42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_VIPER650
48 #include <sys/types.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpExponentialMap.h>
53 #include <visp/vpDebug.h>
54 #include <visp/vpVelocityTwistMatrix.h>
55 #include <visp/vpThetaUVector.h>
56 #include <visp/vpRobot.h>
57 #include <visp/vpRobotViper650.h>
63 bool vpRobotViper650::robotAlreadyCreated =
false;
85 void emergencyStopViper650(
int signo)
87 std::cout <<
"Stop the Viper650 application by signal ("
88 << signo <<
"): " << (char)7 ;
92 std::cout <<
"SIGINT (stop by ^C) " << std::endl ; break ;
94 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl ; break ;
96 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
98 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
100 std::cout <<
"SIGQUIT " << std::endl ; break ;
102 std::cout << signo << std::endl ;
106 PrimitiveSTOP_Viper650();
107 std::cout <<
"Robot was stopped\n";
112 fprintf(stdout,
"Application ");
114 kill(getpid(), SIGKILL);
177 vpRobotViper650::vpRobotViper650 (
bool verbose)
202 signal(SIGINT, emergencyStopViper650);
203 signal(SIGBUS, emergencyStopViper650) ;
204 signal(SIGSEGV, emergencyStopViper650) ;
205 signal(SIGKILL, emergencyStopViper650);
206 signal(SIGQUIT, emergencyStopViper650);
210 std::cout <<
"Open communication with MotionBlox.\n";
221 vpRobotViper650::robotAlreadyCreated =
true;
258 time_prev_getvel = 0;
259 first_time_getvel =
true;
264 first_time_getdis =
true;
267 Try( InitializeConnection(
verbose_) );
270 Try( InitializeNode_Viper650() );
272 Try( PrimitiveRESET_Viper650() );
275 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
281 UInt32 HIPowerStatus;
283 Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
289 std::cout <<
"Robot status: ";
290 switch(EStopStatus) {
293 if (HIPowerStatus == 0)
294 std::cout <<
"Power is OFF" << std::endl;
296 std::cout <<
"Power is ON" << std::endl;
301 if (HIPowerStatus == 0)
302 std::cout <<
"Power is OFF" << std::endl;
304 std::cout <<
"Power is ON" << std::endl;
306 case ESTOP_ACTIVATED:
308 std::cout <<
"Emergency stop is activated" << std::endl;
311 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
312 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
315 std::cout << std::endl;
332 if (TryStt == -20001)
333 printf(
"No connection detected. Check if the robot is powered on \n"
334 "and if the firewire link exist between the MotionBlox and this computer.\n");
335 else if (TryStt == -675)
336 printf(
" Timeout enabling power...\n");
340 PrimitivePOWEROFF_Viper650();
342 ShutDownConnection();
344 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
346 "Cannot open connexion with the motionblox");
420 for (
unsigned int i=0; i < 3; i ++) {
421 eMc_pose[i] =
etc[i];
422 eMc_pose[i+3] =
erc[i];
425 Try( PrimitiveCONST_Viper650(eMc_pose) );
460 UInt32 HIPowerStatus;
461 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
473 ShutDownConnection();
475 vpRobotViper650::robotAlreadyCreated =
false;
499 Try( PrimitiveSTOP_Viper650() );
505 std::cout <<
"Change the control mode from velocity to position control.\n";
506 Try( PrimitiveSTOP_Viper650() );
516 std::cout <<
"Change the control mode from stop to velocity control.\n";
549 Try( PrimitiveSTOP_Viper650() );
556 "Cannot stop robot motion.");
575 UInt32 HIPowerStatus;
577 bool firsttime =
true;
578 unsigned int nitermax = 10;
580 for (
unsigned int i=0; i<nitermax; i++) {
581 Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
583 if (EStopStatus == ESTOP_AUTO) {
587 else if (EStopStatus == ESTOP_MANUAL) {
591 else if (EStopStatus == ESTOP_ACTIVATED) {
594 std::cout <<
"Emergency stop is activated! \n"
595 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
598 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
603 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
604 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
606 ShutDownConnection();
611 if (EStopStatus == ESTOP_ACTIVATED)
612 std::cout << std::endl;
614 if (EStopStatus == ESTOP_ACTIVATED) {
615 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
617 "Cannot power on the robot.");
620 if (HIPowerStatus == 0) {
621 fprintf(stdout,
"Power ON the Viper650 robot\n");
624 Try( PrimitivePOWERON_Viper650() );
631 "Cannot power off the robot.");
650 UInt32 HIPowerStatus;
651 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
655 if (HIPowerStatus == 1) {
656 fprintf(stdout,
"Power OFF the Viper650 robot\n");
659 Try( PrimitivePOWEROFF_Viper650() );
666 "Cannot power off the robot.");
687 UInt32 HIPowerStatus;
688 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
692 if (HIPowerStatus == 1) {
700 "Cannot get the power status.");
760 Try( PrimitiveACQ_POS_J_Viper650(position, ×tamp) );
764 for (
unsigned int i=0; i <
njoint; i++)
797 Try( PrimitiveACQ_POS_Viper650(position, ×tamp) );
801 for (
unsigned int i=0; i <
njoint; i++)
855 positioningVelocity = velocity;
866 return positioningVelocity;
955 "Modification of the robot state");
967 Try( PrimitiveACQ_POS_Viper650(q.
data, ×tamp) );
979 for (
unsigned int i=0; i < 3; i++) {
980 txyz[i] = position[i];
981 rxyz[i] = position[i+3];
997 Try( PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity) );
998 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1008 destination = position;
1013 Try( PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity) );
1014 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1020 vpRxyzVector rxyz(position[3],position[4],position[5]);
1024 for (
unsigned int i=0; i <3; i++) {
1025 destination[i] = position[i];
1028 int configuration = 0;
1031 Try( PrimitiveMOVE_C_Viper650(destination.
data, configuration,
1032 positioningVelocity) );
1033 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1039 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1041 "Positionning error: "
1042 "Mixt frame not implemented.");
1048 if (TryStt == InvalidPosition || TryStt == -1023)
1049 std::cout <<
" : Position out of range.\n";
1051 else if (TryStt == -3019) {
1053 std::cout <<
" : Joint position out of range.\n";
1055 std::cout <<
" : Cartesian position leads to a joint position out of range.\n";
1057 else if (TryStt < 0)
1058 std::cout <<
" : Unknown error (see Fabien).\n";
1059 else if (error == -1)
1060 std::cout <<
"Position out of range.\n";
1062 if (TryStt < 0 || error < 0) {
1065 "Position out of range.");
1146 position[0] = pos1 ;
1147 position[1] = pos2 ;
1148 position[2] = pos3 ;
1149 position[3] = pos4 ;
1150 position[4] = pos5 ;
1151 position[5] = pos6 ;
1211 "Bad position in filename.");
1300 Try( PrimitiveACQ_POS_J_Viper650(position.
data, ×tamp) );
1307 Try( PrimitiveACQ_POS_C_Viper650(position.
data, ×tamp) );
1311 for (
unsigned int i=3; i <6; i++)
1314 vpRzyzVector rzyz(position[3], position[4], position[5]);
1319 for (
unsigned int i=0; i <3; i++)
1320 position[i+3] = rxyz[i];
1330 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1332 "Cannot get position in mixt frame: "
1342 "Cannot get position.");
1383 for (
unsigned int j=0;j<3;j++)
1384 RxyzVect[j]=posRxyz[j+3];
1389 for (
unsigned int j=0;j<3;j++)
1391 position[j]=posRxyz[j];
1392 position[j+3]=RtuVect[j];
1418 PrimitiveACQ_TIME_Viper650(×tamp);
1500 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1502 "Cannot send a velocity to the robot "
1503 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1516 for (
unsigned int i=0; i<3; i++)
1518 for (
unsigned int i=3; i<6; i++)
1529 for (
unsigned int i=0; i<6; i++)
1542 Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPCAM_VIPER650) );
1550 Try( PrimitiveMOVESPEED_Viper650(vel_sat.
data) );
1555 std::cout <<
"Vitesse ref appliquee: " << vel_sat.
t();
1556 Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPFIX_VIPER650) );
1565 "Case not taken in account.");
1572 if (TryStt == VelStopOnJoint) {
1573 UInt32 axisInJoint[
njoint];
1574 PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1575 for (
unsigned int i=0; i <
njoint; i ++) {
1577 std::cout <<
"\nWarning: Velocity control stopped: axis "
1578 << i+1 <<
" on joint limit!" <<std::endl;
1582 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1583 if (TryString != NULL) {
1585 printf(
" Error sentence %s\n", TryString);
1676 Try( PrimitiveACQ_POS_J_Viper650(q_cur.
data, ×tamp) );
1677 time_cur = timestamp;
1683 if ( ! first_time_getvel ) {
1688 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1697 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1703 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1718 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1726 for (
unsigned int i=0; i < 3; i++) {
1728 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1730 velocity[i+3] = thetaU[i];
1734 velocity /= (time_cur - time_prev_getvel);
1740 first_time_getvel =
false;
1744 fMc_prev_getvel = fMc_cur;
1747 q_prev_getvel = q_cur;
1750 time_prev_getvel = time_cur;
1757 "Cannot get velocity.");
1920 fd = fopen(filename,
"r") ;
1924 char line[FILENAME_MAX];
1925 char dummy[FILENAME_MAX];
1927 bool sortie =
false;
1931 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1932 if ( strncmp (line,
"#", 1) != 0) {
1934 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1946 while ( sortie !=
true );
1950 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1952 &q[0], &q[1], &q[2],
1953 &q[3], &q[4], &q[5]);
1990 fd = fopen(filename,
"w") ;
1995 #Viper650 - Position - Version 1.00\n\
1998 # Joint position in degrees\n\
2003 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2055 vpRobotViper650::getCameraDisplacement(
vpColVector &displacement)
2071 vpRobotViper650::getArticularDisplacement(
vpColVector &displacement)
2110 Try( PrimitiveACQ_POS_Viper650(q, ×tamp) );
2111 for (
unsigned int i=0; i <
njoint; i ++) {
2115 if ( ! first_time_getdis ) {
2118 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2124 displacement = q_cur - q_prev_getdis;
2129 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2135 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2142 first_time_getdis =
false;
2146 q_prev_getdis = q_cur;
2152 "Cannot get velocity.");
2174 Try( PrimitiveTFS_BIAS_Viper650() );
2183 "Cannot bias the force/torque sensor.");
2233 Try( PrimitiveTFS_ACQ_Viper650(H.
data) );
2239 "Cannot get force/torque measures.");
2251 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
2252 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2257 "Cannot enable joint limits on axis 6.");
2273 Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1) );
2274 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2279 "Cannot disable joint limits on axis 6.");
static const double defaultPositioningVelocity
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotViper650(void)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setVerbose(bool verbose)
void setPositioningVelocity(const double velocity)
double getMaxTranslationVelocity(void) const
Initialize the position controller.
class that defines a generic virtual robot
Automatic control mode (default).
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
static int wait(double t0, double t)
void move(const char *filename)
double getMaxRotationVelocity(void) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
The vpRotationMatrix considers the particular case of a rotation matrix.
double * data
address of the first element of the data array
static const vpToolType defaultTool
Default tool attached to the robot end effector.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
Emergency stop activated.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParametersProjType
vpRowVector t() const
transpose of Vector
Modelisation of the ADEPT Viper 650 robot.
void enableJoint6Limits() const
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
double getPositioningVelocity(void) const
Manual control mode activated when the dead man switch is in use.
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpToolType
List of possible tools that can be attached to the robot end-effector.
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
static double rad(double deg)
void get_fJe(vpMatrix &fJe)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
static bool savePosFile(const char *filename, const vpColVector &q)
void get_cMe(vpHomogeneousMatrix &cMe) const
static double deg(double rad)
void biasForceTorqueSensor() const
Class that provides a data structure for the column vectors as well as a set of operations on these v...
static bool readPosFile(const char *filename, vpColVector &q)
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
void disableJoint6Limits() const
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Class that consider the case of the Euler angles using the z-y-z convention, where are respectively...
void setToolType(vpViper650::vpToolType tool)
Set the current tool type.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_cMe(vpHomogeneousMatrix &cMe) const
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void getForceTorque(vpColVector &H) const
static const unsigned int njoint
Number of joint.
void get_eJe(vpMatrix &eJe)
bool getPowerState() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void resize(const unsigned int i, const bool flagNullify=true)