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);
198 signal(SIGINT, emergencyStopViper850);
199 signal(SIGBUS, emergencyStopViper850);
200 signal(SIGSEGV, emergencyStopViper850);
201 signal(SIGKILL, emergencyStopViper850);
202 signal(SIGQUIT, emergencyStopViper850);
206 std::cout <<
"Open communication with MotionBlox.\n";
214 positioningVelocity = defaultPositioningVelocity;
216 maxRotationVelocity_joint6 = maxRotationVelocity;
218 vpRobotViper850::robotAlreadyCreated =
true;
258 time_prev_getvel = 0;
259 first_time_getvel =
true;
264 first_time_getdis =
true;
266 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
267 std::string calibfile;
268 #ifdef VISP_HAVE_VIPER850_DATA
269 calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
274 "data to retrive ATI F/T calib "
282 Try(InitializeConnection(
verbose_));
285 Try(InitializeNode_Viper850());
287 Try(PrimitiveRESET_Viper850());
290 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
296 UInt32 HIPowerStatus;
298 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
303 std::cout <<
"Robot status: ";
304 switch (EStopStatus) {
307 if (HIPowerStatus == 0)
308 std::cout <<
"Power is OFF" << std::endl;
310 std::cout <<
"Power is ON" << std::endl;
315 if (HIPowerStatus == 0)
316 std::cout <<
"Power is OFF" << std::endl;
318 std::cout <<
"Power is ON" << std::endl;
320 case ESTOP_ACTIVATED:
322 std::cout <<
"Emergency stop is activated" << std::endl;
325 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
326 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
329 std::cout << std::endl;
347 if (TryStt == -20001)
348 printf(
"No connection detected. Check if the robot is powered on \n"
349 "and if the firewire link exist between the MotionBlox and this "
351 else if (TryStt == -675)
352 printf(
" Timeout enabling power...\n");
356 PrimitivePOWEROFF_Viper850();
358 ShutDownConnection();
360 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
444 for (
unsigned int i = 0; i < 3; i++) {
445 eMc_pose[i] =
etc[i];
446 eMc_pose[i + 3] =
erc[i];
449 Try(PrimitiveCONST_Viper850(eMc_pose));
524 for (
unsigned int i = 0; i < 3; i++) {
525 eMc_pose[i] =
etc[i];
526 eMc_pose[i + 3] =
erc[i];
529 Try(PrimitiveCONST_Viper850(eMc_pose));
590 for (
unsigned int i = 0; i < 3; i++) {
591 eMc_pose[i] =
etc[i];
592 eMc_pose[i + 3] =
erc[i];
595 Try(PrimitiveCONST_Viper850(eMc_pose));
620 for (
unsigned int i = 0; i < 3; i++) {
621 eMc_pose[i] =
etc[i];
622 eMc_pose[i + 3] =
erc[i];
625 Try(PrimitiveCONST_Viper850(eMc_pose));
652 for (
unsigned int i = 0; i < 3; i++) {
653 eMc_pose[i] =
etc[i];
654 eMc_pose[i + 3] =
erc[i];
657 Try(PrimitiveCONST_Viper850(eMc_pose));
676 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
685 UInt32 HIPowerStatus;
686 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
697 ShutDownConnection();
699 vpRobotViper850::robotAlreadyCreated =
false;
719 Try(PrimitiveSTOP_Viper850());
726 std::cout <<
"Change the control mode from velocity to position control.\n";
727 Try(PrimitiveSTOP_Viper850());
737 std::cout <<
"Change the control mode from stop to velocity control.\n";
768 Try(PrimitiveSTOP_Viper850());
792 UInt32 HIPowerStatus;
794 bool firsttime =
true;
795 unsigned int nitermax = 10;
797 for (
unsigned int i = 0; i < nitermax; i++) {
798 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
799 if (EStopStatus == ESTOP_AUTO) {
802 }
else if (EStopStatus == ESTOP_MANUAL) {
805 }
else if (EStopStatus == ESTOP_ACTIVATED) {
808 std::cout <<
"Emergency stop is activated! \n"
809 <<
"Check the emergency stop button and push the yellow "
810 "button before continuing."
814 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
818 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
819 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
821 ShutDownConnection();
826 if (EStopStatus == ESTOP_ACTIVATED)
827 std::cout << std::endl;
829 if (EStopStatus == ESTOP_ACTIVATED) {
830 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
834 if (HIPowerStatus == 0) {
835 fprintf(stdout,
"Power ON the Viper850 robot\n");
838 Try(PrimitivePOWERON_Viper850());
862 UInt32 HIPowerStatus;
863 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
866 if (HIPowerStatus == 1) {
867 fprintf(stdout,
"Power OFF the Viper850 robot\n");
870 Try(PrimitivePOWEROFF_Viper850());
896 UInt32 HIPowerStatus;
897 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
900 if (HIPowerStatus == 1) {
960 Try(PrimitiveACQ_POS_J_Viper850(position, ×tamp));
964 for (
unsigned int i = 0; i <
njoint; i++)
993 Try(PrimitiveACQ_POS_Viper850(position, ×tamp));
997 for (
unsigned int i = 0; i <
njoint; i++)
1135 "Modification of the robot state");
1147 Try(PrimitiveACQ_POS_Viper850(q.data, ×tamp));
1159 for (
unsigned int i = 0; i < 3; i++) {
1160 txyz[i] = position[i];
1161 rxyz[i] = position[i + 3];
1176 destination.rad2deg();
1177 Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1178 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1187 destination = position;
1193 Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1194 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1199 vpRxyzVector rxyz(position[3], position[4], position[5]);
1203 for (
unsigned int i = 0; i < 3; i++) {
1204 destination[i] = position[i];
1207 int configuration = 0;
1211 Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, positioningVelocity));
1212 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1218 "Mixt frame not implemented.");
1222 "End-effector frame not implemented.");
1227 if (TryStt == InvalidPosition || TryStt == -1023)
1228 std::cout <<
" : Position out of range.\n";
1229 else if (TryStt == -3019) {
1231 std::cout <<
" : Joint position out of range.\n";
1233 std::cout <<
" : Cartesian position leads to a joint position out of "
1235 }
else if (TryStt < 0)
1236 std::cout <<
" : Unknown error (see Fabien).\n";
1237 else if (error == -1)
1238 std::cout <<
"Position out of range.\n";
1240 if (TryStt < 0 || error < 0) {
1313 double pos3,
double pos4,
double pos5,
double pos6)
1464 Try(PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp));
1472 Try(PrimitiveACQ_POS_J_Viper850(q.
data, ×tamp));
1483 for (
unsigned int i = 0; i < 3; i++) {
1484 position[i] = fMc[i][3];
1485 position[i + 3] = rxyz[i];
1516 PrimitiveACQ_TIME_Viper850(×tamp);
1553 for (
unsigned int j = 0; j < 3; j++)
1554 RxyzVect[j] = posRxyz[j + 3];
1559 for (
unsigned int j = 0; j < 3; j++) {
1560 position[j] = posRxyz[j];
1561 position[j + 3] = RtuVect[j];
1582 for (
unsigned int j = 0; j < 3; j++)
1583 RxyzVect[j] = posRxyz[j + 3];
1588 for (
unsigned int j = 0; j < 3; j++) {
1589 position[j] = posRxyz[j];
1590 position[j + 3] = RtuVect[j];
1675 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1677 "Cannot send a velocity to the robot "
1678 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1692 for (
unsigned int i = 0; i < 3; i++)
1694 for (
unsigned int i = 3; i < 6; i++)
1708 for (
unsigned int i = 0; i < 6; i++)
1711 for (
unsigned int i = 0; i < 5; i++)
1726 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1736 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.
data, REPCAM_VIPER850));
1744 Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1750 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1759 "Case not taken in account.");
1766 if (TryStt == VelStopOnJoint) {
1767 UInt32 axisInJoint[
njoint];
1768 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1769 for (
unsigned int i = 0; i <
njoint; i++) {
1771 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1774 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1775 if (TryString != NULL) {
1777 printf(
" Error sentence %s\n", TryString);
1861 Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp));
1862 time_cur = timestamp;
1868 if (!first_time_getvel) {
1873 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1882 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1888 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1903 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1911 for (
unsigned int i = 0; i < 3; i++) {
1913 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1915 velocity[i + 3] = thetaU[i];
1919 velocity /= (time_cur - time_prev_getvel);
1924 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1928 first_time_getvel =
false;
1932 fMc_prev_getvel = fMc_cur;
1935 q_prev_getvel = q_cur;
1938 time_prev_getvel = time_cur;
2102 std::ifstream fd(filename.c_str(), std::ios::in);
2104 if (!fd.is_open()) {
2109 std::string key(
"R:");
2110 std::string id(
"#Viper850 - Position");
2111 bool pos_found =
false;
2116 while (std::getline(fd, line)) {
2119 if (!(line.compare(0,
id.size(), id) == 0)) {
2120 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2124 if ((line.compare(0, 1,
"#") == 0)) {
2127 if ((line.compare(0, key.size(), key) == 0)) {
2130 if (chain.size() <
njoint + 1)
2132 if (chain.size() <
njoint + 1)
2135 std::istringstream ss(line);
2138 for (
unsigned int i = 0; i <
njoint; i++)
2151 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2185 fd = fopen(filename.c_str(),
"w");
2190 #Viper850 - Position - Version 1.00\n\
2193 # Joint position in degrees\n\
2259 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2260 for (
unsigned int i = 0; i <
njoint; i++) {
2264 if (!first_time_getdis) {
2267 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2272 displacement = q_cur - q_prev_getdis;
2277 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2282 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2286 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2291 first_time_getdis =
false;
2295 q_prev_getdis = q_cur;
2313 #if defined(USE_ATI_DAQ)
2314 #if defined(VISP_HAVE_COMEDI)
2318 "apt-get install libcomedi-dev"));
2320 #else // Use serial link
2323 Try(PrimitiveTFS_BIAS_Viper850());
2345 #if defined(USE_ATI_DAQ)
2346 #if defined(VISP_HAVE_COMEDI)
2350 "apt-get install libcomedi-dev"));
2352 #else // Use serial link
2398 #if defined(USE_ATI_DAQ)
2399 #if defined(VISP_HAVE_COMEDI)
2404 "apt-get install libcomedi-dev"));
2406 #else // Use serial link
2411 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2460 #if defined(USE_ATI_DAQ)
2461 #if defined(VISP_HAVE_COMEDI)
2466 "apt-get install libcomedi-dev"));
2468 #else // Use serial link
2473 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2494 Try(PrimitivePneumaticGripper_Viper850(1));
2495 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2512 Try(PrimitivePneumaticGripper_Viper850(0));
2513 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2528 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2529 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2550 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2551 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2597 maxRotationVelocity_joint6 = w6_max;
2610 #elif !defined(VISP_BUILD_SHARED_LIBS)
2613 void dummy_vpRobotViper850(){};