42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_VIPER850
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/vpRobot.h>
57 #include <visp/vpRobotViper850.h>
63 bool vpRobotViper850::robotAlreadyCreated =
false;
85 void emergencyStopViper850(
int signo)
87 std::cout <<
"Stop the Viper850 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_Viper850();
107 std::cout <<
"Robot was stopped\n";
112 fprintf(stdout,
"Application ");
114 kill(getpid(), SIGKILL);
177 vpRobotViper850::vpRobotViper850 (
bool verbose)
202 signal(SIGINT, emergencyStopViper850);
203 signal(SIGBUS, emergencyStopViper850) ;
204 signal(SIGSEGV, emergencyStopViper850) ;
205 signal(SIGKILL, emergencyStopViper850);
206 signal(SIGQUIT, emergencyStopViper850);
210 std::cout <<
"Open communication with MotionBlox.\n";
221 vpRobotViper850::robotAlreadyCreated =
true;
258 time_prev_getvel = 0;
259 first_time_getvel =
true;
264 first_time_getdis =
true;
268 Try( InitializeConnection(
verbose_) );
271 Try( InitializeNode_Viper850() );
273 Try( PrimitiveRESET_Viper850() );
276 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
282 UInt32 HIPowerStatus;
284 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
290 std::cout <<
"Robot status: ";
291 switch(EStopStatus) {
294 if (HIPowerStatus == 0)
295 std::cout <<
"Power is OFF" << std::endl;
297 std::cout <<
"Power is ON" << std::endl;
302 if (HIPowerStatus == 0)
303 std::cout <<
"Power is OFF" << std::endl;
305 std::cout <<
"Power is ON" << std::endl;
307 case ESTOP_ACTIVATED:
309 std::cout <<
"Emergency stop is activated" << std::endl;
312 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
313 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
316 std::cout << std::endl;
333 if (TryStt == -20001)
334 printf(
"No connection detected. Check if the robot is powered on \n"
335 "and if the firewire link exist between the MotionBlox and this computer.\n");
336 else if (TryStt == -675)
337 printf(
" Timeout enabling power...\n");
341 PrimitivePOWEROFF_Viper850();
343 ShutDownConnection();
345 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
347 "Cannot open connexion with the motionblox");
421 for (
unsigned int i=0; i < 3; i ++) {
422 eMc_pose[i] =
etc[i];
423 eMc_pose[i+3] =
erc[i];
426 Try( PrimitiveCONST_Viper850(eMc_pose) );
461 UInt32 HIPowerStatus;
462 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
474 ShutDownConnection();
476 vpRobotViper850::robotAlreadyCreated =
false;
500 Try( PrimitiveSTOP_Viper850() );
506 std::cout <<
"Change the control mode from velocity to position control.\n";
507 Try( PrimitiveSTOP_Viper850() );
517 std::cout <<
"Change the control mode from stop to velocity control.\n";
550 Try( PrimitiveSTOP_Viper850() );
557 "Cannot stop robot motion.");
576 UInt32 HIPowerStatus;
578 bool firsttime =
true;
579 unsigned int nitermax = 10;
581 for (
unsigned int i=0; i<nitermax; i++) {
582 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
584 if (EStopStatus == ESTOP_AUTO) {
588 else if (EStopStatus == ESTOP_MANUAL) {
592 else if (EStopStatus == ESTOP_ACTIVATED) {
595 std::cout <<
"Emergency stop is activated! \n"
596 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
599 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
604 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
605 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
607 ShutDownConnection();
612 if (EStopStatus == ESTOP_ACTIVATED)
613 std::cout << std::endl;
615 if (EStopStatus == ESTOP_ACTIVATED) {
616 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
618 "Cannot power on the robot.");
621 if (HIPowerStatus == 0) {
622 fprintf(stdout,
"Power ON the Viper850 robot\n");
625 Try( PrimitivePOWERON_Viper850() );
632 "Cannot power off the robot.");
651 UInt32 HIPowerStatus;
652 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
656 if (HIPowerStatus == 1) {
657 fprintf(stdout,
"Power OFF the Viper850 robot\n");
660 Try( PrimitivePOWEROFF_Viper850() );
667 "Cannot power off the robot.");
688 UInt32 HIPowerStatus;
689 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
693 if (HIPowerStatus == 1) {
701 "Cannot get the power status.");
761 Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
765 for (
unsigned int i=0; i <
njoint; i++)
798 Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
802 for (
unsigned int i=0; i <
njoint; i++)
856 positioningVelocity = velocity;
867 return positioningVelocity;
956 "Modification of the robot state");
968 Try( PrimitiveACQ_POS_Viper850(q.
data, ×tamp) );
980 for (
unsigned int i=0; i < 3; i++) {
981 txyz[i] = position[i];
982 rxyz[i] = position[i+3];
998 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
999 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1009 destination = position;
1014 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
1015 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1021 vpRxyzVector rxyz(position[3],position[4],position[5]);
1025 for (
unsigned int i=0; i <3; i++) {
1026 destination[i] = position[i];
1029 int configuration = 0;
1032 Try( PrimitiveMOVE_C_Viper850(destination.
data, configuration,
1033 positioningVelocity) );
1034 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1040 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1042 "Positionning error: "
1043 "Mixt frame not implemented.");
1049 if (TryStt == InvalidPosition || TryStt == -1023)
1050 std::cout <<
" : Position out of range.\n";
1051 else if (TryStt == -3019) {
1053 std::cout <<
" : Joint position out of range.\n";
1055 std::cout <<
" : Cartesian position leads to a joint position out of range.\n";
1057 else if (TryStt < 0)
1058 std::cout <<
" : Unknown error (see Fabien).\n";
1059 else if (error == -1)
1060 std::cout <<
"Position out of range.\n";
1062 if (TryStt < 0 || error < 0) {
1065 "Position out of range.");
1146 position[0] = pos1 ;
1147 position[1] = pos2 ;
1148 position[2] = pos3 ;
1149 position[3] = pos4 ;
1150 position[4] = pos5 ;
1151 position[5] = pos6 ;
1211 "Bad position in filename.");
1299 Try( PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp) );
1306 Try( PrimitiveACQ_POS_C_Viper850(position.
data, ×tamp) );
1310 for (
unsigned int i=3; i <6; i++)
1313 vpRzyzVector rzyz(position[3], position[4], position[5]);
1318 for (
unsigned int i=0; i <3; i++)
1319 position[i+3] = rxyz[i];
1329 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1331 "Cannot get position in mixt frame: "
1341 "Cannot get position.");
1353 PrimitiveACQ_TIME_Viper850(×tamp);
1391 for (
unsigned int j=0;j<3;j++)
1392 RxyzVect[j]=posRxyz[j+3];
1397 for (
unsigned int j=0;j<3;j++)
1399 position[j]=posRxyz[j];
1400 position[j+3]=RtuVect[j];
1421 for (
unsigned int j=0;j<3;j++)
1422 RxyzVect[j]=posRxyz[j+3];
1427 for (
unsigned int j=0;j<3;j++)
1429 position[j]=posRxyz[j];
1430 position[j+3]=RtuVect[j];
1511 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1513 "Cannot send a velocity to the robot "
1514 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1527 for (
unsigned int i=0; i<3; i++)
1529 for (
unsigned int i=3; i<6; i++)
1540 for (
unsigned int i=0; i<6; i++)
1553 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850) );
1561 Try( PrimitiveMOVESPEED_Viper850(vel_sat.
data) );
1566 std::cout <<
"Vitesse ref appliquee: " << vel_sat.
t();
1567 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850) );
1576 "Case not taken in account.");
1583 if (TryStt == VelStopOnJoint) {
1584 UInt32 axisInJoint[
njoint];
1585 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1586 for (
unsigned int i=0; i <
njoint; i ++) {
1588 std::cout <<
"\nWarning: Velocity control stopped: axis "
1589 << i+1 <<
" on joint limit!" <<std::endl;
1593 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1594 if (TryString != NULL) {
1596 printf(
" Error sentence %s\n", TryString);
1688 Try( PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp) );
1689 time_cur = timestamp;
1695 if ( ! first_time_getvel ) {
1700 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1709 velocity = (q_cur - q_prev_getvel)
1710 / (time_cur - time_prev_getvel);
1716 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1731 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1739 for (
unsigned int i=0; i < 3; i++) {
1741 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1743 velocity[i+3] = thetaU[i];
1747 velocity /= (time_cur - time_prev_getvel);
1753 first_time_getvel =
false;
1757 fMc_prev_getvel = fMc_cur;
1760 q_prev_getvel = q_cur;
1763 time_prev_getvel = time_cur;
1770 "Cannot get velocity.");
1934 fd = fopen(filename,
"r") ;
1938 char line[FILENAME_MAX];
1939 char dummy[FILENAME_MAX];
1941 bool sortie =
false;
1945 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1946 if ( strncmp (line,
"#", 1) != 0) {
1948 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1960 while ( sortie !=
true );
1964 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1966 &q[0], &q[1], &q[2],
1967 &q[3], &q[4], &q[5]);
2004 fd = fopen(filename,
"w") ;
2009 #Viper - Position - Version 1.0\n\
2012 # Joint position in degrees\n\
2017 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2069 vpRobotViper850::getCameraDisplacement(
vpColVector &displacement)
2085 vpRobotViper850::getArticularDisplacement(
vpColVector &displacement)
2124 Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
2125 for (
unsigned int i=0; i <
njoint; i ++) {
2129 if ( ! first_time_getdis ) {
2132 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2138 displacement = q_cur - q_prev_getdis;
2143 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2149 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2156 first_time_getdis =
false;
2160 q_prev_getdis = q_cur;
2166 "Cannot get velocity.");
2188 Try( PrimitiveTFS_BIAS_Viper850() );
2197 "Cannot bias the force/torque sensor.");
2247 Try( PrimitiveTFS_ACQ_Viper850(H.
data) );
2253 "Cannot get force/torque measures.");
2266 Try( PrimitiveGripper_Viper850(1) );
2267 std::cout <<
"Open the gripper..." << std::endl;
2272 "Cannot open the gripper.");
2286 Try( PrimitiveGripper_Viper850(0) );
2287 std::cout <<
"Close the gripper..." << std::endl;
2292 "Cannot close the gripper.");
2304 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
2305 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2310 "Cannot enable joint limits on axis 6.");
2326 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1) );
2327 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2332 "Cannot disable joint limits on axis 6.");