38 #include <visp3/core/vpConfig.h>
40 #ifdef VISP_HAVE_AFMA6
44 #include <sys/types.h>
47 #include <visp3/robot/vpRobotException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpDebug.h>
50 #include <visp3/core/vpVelocityTwistMatrix.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/robot/vpRobotAfma6.h>
53 #include <visp3/core/vpRotationMatrix.h>
59 bool vpRobotAfma6::robotAlreadyCreated =
false;
81 void emergencyStopAfma6(
int signo)
83 std::cout <<
"Stop the Afma6 application by signal ("
84 << signo <<
"): " << (char)7 ;
88 std::cout <<
"SIGINT (stop by ^C) " << std::endl ; break ;
90 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl ; break ;
92 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
94 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
96 std::cout <<
"SIGQUIT " << std::endl ; break ;
98 std::cout << signo << std::endl ;
102 PrimitiveSTOP_Afma6();
103 std::cout <<
"Robot was stopped\n";
108 fprintf(stdout,
"Application ");
110 kill(getpid(), SIGKILL);
154 vpRobotAfma6::vpRobotAfma6 (
bool verbose)
179 signal(SIGINT, emergencyStopAfma6);
180 signal(SIGBUS, emergencyStopAfma6) ;
181 signal(SIGSEGV, emergencyStopAfma6) ;
182 signal(SIGKILL, emergencyStopAfma6);
183 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;
244 Try( InitializeConnection(
verbose_) );
247 Try( InitializeNode_Afma6() );
249 Try( PrimitiveRESET_Afma6() );
255 UInt32 HIPowerStatus;
257 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 if (EStopStatus == ESTOP_AUTO) {
522 else if (EStopStatus == ESTOP_MANUAL) {
525 else if (EStopStatus == ESTOP_ACTIVATED) {
527 std::cout <<
"Emergency stop is activated! \n"
528 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
531 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
536 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
537 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
539 ShutDownConnection();
544 if (EStopStatus == ESTOP_ACTIVATED)
545 std::cout << std::endl;
547 if (EStopStatus == ESTOP_ACTIVATED) {
548 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
550 "Cannot power on the robot.");
553 if (HIPowerStatus == 0) {
554 fprintf(stdout,
"Power ON the Afma6 robot\n");
557 Try( PrimitivePOWERON_Afma6() );
564 "Cannot power off the robot.");
583 UInt32 HIPowerStatus;
584 Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
588 if (HIPowerStatus == 1) {
589 fprintf(stdout,
"Power OFF the Afma6 robot\n");
592 Try( PrimitivePOWEROFF_Afma6() );
599 "Cannot power off the robot.");
620 UInt32 HIPowerStatus;
621 Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
625 if (HIPowerStatus == 1) {
633 "Cannot get the power status.");
691 Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
695 for (
unsigned int i=0; i <
njoint; i++)
739 Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
743 for (
unsigned int i=0; i <
njoint; i++)
788 positioningVelocity = velocity;
799 return positioningVelocity;
888 for (
unsigned int i=0; i < 3; i++) {
889 position[i] = pose[i];
890 position[i+3] = rxyz[i];
894 "Positionning error: "
895 "Joint frame not implemented for pose positionning.");
989 "Modification of the robot state");
993 double _destination[6];
1001 Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
1004 for (
unsigned int i=0; i <
njoint; i++)
1014 for (
unsigned int i=0; i < 3; i++) {
1015 txyz[i] = position[i];
1016 rxyz[i] = position[i+3];
1027 bool nearest =
true;
1030 for (
unsigned int i=0; i <
njoint; i ++) {
1031 _destination[i] = q[i];
1033 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1034 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1044 for (
unsigned int i=0; i <
njoint; i ++) {
1045 _destination[i] = position[i];
1047 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1048 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1056 for (
unsigned int i=0; i < 3; i++) {
1057 txyz[i] = position[i];
1058 rxyz[i] = position[i+3];
1066 bool nearest =
true;
1069 for (
unsigned int i=0; i <
njoint; i ++) {
1070 _destination[i] = q[i];
1072 Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1073 Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1084 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1086 "Positionning error: "
1087 "Mixt frame not implemented.");
1093 if (TryStt == InvalidPosition || TryStt == -1023)
1094 std::cout <<
" : Position out of range.\n";
1095 else if (TryStt < 0)
1096 std::cout <<
" : Unknown error (see Fabien).\n";
1097 else if (error == -1)
1098 std::cout <<
"Position out of range.\n";
1100 if (TryStt < 0 || error < 0) {
1103 "Position out of range.");
1186 position[0] = pos1 ;
1187 position[1] = pos2 ;
1188 position[2] = pos3 ;
1189 position[3] = pos4 ;
1190 position[4] = pos5 ;
1191 position[5] = pos6 ;
1253 "Bad position in filename.");
1328 Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
1329 for (
unsigned int i=0; i <
njoint; i ++) {
1330 position[i] = _q[i];
1337 Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
1340 for (
unsigned int i=0; i <
njoint; i++)
1353 for (
unsigned int i=0; i < 3; i++) {
1354 position[i] = fMc[i][3];
1355 position[i+3] = rxyz[i];
1360 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1362 "Cannot get position in mixt frame: "
1372 "Cannot get position.");
1383 PrimitiveACQ_TIME_Afma6(×tamp);
1419 for(
unsigned int j=0;j<3;j++)
1420 RxyzVect[j]=posRxyz[j+3];
1425 for(
unsigned int j=0;j<3;j++)
1427 position[j]=posRxyz[j];
1428 position[j+3]=RtuVect[j];
1509 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1511 "Cannot send a velocity to the robot "
1512 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1517 for (
unsigned int i=0; i<3; i++)
1519 for (
unsigned int i=3; i<6; i++)
1528 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPCAM_AFMA6) );
1533 Try( PrimitiveMOVESPEED_Afma6(vel_sat.
data) );
1537 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPFIX_AFMA6) );
1541 Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPMIX_AFMA6) );
1546 "Case not taken in account.");
1553 if (TryStt == VelStopOnJoint) {
1554 Int32 axisInJoint[
njoint];
1555 PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1556 for (
unsigned int i=0; i <
njoint; i ++) {
1558 std::cout <<
"\nWarning: Velocity control stopped: axis "
1559 << i+1 <<
" on joint limit!" <<std::endl;
1563 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1564 if (TryString != NULL) {
1566 printf(
" Error sentence %s\n", TryString);
1652 Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
1653 time_cur = timestamp;
1654 for (
unsigned int i=0; i <
njoint; i ++) {
1661 if ( ! first_time_getvel ) {
1666 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1675 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1681 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1696 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1704 for (
unsigned int i=0; i < 3; i++) {
1706 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1708 velocity[i+3] = thetaU[i];
1712 velocity /= (time_cur - time_prev_getvel);
1718 first_time_getvel =
false;
1722 fMc_prev_getvel = fMc_cur;
1725 q_prev_getvel = q_cur;
1728 time_prev_getvel = time_cur;
1735 "Cannot get velocity.");
1875 fd = fopen(filename,
"r") ;
1879 char line[FILENAME_MAX];
1880 char dummy[FILENAME_MAX];
1882 bool sortie =
false;
1886 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1887 if ( strncmp (line,
"#", 1) != 0) {
1889 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1901 while ( sortie !=
true );
1905 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1907 &q[0], &q[1], &q[2],
1908 &q[3], &q[4], &q[5]);
1911 for (
unsigned int i=3; i <
njoint; i ++)
1947 fd = fopen(filename,
"w") ;
1952 #AFMA6 - Position - Version 2.01\n\
1955 # Joint position: X, Y, Z: translations in meters\n\
1956 # A, B, C: rotations in degrees\n\
1961 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2027 Try( PrimitiveGripper_Afma6(1) );
2028 std::cout <<
"Open the gripper..." << std::endl;
2033 "Cannot open the gripper.");
2048 Try( PrimitiveGripper_Afma6(0) );
2049 std::cout <<
"Close the gripper..." << std::endl;
2054 "Cannot close the gripper.");
2072 vpRobotAfma6::getCameraDisplacement(
vpColVector &displacement)
2088 vpRobotAfma6::getArticularDisplacement(
vpColVector &displacement)
2128 Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
2129 for (
unsigned int i=0; i <
njoint; i ++) {
2136 if ( ! first_time_getdis ) {
2140 c_prevMc_cur = fMc_prev_getdis.
inverse() * fMc_cur;
2150 for (
unsigned int i=0; i<3; i++) {
2151 displacement[i] = t[i];
2152 displacement[i+3] = rxyz[i];
2158 displacement = q_cur - q_prev_getdis;
2163 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2169 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2176 first_time_getdis =
false;
2180 q_prev_getdis = q_cur;
2183 fMc_prev_getdis = fMc_cur;
2189 "Cannot get velocity.");
2206 Int32 axisInJoint[
njoint];
2211 Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
2213 for (
unsigned int i=0; i <
njoint; i ++) {
2214 if (axisInJoint[i]){
2215 std::cout <<
"\nWarning: Velocity control stopped: axis "
2216 << i+1 <<
" on joint limit!" <<std::endl;
2217 jointsStatus[i] = axisInJoint[i];
2221 jointsStatus[i] = 0;
2229 "Cannot check joint limits.");
2236 #elif !defined(VISP_BUILD_SHARED_LIBS)
2238 void dummy_vpRobotAfma6() {};
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Modelisation of Irisa's gantry robot named Afma6.
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
bool checkJointLimits(vpColVector &jointsStatus)
static const unsigned int njoint
Number of joint.
void get_eJe(vpMatrix &_eJe)
Error that can be emited by the vpRobot class and its derivates.
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
static bool savePosFile(const char *filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
double getMaxTranslationVelocity(void) const
Initialize the position controller.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
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)
Type * data
Address of the first element of the data array.
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.
Implementation of a rotation matrix and operations on such kind of matrices.
void get_cMe(vpHomogeneousMatrix &_cMe) const
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Initialize the velocity controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
vpCameraParametersProjType
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_fJe(vpMatrix &_fJe)
void extract(vpRotationMatrix &R) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Implementation of a velocity twist matrix and operations on such kind of matrices.
static double rad(double deg)
void move(const char *filename)
static double deg(double rad)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Implementation of a rotation vector as Euler angle minimal representation.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
static bool readPosFile(const char *filename, vpColVector &q)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static const double defaultPositioningVelocity
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void resize(const unsigned int i, const bool flagNullify=true)
void get_cVe(vpVelocityTwistMatrix &_cVe) const