36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_VIPER850
42 #include <sys/types.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpExponentialMap.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpThetaUVector.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/robot/vpRobot.h>
51 #include <visp3/robot/vpRobotException.h>
52 #include <visp3/robot/vpRobotViper850.h>
58 bool vpRobotViper850::m_robotAlreadyCreated =
false;
80 void emergencyStopViper850(
int signo)
82 std::cout <<
"Stop the Viper850 application by signal (" << signo <<
"): " << (char)7;
85 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
88 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
91 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
94 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
97 std::cout <<
"SIGQUIT " << std::endl;
100 std::cout << signo << std::endl;
104 PrimitiveSTOP_Viper850();
105 std::cout <<
"Robot was stopped\n";
110 fprintf(stdout,
"Application ");
112 kill(getpid(), SIGKILL);
195 signal(SIGINT, emergencyStopViper850);
196 signal(SIGBUS, emergencyStopViper850);
197 signal(SIGSEGV, emergencyStopViper850);
198 signal(SIGKILL, emergencyStopViper850);
199 signal(SIGQUIT, emergencyStopViper850);
203 std::cout <<
"Open communication with MotionBlox.\n";
215 vpRobotViper850::m_robotAlreadyCreated =
true;
253 m_q_prev_getvel.
resize(6);
255 m_time_prev_getvel = 0;
256 m_first_time_getvel =
true;
259 m_q_prev_getdis.
resize(6);
261 m_first_time_getdis =
true;
263 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
264 std::string calibfile;
265 #ifdef VISP_HAVE_VIPER850_DATA
266 calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
271 "data to retrieve ATI F/T calib "
279 Try(InitializeConnection(
verbose_));
282 Try(InitializeNode_Viper850());
284 Try(PrimitiveRESET_Viper850());
287 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
293 UInt32 HIPowerStatus;
295 Try(PrimitiveSTATUS_Viper850(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
300 std::cout <<
"Robot status: ";
301 switch (EStopStatus) {
303 m_controlMode =
AUTO;
304 if (HIPowerStatus == 0)
305 std::cout <<
"Power is OFF" << std::endl;
307 std::cout <<
"Power is ON" << std::endl;
312 if (HIPowerStatus == 0)
313 std::cout <<
"Power is OFF" << std::endl;
315 std::cout <<
"Power is ON" << std::endl;
317 case ESTOP_ACTIVATED:
318 m_controlMode =
ESTOP;
319 std::cout <<
"Emergency stop is activated" << std::endl;
322 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
323 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
326 std::cout << std::endl;
344 if (TryStt == -20001)
345 printf(
"No connection detected. Check if the robot is powered on \n"
346 "and if the firewire link exist between the MotionBlox and this "
348 else if (TryStt == -675)
349 printf(
" Timeout enabling power...\n");
353 PrimitivePOWEROFF_Viper850();
355 ShutDownConnection();
357 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
441 for (
unsigned int i = 0; i < 3; i++) {
442 eMc_pose[i] =
etc[i];
443 eMc_pose[i + 3] =
erc[i];
446 Try(PrimitiveCONST_Viper850(eMc_pose));
521 for (
unsigned int i = 0; i < 3; i++) {
522 eMc_pose[i] =
etc[i];
523 eMc_pose[i + 3] =
erc[i];
526 Try(PrimitiveCONST_Viper850(eMc_pose));
587 for (
unsigned int i = 0; i < 3; i++) {
588 eMc_pose[i] =
etc[i];
589 eMc_pose[i + 3] =
erc[i];
592 Try(PrimitiveCONST_Viper850(eMc_pose));
617 for (
unsigned int i = 0; i < 3; i++) {
618 eMc_pose[i] =
etc[i];
619 eMc_pose[i + 3] =
erc[i];
622 Try(PrimitiveCONST_Viper850(eMc_pose));
649 for (
unsigned int i = 0; i < 3; i++) {
650 eMc_pose[i] =
etc[i];
651 eMc_pose[i + 3] =
erc[i];
654 Try(PrimitiveCONST_Viper850(eMc_pose));
673 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
682 UInt32 HIPowerStatus;
683 Try(PrimitiveSTATUS_Viper850(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
694 ShutDownConnection();
696 vpRobotViper850::m_robotAlreadyCreated =
false;
716 Try(PrimitiveSTOP_Viper850());
723 std::cout <<
"Change the control mode from velocity to position control.\n";
724 Try(PrimitiveSTOP_Viper850());
734 std::cout <<
"Change the control mode from stop to velocity control.\n";
765 Try(PrimitiveSTOP_Viper850());
789 UInt32 HIPowerStatus;
791 bool firsttime =
true;
792 unsigned int nitermax = 10;
794 for (
unsigned int i = 0; i < nitermax; i++) {
795 Try(PrimitiveSTATUS_Viper850(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
796 if (EStopStatus == ESTOP_AUTO) {
797 m_controlMode =
AUTO;
799 }
else if (EStopStatus == ESTOP_MANUAL) {
802 }
else if (EStopStatus == ESTOP_ACTIVATED) {
803 m_controlMode =
ESTOP;
805 std::cout <<
"Emergency stop is activated! \n"
806 <<
"Check the emergency stop button and push the yellow "
807 "button before continuing."
811 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
815 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
816 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
818 ShutDownConnection();
823 if (EStopStatus == ESTOP_ACTIVATED)
824 std::cout << std::endl;
826 if (EStopStatus == ESTOP_ACTIVATED) {
827 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
831 if (HIPowerStatus == 0) {
832 fprintf(stdout,
"Power ON the Viper850 robot\n");
835 Try(PrimitivePOWERON_Viper850());
859 UInt32 HIPowerStatus;
860 Try(PrimitiveSTATUS_Viper850(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
863 if (HIPowerStatus == 1) {
864 fprintf(stdout,
"Power OFF the Viper850 robot\n");
867 Try(PrimitivePOWEROFF_Viper850());
893 UInt32 HIPowerStatus;
894 Try(PrimitiveSTATUS_Viper850(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
897 if (HIPowerStatus == 1) {
957 Try(PrimitiveACQ_POS_J_Viper850(position, ×tamp));
961 for (
unsigned int i = 0; i <
njoint; i++)
990 Try(PrimitiveACQ_POS_Viper850(position, ×tamp));
994 for (
unsigned int i = 0; i <
njoint; i++)
1131 "Modification of the robot state");
1143 Try(PrimitiveACQ_POS_Viper850(q.
data, ×tamp));
1155 for (
unsigned int i = 0; i < 3; i++) {
1156 txyz[i] = position[i];
1157 rxyz[i] = position[i + 3];
1173 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_positioningVelocity));
1174 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1183 destination = position;
1189 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_positioningVelocity));
1190 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1195 vpRxyzVector rxyz(position[3], position[4], position[5]);
1199 for (
unsigned int i = 0; i < 3; i++) {
1200 destination[i] = position[i];
1203 int configuration = 0;
1207 Try(PrimitiveMOVE_C_Viper850(destination.
data, configuration, m_positioningVelocity));
1208 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1214 "Mixt frame not implemented.");
1218 "End-effector frame not implemented.");
1223 if (TryStt == InvalidPosition || TryStt == -1023)
1224 std::cout <<
" : Position out of range.\n";
1225 else if (TryStt == -3019) {
1227 std::cout <<
" : Joint position out of range.\n";
1229 std::cout <<
" : Cartesian position leads to a joint position out of "
1231 }
else if (TryStt < 0)
1232 std::cout <<
" : Unknown error (see Fabien).\n";
1233 else if (error == -1)
1234 std::cout <<
"Position out of range.\n";
1236 if (TryStt < 0 || error < 0) {
1309 double pos4,
double pos5,
double pos6)
1460 Try(PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp));
1468 Try(PrimitiveACQ_POS_J_Viper850(q.
data, ×tamp));
1479 for (
unsigned int i = 0; i < 3; i++) {
1480 position[i] = fMc[i][3];
1481 position[i + 3] = rxyz[i];
1512 PrimitiveACQ_TIME_Viper850(×tamp);
1549 for (
unsigned int j = 0; j < 3; j++)
1550 RxyzVect[j] = posRxyz[j + 3];
1555 for (
unsigned int j = 0; j < 3; j++) {
1556 position[j] = posRxyz[j];
1557 position[j + 3] = RtuVect[j];
1578 for (
unsigned int j = 0; j < 3; j++)
1579 RxyzVect[j] = posRxyz[j + 3];
1584 for (
unsigned int j = 0; j < 3; j++) {
1585 position[j] = posRxyz[j];
1586 position[j + 3] = RtuVect[j];
1671 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1673 "Cannot send a velocity to the robot "
1674 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1688 for (
unsigned int i = 0; i < 3; i++)
1690 for (
unsigned int i = 3; i < 6; i++)
1704 for (
unsigned int i = 0; i < 6; i++)
1707 for (
unsigned int i = 0; i < 5; i++)
1722 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850));
1732 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.
data, REPCAM_VIPER850));
1740 Try(PrimitiveMOVESPEED_Viper850(vel_sat.
data));
1746 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850));
1755 "Case not taken in account.");
1762 if (TryStt == VelStopOnJoint) {
1763 UInt32 axisInJoint[
njoint];
1764 PrimitiveSTATUS_Viper850(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, axisInJoint,
nullptr);
1765 for (
unsigned int i = 0; i <
njoint; i++) {
1767 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1770 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1771 if (TryString !=
nullptr) {
1773 printf(
" Error sentence %s\n", TryString);
1856 Try(PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp));
1857 time_cur = timestamp;
1865 if (!m_first_time_getvel) {
1870 eMe = m_fMe_prev_getvel.
inverse() * fMe_cur;
1880 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1889 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1895 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1910 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1918 for (
unsigned int i = 0; i < 3; i++) {
1920 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1922 velocity[i + 3] = thetaU[i];
1926 velocity /= (time_cur - m_time_prev_getvel);
1931 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1935 m_first_time_getvel =
false;
1939 m_fMe_prev_getvel = fMe_cur;
1941 m_fMc_prev_getvel = fMc_cur;
1944 m_q_prev_getvel = q_cur;
1947 m_time_prev_getvel = time_cur;
2111 std::ifstream fd(filename.c_str(), std::ios::in);
2113 if (!fd.is_open()) {
2118 std::string key(
"R:");
2119 std::string id(
"#Viper850 - Position");
2120 bool pos_found =
false;
2125 while (std::getline(fd, line)) {
2128 if (!(line.compare(0,
id.size(),
id) == 0)) {
2129 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2133 if ((line.compare(0, 1,
"#") == 0)) {
2136 if ((line.compare(0, key.size(), key) == 0)) {
2139 if (chain.size() <
njoint + 1)
2141 if (chain.size() <
njoint + 1)
2144 std::istringstream ss(line);
2147 for (
unsigned int i = 0; i <
njoint; i++)
2160 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2194 fd = fopen(filename.c_str(),
"w");
2199 #Viper850 - Position - Version 1.00\n\
2202 # Joint position in degrees\n\
2268 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2269 for (
unsigned int i = 0; i <
njoint; i++) {
2273 if (!m_first_time_getdis) {
2276 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2281 displacement = q_cur - m_q_prev_getdis;
2286 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2291 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2295 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2300 m_first_time_getdis =
false;
2304 m_q_prev_getdis = q_cur;
2322 #if defined(USE_ATI_DAQ)
2323 #if defined(VISP_HAVE_COMEDI)
2327 "apt-get install libcomedi-dev"));
2332 Try(PrimitiveTFS_BIAS_Viper850());
2354 #if defined(USE_ATI_DAQ)
2355 #if defined(VISP_HAVE_COMEDI)
2359 "apt-get install libcomedi-dev"));
2407 #if defined(USE_ATI_DAQ)
2408 #if defined(VISP_HAVE_COMEDI)
2413 "apt-get install libcomedi-dev"));
2420 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2469 #if defined(USE_ATI_DAQ)
2470 #if defined(VISP_HAVE_COMEDI)
2475 "apt-get install libcomedi-dev"));
2482 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2503 Try(PrimitivePneumaticGripper_Viper850(1));
2504 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2521 Try(PrimitivePneumaticGripper_Viper850(0));
2522 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2537 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2538 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2559 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2560 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2606 maxRotationVelocity_joint6 = w6_max;
2619 #elif !defined(VISP_BUILD_SHARED_LIBS)
2622 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 emitted 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 emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
vpColVector getForceTorque() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) override
void get_cVe(vpVelocityTwistMatrix &cVe) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) override
void get_fJe(vpMatrix &fJe) override
virtual ~vpRobotViper850(void)
static const double m_defaultPositioningVelocity
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
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)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
double getPositioningVelocity(void) const
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_eJe(vpMatrix &eJe) override
void setPositioningVelocity(double velocity)
void get_cMe(vpHomogeneousMatrix &cMe) const
void move(const std::string &filename)
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)
Modelization 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)