36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_VIPER650
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/vpRobotViper650.h>
58 bool vpRobotViper650::m_robotAlreadyCreated =
false;
80 void emergencyStopViper650(
int signo)
82 std::cout <<
"Stop the Viper650 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_Viper650();
105 std::cout <<
"Robot was stopped\n";
110 fprintf(stdout,
"Application ");
112 kill(getpid(), SIGKILL);
196 signal(SIGINT, emergencyStopViper650);
197 signal(SIGBUS, emergencyStopViper650);
198 signal(SIGSEGV, emergencyStopViper650);
199 signal(SIGKILL, emergencyStopViper650);
200 signal(SIGQUIT, emergencyStopViper650);
204 std::cout <<
"Open communication with MotionBlox.\n";
216 vpRobotViper650::m_robotAlreadyCreated =
true;
252 m_q_prev_getvel.
resize(6);
254 m_time_prev_getvel = 0;
255 m_first_time_getvel =
true;
258 m_q_prev_getdis.
resize(6);
260 m_first_time_getdis =
true;
263 Try(InitializeConnection(
verbose_));
266 Try(InitializeNode_Viper650());
268 Try(PrimitiveRESET_Viper650());
271 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
277 UInt32 HIPowerStatus;
279 Try(PrimitiveSTATUS_Viper650(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
284 std::cout <<
"Robot status: ";
285 switch (EStopStatus) {
287 m_controlMode =
AUTO;
288 if (HIPowerStatus == 0)
289 std::cout <<
"Power is OFF" << std::endl;
291 std::cout <<
"Power is ON" << std::endl;
296 if (HIPowerStatus == 0)
297 std::cout <<
"Power is OFF" << std::endl;
299 std::cout <<
"Power is ON" << std::endl;
301 case ESTOP_ACTIVATED:
302 m_controlMode =
ESTOP;
303 std::cout <<
"Emergency stop is activated" << std::endl;
306 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
307 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
310 std::cout << std::endl;
328 if (TryStt == -20001)
329 printf(
"No connection detected. Check if the robot is powered on \n"
330 "and if the firewire link exist between the MotionBlox and this "
332 else if (TryStt == -675)
333 printf(
" Timeout enabling power...\n");
337 PrimitivePOWEROFF_Viper650();
339 ShutDownConnection();
341 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
425 for (
unsigned int i = 0; i < 3; i++) {
426 eMc_pose[i] =
etc[i];
427 eMc_pose[i + 3] =
erc[i];
430 Try(PrimitiveCONST_Viper650(eMc_pose));
505 for (
unsigned int i = 0; i < 3; i++) {
506 eMc_pose[i] =
etc[i];
507 eMc_pose[i + 3] =
erc[i];
510 Try(PrimitiveCONST_Viper650(eMc_pose));
571 for (
unsigned int i = 0; i < 3; i++) {
572 eMc_pose[i] =
etc[i];
573 eMc_pose[i + 3] =
erc[i];
576 Try(PrimitiveCONST_Viper650(eMc_pose));
601 for (
unsigned int i = 0; i < 3; i++) {
602 eMc_pose[i] =
etc[i];
603 eMc_pose[i + 3] =
erc[i];
606 Try(PrimitiveCONST_Viper650(eMc_pose));
633 for (
unsigned int i = 0; i < 3; i++) {
634 eMc_pose[i] =
etc[i];
635 eMc_pose[i + 3] =
erc[i];
638 Try(PrimitiveCONST_Viper650(eMc_pose));
662 UInt32 HIPowerStatus;
663 Try(PrimitiveSTATUS_Viper650(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
674 ShutDownConnection();
676 vpRobotViper650::m_robotAlreadyCreated =
false;
696 Try(PrimitiveSTOP_Viper650());
703 std::cout <<
"Change the control mode from velocity to position control.\n";
704 Try(PrimitiveSTOP_Viper650());
714 std::cout <<
"Change the control mode from stop to velocity control.\n";
745 Try(PrimitiveSTOP_Viper650());
769 UInt32 HIPowerStatus;
771 bool firsttime =
true;
772 unsigned int nitermax = 10;
774 for (
unsigned int i = 0; i < nitermax; i++) {
775 Try(PrimitiveSTATUS_Viper650(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
776 if (EStopStatus == ESTOP_AUTO) {
777 m_controlMode =
AUTO;
779 }
else if (EStopStatus == ESTOP_MANUAL) {
782 }
else if (EStopStatus == ESTOP_ACTIVATED) {
783 m_controlMode =
ESTOP;
785 std::cout <<
"Emergency stop is activated! \n"
786 <<
"Check the emergency stop button and push the yellow "
787 "button before continuing."
791 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
795 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
796 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
798 ShutDownConnection();
803 if (EStopStatus == ESTOP_ACTIVATED)
804 std::cout << std::endl;
806 if (EStopStatus == ESTOP_ACTIVATED) {
807 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
811 if (HIPowerStatus == 0) {
812 fprintf(stdout,
"Power ON the Viper650 robot\n");
815 Try(PrimitivePOWERON_Viper650());
839 UInt32 HIPowerStatus;
840 Try(PrimitiveSTATUS_Viper650(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
843 if (HIPowerStatus == 1) {
844 fprintf(stdout,
"Power OFF the Viper650 robot\n");
847 Try(PrimitivePOWEROFF_Viper650());
873 UInt32 HIPowerStatus;
874 Try(PrimitiveSTATUS_Viper650(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
877 if (HIPowerStatus == 1) {
937 Try(PrimitiveACQ_POS_J_Viper650(position, ×tamp));
941 for (
unsigned int i = 0; i <
njoint; i++)
970 Try(PrimitiveACQ_POS_Viper650(position, ×tamp));
974 for (
unsigned int i = 0; i <
njoint; i++)
1112 "Modification of the robot state");
1124 Try(PrimitiveACQ_POS_Viper650(q.
data, ×tamp));
1136 for (
unsigned int i = 0; i < 3; i++) {
1137 txyz[i] = position[i];
1138 rxyz[i] = position[i + 3];
1154 Try(PrimitiveMOVE_J_Viper650(destination.
data, m_positioningVelocity));
1155 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1164 destination = position;
1170 Try(PrimitiveMOVE_J_Viper650(destination.
data, m_positioningVelocity));
1171 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1176 vpRxyzVector rxyz(position[3], position[4], position[5]);
1180 for (
unsigned int i = 0; i < 3; i++) {
1181 destination[i] = position[i];
1184 int configuration = 0;
1188 Try(PrimitiveMOVE_C_Viper650(destination.
data, configuration, m_positioningVelocity));
1189 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1195 "Mixt frame not implemented.");
1199 "Mixt frame not implemented.");
1204 if (TryStt == InvalidPosition || TryStt == -1023)
1205 std::cout <<
" : Position out of range.\n";
1207 else if (TryStt == -3019) {
1209 std::cout <<
" : Joint position out of range.\n";
1211 std::cout <<
" : Cartesian position leads to a joint position out of "
1213 }
else if (TryStt < 0)
1214 std::cout <<
" : Unknown error (see Fabien).\n";
1215 else if (error == -1)
1216 std::cout <<
"Position out of range.\n";
1218 if (TryStt < 0 || error < 0) {
1291 double pos4,
double pos5,
double pos6)
1442 Try(PrimitiveACQ_POS_J_Viper650(position.
data, ×tamp));
1450 Try(PrimitiveACQ_POS_J_Viper650(q.
data, ×tamp));
1461 for (
unsigned int i = 0; i < 3; i++) {
1462 position[i] = fMc[i][3];
1463 position[i + 3] = rxyz[i];
1522 for (
unsigned int j = 0; j < 3; j++)
1523 RxyzVect[j] = posRxyz[j + 3];
1528 for (
unsigned int j = 0; j < 3; j++) {
1529 position[j] = posRxyz[j];
1530 position[j + 3] = RtuVect[j];
1557 PrimitiveACQ_TIME_Viper650(×tamp);
1642 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1644 "Cannot send a velocity to the robot "
1645 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1659 for (
unsigned int i = 0; i < 3; i++)
1661 for (
unsigned int i = 3; i < 6; i++)
1674 for (
unsigned int i = 0; i < 6; i++)
1677 for (
unsigned int i = 0; i < 5; i++)
1692 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPCAM_VIPER650));
1702 Try(PrimitiveMOVESPEED_CART_Viper650(v_c.
data, REPCAM_VIPER650));
1710 Try(PrimitiveMOVESPEED_Viper650(vel_sat.
data));
1716 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPFIX_VIPER650));
1725 "Case not taken in account.");
1732 if (TryStt == VelStopOnJoint) {
1733 UInt32 axisInJoint[
njoint];
1734 PrimitiveSTATUS_Viper650(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, axisInJoint,
nullptr);
1735 for (
unsigned int i = 0; i <
njoint; i++) {
1737 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1740 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1741 if (TryString !=
nullptr) {
1743 printf(
" Error sentence %s\n", TryString);
1826 Try(PrimitiveACQ_POS_J_Viper650(q_cur.
data, ×tamp));
1827 time_cur = timestamp;
1835 if (!m_first_time_getvel) {
1840 eMe = m_fMe_prev_getvel.
inverse() * fMe_cur;
1850 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1859 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1865 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1880 cMc = m_fMc_prev_getvel.
inverse() * fMc_cur;
1888 for (
unsigned int i = 0; i < 3; i++) {
1890 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1892 velocity[i + 3] = thetaU[i];
1896 velocity /= (time_cur - m_time_prev_getvel);
1901 "vpRobotViper650::getVelocity() not implemented in end-effector"));
1905 m_first_time_getvel =
false;
1909 m_fMe_prev_getvel = fMe_cur;
1911 m_fMc_prev_getvel = fMc_cur;
1914 m_q_prev_getvel = q_cur;
1917 m_time_prev_getvel = time_cur;
2081 std::ifstream fd(filename.c_str(), std::ios::in);
2083 if (!fd.is_open()) {
2088 std::string key(
"R:");
2089 std::string id(
"#Viper650 - Position");
2090 bool pos_found =
false;
2095 while (std::getline(fd, line)) {
2098 if (!(line.compare(0,
id.size(),
id) == 0)) {
2099 std::cout <<
"Error: this position file " << filename <<
" is not for Viper650 robot" << std::endl;
2103 if ((line.compare(0, 1,
"#") == 0)) {
2106 if ((line.compare(0, key.size(), key) == 0)) {
2109 if (chain.size() <
njoint + 1)
2111 if (chain.size() <
njoint + 1)
2114 std::istringstream ss(line);
2117 for (
unsigned int i = 0; i <
njoint; i++)
2130 std::cout <<
"Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2164 fd = fopen(filename.c_str(),
"w");
2169 #Viper650 - Position - Version 1.00\n\
2172 # Joint position in degrees\n\
2238 Try(PrimitiveACQ_POS_Viper650(q, ×tamp));
2239 for (
unsigned int i = 0; i <
njoint; i++) {
2243 if (!m_first_time_getdis) {
2246 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2251 displacement = q_cur - m_q_prev_getdis;
2256 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2261 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2265 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2270 m_first_time_getdis =
false;
2274 m_q_prev_getdis = q_cur;
2300 Try(PrimitiveTFS_BIAS_Viper650());
2357 Try(PrimitiveTFS_ACQ_Viper650(H.
data));
2409 Try(PrimitiveTFS_ACQ_Viper650(H.
data));
2429 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2430 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2451 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2452 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2497 m_maxRotationVelocity_joint6 = w6_max;
2519 Try(PrimitivePneumaticGripper_Viper650(1));
2520 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2537 Try(PrimitivePneumaticGripper_Viper650(0));
2538 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2545 #elif !defined(VISP_BUILD_SHARED_LIBS)
2548 void dummy_vpRobotViper650(){};
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)
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.
void move(const std::string &filename)
void set_eMc(const vpHomogeneousMatrix &eMc_)
static bool savePosFile(const std::string &filename, const vpColVector &q)
static const double m_defaultPositioningVelocity
virtual ~vpRobotViper650(void)
void get_eJe(vpMatrix &eJe) override
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPositioningVelocity(double velocity)
void enableJoint6Limits() const
void get_cVe(vpVelocityTwistMatrix &cVe) const
vpColVector getForceTorque() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) override
bool getPowerState() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocityJoint6(double w6_max)
void disableJoint6Limits() const
void biasForceTorqueSensor() const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) override
void get_cMe(vpHomogeneousMatrix &cMe) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
double getMaxRotationVelocityJoint6() const
void get_fJe(vpMatrix &fJe) override
void setMaxRotationVelocity(double w_max)
static bool readPosFile(const std::string &filename, vpColVector &q)
double getPositioningVelocity(void) const
void closeGripper() const
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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 650 robot.
vpCameraParameters::vpCameraParametersProjType projModel
static const vpToolType defaultTool
Default tool attached to the robot end effector.
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)