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() );
278 UInt32 HIPowerStatus;
280 Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
286 std::cout <<
"Robot status: ";
287 switch(EStopStatus) {
290 if (HIPowerStatus == 0)
291 std::cout <<
"Power is OFF" << std::endl;
293 std::cout <<
"Power is ON" << std::endl;
298 if (HIPowerStatus == 0)
299 std::cout <<
"Power is OFF" << std::endl;
301 std::cout <<
"Power is ON" << std::endl;
303 case ESTOP_ACTIVATED:
305 std::cout <<
"Emergency stop is activated" << std::endl;
308 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
309 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
312 std::cout << std::endl;
329 if (TryStt == -20001)
330 printf(
"No connection detected. Check if the robot is powered on \n"
331 "and if the firewire link exist between the MotionBlox and this computer.\n");
332 else if (TryStt == -675)
333 printf(
" Timeout enabling power...\n");
337 PrimitivePOWEROFF_Viper650();
339 ShutDownConnection();
341 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
343 "Cannot open connexion with the motionblox");
417 for (
unsigned int i=0; i < 3; i ++) {
418 eMc_pose[i] =
etc[i];
419 eMc_pose[i+3] =
erc[i];
422 Try( PrimitiveCONST_Viper650(eMc_pose) );
457 UInt32 HIPowerStatus;
458 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
470 ShutDownConnection();
472 vpRobotViper650::robotAlreadyCreated =
false;
496 Try( PrimitiveSTOP_Viper650() );
502 std::cout <<
"Change the control mode from velocity to position control.\n";
503 Try( PrimitiveSTOP_Viper650() );
513 std::cout <<
"Change the control mode from stop to velocity control.\n";
546 Try( PrimitiveSTOP_Viper650() );
553 "Cannot stop robot motion.");
572 UInt32 HIPowerStatus;
574 bool firsttime =
true;
575 unsigned int nitermax = 10;
577 for (
unsigned int i=0; i<nitermax; i++) {
578 Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
580 if (EStopStatus == ESTOP_AUTO) {
584 else if (EStopStatus == ESTOP_MANUAL) {
588 else if (EStopStatus == ESTOP_ACTIVATED) {
591 std::cout <<
"Emergency stop is activated! \n"
592 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
595 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
600 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
601 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
603 ShutDownConnection();
608 if (EStopStatus == ESTOP_ACTIVATED)
609 std::cout << std::endl;
611 if (EStopStatus == ESTOP_ACTIVATED) {
612 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
614 "Cannot power on the robot.");
617 if (HIPowerStatus == 0) {
618 fprintf(stdout,
"Power ON the Viper650 robot\n");
621 Try( PrimitivePOWERON_Viper650() );
628 "Cannot power off the robot.");
647 UInt32 HIPowerStatus;
648 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
652 if (HIPowerStatus == 1) {
653 fprintf(stdout,
"Power OFF the Viper650 robot\n");
656 Try( PrimitivePOWEROFF_Viper650() );
663 "Cannot power off the robot.");
684 UInt32 HIPowerStatus;
685 Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
689 if (HIPowerStatus == 1) {
697 "Cannot get the power status.");
757 Try( PrimitiveACQ_POS_J_Viper650(position, ×tamp) );
761 for (
unsigned int i=0; i <
njoint; i++)
794 Try( PrimitiveACQ_POS_Viper650(position, ×tamp) );
798 for (
unsigned int i=0; i <
njoint; i++)
852 positioningVelocity = velocity;
863 return positioningVelocity;
952 "Modification of the robot state");
964 Try( PrimitiveACQ_POS_Viper650(q.
data, ×tamp) );
976 for (
unsigned int i=0; i < 3; i++) {
977 txyz[i] = position[i];
978 rxyz[i] = position[i+3];
994 Try( PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity) );
995 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1005 destination = position;
1010 Try( PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity) );
1011 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1017 vpRxyzVector rxyz(position[3],position[4],position[5]);
1021 for (
unsigned int i=0; i <3; i++) {
1022 destination[i] = position[i];
1025 int configuration = 0;
1028 Try( PrimitiveMOVE_C_Viper650(destination.
data, configuration,
1029 positioningVelocity) );
1030 Try( WaitState_Viper650(ETAT_ATTENTE_AFMA6, 1000) );
1036 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1038 "Positionning error: "
1039 "Mixt frame not implemented.");
1045 if (TryStt == InvalidPosition || TryStt == -1023)
1046 std::cout <<
" : Position out of range.\n";
1048 else if (TryStt == -3019) {
1050 std::cout <<
" : Joint position out of range.\n";
1052 std::cout <<
" : Cartesian position leads to a joint position out of range.\n";
1054 else if (TryStt < 0)
1055 std::cout <<
" : Unknown error (see Fabien).\n";
1056 else if (error == -1)
1057 std::cout <<
"Position out of range.\n";
1059 if (TryStt < 0 || error < 0) {
1062 "Position out of range.");
1143 position[0] = pos1 ;
1144 position[1] = pos2 ;
1145 position[2] = pos3 ;
1146 position[3] = pos4 ;
1147 position[4] = pos5 ;
1148 position[5] = pos6 ;
1208 "Bad position in filename.");
1297 Try( PrimitiveACQ_POS_J_Viper650(position.
data, ×tamp) );
1304 Try( PrimitiveACQ_POS_C_Viper650(position.
data, ×tamp) );
1308 for (
unsigned int i=3; i <6; i++)
1311 vpRzyzVector rzyz(position[3], position[4], position[5]);
1316 for (
unsigned int i=0; i <3; i++)
1317 position[i+3] = rxyz[i];
1327 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1329 "Cannot get position in mixt frame: "
1339 "Cannot get position.");
1380 for (
unsigned int j=0;j<3;j++)
1381 RxyzVect[j]=posRxyz[j+3];
1386 for (
unsigned int j=0;j<3;j++)
1388 position[j]=posRxyz[j];
1389 position[j+3]=RtuVect[j];
1415 PrimitiveACQ_TIME_Viper650(×tamp);
1497 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1499 "Cannot send a velocity to the robot "
1500 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1513 for (
unsigned int i=0; i<3; i++)
1515 for (
unsigned int i=3; i<6; i++)
1526 for (
unsigned int i=0; i<6; i++)
1539 Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPCAM_VIPER650) );
1547 Try( PrimitiveMOVESPEED_Viper650(vel_sat.
data) );
1552 std::cout <<
"Vitesse ref appliquee: " << vel_sat.
t();
1553 Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPFIX_VIPER650) );
1562 "Case not taken in account.");
1569 if (TryStt == VelStopOnJoint) {
1570 UInt32 axisInJoint[
njoint];
1571 PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1572 for (
unsigned int i=0; i <
njoint; i ++) {
1574 std::cout <<
"\nWarning: Velocity control stopped: axis "
1575 << i+1 <<
" on joint limit!" <<std::endl;
1579 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1580 if (TryString != NULL) {
1582 printf(
" Error sentence %s\n", TryString);
1673 Try( PrimitiveACQ_POS_J_Viper650(q_cur.
data, ×tamp) );
1674 time_cur = timestamp;
1680 if ( ! first_time_getvel ) {
1685 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1694 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1700 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1715 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1723 for (
unsigned int i=0; i < 3; i++) {
1725 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1727 velocity[i+3] = thetaU[i];
1731 velocity /= (time_cur - time_prev_getvel);
1737 first_time_getvel =
false;
1741 fMc_prev_getvel = fMc_cur;
1744 q_prev_getvel = q_cur;
1747 time_prev_getvel = time_cur;
1754 "Cannot get velocity.");
1917 fd = fopen(filename,
"r") ;
1921 char line[FILENAME_MAX];
1922 char dummy[FILENAME_MAX];
1924 bool sortie =
false;
1928 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1929 if ( strncmp (line,
"#", 1) != 0) {
1931 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1943 while ( sortie !=
true );
1947 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1949 &q[0], &q[1], &q[2],
1950 &q[3], &q[4], &q[5]);
1987 fd = fopen(filename,
"w") ;
1992 #Viper - Position - Version 1.0\n\
1995 # Joint position in degrees\n\
2000 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2052 vpRobotViper650::getCameraDisplacement(
vpColVector &displacement)
2068 vpRobotViper650::getArticularDisplacement(
vpColVector &displacement)
2107 Try( PrimitiveACQ_POS_Viper650(q, ×tamp) );
2108 for (
unsigned int i=0; i <
njoint; i ++) {
2112 if ( ! first_time_getdis ) {
2115 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2121 displacement = q_cur - q_prev_getdis;
2126 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2132 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2139 first_time_getdis =
false;
2143 q_prev_getdis = q_cur;
2149 "Cannot get velocity.");
2171 Try( PrimitiveTFS_BIAS_Viper650() );
2180 "Cannot bias the force/torque sensor.");
2230 Try( PrimitiveTFS_ACQ_Viper650(H.
data) );
2236 "Cannot get force/torque measures.");
static const double defaultPositioningVelocity
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
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 get_eJe(const vpColVector &q, vpMatrix &eJe)
void setVerbose(bool verbose)
void setPositioningVelocity(const double velocity)
virtual vpRobotStateType getRobotState(void)
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 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)
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q)
The vpRotationMatrix considers the particular case of a rotation matrix.
double * data
address of the first element of the data array
double getPositioningVelocity(void)
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 extract(vpRotationMatrix &R) 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.
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
void get_fJe(vpMatrix &fJe)
void get_cMe(vpHomogeneousMatrix &cMe)
static bool savePosFile(const char *filename, const vpColVector &q)
void getForceTorque(vpColVector &H)
static double deg(double rad)
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
void get_cMe(vpHomogeneousMatrix &cMe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
void biasForceTorqueSensor()
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)
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix get_fMc(const vpColVector &q)
void get_eJe(vpMatrix &eJe)
void get_cVe(vpVelocityTwistMatrix &cVe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void resize(const unsigned int i, const bool flagNullify=true)