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() );
279 UInt32 HIPowerStatus;
281 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
287 std::cout <<
"Robot status: ";
288 switch(EStopStatus) {
291 if (HIPowerStatus == 0)
292 std::cout <<
"Power is OFF" << std::endl;
294 std::cout <<
"Power is ON" << std::endl;
299 if (HIPowerStatus == 0)
300 std::cout <<
"Power is OFF" << std::endl;
302 std::cout <<
"Power is ON" << std::endl;
304 case ESTOP_ACTIVATED:
306 std::cout <<
"Emergency stop is activated" << std::endl;
309 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
310 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
313 std::cout << std::endl;
330 if (TryStt == -20001)
331 printf(
"No connection detected. Check if the robot is powered on \n"
332 "and if the firewire link exist between the MotionBlox and this computer.\n");
333 else if (TryStt == -675)
334 printf(
" Timeout enabling power...\n");
338 PrimitivePOWEROFF_Viper850();
340 ShutDownConnection();
342 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
344 "Cannot open connexion with the motionblox");
418 for (
unsigned int i=0; i < 3; i ++) {
419 eMc_pose[i] =
etc[i];
420 eMc_pose[i+3] =
erc[i];
423 Try( PrimitiveCONST_Viper850(eMc_pose) );
458 UInt32 HIPowerStatus;
459 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
471 ShutDownConnection();
473 vpRobotViper850::robotAlreadyCreated =
false;
497 Try( PrimitiveSTOP_Viper850() );
503 std::cout <<
"Change the control mode from velocity to position control.\n";
504 Try( PrimitiveSTOP_Viper850() );
514 std::cout <<
"Change the control mode from stop to velocity control.\n";
547 Try( PrimitiveSTOP_Viper850() );
554 "Cannot stop robot motion.");
573 UInt32 HIPowerStatus;
575 bool firsttime =
true;
576 unsigned int nitermax = 10;
578 for (
unsigned int i=0; i<nitermax; i++) {
579 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
581 if (EStopStatus == ESTOP_AUTO) {
585 else if (EStopStatus == ESTOP_MANUAL) {
589 else if (EStopStatus == ESTOP_ACTIVATED) {
592 std::cout <<
"Emergency stop is activated! \n"
593 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
596 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
601 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
602 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
604 ShutDownConnection();
609 if (EStopStatus == ESTOP_ACTIVATED)
610 std::cout << std::endl;
612 if (EStopStatus == ESTOP_ACTIVATED) {
613 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
615 "Cannot power on the robot.");
618 if (HIPowerStatus == 0) {
619 fprintf(stdout,
"Power ON the Viper850 robot\n");
622 Try( PrimitivePOWERON_Viper850() );
629 "Cannot power off the robot.");
648 UInt32 HIPowerStatus;
649 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
653 if (HIPowerStatus == 1) {
654 fprintf(stdout,
"Power OFF the Viper850 robot\n");
657 Try( PrimitivePOWEROFF_Viper850() );
664 "Cannot power off the robot.");
685 UInt32 HIPowerStatus;
686 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
690 if (HIPowerStatus == 1) {
698 "Cannot get the power status.");
758 Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
762 for (
unsigned int i=0; i <
njoint; i++)
795 Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
799 for (
unsigned int i=0; i <
njoint; i++)
853 positioningVelocity = velocity;
864 return positioningVelocity;
953 "Modification of the robot state");
965 Try( PrimitiveACQ_POS_Viper850(q.
data, ×tamp) );
977 for (
unsigned int i=0; i < 3; i++) {
978 txyz[i] = position[i];
979 rxyz[i] = position[i+3];
995 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
996 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1006 destination = position;
1011 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
1012 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1018 vpRxyzVector rxyz(position[3],position[4],position[5]);
1022 for (
unsigned int i=0; i <3; i++) {
1023 destination[i] = position[i];
1026 int configuration = 0;
1029 Try( PrimitiveMOVE_C_Viper850(destination.
data, configuration,
1030 positioningVelocity) );
1031 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1037 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1039 "Positionning error: "
1040 "Mixt frame not implemented.");
1046 if (TryStt == InvalidPosition || TryStt == -1023)
1047 std::cout <<
" : Position out of range.\n";
1048 else if (TryStt == -3019) {
1050 std::cout <<
" : Joint position out of range.\n";
1052 std::cout <<
" : Cartesian position leads to a joint position out of range.\n";
1054 else if (TryStt < 0)
1055 std::cout <<
" : Unknown error (see Fabien).\n";
1056 else if (error == -1)
1057 std::cout <<
"Position out of range.\n";
1059 if (TryStt < 0 || error < 0) {
1062 "Position out of range.");
1143 position[0] = pos1 ;
1144 position[1] = pos2 ;
1145 position[2] = pos3 ;
1146 position[3] = pos4 ;
1147 position[4] = pos5 ;
1148 position[5] = pos6 ;
1208 "Bad position in filename.");
1296 Try( PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp) );
1303 Try( PrimitiveACQ_POS_C_Viper850(position.
data, ×tamp) );
1307 for (
unsigned int i=3; i <6; i++)
1310 vpRzyzVector rzyz(position[3], position[4], position[5]);
1315 for (
unsigned int i=0; i <3; i++)
1316 position[i+3] = rxyz[i];
1326 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1328 "Cannot get position in mixt frame: "
1338 "Cannot get position.");
1350 PrimitiveACQ_TIME_Viper850(×tamp);
1388 for (
unsigned int j=0;j<3;j++)
1389 RxyzVect[j]=posRxyz[j+3];
1394 for (
unsigned int j=0;j<3;j++)
1396 position[j]=posRxyz[j];
1397 position[j+3]=RtuVect[j];
1418 for (
unsigned int j=0;j<3;j++)
1419 RxyzVect[j]=posRxyz[j+3];
1424 for (
unsigned int j=0;j<3;j++)
1426 position[j]=posRxyz[j];
1427 position[j+3]=RtuVect[j];
1508 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1510 "Cannot send a velocity to the robot "
1511 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1524 for (
unsigned int i=0; i<3; i++)
1526 for (
unsigned int i=3; i<6; i++)
1537 for (
unsigned int i=0; i<6; i++)
1550 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850) );
1558 Try( PrimitiveMOVESPEED_Viper850(vel_sat.
data) );
1563 std::cout <<
"Vitesse ref appliquee: " << vel_sat.
t();
1564 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850) );
1573 "Case not taken in account.");
1580 if (TryStt == VelStopOnJoint) {
1581 UInt32 axisInJoint[
njoint];
1582 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1583 for (
unsigned int i=0; i <
njoint; i ++) {
1585 std::cout <<
"\nWarning: Velocity control stopped: axis "
1586 << i+1 <<
" on joint limit!" <<std::endl;
1590 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1591 if (TryString != NULL) {
1593 printf(
" Error sentence %s\n", TryString);
1685 Try( PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp) );
1686 time_cur = timestamp;
1692 if ( ! first_time_getvel ) {
1697 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1706 velocity = (q_cur - q_prev_getvel)
1707 / (time_cur - time_prev_getvel);
1713 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1728 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1736 for (
unsigned int i=0; i < 3; i++) {
1738 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1740 velocity[i+3] = thetaU[i];
1744 velocity /= (time_cur - time_prev_getvel);
1750 first_time_getvel =
false;
1754 fMc_prev_getvel = fMc_cur;
1757 q_prev_getvel = q_cur;
1760 time_prev_getvel = time_cur;
1767 "Cannot get velocity.");
1931 fd = fopen(filename,
"r") ;
1935 char line[FILENAME_MAX];
1936 char dummy[FILENAME_MAX];
1938 bool sortie =
false;
1942 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1943 if ( strncmp (line,
"#", 1) != 0) {
1945 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1957 while ( sortie !=
true );
1961 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1963 &q[0], &q[1], &q[2],
1964 &q[3], &q[4], &q[5]);
2001 fd = fopen(filename,
"w") ;
2006 #Viper - Position - Version 1.0\n\
2009 # Joint position in degrees\n\
2014 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2066 vpRobotViper850::getCameraDisplacement(
vpColVector &displacement)
2082 vpRobotViper850::getArticularDisplacement(
vpColVector &displacement)
2121 Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
2122 for (
unsigned int i=0; i <
njoint; i ++) {
2126 if ( ! first_time_getdis ) {
2129 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2135 displacement = q_cur - q_prev_getdis;
2140 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2146 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2153 first_time_getdis =
false;
2157 q_prev_getdis = q_cur;
2163 "Cannot get velocity.");
2185 Try( PrimitiveTFS_BIAS_Viper850() );
2194 "Cannot bias the force/torque sensor.");
2244 Try( PrimitiveTFS_ACQ_Viper850(H.
data) );
2250 "Cannot get force/torque measures.");
2263 Try( PrimitiveGripper_Viper850(1) );
2264 std::cout <<
"Open the gripper..." << std::endl;
2269 "Cannot open the gripper.");
2283 Try( PrimitiveGripper_Viper850(0) );
2284 std::cout <<
"Close the gripper..." << std::endl;
2289 "Cannot close the gripper.");