42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_AFMA4
49 #include <visp/vpRobotException.h>
50 #include <visp/vpExponentialMap.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpVelocityTwistMatrix.h>
53 #include <visp/vpThetaUVector.h>
54 #include <visp/vpRobotAfma4.h>
60 bool vpRobotAfma4::robotAlreadyCreated =
false;
83 void emergencyStopAfma4(
int signo)
85 std::cout <<
"Stop the Afma4 application by signal ("
86 << signo <<
"): " << (char)7 ;
90 std::cout <<
"SIGINT (stop by ^C) " << std::endl ; break ;
92 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl ; break ;
94 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
96 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
98 std::cout <<
"SIGQUIT " << std::endl ; break ;
100 std::cout << signo << std::endl ;
104 PrimitiveSTOP_Afma4();
105 std::cout <<
"Robot was stopped\n";
110 fprintf(stdout,
"Application ");
112 kill(getpid(), SIGKILL);
157 signal(SIGINT, emergencyStopAfma4);
158 signal(SIGBUS, emergencyStopAfma4) ;
159 signal(SIGSEGV, emergencyStopAfma4) ;
160 signal(SIGKILL, emergencyStopAfma4);
161 signal(SIGQUIT, emergencyStopAfma4);
163 std::cout <<
"Open communication with MotionBlox.\n";
174 vpRobotAfma4::robotAlreadyCreated =
true;
201 time_prev_getvel = 0;
202 first_time_getvel =
true;
207 first_time_getdis =
true;
210 Try( stt = InitializeConnection() );
212 if (stt != SUCCESS) {
213 vpERROR_TRACE (
"Cannot open connexion with the motionblox.");
215 "Cannot open connexion with the motionblox");
219 Try( stt = InitializeNode_Afma4() );
221 if (stt != SUCCESS) {
222 vpERROR_TRACE (
"Cannot open connexion with the motionblox.");
224 "Cannot open connexion with the motionblox");
226 Try( PrimitiveRESET_Afma4() );
229 UInt32 HIPowerStatus;
231 Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
236 std::cout <<
"Robot status: ";
237 switch(EStopStatus) {
240 if (HIPowerStatus == 0)
241 std::cout <<
"Power is OFF" << std::endl;
243 std::cout <<
"Power is ON" << std::endl;
245 case ESTOP_ACTIVATED:
246 std::cout <<
"Emergency stop is activated" << std::endl;
249 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
250 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
253 std::cout << std::endl;
266 if (TryStt == -20001)
267 printf(
"No connection detected. Check if the robot is powered on \n"
268 "and if the firewire link exist between the MotionBlox and this computer.\n");
269 else if (TryStt == -675)
270 printf(
" Timeout enabling power...\n");
274 PrimitivePOWEROFF_Afma4();
276 ShutDownConnection();
278 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
280 "Cannot open connexion with the motionblox");
303 UInt32 HIPowerStatus;
304 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
316 ShutDownConnection();
318 vpRobotAfma4::robotAlreadyCreated =
false;
342 Try( PrimitiveSTOP_Afma4() );
348 std::cout <<
"Change the control mode from velocity to position control.\n";
349 Try( PrimitiveSTOP_Afma4() );
359 std::cout <<
"Change the control mode from stop to velocity control.\n";
388 Try( PrimitiveSTOP_Afma4() );
395 "Cannot stop robot motion.");
415 UInt32 HIPowerStatus;
417 bool firsttime =
true;
420 for (
int i=0; i<nitermax; i++) {
421 Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
423 switch(EStopStatus) {
424 case ESTOP_AUTO:
break;
425 case ESTOP_MANUAL:
break;
426 case ESTOP_ACTIVATED:
428 std::cout <<
"Emergency stop is activated! \n"
429 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
432 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
437 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
438 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
440 ShutDownConnection();
445 std::cout << std::endl;
447 if (EStopStatus == ESTOP_ACTIVATED) {
448 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
450 "Cannot power on the robot.");
453 if (HIPowerStatus == 0) {
454 fprintf(stdout,
"Power ON the Afma4 robot\n");
457 Try( PrimitivePOWERON_Afma4() );
464 "Cannot power off the robot.");
483 UInt32 HIPowerStatus;
484 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
488 if (HIPowerStatus == 1) {
489 fprintf(stdout,
"Power OFF the Afma4 robot\n");
492 Try( PrimitivePOWEROFF_Afma4() );
499 "Cannot power off the robot.");
520 UInt32 HIPowerStatus;
521 Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
525 if (HIPowerStatus == 1) {
533 "Cannot get the power status.");
568 double position[this->
njoint];
571 Try( PrimitiveACQ_POS_Afma4(position) );
575 for (
unsigned int i=0; i <
njoint; i++)
618 double position[this->
njoint];
621 Try( PrimitiveACQ_POS_Afma4(position) );
625 for (
unsigned int i=0; i <
njoint; i++)
658 Try( PrimitiveACQ_POS_Afma4(position) );
662 for (
unsigned int i=0; i <
njoint; i++)
704 positioningVelocity = velocity;
715 return positioningVelocity;
797 "Modification of the robot state");
805 vpERROR_TRACE (
"Positionning error. Reference frame not implemented");
807 "Positionning error: "
808 "Reference frame not implemented.");
811 vpERROR_TRACE (
"Positionning error. Camera frame not implemented");
813 "Positionning error: "
814 "Camera frame not implemented.");
817 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
819 "Positionning error: "
820 "Mixt frame not implemented.");
831 "Positionning error: bad vector dimension.");
836 Try( PrimitiveMOVE_Afma4(position.
data, positioningVelocity) );
837 Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
840 if (TryStt == InvalidPosition || TryStt == -1023)
841 std::cout <<
" : Position out of range.\n";
843 std::cout <<
" : Unknown error (see Fabien).\n";
844 else if (error == -1)
845 std::cout <<
"Position out of range.\n";
847 if (TryStt < 0 || error < 0) {
850 "Position out of range.");
970 "Bad position in filename.");
1049 Try( PrimitiveACQ_POS_Afma4(_q) );
1050 for (
unsigned int i=0; i < this->
njoint; i ++) {
1051 position[i] = _q[i];
1058 Try( PrimitiveACQ_POS_Afma4(_q) );
1061 for (
unsigned int i=0; i < this->
njoint; i++)
1074 for (
unsigned int i=0; i < 3; i++) {
1075 position[i] = fMc[i][3];
1076 position[i+3] = rxyz[i];
1081 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1083 "Cannot get position in mixt frame: "
1093 "Cannot get position.");
1153 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1155 "Cannot send a velocity to the robot "
1156 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1165 vpERROR_TRACE (
"Bad dimension of the velocity vector in camera frame");
1167 "Bad dimension of the velocity vector "
1174 vpERROR_TRACE (
"Bad dimension of the articular velocity vector");
1176 "Bad dimension of the articular "
1177 "velocity vector ");
1183 "in the reference frame: "
1184 "functionality not implemented");
1186 "Cannot send a velocity to the robot "
1187 "in the reference frame:"
1188 "functionality not implemented");
1193 "in the mixt frame: "
1194 "functionality not implemented");
1196 "Cannot send a velocity to the robot "
1197 "in the mixt frame:"
1198 "functionality not implemented");
1203 "Case not taken in account.");
1205 "Cannot send a velocity to the robot ");
1219 for (
int i=0; i<3; i++) {
1232 joint_vel = eJe * velocity;
1246 vpMatrix eJe_inverse = fJe_inverse * fVe;
1251 joint_vel = eJe_inverse * eVc * velocity;
1273 Try( PrimitiveMOVESPEED_Afma4(joint_vel.
data) );
1277 if (TryStt == VelStopOnJoint) {
1278 UInt32 axisInJoint[
njoint];
1279 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1280 for (
unsigned int i=0; i <
njoint; i ++) {
1282 std::cout <<
"\nWarning: Velocity control stopped: axis "
1283 << i+1 <<
" on joint limit!" <<std::endl;
1287 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1288 if (TryString != NULL) {
1290 printf(
" Error sentence %s\n", TryString);
1377 Try( PrimitiveACQ_POS_Afma4(q) );
1378 for (
unsigned int i=0; i < this->
njoint; i ++) {
1385 if ( ! first_time_getvel ) {
1390 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1399 velocity = (q_cur - q_prev_getvel)
1400 / (time_cur - time_prev_getvel);
1406 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1421 "functionality not implemented");
1423 "Cannot get a displacement in the mixt frame:"
1424 "functionality not implemented");
1431 first_time_getvel =
false;
1435 fMc_prev_getvel = fMc_cur;
1438 q_prev_getvel = q_cur;
1441 time_prev_getvel = time_cur;
1448 "Cannot get velocity.");
1555 fd = fopen(filename,
"r") ;
1559 char line[FILENAME_MAX];
1560 char dummy[FILENAME_MAX];
1562 bool sortie =
false;
1566 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1567 if ( strncmp (line,
"#", 1) != 0) {
1569 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1581 while ( sortie !=
true );
1585 sscanf(line,
"%s %lf %lf %lf %lf",
1587 &q[0], &q[1], &q[2], &q[3]);
1627 fd = fopen(filename,
"w") ;
1632 #AFMA4 - Position - Version 2.01\n\
1635 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1636 # Y : vertical translation in meters (joint 2)\n\
1637 # A : pan rotation of the camera in degrees (joint 4)\n\
1638 # B : tilt rotation of the camera in degrees (joint 5)\n\
1642 fprintf(fd,
"R: %lf %lf %lf %lf\n",
1741 Try( PrimitiveACQ_POS_Afma4(q) );
1742 for (
unsigned int i=0; i <
njoint; i ++) {
1746 if ( ! first_time_getdis ) {
1749 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1755 displacement = q_cur - q_prev_getdis;
1760 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1766 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1773 first_time_getdis =
false;
1777 q_prev_getdis = q_cur;
1783 "Cannot get velocity.");
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
void get_eJe(const vpColVector &q, vpMatrix &eJe)
void get_cVf(vpVelocityTwistMatrix &cVf)
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe)
Error that can be emited by the vpRobot class and its derivates.
static bool savePosFile(const char *filename, const vpColVector &q)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
virtual ~vpRobotAfma4(void)
virtual vpRobotStateType getRobotState(void)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double getMaxTranslationVelocity(void) const
void get_cMe(vpHomogeneousMatrix &cMe)
Initialize the position controller.
class that defines a generic virtual robot
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse)
double getMaxRotationVelocity(void) const
void move(const char *filename)
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
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Initialize the velocity controller.
void get_cVe(vpVelocityTwistMatrix &cVe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void getCameraDisplacement(vpColVector &displacement)
void setPositioningVelocity(const double velocity)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void getArticularDisplacement(vpColVector &displacement)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
static const unsigned int njoint
Number of joint.
Modelisation of Irisa's cylindrical robot named Afma4.
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
static double rad(double deg)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
void get_eJe(vpMatrix &eJe)
static bool readPosFile(const char *filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf)
vpHomogeneousMatrix inverse() const
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void get_fJe(vpMatrix &fJe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
double getPositioningVelocity(void)
static double measureTimeSecond()
unsigned int getRows() const
Return the number of rows of the matrix.
static const double defaultPositioningVelocity
void buildFrom(const double phi, const double theta, const double psi)
Class that consider the case of a translation vector.
void get_cMe(vpHomogeneousMatrix &cMe)
void resize(const unsigned int i, const bool flagNullify=true)