42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_AFMA6
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/vpRobotAfma6.h>
55 #include <visp/vpRotationMatrix.h>
61 bool vpRobotAfma6::robotAlreadyCreated =
false;
83 void emergencyStopAfma6(
int signo)
85 std::cout <<
"Stop the Afma6 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_Afma6();
105 std::cout <<
"Robot was stopped\n";
110 fprintf(stdout,
"Application ");
112 kill(getpid(), SIGKILL);
181 signal(SIGINT, emergencyStopAfma6);
182 signal(SIGBUS, emergencyStopAfma6) ;
183 signal(SIGSEGV, emergencyStopAfma6) ;
184 signal(SIGKILL, emergencyStopAfma6);
185 signal(SIGQUIT, emergencyStopAfma6);
187 std::cout <<
"Open communication with MotionBlox.\n";
198 vpRobotAfma6::robotAlreadyCreated =
true;
235 time_prev_getvel = 0;
236 first_time_getvel =
true;
241 first_time_getdis =
true;
245 Try( InitializeConnection() );
248 Try( InitializeNode_Afma6() );
250 Try( PrimitiveRESET_Afma6() );
256 UInt32 HIPowerStatus;
258 Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
263 std::cout <<
"Robot status: ";
264 switch(EStopStatus) {
267 if (HIPowerStatus == 0)
268 std::cout <<
"Power is OFF" << std::endl;
270 std::cout <<
"Power is ON" << std::endl;
272 case ESTOP_ACTIVATED:
273 std::cout <<
"Emergency stop is activated" << std::endl;
276 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
277 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
280 std::cout << std::endl;
293 if (TryStt == -20001)
294 printf(
"No connection detected. Check if the robot is powered on \n"
295 "and if the firewire link exist between the MotionBlox and this computer.\n");
296 else if (TryStt == -675)
297 printf(
" Timeout enabling power...\n");
301 PrimitivePOWEROFF_Afma6();
303 ShutDownConnection();
305 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
307 "Cannot open connexion with the motionblox");
364 for (
unsigned int i=0; i < 3; i ++) {
365 eMc_pose[i] =
_etc[i];
366 eMc_pose[i+3] =
_erc[i];
369 Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
400 UInt32 HIPowerStatus;
401 Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
413 ShutDownConnection();
415 vpRobotAfma6::robotAlreadyCreated =
false;
438 Try( PrimitiveSTOP_Afma6() );
444 std::cout <<
"Change the control mode from velocity to position control.\n";
445 Try( PrimitiveSTOP_Afma6() );
455 std::cout <<
"Change the control mode from stop to velocity control.\n";
485 Try( PrimitiveSTOP_Afma6() );
492 "Cannot stop robot motion.");
511 UInt32 HIPowerStatus;
513 bool firsttime =
true;
514 unsigned int nitermax = 10;
516 for (
unsigned int i=0; i<nitermax; i++) {
517 Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
519 switch(EStopStatus) {
520 case ESTOP_AUTO:
break;
521 case ESTOP_MANUAL:
break;
522 case ESTOP_ACTIVATED:
524 std::cout <<
"Emergency stop is activated! \n"
525 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
528 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
533 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
534 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
536 ShutDownConnection();
541 std::cout << std::endl;
543 if (EStopStatus == ESTOP_ACTIVATED) {
544 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
546 "Cannot power on the robot.");
549 if (HIPowerStatus == 0) {
550 fprintf(stdout,
"Power ON the Afma6 robot\n");
553 Try( PrimitivePOWERON_Afma6() );
560 "Cannot power off the robot.");
579 UInt32 HIPowerStatus;
580 Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
584 if (HIPowerStatus == 1) {
585 fprintf(stdout,
"Power OFF the Afma6 robot\n");
588 Try( PrimitivePOWEROFF_Afma6() );
595 "Cannot power off the robot.");
616 UInt32 HIPowerStatus;
617 Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
621 if (HIPowerStatus == 1) {
629 "Cannot get the power status.");
686 Try( PrimitiveACQ_POS_Afma6(position) );
690 for (
unsigned int i=0; i <
njoint; i++)
733 Try( PrimitiveACQ_POS_Afma6(position) );
737 for (
unsigned int i=0; i <
njoint; i++)
782 positioningVelocity = velocity;
793 return positioningVelocity;
882 for (
unsigned int i=0; i < 3; i++) {
883 position[i] = pose[i];
884 position[i+3] = rxyz[i];
888 "Positionning error: "
889 "Joint frame not implemented for pose positionning.");
983 "Modification of the robot state");
987 double _destination[6];
994 Try( PrimitiveACQ_POS_Afma6(_q) );
997 for (
unsigned int i=0; i <
njoint; i++)
1007 for (
unsigned int i=0; i < 3; i++) {
1008 txyz[i] = position[i];
1009 rxyz[i] = position[i+3];
1020 bool nearest =
true;
1023 for (
unsigned int i=0; i <
njoint; i ++) {
1024 _destination[i] = q[i];
1026 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1027 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1037 for (
unsigned int i=0; i <
njoint; i ++) {
1038 _destination[i] = position[i];
1040 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1041 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1049 for (
unsigned int i=0; i < 3; i++) {
1050 txyz[i] = position[i];
1051 rxyz[i] = position[i+3];
1059 bool nearest =
true;
1062 for (
unsigned int i=0; i <
njoint; i ++) {
1063 _destination[i] = q[i];
1065 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1066 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1077 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1079 "Positionning error: "
1080 "Mixt frame not implemented.");
1086 if (TryStt == InvalidPosition || TryStt == -1023)
1087 std::cout <<
" : Position out of range.\n";
1088 else if (TryStt < 0)
1089 std::cout <<
" : Unknown error (see Fabien).\n";
1090 else if (error == -1)
1091 std::cout <<
"Position out of range.\n";
1093 if (TryStt < 0 || error < 0) {
1096 "Position out of range.");
1179 position[0] = pos1 ;
1180 position[1] = pos2 ;
1181 position[2] = pos3 ;
1182 position[3] = pos4 ;
1183 position[4] = pos5 ;
1184 position[5] = pos6 ;
1246 "Bad position in filename.");
1319 Try( PrimitiveACQ_POS_Afma6(_q) );
1320 for (
unsigned int i=0; i <
njoint; i ++) {
1321 position[i] = _q[i];
1328 Try( PrimitiveACQ_POS_Afma6(_q) );
1331 for (
unsigned int i=0; i <
njoint; i++)
1344 for (
unsigned int i=0; i < 3; i++) {
1345 position[i] = fMc[i][3];
1346 position[i+3] = rxyz[i];
1351 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1353 "Cannot get position in mixt frame: "
1363 "Cannot get position.");
1386 for(
unsigned int j=0;j<3;j++)
1387 RxyzVect[j]=posRxyz[j+3];
1392 for(
unsigned int j=0;j<3;j++)
1394 position[j]=posRxyz[j];
1395 position[j+3]=RtuVect[j];
1460 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1462 "Cannot send a velocity to the robot "
1463 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1468 for (
int i=0; i<3; i++)
1470 for (
int i=3; i<6; i++)
1479 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPCAM) );
1484 Try( PrimitiveMOVESPEED_Afma6(vel_sat.
data) );
1488 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPFIX) );
1492 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPMIX) );
1497 "Case not taken in account.");
1504 if (TryStt == VelStopOnJoint) {
1505 Int32 axisInJoint[
njoint];
1506 PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1507 for (
unsigned int i=0; i <
njoint; i ++) {
1509 std::cout <<
"\nWarning: Velocity control stopped: axis "
1510 << i+1 <<
" on joint limit!" <<std::endl;
1514 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1515 if (TryString != NULL) {
1517 printf(
" Error sentence %s\n", TryString);
1605 Try( PrimitiveACQ_POS_Afma6(q) );
1606 for (
unsigned int i=0; i <
njoint; i ++) {
1613 if ( ! first_time_getvel ) {
1618 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1627 velocity = (q_cur - q_prev_getvel)
1628 / (time_cur - time_prev_getvel);
1634 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1649 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1657 for (
unsigned int i=0; i < 3; i++) {
1659 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1661 velocity[i+3] = thetaU[i];
1665 velocity /= (time_cur - time_prev_getvel);
1671 first_time_getvel =
false;
1675 fMc_prev_getvel = fMc_cur;
1678 q_prev_getvel = q_cur;
1681 time_prev_getvel = time_cur;
1688 "Cannot get velocity.");
1795 fd = fopen(filename,
"r") ;
1799 char line[FILENAME_MAX];
1800 char dummy[FILENAME_MAX];
1802 bool sortie =
false;
1806 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1807 if ( strncmp (line,
"#", 1) != 0) {
1809 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1821 while ( sortie !=
true );
1825 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1827 &q[0], &q[1], &q[2],
1828 &q[3], &q[4], &q[5]);
1831 for (
unsigned int i=3; i <
njoint; i ++)
1867 fd = fopen(filename,
"w") ;
1872 #AFMA6 - Position - Version 2.01\n\
1875 # Joint position: X, Y, Z: translations in meters\n\
1876 # A, B, C: rotations in degrees\n\
1881 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
1947 Try( PrimitiveGripper_Afma6(1) );
1948 std::cout <<
"Open the gripper..." << std::endl;
1953 "Cannot open the gripper.");
1968 Try( PrimitiveGripper_Afma6(0) );
1969 std::cout <<
"Close the gripper..." << std::endl;
1974 "Cannot close the gripper.");
2047 Try( PrimitiveACQ_POS_Afma6(q) );
2048 for (
unsigned int i=0; i <
njoint; i ++) {
2055 if ( ! first_time_getdis ) {
2059 c_prevMc_cur = fMc_prev_getdis.
inverse() * fMc_cur;
2069 for (
unsigned int i=0; i<3; i++) {
2070 displacement[i] = t[i];
2071 displacement[i+3] = rxyz[i];
2077 displacement = q_cur - q_prev_getdis;
2082 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2088 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2095 first_time_getdis =
false;
2099 q_prev_getdis = q_cur;
2102 fMc_prev_getdis = fMc_cur;
2108 "Cannot get velocity.");
2125 Int32 axisInJoint[
njoint];
2130 Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
2132 for (
unsigned int i=0; i <
njoint; i ++) {
2133 if (axisInJoint[i]){
2134 std::cout <<
"\nWarning: Velocity control stopped: axis "
2135 << i+1 <<
" on joint limit!" <<std::endl;
2136 jointsStatus[i] = axisInJoint[i];
2140 jointsStatus[i] = 0;
2148 "Cannot check joint limits.");
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Modelisation of Irisa's gantry robot named Afma6.
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
bool checkJointLimits(vpColVector &jointsStatus)
static const unsigned int njoint
Number of joint.
void get_eJe(vpMatrix &_eJe)
void getCameraDisplacement(vpColVector &displacement)
Error that can be emited by the vpRobot class and its derivates.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static bool savePosFile(const char *filename, const vpColVector &q)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
virtual vpRobotStateType getRobotState(void)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
double getMaxTranslationVelocity(void) const
Initialize the position controller.
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
class that defines a generic virtual robot
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
void getArticularDisplacement(vpColVector &displacement)
double getPositioningVelocity(void)
double getMaxRotationVelocity(void) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual ~vpRobotAfma6(void)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setPositioningVelocity(const double velocity)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
The vpRotationMatrix considers the particular case of a rotation matrix.
double * data
address of the first element of the data array
void get_cMe(vpHomogeneousMatrix &cMe)
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Initialize the velocity controller.
void get_eJe(const vpColVector &q, vpMatrix &eJe)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParametersProjType
void get_fJe(vpMatrix &_fJe)
void extract(vpRotationMatrix &R) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void get_cVe(vpVelocityTwistMatrix &_cVe)
static double rad(double deg)
void get_cMe(vpHomogeneousMatrix &_cMe)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true)
void move(const char *filename)
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...
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
void get_fJe(const vpColVector &q, vpMatrix &fJe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
static double measureTimeSecond()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
static bool readPosFile(const char *filename, vpColVector &q)
void buildFrom(const double phi, const double theta, const double psi)
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
static const double defaultPositioningVelocity
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void resize(const unsigned int i, const bool flagNullify=true)