Visual Servoing Platform version 3.5.0
vpRobotAfma4.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Irisa's Afma4 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_AFMA4
42
43#include <signal.h>
44#include <stdlib.h>
45#include <sys/types.h>
46#include <unistd.h>
47
48#include <visp3/core/vpDebug.h>
49#include <visp3/core/vpExponentialMap.h>
50#include <visp3/core/vpIoTools.h>
51#include <visp3/core/vpThetaUVector.h>
52#include <visp3/core/vpVelocityTwistMatrix.h>
53#include <visp3/robot/vpRobotAfma4.h>
54#include <visp3/robot/vpRobotException.h>
55
56/* ---------------------------------------------------------------------- */
57/* --- STATIC ----------------------------------------------------------- */
58/* ---------------------------------------------------------------------- */
59
60bool vpRobotAfma4::robotAlreadyCreated = false;
61
70
71/* ---------------------------------------------------------------------- */
72/* --- EMERGENCY STOP --------------------------------------------------- */
73/* ---------------------------------------------------------------------- */
74
82void emergencyStopAfma4(int signo)
83{
84 std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
85 switch (signo) {
86 case SIGINT:
87 std::cout << "SIGINT (stop by ^C) " << std::endl;
88 break;
89 case SIGBUS:
90 std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
91 break;
92 case SIGSEGV:
93 std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
94 break;
95 case SIGKILL:
96 std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
97 break;
98 case SIGQUIT:
99 std::cout << "SIGQUIT " << std::endl;
100 break;
101 default:
102 std::cout << signo << std::endl;
103 }
104 // std::cout << "Emergency stop called\n";
105 // PrimitiveESTOP_Afma4();
106 PrimitiveSTOP_Afma4();
107 std::cout << "Robot was stopped\n";
108
109 // Free allocated resources
110 // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
111
112 fprintf(stdout, "Application ");
113 fflush(stdout);
114 kill(getpid(), SIGKILL);
115 exit(1);
116}
117
118/* ---------------------------------------------------------------------- */
119/* --- CONSTRUCTOR ------------------------------------------------------ */
120/* ---------------------------------------------------------------------- */
121
133vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
134{
135
136 /*
137 #define SIGHUP 1 // hangup
138 #define SIGINT 2 // interrupt (rubout)
139 #define SIGQUIT 3 // quit (ASCII FS)
140 #define SIGILL 4 // illegal instruction (not reset when caught)
141 #define SIGTRAP 5 // trace trap (not reset when caught)
142 #define SIGIOT 6 // IOT instruction
143 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
144 #define SIGEMT 7 // EMT instruction
145 #define SIGFPE 8 // floating point exception
146 #define SIGKILL 9 // kill (cannot be caught or ignored)
147 #define SIGBUS 10 // bus error
148 #define SIGSEGV 11 // segmentation violation
149 #define SIGSYS 12 // bad argument to system call
150 #define SIGPIPE 13 // write on a pipe with no one to read it
151 #define SIGALRM 14 // alarm clock
152 #define SIGTERM 15 // software termination signal from kill
153 */
154
155 signal(SIGINT, emergencyStopAfma4);
156 signal(SIGBUS, emergencyStopAfma4);
157 signal(SIGSEGV, emergencyStopAfma4);
158 signal(SIGKILL, emergencyStopAfma4);
159 signal(SIGQUIT, emergencyStopAfma4);
160
161 setVerbose(verbose);
162 if (verbose_)
163 std::cout << "Open communication with MotionBlox.\n";
164 try {
165 this->init();
167 } catch (...) {
168 // vpERROR_TRACE("Error caught") ;
169 throw;
170 }
171 positioningVelocity = defaultPositioningVelocity;
172
173 vpRobotAfma4::robotAlreadyCreated = true;
174
175 return;
176}
177
178/* ------------------------------------------------------------------------ */
179/* --- INITIALISATION ----------------------------------------------------- */
180/* ------------------------------------------------------------------------ */
181
191{
192 int stt;
193 InitTry;
194
195 // Initialise private variables used to compute the measured velocities
196 q_prev_getvel.resize(4);
197 q_prev_getvel = 0;
198 time_prev_getvel = 0;
199 first_time_getvel = true;
200
201 // Initialise private variables used to compute the measured displacement
202 q_prev_getdis.resize(4);
203 q_prev_getdis = 0;
204 first_time_getdis = true;
205
206 // Initialize the firewire connection
207 Try(stt = InitializeConnection(verbose_));
208
209 if (stt != SUCCESS) {
210 vpERROR_TRACE("Cannot open connection with the motionblox.");
211 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
212 }
213
214 // Connect to the servoboard using the servo board GUID
215 Try(stt = InitializeNode_Afma4());
216
217 if (stt != SUCCESS) {
218 vpERROR_TRACE("Cannot open connection with the motionblox.");
219 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
220 }
221 Try(PrimitiveRESET_Afma4());
222
223 // Look if the power is on or off
224 UInt32 HIPowerStatus;
225 UInt32 EStopStatus;
226 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
227 CAL_Wait(0.1);
228
229 // Print the robot status
230 if (verbose_) {
231 std::cout << "Robot status: ";
232 switch (EStopStatus) {
233 case ESTOP_AUTO:
234 case ESTOP_MANUAL:
235 if (HIPowerStatus == 0)
236 std::cout << "Power is OFF" << std::endl;
237 else
238 std::cout << "Power is ON" << std::endl;
239 break;
240 case ESTOP_ACTIVATED:
241 std::cout << "Emergency stop is activated" << std::endl;
242 break;
243 default:
244 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
245 std::cout << "You have to call Adept for maintenance..." << std::endl;
246 // Free allocated resources
247 }
248 std::cout << std::endl;
249 }
250 // get real joint min/max from the MotionBlox
251 Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
252 // for (unsigned int i=0; i < njoint; i++) {
253 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
254 // _joint_max[i]);
255 // }
256
257 // If an error occur in the low level controller, goto here
258 // CatchPrint();
259 Catch();
260
261 // Test if an error occurs
262 if (TryStt == -20001)
263 printf("No connection detected. Check if the robot is powered on \n"
264 "and if the firewire link exist between the MotionBlox and this "
265 "computer.\n");
266 else if (TryStt == -675)
267 printf(" Timeout enabling power...\n");
268
269 if (TryStt < 0) {
270 // Power off the robot
271 PrimitivePOWEROFF_Afma4();
272 // Free allocated resources
273 ShutDownConnection();
274
275 std::cout << "Cannot open connection with the motionblox..." << std::endl;
276 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
277 }
278 return;
279}
280
281/* ------------------------------------------------------------------------ */
282/* --- DESTRUCTOR --------------------------------------------------------- */
283/* ------------------------------------------------------------------------ */
284
292{
293 InitTry;
294
296
297 // Look if the power is on or off
298 UInt32 HIPowerStatus;
299 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
300 CAL_Wait(0.1);
301
302 // if (HIPowerStatus == 1) {
303 // fprintf(stdout, "Power OFF the robot\n");
304 // fflush(stdout);
305
306 // Try( PrimitivePOWEROFF_Afma4() );
307 // }
308
309 // Free allocated resources
310 ShutDownConnection();
311
312 vpRobotAfma4::robotAlreadyCreated = false;
313
314 CatchPrint();
315 return;
316}
317
325{
326 InitTry;
327
328 switch (newState) {
329 case vpRobot::STATE_STOP: {
331 Try(PrimitiveSTOP_Afma4());
332 }
333 break;
334 }
337 std::cout << "Change the control mode from velocity to position control.\n";
338 Try(PrimitiveSTOP_Afma4());
339 } else {
340 // std::cout << "Change the control mode from stop to position
341 // control.\n";
342 }
343 this->powerOn();
344 break;
345 }
348 std::cout << "Change the control mode from stop to velocity control.\n";
349 }
350 this->powerOn();
351 break;
352 }
353 default:
354 break;
355 }
356
357 CatchPrint();
358
359 return vpRobot::setRobotState(newState);
360}
361
362/* ------------------------------------------------------------------------ */
363/* --- STOP --------------------------------------------------------------- */
364/* ------------------------------------------------------------------------ */
365
374{
375 InitTry;
376 Try(PrimitiveSTOP_Afma4());
378
379 CatchPrint();
380 if (TryStt < 0) {
381 vpERROR_TRACE("Cannot stop robot motion");
382 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
383 }
384}
385
396{
397 InitTry;
398
399 // Look if the power is on or off
400 UInt32 HIPowerStatus;
401 UInt32 EStopStatus;
402 bool firsttime = true;
403 unsigned int nitermax = 10;
404
405 for (unsigned int i = 0; i < nitermax; i++) {
406 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
407 if (EStopStatus == ESTOP_AUTO) {
408 break; // exit for loop
409 } else if (EStopStatus == ESTOP_MANUAL) {
410 break; // exit for loop
411 } else if (EStopStatus == ESTOP_ACTIVATED) {
412 if (firsttime) {
413 std::cout << "Emergency stop is activated! \n"
414 << "Check the emergency stop button and push the yellow "
415 "button before continuing."
416 << std::endl;
417 firsttime = false;
418 }
419 fprintf(stdout, "Remaining time %us \r", nitermax - i);
420 fflush(stdout);
421 CAL_Wait(1);
422 } else {
423 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
424 std::cout << "You have to call Adept for maintenance..." << std::endl;
425 // Free allocated resources
426 ShutDownConnection();
427 throw(vpRobotException(vpRobotException::lowLevelError, "Error in the emergency chain"));
428 }
429 }
430
431 if (EStopStatus == ESTOP_ACTIVATED)
432 std::cout << std::endl;
433
434 if (EStopStatus == ESTOP_ACTIVATED) {
435 std::cout << "Sorry, cannot power on the robot." << std::endl;
436 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
437 }
438
439 if (HIPowerStatus == 0) {
440 fprintf(stdout, "Power ON the Afma4 robot\n");
441 fflush(stdout);
442
443 Try(PrimitivePOWERON_Afma4());
444 }
445
446 CatchPrint();
447 if (TryStt < 0) {
448 vpERROR_TRACE("Cannot power on the robot");
449 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
450 }
451}
452
463{
464 InitTry;
465
466 // Look if the power is on or off
467 UInt32 HIPowerStatus;
468 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
469 CAL_Wait(0.1);
470
471 if (HIPowerStatus == 1) {
472 fprintf(stdout, "Power OFF the Afma4 robot\n");
473 fflush(stdout);
474
475 Try(PrimitivePOWEROFF_Afma4());
476 }
477
478 CatchPrint();
479 if (TryStt < 0) {
480 vpERROR_TRACE("Cannot power off the robot");
481 throw vpRobotException(vpRobotException::communicationError, "Cannot power off the robot.");
482 }
483}
484
497{
498 InitTry;
499 bool status = false;
500 // Look if the power is on or off
501 UInt32 HIPowerStatus;
502 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
503 CAL_Wait(0.1);
504
505 if (HIPowerStatus == 1) {
506 status = true;
507 }
508
509 CatchPrint();
510 if (TryStt < 0) {
511 vpERROR_TRACE("Cannot get the power status");
512 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
513 }
514 return status;
515}
516
527{
529 vpAfma4::get_cMe(cMe);
530
531 cVe.buildFrom(cMe);
532}
533
544{
545 double position[this->njoint];
546 double timestamp;
547
548 InitTry;
549 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
550 CatchPrint();
551
552 vpColVector q(this->njoint);
553 for (unsigned int i = 0; i < njoint; i++)
554 q[i] = position[i];
555
556 try {
557 vpAfma4::get_cVf(q, cVf);
558 } catch (...) {
559 vpERROR_TRACE("catch exception ");
560 throw;
561 }
562}
563
575
589{
590
591 double position[this->njoint];
592 double timestamp;
593
594 InitTry;
595 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
596 CatchPrint();
597
598 vpColVector q(this->njoint);
599 for (unsigned int i = 0; i < njoint; i++)
600 q[i] = position[i];
601
602 try {
604 } catch (...) {
605 vpERROR_TRACE("catch exception ");
606 throw;
607 }
608}
609
625{
626 double position[6];
627 double timestamp;
628
629 InitTry;
630 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
631 CatchPrint();
632
633 vpColVector q(6);
634 for (unsigned int i = 0; i < njoint; i++)
635 q[i] = position[i];
636
637 try {
639 } catch (...) {
640 vpERROR_TRACE("Error caught");
641 throw;
642 }
643}
672void vpRobotAfma4::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
673
679double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; }
680
754{
755
757 vpERROR_TRACE("Robot was not in position-based control\n"
758 "Modification of the robot state");
760 }
761
762 int error = 0;
763
764 switch (frame) {
766 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
767 "Reference frame not implemented.");
768 break;
770 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
771 "Camera frame not implemented.");
772 break;
774 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
775 "Mixt frame not implemented.");
776 break;
778 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
779 "End-effector frame not implemented.");
780 break;
781
783 break;
784 }
785 }
786 if (position.getRows() != this->njoint) {
787 vpERROR_TRACE("Positionning error: bad vector dimension.");
788 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension.");
789 }
790
791 InitTry;
792
793 Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
794 Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
795
796 CatchPrint();
797 if (TryStt == InvalidPosition || TryStt == -1023)
798 std::cout << " : Position out of range.\n";
799 else if (TryStt < 0)
800 std::cout << " : Unknown error (see Fabien).\n";
801 else if (error == -1)
802 std::cout << "Position out of range.\n";
803
804 if (TryStt < 0 || error < 0) {
805 vpERROR_TRACE("Positionning error.");
806 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
807 }
808
809 return;
810}
811
862void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
863 const double q4, const double q5)
864{
865 try {
866 vpColVector position(this->njoint);
867 position[0] = q1;
868 position[1] = q2;
869 position[2] = q4;
870 position[3] = q5;
871
872 setPosition(frame, position);
873 } catch (...) {
874 vpERROR_TRACE("Error caught");
875 throw;
876 }
877}
878
908void vpRobotAfma4::setPosition(const char *filename)
909{
910 vpColVector q;
911 bool ret;
912
913 ret = this->readPosFile(filename, q);
914
915 if (ret == false) {
916 vpERROR_TRACE("Bad position in \"%s\"", filename);
917 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
918 }
921}
922
928{
929 double timestamp;
930 PrimitiveACQ_TIME_Afma4(&timestamp);
931 return timestamp;
932}
933
994void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
995{
996
997 InitTry;
998
999 position.resize(this->njoint);
1000
1001 switch (frame) {
1002 case vpRobot::CAMERA_FRAME: {
1003 position = 0;
1004 return;
1005 }
1007 double _q[njoint];
1008 Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1009 for (unsigned int i = 0; i < this->njoint; i++) {
1010 position[i] = _q[i];
1011 }
1012
1013 return;
1014 }
1016 double _q[njoint];
1017 Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1018
1019 vpColVector q(this->njoint);
1020 for (unsigned int i = 0; i < this->njoint; i++)
1021 q[i] = _q[i];
1022
1023 // Compute fMc
1025 vpAfma4::get_fMc(q, fMc);
1026
1027 // From fMc extract the pose
1028 vpRotationMatrix fRc;
1029 fMc.extract(fRc);
1030 vpRxyzVector rxyz;
1031 rxyz.buildFrom(fRc);
1032
1033 for (unsigned int i = 0; i < 3; i++) {
1034 position[i] = fMc[i][3]; // translation x,y,z
1035 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1036 }
1037 break;
1038 }
1039 case vpRobot::MIXT_FRAME: {
1040 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1041 "not implemented");
1042 }
1044 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1045 "not implemented");
1046 }
1047 }
1048
1049 CatchPrint();
1050 if (TryStt < 0) {
1051 vpERROR_TRACE("Cannot get position.");
1052 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1053 }
1054
1055 return;
1056}
1057
1069{
1070 double timestamp;
1071 getPosition(frame, position, timestamp);
1072}
1073
1128{
1129
1131 vpERROR_TRACE("Cannot send a velocity to the robot "
1132 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1134 "Cannot send a velocity to the robot "
1135 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1136 }
1137
1138 // Check the dimension of the velocity vector to see if it is
1139 // compatible with the requested frame
1140 switch (frame) {
1141 case vpRobot::CAMERA_FRAME: {
1142 // if (vel.getRows() != 2) {
1143 if (vel.getRows() != 6) {
1144 vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1145 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1146 "in camera frame");
1147 }
1148 break;
1149 }
1151 if (vel.getRows() != this->njoint) {
1152 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1153 "velocity vector ");
1154 }
1155 break;
1156 }
1158 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1159 "in the reference frame:"
1160 "functionality not implemented");
1161 }
1162 case vpRobot::MIXT_FRAME: {
1163 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1164 "in the mixt frame:"
1165 "functionality not implemented");
1166 }
1168 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1169 "in the end-effector frame:"
1170 "functionality not implemented");
1171 }
1172 default: {
1173 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
1174 }
1175 }
1176
1177 //
1178 // Velocities saturation with normalization
1179 //
1180 vpColVector joint_vel(this->njoint);
1181
1182 // Case of the camera frame where we control only 2 dof
1183 if (frame == vpRobot::CAMERA_FRAME) {
1184 vpColVector vel_max(6);
1185
1186 for (unsigned int i = 0; i < 3; i++) {
1187 vel_max[i] = getMaxTranslationVelocity();
1188 vel_max[i + 3] = getMaxRotationVelocity();
1189 }
1190
1191 vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1192
1193#if 0 // ok
1194 vpMatrix eJe(4,6);
1195 eJe = 0;
1196 eJe[2][4] = -1;
1197 eJe[3][3] = 1;
1198
1199 joint_vel = eJe * velocity; // Compute the articular velocity
1200#endif
1201 vpColVector q;
1203 vpMatrix fJe_inverse;
1204 get_fJe_inverse(q, fJe_inverse);
1206 get_fMe(q, fMe);
1208 t = 0;
1209 vpRotationMatrix fRe;
1210 fMe.extract(fRe);
1211 vpVelocityTwistMatrix fVe(t, fRe);
1212 // compute the inverse jacobian in the end-effector frame
1213 vpMatrix eJe_inverse = fJe_inverse * fVe;
1214
1215 // Transform the velocities from camera to end-effector frame
1217 eVc.buildFrom(this->_eMc);
1218 joint_vel = eJe_inverse * eVc * velocity;
1219
1220 // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1221 // joint_vel[2], joint_vel[3]);
1222 }
1223
1224 // Case of the joint control where we control all the joints
1225 else if (frame == vpRobot::ARTICULAR_FRAME) {
1226
1227 vpColVector vel_max(4);
1228
1229 vel_max[0] = getMaxRotationVelocity();
1230 vel_max[1] = getMaxTranslationVelocity();
1231 vel_max[2] = getMaxRotationVelocity();
1232 vel_max[3] = getMaxRotationVelocity();
1233
1234 joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1235 }
1236
1237 InitTry;
1238
1239 // Send a joint velocity to the low level controller
1240 Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
1241
1242 Catch();
1243 if (TryStt < 0) {
1244 if (TryStt == VelStopOnJoint) {
1245 UInt32 axisInJoint[njoint];
1246 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1247 for (unsigned int i = 0; i < njoint; i++) {
1248 if (axisInJoint[i])
1249 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1250 }
1251 } else {
1252 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1253 if (TryString != NULL) {
1254 // The statement is in TryString, but we need to check the validity
1255 printf(" Error sentence %s\n", TryString); // Print the TryString
1256 } else {
1257 printf("\n");
1258 }
1259 }
1260 }
1261
1262 return;
1263}
1264
1265/* ------------------------------------------------------------------------ */
1266/* --- GET ---------------------------------------------------------------- */
1267/* ------------------------------------------------------------------------ */
1268
1316void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1317{
1318
1319 switch (frame) {
1321 velocity.resize(this->njoint);
1322 break;
1323 default:
1324 velocity.resize(6);
1325 }
1326
1327 velocity = 0;
1328
1329 double q[4];
1330 vpColVector q_cur(4);
1331 vpHomogeneousMatrix fMc_cur;
1332 vpHomogeneousMatrix cMc; // camera displacement
1333 double time_cur;
1334
1335 InitTry;
1336
1337 // Get the current joint position
1338 Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1339 time_cur = timestamp;
1340
1341 for (unsigned int i = 0; i < this->njoint; i++) {
1342 q_cur[i] = q[i];
1343 }
1344
1345 // Get the camera pose from the direct kinematics
1346 vpAfma4::get_fMc(q_cur, fMc_cur);
1347
1348 if (!first_time_getvel) {
1349
1350 switch (frame) {
1351 case vpRobot::CAMERA_FRAME: {
1352 // Compute the displacement of the camera since the previous call
1353 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1354
1355 // Compute the velocity of the camera from this displacement
1356 velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1357
1358 break;
1359 }
1360
1362 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1363 break;
1364 }
1365
1367 // Compute the displacement of the camera since the previous call
1368 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1369
1370 // Compute the velocity of the camera from this displacement
1371 vpColVector v;
1372 v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1373
1374 // Express this velocity in the reference frame
1375 vpVelocityTwistMatrix fVc(fMc_cur);
1376 velocity = fVc * v;
1377
1378 break;
1379 }
1380
1381 case vpRobot::MIXT_FRAME: {
1382 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1383 "functionality not implemented");
1384
1385 break;
1386 }
1388 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
1389 "functionality not implemented");
1390
1391 break;
1392 }
1393 }
1394 } else {
1395 first_time_getvel = false;
1396 }
1397
1398 // Memorize the camera pose for the next call
1399 fMc_prev_getvel = fMc_cur;
1400
1401 // Memorize the joint position for the next call
1402 q_prev_getvel = q_cur;
1403
1404 // Memorize the time associated to the joint position for the next call
1405 time_prev_getvel = time_cur;
1406
1407 CatchPrint();
1408 if (TryStt < 0) {
1409 vpERROR_TRACE("Cannot get velocity.");
1410 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1411 }
1412}
1413
1423{
1424 double timestamp;
1425 getVelocity(frame, velocity, timestamp);
1426}
1427
1468{
1469 vpColVector velocity;
1470 getVelocity(frame, velocity, timestamp);
1471
1472 return velocity;
1473}
1474
1484{
1485 vpColVector velocity;
1486 double timestamp;
1487 getVelocity(frame, velocity, timestamp);
1488
1489 return velocity;
1490}
1491
1543bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1544{
1545 std::ifstream fd(filename.c_str(), std::ios::in);
1546
1547 if (!fd.is_open()) {
1548 return false;
1549 }
1550
1551 std::string line;
1552 std::string key("R:");
1553 std::string id("#AFMA4 - Position");
1554 bool pos_found = false;
1555 int lineNum = 0;
1556
1557 q.resize(njoint);
1558
1559 while (std::getline(fd, line)) {
1560 lineNum++;
1561 if (lineNum == 1) {
1562 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1563 std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1564 return false;
1565 }
1566 }
1567 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1568 continue;
1569 }
1570 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1571 // check if there are at least njoint values in the line
1572 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1573 if (chain.size() < njoint + 1) // try to split with tab separator
1574 chain = vpIoTools::splitChain(line, std::string("\t"));
1575 if (chain.size() < njoint + 1)
1576 continue;
1577
1578 std::istringstream ss(line);
1579 std::string key_;
1580 ss >> key_;
1581 for (unsigned int i = 0; i < njoint; i++)
1582 ss >> q[i];
1583 pos_found = true;
1584 break;
1585 }
1586 }
1587
1588 // converts rotations from degrees into radians
1589 q[0] = vpMath::rad(q[0]);
1590 q[2] = vpMath::rad(q[2]);
1591 q[3] = vpMath::rad(q[3]);
1592
1593 fd.close();
1594
1595 if (!pos_found) {
1596 std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1597 return false;
1598 }
1599
1600 return true;
1601}
1602
1626bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1627{
1628
1629 FILE *fd;
1630 fd = fopen(filename.c_str(), "w");
1631 if (fd == NULL)
1632 return false;
1633
1634 fprintf(fd, "\
1635#AFMA4 - Position - Version 2.01\n\
1636#\n\
1637# R: X Y A B\n\
1638# Joint position: X : rotation of the turret in degrees (joint 1)\n\
1639# Y : vertical translation in meters (joint 2)\n\
1640# A : pan rotation of the camera in degrees (joint 4)\n\
1641# B : tilt rotation of the camera in degrees (joint 5)\n\
1642#\n\n");
1643
1644 // Save positions in mm and deg
1645 fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1646
1647 fclose(fd);
1648 return (true);
1649}
1650
1661void vpRobotAfma4::move(const char *filename)
1662{
1663 vpColVector q;
1664
1665 this->readPosFile(filename, q);
1667 this->setPositioningVelocity(10);
1669}
1670
1690{
1691 displacement.resize(6);
1692 displacement = 0;
1693
1694 double q[6];
1695 vpColVector q_cur(6);
1696 double timestamp;
1697
1698 InitTry;
1699
1700 // Get the current joint position
1701 Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1702 for (unsigned int i = 0; i < njoint; i++) {
1703 q_cur[i] = q[i];
1704 }
1705
1706 if (!first_time_getdis) {
1707 switch (frame) {
1708 case vpRobot::CAMERA_FRAME: {
1709 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1710 return;
1711 }
1712
1714 displacement = q_cur - q_prev_getdis;
1715 break;
1716 }
1717
1719 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1720 return;
1721 }
1722
1723 case vpRobot::MIXT_FRAME: {
1724 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1725 return;
1726 }
1728 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1729 return;
1730 }
1731 }
1732 } else {
1733 first_time_getdis = false;
1734 }
1735
1736 // Memorize the joint position for the next call
1737 q_prev_getdis = q_cur;
1738
1739 CatchPrint();
1740 if (TryStt < 0) {
1741 vpERROR_TRACE("Cannot get velocity.");
1742 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1743 }
1744}
1745
1746#elif !defined(VISP_BUILD_SHARED_LIBS)
1747// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
1748// symbols
1749void dummy_vpRobotAfma4(){};
1750#endif
Modelisation of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:111
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:450
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:176
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:309
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:395
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:506
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:348
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:259
double _joint_min[4]
Definition: vpAfma4.h:150
double _joint_max[4]
Definition: vpAfma4.h:149
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_cVf(vpVelocityTwistMatrix &cVf) const
virtual ~vpRobotAfma4(void)
double getPositioningVelocity(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
bool getPowerState()
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_fJe(vpMatrix &fJe)
void move(const char *filename)
static const double defaultPositioningVelocity
Definition: vpRobotAfma4.h:218
void get_eJe(vpMatrix &eJe)
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void init(void)
double getTime() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Error that can be emited by the vpRobot class and its derivates.
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void setVerbose(bool verbose)
Definition: vpRobot.h:159
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393