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::m_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";
218 vpRobotViper850::m_robotAlreadyCreated =
true;
256 m_q_prev_getvel.
resize(6);
258 m_time_prev_getvel = 0;
259 m_first_time_getvel =
true;
262 m_q_prev_getdis.
resize(6);
264 m_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) {
306 m_controlMode =
AUTO;
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:
321 m_controlMode =
ESTOP;
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::m_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) {
800 m_controlMode =
AUTO;
802 }
else if (EStopStatus == ESTOP_MANUAL) {
805 }
else if (EStopStatus == ESTOP_ACTIVATED) {
806 m_controlMode =
ESTOP;
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++)
1134 "Modification of the robot state");
1146 Try(PrimitiveACQ_POS_Viper850(q.
data, ×tamp));
1158 for (
unsigned int i = 0; i < 3; i++) {
1159 txyz[i] = position[i];
1160 rxyz[i] = position[i + 3];
1176 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_positioningVelocity));
1177 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1186 destination = position;
1192 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_positioningVelocity));
1193 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1198 vpRxyzVector rxyz(position[3], position[4], position[5]);
1202 for (
unsigned int i = 0; i < 3; i++) {
1203 destination[i] = position[i];
1206 int configuration = 0;
1210 Try(PrimitiveMOVE_C_Viper850(destination.
data, configuration, m_positioningVelocity));
1211 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1217 "Mixt frame not implemented.");
1221 "End-effector frame not implemented.");
1226 if (TryStt == InvalidPosition || TryStt == -1023)
1227 std::cout <<
" : Position out of range.\n";
1228 else if (TryStt == -3019) {
1230 std::cout <<
" : Joint position out of range.\n";
1232 std::cout <<
" : Cartesian position leads to a joint position out of "
1234 }
else if (TryStt < 0)
1235 std::cout <<
" : Unknown error (see Fabien).\n";
1236 else if (error == -1)
1237 std::cout <<
"Position out of range.\n";
1239 if (TryStt < 0 || error < 0) {
1312 double pos4,
double pos5,
double pos6)
1463 Try(PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp));
1471 Try(PrimitiveACQ_POS_J_Viper850(q.
data, ×tamp));
1482 for (
unsigned int i = 0; i < 3; i++) {
1483 position[i] = fMc[i][3];
1484 position[i + 3] = rxyz[i];
1515 PrimitiveACQ_TIME_Viper850(×tamp);
1552 for (
unsigned int j = 0; j < 3; j++)
1553 RxyzVect[j] = posRxyz[j + 3];
1558 for (
unsigned int j = 0; j < 3; j++) {
1559 position[j] = posRxyz[j];
1560 position[j + 3] = RtuVect[j];
1581 for (
unsigned int j = 0; j < 3; j++)
1582 RxyzVect[j] = posRxyz[j + 3];
1587 for (
unsigned int j = 0; j < 3; j++) {
1588 position[j] = posRxyz[j];
1589 position[j + 3] = RtuVect[j];
1674 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1676 "Cannot send a velocity to the robot "
1677 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1691 for (
unsigned int i = 0; i < 3; i++)
1693 for (
unsigned int i = 3; i < 6; i++)
1707 for (
unsigned int i = 0; i < 6; i++)
1710 for (
unsigned int i = 0; i < 5; i++)
1725 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850));
1735 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.
data, REPCAM_VIPER850));
1743 Try(PrimitiveMOVESPEED_Viper850(vel_sat.
data));
1749 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850));
1758 "Case not taken in account.");
1765 if (TryStt == VelStopOnJoint) {
1766 UInt32 axisInJoint[
njoint];
1767 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1768 for (
unsigned int i = 0; i <
njoint; i++) {
1770 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1773 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1774 if (TryString != NULL) {
1776 printf(
" Error sentence %s\n", TryString);
1859 Try(PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp));
1860 time_cur = timestamp;
1868 if (!m_first_time_getvel) {
1873 eMe = m_fMe_prev_getvel.
inverse() * fMe_cur;
1883 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1892 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1898 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1913 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1921 for (
unsigned int i = 0; i < 3; i++) {
1923 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1925 velocity[i + 3] = thetaU[i];
1929 velocity /= (time_cur - m_time_prev_getvel);
1934 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1938 m_first_time_getvel =
false;
1942 m_fMe_prev_getvel = fMe_cur;
1944 m_fMc_prev_getvel = fMc_cur;
1947 m_q_prev_getvel = q_cur;
1950 m_time_prev_getvel = time_cur;
2114 std::ifstream fd(filename.c_str(), std::ios::in);
2116 if (!fd.is_open()) {
2121 std::string key(
"R:");
2122 std::string id(
"#Viper850 - Position");
2123 bool pos_found =
false;
2128 while (std::getline(fd, line)) {
2131 if (!(line.compare(0,
id.size(),
id) == 0)) {
2132 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2136 if ((line.compare(0, 1,
"#") == 0)) {
2139 if ((line.compare(0, key.size(), key) == 0)) {
2142 if (chain.size() <
njoint + 1)
2144 if (chain.size() <
njoint + 1)
2147 std::istringstream ss(line);
2150 for (
unsigned int i = 0; i <
njoint; i++)
2163 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2197 fd = fopen(filename.c_str(),
"w");
2202 #Viper850 - Position - Version 1.00\n\
2205 # Joint position in degrees\n\
2271 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2272 for (
unsigned int i = 0; i <
njoint; i++) {
2276 if (!m_first_time_getdis) {
2279 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2284 displacement = q_cur - m_q_prev_getdis;
2289 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2294 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2298 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2303 m_first_time_getdis =
false;
2307 m_q_prev_getdis = q_cur;
2325 #if defined(USE_ATI_DAQ)
2326 #if defined(VISP_HAVE_COMEDI)
2330 "apt-get install libcomedi-dev"));
2335 Try(PrimitiveTFS_BIAS_Viper850());
2357 #if defined(USE_ATI_DAQ)
2358 #if defined(VISP_HAVE_COMEDI)
2362 "apt-get install libcomedi-dev"));
2410 #if defined(USE_ATI_DAQ)
2411 #if defined(VISP_HAVE_COMEDI)
2416 "apt-get install libcomedi-dev"));
2423 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2472 #if defined(USE_ATI_DAQ)
2473 #if defined(VISP_HAVE_COMEDI)
2478 "apt-get install libcomedi-dev"));
2485 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2506 Try(PrimitivePneumaticGripper_Viper850(1));
2507 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2524 Try(PrimitivePneumaticGripper_Viper850(0));
2525 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2540 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2541 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2562 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2563 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2609 maxRotationVelocity_joint6 = w6_max;
2622 #elif !defined(VISP_BUILD_SHARED_LIBS)
2625 void dummy_vpRobotViper850(){};
Type * data
Address of the first element of the data array.
vpCameraParametersProjType
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
@ functionNotImplementedError
Function not implemented.
static vpColVector inverse(const vpHomogeneousMatrix &M)
vpColVector getForceTorque() const
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Implementation of a pose vector and operations on poses.
Error that can be emited by the vpRobot class and its derivates.
@ positionOutOfRangeError
vpColVector getForceTorque() const
void get_cVe(vpVelocityTwistMatrix &cVe) const
virtual ~vpRobotViper850(void)
static const double m_defaultPositioningVelocity
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocity(double w_max)
void closeGripper() const
void setMaxRotationVelocityJoint6(double w6_max)
void biasForceTorqueSensor()
void unbiasForceTorqueSensor()
void disableJoint6Limits() const
void enableJoint6Limits() const
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_eJe(vpMatrix &eJe)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
double getPositioningVelocity(void) const
static bool readPosFile(const std::string &filename, vpColVector &q)
void setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_cMe(vpHomogeneousMatrix &cMe) const
void move(const std::string &filename)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getMaxRotationVelocityJoint6() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
Class that defines a generic virtual robot.
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void setVerbose(bool verbose)
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
double getMaxRotationVelocity(void) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double getMaxTranslationVelocity(void) const
double maxRotationVelocity
void setMaxRotationVelocity(double maxVr)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 850 robot.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
vpCameraParameters::vpCameraParametersProjType projModel
vpToolType
List of possible tools that can be attached to the robot end-effector.
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
static const unsigned int njoint
Number of joint.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)