39#include <visp3/core/vpConfig.h>
41#ifdef VISP_HAVE_VIPER850
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>
61bool vpRobotViper850::m_robotAlreadyCreated =
false;
83void 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++)
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];
1177 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_positioningVelocity));
1178 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1187 destination = position;
1193 Try(PrimitiveMOVE_J_Viper850(destination.
data, m_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, m_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);
1860 Try(PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp));
1861 time_cur = timestamp;
1869 if (!m_first_time_getvel) {
1874 eMe = m_fMe_prev_getvel.
inverse() * fMe_cur;
1884 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1893 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1899 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1914 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1922 for (
unsigned int i = 0; i < 3; i++) {
1924 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1926 velocity[i + 3] = thetaU[i];
1930 velocity /= (time_cur - m_time_prev_getvel);
1935 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1939 m_first_time_getvel =
false;
1943 m_fMe_prev_getvel = fMe_cur;
1945 m_fMc_prev_getvel = fMc_cur;
1948 m_q_prev_getvel = q_cur;
1951 m_time_prev_getvel = time_cur;
2115 std::ifstream fd(filename.c_str(), std::ios::in);
2117 if (!fd.is_open()) {
2122 std::string key(
"R:");
2123 std::string id(
"#Viper850 - Position");
2124 bool pos_found =
false;
2129 while (std::getline(fd, line)) {
2132 if (!(line.compare(0,
id.size(),
id) == 0)) {
2133 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2137 if ((line.compare(0, 1,
"#") == 0)) {
2140 if ((line.compare(0, key.size(), key) == 0)) {
2143 if (chain.size() <
njoint + 1)
2145 if (chain.size() <
njoint + 1)
2148 std::istringstream ss(line);
2151 for (
unsigned int i = 0; i <
njoint; i++)
2164 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2198 fd = fopen(filename.c_str(),
"w");
2203#Viper850 - Position - Version 1.00\n\
2206# Joint position in degrees\n\
2272 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2273 for (
unsigned int i = 0; i <
njoint; i++) {
2277 if (!m_first_time_getdis) {
2280 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2285 displacement = q_cur - m_q_prev_getdis;
2290 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2295 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2299 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2304 m_first_time_getdis =
false;
2308 m_q_prev_getdis = q_cur;
2326#if defined(USE_ATI_DAQ)
2327#if defined(VISP_HAVE_COMEDI)
2331 "apt-get install libcomedi-dev"));
2336 Try(PrimitiveTFS_BIAS_Viper850());
2358#if defined(USE_ATI_DAQ)
2359#if defined(VISP_HAVE_COMEDI)
2363 "apt-get install libcomedi-dev"));
2411#if defined(USE_ATI_DAQ)
2412#if defined(VISP_HAVE_COMEDI)
2417 "apt-get install libcomedi-dev"));
2424 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2473#if defined(USE_ATI_DAQ)
2474#if defined(VISP_HAVE_COMEDI)
2479 "apt-get install libcomedi-dev"));
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)
2626void 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)