42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_AFMA4
48 #include <sys/types.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpExponentialMap.h>
53 #include <visp/vpDebug.h>
54 #include <visp/vpVelocityTwistMatrix.h>
55 #include <visp/vpThetaUVector.h>
56 #include <visp/vpRobotAfma4.h>
62 bool vpRobotAfma4::robotAlreadyCreated =
false;
85 void emergencyStopAfma4(
int signo)
87 std::cout <<
"Stop the Afma4 application by signal ("
88 << signo <<
"): " << (char)7 ;
92 std::cout <<
"SIGINT (stop by ^C) " << std::endl ; break ;
94 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl ; break ;
96 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
98 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
100 std::cout <<
"SIGQUIT " << std::endl ; break ;
102 std::cout << signo << std::endl ;
106 PrimitiveSTOP_Afma4();
107 std::cout <<
"Robot was stopped\n";
112 fprintf(stdout,
"Application ");
114 kill(getpid(), SIGKILL);
134 vpRobotAfma4::vpRobotAfma4 (
bool verbose)
159 signal(SIGINT, emergencyStopAfma4);
160 signal(SIGBUS, emergencyStopAfma4) ;
161 signal(SIGSEGV, emergencyStopAfma4) ;
162 signal(SIGKILL, emergencyStopAfma4);
163 signal(SIGQUIT, emergencyStopAfma4);
167 std::cout <<
"Open communication with MotionBlox.\n";
178 vpRobotAfma4::robotAlreadyCreated =
true;
205 time_prev_getvel = 0;
206 first_time_getvel =
true;
211 first_time_getdis =
true;
214 Try( stt = InitializeConnection(
verbose_) );
216 if (stt != SUCCESS) {
217 vpERROR_TRACE (
"Cannot open connexion with the motionblox.");
219 "Cannot open connexion with the motionblox");
223 Try( stt = InitializeNode_Afma4() );
225 if (stt != SUCCESS) {
226 vpERROR_TRACE (
"Cannot open connexion with the motionblox.");
228 "Cannot open connexion with the motionblox");
230 Try( PrimitiveRESET_Afma4() );
233 UInt32 HIPowerStatus;
235 Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
241 std::cout <<
"Robot status: ";
242 switch(EStopStatus) {
245 if (HIPowerStatus == 0)
246 std::cout <<
"Power is OFF" << std::endl;
248 std::cout <<
"Power is ON" << std::endl;
250 case ESTOP_ACTIVATED:
251 std::cout <<
"Emergency stop is activated" << std::endl;
254 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
255 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
258 std::cout << std::endl;
271 if (TryStt == -20001)
272 printf(
"No connection detected. Check if the robot is powered on \n"
273 "and if the firewire link exist between the MotionBlox and this computer.\n");
274 else if (TryStt == -675)
275 printf(
" Timeout enabling power...\n");
279 PrimitivePOWEROFF_Afma4();
281 ShutDownConnection();
283 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
285 "Cannot open connexion with the motionblox");
308 UInt32 HIPowerStatus;
309 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
321 ShutDownConnection();
323 vpRobotAfma4::robotAlreadyCreated =
false;
347 Try( PrimitiveSTOP_Afma4() );
353 std::cout <<
"Change the control mode from velocity to position control.\n";
354 Try( PrimitiveSTOP_Afma4() );
364 std::cout <<
"Change the control mode from stop to velocity control.\n";
393 Try( PrimitiveSTOP_Afma4() );
400 "Cannot stop robot motion.");
420 UInt32 HIPowerStatus;
422 bool firsttime =
true;
423 unsigned int nitermax = 10;
425 for (
unsigned int i=0; i<nitermax; i++) {
426 Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
428 if (EStopStatus == ESTOP_AUTO) {
431 else if (EStopStatus == ESTOP_MANUAL) {
434 else if (EStopStatus == ESTOP_ACTIVATED) {
436 std::cout <<
"Emergency stop is activated! \n"
437 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
440 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
445 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
446 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
448 ShutDownConnection();
453 if (EStopStatus == ESTOP_ACTIVATED)
454 std::cout << std::endl;
456 if (EStopStatus == ESTOP_ACTIVATED) {
457 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
459 "Cannot power on the robot.");
462 if (HIPowerStatus == 0) {
463 fprintf(stdout,
"Power ON the Afma4 robot\n");
466 Try( PrimitivePOWERON_Afma4() );
473 "Cannot power off the robot.");
492 UInt32 HIPowerStatus;
493 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
497 if (HIPowerStatus == 1) {
498 fprintf(stdout,
"Power OFF the Afma4 robot\n");
501 Try( PrimitivePOWEROFF_Afma4() );
508 "Cannot power off the robot.");
529 UInt32 HIPowerStatus;
530 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
534 if (HIPowerStatus == 1) {
542 "Cannot get the power status.");
577 double position[this->
njoint];
581 Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
585 for (
unsigned int i=0; i <
njoint; i++)
628 double position[this->
njoint];
632 Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
636 for (
unsigned int i=0; i <
njoint; i++)
668 Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
672 for (
unsigned int i=0; i <
njoint; i++)
714 positioningVelocity = velocity;
725 return positioningVelocity;
807 "Modification of the robot state");
815 vpERROR_TRACE (
"Positionning error. Reference frame not implemented");
817 "Positionning error: "
818 "Reference frame not implemented.");
821 vpERROR_TRACE (
"Positionning error. Camera frame not implemented");
823 "Positionning error: "
824 "Camera frame not implemented.");
827 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
829 "Positionning error: "
830 "Mixt frame not implemented.");
841 "Positionning error: bad vector dimension.");
846 Try( PrimitiveMOVE_Afma4(position.
data, positioningVelocity) );
847 Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
850 if (TryStt == InvalidPosition || TryStt == -1023)
851 std::cout <<
" : Position out of range.\n";
853 std::cout <<
" : Unknown error (see Fabien).\n";
854 else if (error == -1)
855 std::cout <<
"Position out of range.\n";
857 if (TryStt < 0 || error < 0) {
860 "Position out of range.");
980 "Bad position in filename.");
992 PrimitiveACQ_TIME_Afma4(×tamp);
1071 Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
1072 for (
unsigned int i=0; i < this->
njoint; i ++) {
1073 position[i] = _q[i];
1080 Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
1083 for (
unsigned int i=0; i < this->
njoint; i++)
1096 for (
unsigned int i=0; i < 3; i++) {
1097 position[i] = fMc[i][3];
1098 position[i+3] = rxyz[i];
1103 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1105 "Cannot get position in mixt frame: "
1115 "Cannot get position.");
1191 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1193 "Cannot send a velocity to the robot "
1194 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1203 vpERROR_TRACE (
"Bad dimension of the velocity vector in camera frame");
1205 "Bad dimension of the velocity vector "
1212 vpERROR_TRACE (
"Bad dimension of the articular velocity vector");
1214 "Bad dimension of the articular "
1215 "velocity vector ");
1221 "in the reference frame: "
1222 "functionality not implemented");
1224 "Cannot send a velocity to the robot "
1225 "in the reference frame:"
1226 "functionality not implemented");
1231 "in the mixt frame: "
1232 "functionality not implemented");
1234 "Cannot send a velocity to the robot "
1235 "in the mixt frame:"
1236 "functionality not implemented");
1241 "Case not taken in account.");
1243 "Cannot send a velocity to the robot ");
1257 for (
unsigned int i=0; i<3; i++) {
1270 joint_vel = eJe * velocity;
1284 vpMatrix eJe_inverse = fJe_inverse * fVe;
1289 joint_vel = eJe_inverse * eVc * velocity;
1311 Try( PrimitiveMOVESPEED_Afma4(joint_vel.
data) );
1315 if (TryStt == VelStopOnJoint) {
1316 UInt32 axisInJoint[
njoint];
1317 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1318 for (
unsigned int i=0; i <
njoint; i ++) {
1320 std::cout <<
"\nWarning: Velocity control stopped: axis "
1321 << i+1 <<
" on joint limit!" <<std::endl;
1325 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1326 if (TryString != NULL) {
1328 printf(
" Error sentence %s\n", TryString);
1414 Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
1415 time_cur = timestamp;
1417 for (
unsigned int i=0; i < this->
njoint; i ++) {
1424 if ( ! first_time_getvel ) {
1429 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1438 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1444 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1459 "functionality not implemented");
1461 "Cannot get a displacement in the mixt frame:"
1462 "functionality not implemented");
1469 first_time_getvel =
false;
1473 fMc_prev_getvel = fMc_cur;
1476 q_prev_getvel = q_cur;
1479 time_prev_getvel = time_cur;
1486 "Cannot get velocity.");
1627 fd = fopen(filename,
"r") ;
1631 char line[FILENAME_MAX];
1632 char dummy[FILENAME_MAX];
1634 bool sortie =
false;
1638 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1639 if ( strncmp (line,
"#", 1) != 0) {
1641 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1653 while ( sortie !=
true );
1657 sscanf(line,
"%s %lf %lf %lf %lf",
1659 &q[0], &q[1], &q[2], &q[3]);
1699 fd = fopen(filename,
"w") ;
1704 #AFMA4 - Position - Version 2.01\n\
1707 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1708 # Y : vertical translation in meters (joint 2)\n\
1709 # A : pan rotation of the camera in degrees (joint 4)\n\
1710 # B : tilt rotation of the camera in degrees (joint 5)\n\
1714 fprintf(fd,
"R: %lf %lf %lf %lf\n",
1759 vpRobotAfma4::getCameraDisplacement(
vpColVector &displacement)
1775 vpRobotAfma4::getArticularDisplacement(
vpColVector &displacement)
1814 Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
1815 for (
unsigned int i=0; i <
njoint; i ++) {
1819 if ( ! first_time_getdis ) {
1822 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1828 displacement = q_cur - q_prev_getdis;
1833 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1839 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1846 first_time_getdis =
false;
1850 q_prev_getdis = q_cur;
1856 "Cannot get velocity.");