39 #include <visp3/core/vpConfig.h>
41 #ifdef VISP_HAVE_VIPER850
45 #include <sys/types.h>
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/vpRobot.h>
54 #include <visp3/robot/vpRobotException.h>
55 #include <visp3/robot/vpRobotViper850.h>
61 bool vpRobotViper850::robotAlreadyCreated =
false;
83 void emergencyStopViper850(
int signo)
85 std::cout <<
"Stop the Viper850 application by signal (" << signo <<
"): " << (char)7;
88 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
91 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
94 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
97 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
100 std::cout <<
"SIGQUIT " << std::endl;
103 std::cout << signo << std::endl;
107 PrimitiveSTOP_Viper850();
108 std::cout <<
"Robot was stopped\n";
113 fprintf(stdout,
"Application ");
115 kill(getpid(), SIGKILL);
199 signal(SIGINT, emergencyStopViper850);
200 signal(SIGBUS, emergencyStopViper850);
201 signal(SIGSEGV, emergencyStopViper850);
202 signal(SIGKILL, emergencyStopViper850);
203 signal(SIGQUIT, emergencyStopViper850);
207 std::cout <<
"Open communication with MotionBlox.\n";
215 positioningVelocity = defaultPositioningVelocity;
217 maxRotationVelocity_joint6 = maxRotationVelocity;
219 vpRobotViper850::robotAlreadyCreated =
true;
259 time_prev_getvel = 0;
260 first_time_getvel =
true;
265 first_time_getdis =
true;
267 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
268 std::string calibfile;
269 #ifdef VISP_HAVE_VIPER850_DATA
270 calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
275 "data to retrive ATI F/T calib "
283 Try(InitializeConnection(
verbose_));
286 Try(InitializeNode_Viper850());
288 Try(PrimitiveRESET_Viper850());
291 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
297 UInt32 HIPowerStatus;
299 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
304 std::cout <<
"Robot status: ";
305 switch (EStopStatus) {
308 if (HIPowerStatus == 0)
309 std::cout <<
"Power is OFF" << std::endl;
311 std::cout <<
"Power is ON" << std::endl;
316 if (HIPowerStatus == 0)
317 std::cout <<
"Power is OFF" << std::endl;
319 std::cout <<
"Power is ON" << std::endl;
321 case ESTOP_ACTIVATED:
323 std::cout <<
"Emergency stop is activated" << std::endl;
326 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
327 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
330 std::cout << std::endl;
348 if (TryStt == -20001)
349 printf(
"No connection detected. Check if the robot is powered on \n"
350 "and if the firewire link exist between the MotionBlox and this "
352 else if (TryStt == -675)
353 printf(
" Timeout enabling power...\n");
357 PrimitivePOWEROFF_Viper850();
359 ShutDownConnection();
361 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
446 for (
unsigned int i = 0; i < 3; i++) {
447 eMc_pose[i] =
etc[i];
448 eMc_pose[i + 3] =
erc[i];
451 Try(PrimitiveCONST_Viper850(eMc_pose));
526 for (
unsigned int i = 0; i < 3; i++) {
527 eMc_pose[i] =
etc[i];
528 eMc_pose[i + 3] =
erc[i];
531 Try(PrimitiveCONST_Viper850(eMc_pose));
592 for (
unsigned int i = 0; i < 3; i++) {
593 eMc_pose[i] =
etc[i];
594 eMc_pose[i + 3] =
erc[i];
597 Try(PrimitiveCONST_Viper850(eMc_pose));
622 for (
unsigned int i = 0; i < 3; i++) {
623 eMc_pose[i] =
etc[i];
624 eMc_pose[i + 3] =
erc[i];
627 Try(PrimitiveCONST_Viper850(eMc_pose));
654 for (
unsigned int i = 0; i < 3; i++) {
655 eMc_pose[i] =
etc[i];
656 eMc_pose[i + 3] =
erc[i];
659 Try(PrimitiveCONST_Viper850(eMc_pose));
678 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
687 UInt32 HIPowerStatus;
688 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
699 ShutDownConnection();
701 vpRobotViper850::robotAlreadyCreated =
false;
721 Try(PrimitiveSTOP_Viper850());
728 std::cout <<
"Change the control mode from velocity to position control.\n";
729 Try(PrimitiveSTOP_Viper850());
739 std::cout <<
"Change the control mode from stop to velocity control.\n";
770 Try(PrimitiveSTOP_Viper850());
794 UInt32 HIPowerStatus;
796 bool firsttime =
true;
797 unsigned int nitermax = 10;
799 for (
unsigned int i = 0; i < nitermax; i++) {
800 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
801 if (EStopStatus == ESTOP_AUTO) {
804 }
else if (EStopStatus == ESTOP_MANUAL) {
807 }
else if (EStopStatus == ESTOP_ACTIVATED) {
810 std::cout <<
"Emergency stop is activated! \n"
811 <<
"Check the emergency stop button and push the yellow "
812 "button before continuing."
816 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
820 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
821 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
823 ShutDownConnection();
828 if (EStopStatus == ESTOP_ACTIVATED)
829 std::cout << std::endl;
831 if (EStopStatus == ESTOP_ACTIVATED) {
832 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
836 if (HIPowerStatus == 0) {
837 fprintf(stdout,
"Power ON the Viper850 robot\n");
840 Try(PrimitivePOWERON_Viper850());
864 UInt32 HIPowerStatus;
865 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
868 if (HIPowerStatus == 1) {
869 fprintf(stdout,
"Power OFF the Viper850 robot\n");
872 Try(PrimitivePOWEROFF_Viper850());
898 UInt32 HIPowerStatus;
899 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
902 if (HIPowerStatus == 1) {
962 Try(PrimitiveACQ_POS_J_Viper850(position, ×tamp));
966 for (
unsigned int i = 0; i <
njoint; i++)
995 Try(PrimitiveACQ_POS_Viper850(position, ×tamp));
999 for (
unsigned int i = 0; i <
njoint; i++)
1139 "Modification of the robot state");
1151 Try(PrimitiveACQ_POS_Viper850(q.data, ×tamp));
1163 for (
unsigned int i = 0; i < 3; i++) {
1164 txyz[i] = position[i];
1165 rxyz[i] = position[i + 3];
1180 destination.rad2deg();
1181 Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1182 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1191 destination = position;
1197 Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1198 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1203 vpRxyzVector rxyz(position[3], position[4], position[5]);
1207 for (
unsigned int i = 0; i < 3; i++) {
1208 destination[i] = position[i];
1211 int configuration = 0;
1215 Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, positioningVelocity));
1216 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1222 "Mixt frame not implemented.");
1226 "End-effector frame not implemented.");
1231 if (TryStt == InvalidPosition || TryStt == -1023)
1232 std::cout <<
" : Position out of range.\n";
1233 else if (TryStt == -3019) {
1235 std::cout <<
" : Joint position out of range.\n";
1237 std::cout <<
" : Cartesian position leads to a joint position out of "
1239 }
else if (TryStt < 0)
1240 std::cout <<
" : Unknown error (see Fabien).\n";
1241 else if (error == -1)
1242 std::cout <<
"Position out of range.\n";
1244 if (TryStt < 0 || error < 0) {
1318 const double pos3,
const double pos4,
const double pos5,
const double pos6)
1471 Try(PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp));
1479 Try(PrimitiveACQ_POS_J_Viper850(q.
data, ×tamp));
1490 for (
unsigned int i = 0; i < 3; i++) {
1491 position[i] = fMc[i][3];
1492 position[i + 3] = rxyz[i];
1523 PrimitiveACQ_TIME_Viper850(×tamp);
1560 for (
unsigned int j = 0; j < 3; j++)
1561 RxyzVect[j] = posRxyz[j + 3];
1566 for (
unsigned int j = 0; j < 3; j++) {
1567 position[j] = posRxyz[j];
1568 position[j + 3] = RtuVect[j];
1589 for (
unsigned int j = 0; j < 3; j++)
1590 RxyzVect[j] = posRxyz[j + 3];
1595 for (
unsigned int j = 0; j < 3; j++) {
1596 position[j] = posRxyz[j];
1597 position[j + 3] = RtuVect[j];
1683 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1685 "Cannot send a velocity to the robot "
1686 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1700 for (
unsigned int i = 0; i < 3; i++)
1702 for (
unsigned int i = 3; i < 6; i++)
1716 for (
unsigned int i = 0; i < 6; i++)
1719 for (
unsigned int i = 0; i < 5; i++)
1734 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1744 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.
data, REPCAM_VIPER850));
1752 Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1758 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1767 "Case not taken in account.");
1774 if (TryStt == VelStopOnJoint) {
1775 UInt32 axisInJoint[
njoint];
1776 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1777 for (
unsigned int i = 0; i <
njoint; i++) {
1779 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1782 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1783 if (TryString != NULL) {
1785 printf(
" Error sentence %s\n", TryString);
1870 Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp));
1871 time_cur = timestamp;
1877 if (!first_time_getvel) {
1882 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1891 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1897 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1912 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1920 for (
unsigned int i = 0; i < 3; i++) {
1922 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1924 velocity[i + 3] = thetaU[i];
1928 velocity /= (time_cur - time_prev_getvel);
1933 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1937 first_time_getvel =
false;
1941 fMc_prev_getvel = fMc_cur;
1944 q_prev_getvel = q_cur;
1947 time_prev_getvel = time_cur;
2113 std::ifstream fd(filename.c_str(), std::ios::in);
2115 if (!fd.is_open()) {
2120 std::string key(
"R:");
2121 std::string id(
"#Viper850 - Position");
2122 bool pos_found =
false;
2127 while (std::getline(fd, line)) {
2130 if (!(line.compare(0,
id.size(), id) == 0)) {
2131 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2135 if ((line.compare(0, 1,
"#") == 0)) {
2138 if ((line.compare(0, key.size(), key) == 0)) {
2141 if (chain.size() <
njoint + 1)
2143 if (chain.size() <
njoint + 1)
2146 std::istringstream ss(line);
2149 for (
unsigned int i = 0; i <
njoint; i++)
2162 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2196 fd = fopen(filename.c_str(),
"w");
2201 #Viper850 - Position - Version 1.00\n\
2204 # Joint position in degrees\n\
2270 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2271 for (
unsigned int i = 0; i <
njoint; i++) {
2275 if (!first_time_getdis) {
2278 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2283 displacement = q_cur - q_prev_getdis;
2288 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2293 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2297 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2302 first_time_getdis =
false;
2306 q_prev_getdis = q_cur;
2324 #if defined(USE_ATI_DAQ)
2325 #if defined(VISP_HAVE_COMEDI)
2329 "apt-get install libcomedi-dev"));
2331 #else // Use serial link
2334 Try(PrimitiveTFS_BIAS_Viper850());
2356 #if defined(USE_ATI_DAQ)
2357 #if defined(VISP_HAVE_COMEDI)
2361 "apt-get install libcomedi-dev"));
2363 #else // Use serial link
2410 #if defined(USE_ATI_DAQ)
2411 #if defined(VISP_HAVE_COMEDI)
2416 "apt-get install libcomedi-dev"));
2418 #else // Use serial link
2423 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2473 #if defined(USE_ATI_DAQ)
2474 #if defined(VISP_HAVE_COMEDI)
2479 "apt-get install libcomedi-dev"));
2481 #else // Use serial link
2486 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2507 Try(PrimitivePneumaticGripper_Viper850(1));
2508 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2525 Try(PrimitivePneumaticGripper_Viper850(0));
2526 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2541 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2542 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2563 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2564 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2610 maxRotationVelocity_joint6 = w6_max;
2623 #elif !defined(VISP_BUILD_SHARED_LIBS)
2626 void dummy_vpRobotViper850(){};