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);
199 signal(SIGINT, emergencyStopViper850);
200 signal(SIGBUS, emergencyStopViper850);
201 signal(SIGSEGV, emergencyStopViper850);
202 signal(SIGKILL, emergencyStopViper850);
203 signal(SIGQUIT, emergencyStopViper850);
207 std::cout <<
"Open communication with MotionBlox.\n";
219 vpRobotViper850::robotAlreadyCreated =
true;
259 time_prev_getvel = 0;
260 first_time_getvel =
true;
265 first_time_getdis =
true;
267 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI) 268 std::string calibfile;
269 #ifdef VISP_HAVE_VIPER850_DATA 270 calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
275 "data to retrive ATI F/T calib " 283 Try(InitializeConnection(
verbose_));
286 Try(InitializeNode_Viper850());
288 Try(PrimitiveRESET_Viper850());
291 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
297 UInt32 HIPowerStatus;
299 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
304 std::cout <<
"Robot status: ";
305 switch (EStopStatus) {
308 if (HIPowerStatus == 0)
309 std::cout <<
"Power is OFF" << std::endl;
311 std::cout <<
"Power is ON" << std::endl;
316 if (HIPowerStatus == 0)
317 std::cout <<
"Power is OFF" << std::endl;
319 std::cout <<
"Power is ON" << std::endl;
321 case ESTOP_ACTIVATED:
323 std::cout <<
"Emergency stop is activated" << std::endl;
326 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
327 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
330 std::cout << std::endl;
348 if (TryStt == -20001)
349 printf(
"No connection detected. Check if the robot is powered on \n" 350 "and if the firewire link exist between the MotionBlox and this " 352 else if (TryStt == -675)
353 printf(
" Timeout enabling power...\n");
357 PrimitivePOWEROFF_Viper850();
359 ShutDownConnection();
361 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
446 for (
unsigned int i = 0; i < 3; i++) {
447 eMc_pose[i] =
etc[i];
448 eMc_pose[i + 3] =
erc[i];
451 Try(PrimitiveCONST_Viper850(eMc_pose));
526 for (
unsigned int i = 0; i < 3; i++) {
527 eMc_pose[i] =
etc[i];
528 eMc_pose[i + 3] =
erc[i];
531 Try(PrimitiveCONST_Viper850(eMc_pose));
592 for (
unsigned int i = 0; i < 3; i++) {
593 eMc_pose[i] =
etc[i];
594 eMc_pose[i + 3] =
erc[i];
597 Try(PrimitiveCONST_Viper850(eMc_pose));
622 for (
unsigned int i = 0; i < 3; i++) {
623 eMc_pose[i] =
etc[i];
624 eMc_pose[i + 3] =
erc[i];
627 Try(PrimitiveCONST_Viper850(eMc_pose));
654 for (
unsigned int i = 0; i < 3; i++) {
655 eMc_pose[i] =
etc[i];
656 eMc_pose[i + 3] =
erc[i];
659 Try(PrimitiveCONST_Viper850(eMc_pose));
678 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI) 687 UInt32 HIPowerStatus;
688 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
699 ShutDownConnection();
701 vpRobotViper850::robotAlreadyCreated =
false;
721 Try(PrimitiveSTOP_Viper850());
728 std::cout <<
"Change the control mode from velocity to position control.\n";
729 Try(PrimitiveSTOP_Viper850());
739 std::cout <<
"Change the control mode from stop to velocity control.\n";
770 Try(PrimitiveSTOP_Viper850());
794 UInt32 HIPowerStatus;
796 bool firsttime =
true;
797 unsigned int nitermax = 10;
799 for (
unsigned int i = 0; i < nitermax; i++) {
800 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
801 if (EStopStatus == ESTOP_AUTO) {
804 }
else if (EStopStatus == ESTOP_MANUAL) {
807 }
else if (EStopStatus == ESTOP_ACTIVATED) {
810 std::cout <<
"Emergency stop is activated! \n" 811 <<
"Check the emergency stop button and push the yellow " 812 "button before continuing." 816 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
820 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
821 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
823 ShutDownConnection();
828 if (EStopStatus == ESTOP_ACTIVATED)
829 std::cout << std::endl;
831 if (EStopStatus == ESTOP_ACTIVATED) {
832 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
836 if (HIPowerStatus == 0) {
837 fprintf(stdout,
"Power ON the Viper850 robot\n");
840 Try(PrimitivePOWERON_Viper850());
864 UInt32 HIPowerStatus;
865 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
868 if (HIPowerStatus == 1) {
869 fprintf(stdout,
"Power OFF the Viper850 robot\n");
872 Try(PrimitivePOWEROFF_Viper850());
898 UInt32 HIPowerStatus;
899 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
902 if (HIPowerStatus == 1) {
962 Try(PrimitiveACQ_POS_J_Viper850(position, ×tamp));
966 for (
unsigned int i = 0; i <
njoint; i++)
995 Try(PrimitiveACQ_POS_Viper850(position, ×tamp));
999 for (
unsigned int i = 0; i <
njoint; i++)
1139 "Modification of the robot state");
1151 Try(PrimitiveACQ_POS_Viper850(q.
data, ×tamp));
1163 for (
unsigned int i = 0; i < 3; i++) {
1164 txyz[i] = position[i];
1165 rxyz[i] = position[i + 3];
1181 Try(PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity));
1182 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1191 destination = position;
1197 Try(PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity));
1198 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1203 vpRxyzVector rxyz(position[3], position[4], position[5]);
1207 for (
unsigned int i = 0; i < 3; i++) {
1208 destination[i] = position[i];
1211 int configuration = 0;
1215 Try(PrimitiveMOVE_C_Viper850(destination.
data, configuration, positioningVelocity));
1216 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1222 "Mixt frame not implemented.");
1226 "End-effector frame not implemented.");
1231 if (TryStt == InvalidPosition || TryStt == -1023)
1232 std::cout <<
" : Position out of range.\n";
1233 else if (TryStt == -3019) {
1235 std::cout <<
" : Joint position out of range.\n";
1237 std::cout <<
" : Cartesian position leads to a joint position out of " 1239 }
else if (TryStt < 0)
1240 std::cout <<
" : Unknown error (see Fabien).\n";
1241 else if (error == -1)
1242 std::cout <<
"Position out of range.\n";
1244 if (TryStt < 0 || error < 0) {
1318 const double pos3,
const double pos4,
const double pos5,
const double pos6)
1471 Try(PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp));
1479 Try(PrimitiveACQ_POS_J_Viper850(q.
data, ×tamp));
1490 for (
unsigned int i = 0; i < 3; i++) {
1491 position[i] = fMc[i][3];
1492 position[i + 3] = rxyz[i];
1523 PrimitiveACQ_TIME_Viper850(×tamp);
1560 for (
unsigned int j = 0; j < 3; j++)
1561 RxyzVect[j] = posRxyz[j + 3];
1566 for (
unsigned int j = 0; j < 3; j++) {
1567 position[j] = posRxyz[j];
1568 position[j + 3] = RtuVect[j];
1589 for (
unsigned int j = 0; j < 3; j++)
1590 RxyzVect[j] = posRxyz[j + 3];
1595 for (
unsigned int j = 0; j < 3; j++) {
1596 position[j] = posRxyz[j];
1597 position[j + 3] = RtuVect[j];
1683 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1685 "Cannot send a velocity to the robot " 1686 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1700 for (
unsigned int i = 0; i < 3; i++)
1702 for (
unsigned int i = 3; i < 6; i++)
1716 for (
unsigned int i = 0; i < 6; i++)
1719 for (
unsigned int i = 0; i < 5; i++)
1734 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850));
1744 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.
data, REPCAM_VIPER850));
1752 Try(PrimitiveMOVESPEED_Viper850(vel_sat.
data));
1758 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850));
1767 "Case not taken in account.");
1774 if (TryStt == VelStopOnJoint) {
1775 UInt32 axisInJoint[
njoint];
1776 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1777 for (
unsigned int i = 0; i <
njoint; i++) {
1779 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1782 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1783 if (TryString != NULL) {
1785 printf(
" Error sentence %s\n", TryString);
1870 Try(PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp));
1871 time_cur = timestamp;
1877 if (!first_time_getvel) {
1882 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1891 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1897 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1912 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1920 for (
unsigned int i = 0; i < 3; i++) {
1922 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1924 velocity[i + 3] = thetaU[i];
1928 velocity /= (time_cur - time_prev_getvel);
1933 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1937 first_time_getvel =
false;
1941 fMc_prev_getvel = fMc_cur;
1944 q_prev_getvel = q_cur;
1947 time_prev_getvel = time_cur;
2113 std::ifstream fd(filename.c_str(), std::ios::in);
2115 if (!fd.is_open()) {
2120 std::string key(
"R:");
2121 std::string id(
"#Viper850 - Position");
2122 bool pos_found =
false;
2127 while (std::getline(fd, line)) {
2130 if (!(line.compare(0,
id.size(), id) == 0)) {
2131 std::cout <<
"Error: this position file " << filename <<
" is not for Viper850 robot" << std::endl;
2135 if ((line.compare(0, 1,
"#") == 0)) {
2138 if ((line.compare(0, key.size(), key) == 0)) {
2141 if (chain.size() <
njoint + 1)
2143 if (chain.size() <
njoint + 1)
2146 std::istringstream ss(line);
2149 for (
unsigned int i = 0; i <
njoint; i++)
2162 std::cout <<
"Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2196 fd = fopen(filename.c_str(),
"w");
2201 #Viper850 - Position - Version 1.00\n\ 2204 # Joint position in degrees\n\ 2270 Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
2271 for (
unsigned int i = 0; i <
njoint; i++) {
2275 if (!first_time_getdis) {
2278 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2283 displacement = q_cur - q_prev_getdis;
2288 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2293 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2297 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2302 first_time_getdis =
false;
2306 q_prev_getdis = q_cur;
2324 #if defined(USE_ATI_DAQ) 2325 #if defined(VISP_HAVE_COMEDI) 2329 "apt-get install libcomedi-dev"));
2331 #else // Use serial link 2334 Try(PrimitiveTFS_BIAS_Viper850());
2356 #if defined(USE_ATI_DAQ) 2357 #if defined(VISP_HAVE_COMEDI) 2361 "apt-get install libcomedi-dev"));
2363 #else // Use serial link 2410 #if defined(USE_ATI_DAQ) 2411 #if defined(VISP_HAVE_COMEDI) 2416 "apt-get install libcomedi-dev"));
2418 #else // Use serial link 2423 Try(PrimitiveTFS_ACQ_Viper850(H.
data));
2473 #if defined(USE_ATI_DAQ) 2474 #if defined(VISP_HAVE_COMEDI) 2479 "apt-get install libcomedi-dev"));
2481 #else // Use serial link 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) 2626 void dummy_vpRobotViper850(){};
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocity(double w_max)
void set_eMc(const vpHomogeneousMatrix &eMc_)
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
VISP_EXPORT int wait(double t0, double t)
Error that can be emited by the vpRobot class and its derivates.
double getPositioningVelocity(void) const
static const vpToolType defaultTool
Default tool attached to the robot end effector.
void closeGripper() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
vpColVector getForceTorque() const
void setPositioningVelocity(const double velocity)
double maxRotationVelocity
double getMaxTranslationVelocity(void) const
void enableJoint6Limits() const
Initialize the position controller.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
error that can be emited by ViSP classes.
Class that defines a generic virtual robot.
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Type * data
Address of the first element of the data array.
void move(const std::string &filename)
void get_eJe(vpMatrix &eJe)
void unbiasForceTorqueSensor()
void disableJoint6Limits() const
double getMaxRotationVelocity(void) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Initialize the velocity controller.
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParametersProjType
VISP_EXPORT void sleepMs(double t)
vpToolType
List of possible tools that can be attached to the robot end-effector.
Modelisation of the ADEPT Viper 850 robot.
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
vpColVector getForceTorque() const
bool getPowerState() const
void get_fJe(vpMatrix &fJe)
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
static double rad(double deg)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
static bool readPosFile(const std::string &filename, vpColVector &q)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setMaxRotationVelocity(const double maxVr)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void get_cMe(vpHomogeneousMatrix &cMe) const
void biasForceTorqueSensor()
void setMaxRotationVelocityJoint6(double w6_max)
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Implementation of column vector and the associated operations.
static const double defaultPositioningVelocity
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
double getMaxRotationVelocityJoint6() const
Implementation of a rotation vector as Euler angle minimal representation.
Emergency stop activated.
vpCameraParameters::vpCameraParametersProjType projModel
Implementation of a rotation vector as Euler angle minimal representation.
virtual ~vpRobotViper850(void)
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
Automatic control mode (default).
Function not implemented.
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static const unsigned int njoint
Number of joint.
void get_cMe(vpHomogeneousMatrix &cMe) const
void resize(const unsigned int i, const bool flagNullify=true)