39 #include <visp3/core/vpConfig.h> 41 #ifdef VISP_HAVE_AFMA6 45 #include <sys/types.h> 48 #include <visp3/core/vpDebug.h> 49 #include <visp3/core/vpExponentialMap.h> 50 #include <visp3/core/vpIoTools.h> 51 #include <visp3/core/vpRotationMatrix.h> 52 #include <visp3/core/vpThetaUVector.h> 53 #include <visp3/core/vpVelocityTwistMatrix.h> 54 #include <visp3/robot/vpRobotAfma6.h> 55 #include <visp3/robot/vpRobotException.h> 61 bool vpRobotAfma6::robotAlreadyCreated =
false;
83 void emergencyStopAfma6(
int signo)
85 std::cout <<
"Stop the Afma6 application by signal (" << signo <<
"): " << (char)7;
88 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
91 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
94 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
97 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
100 std::cout <<
"SIGQUIT " << std::endl;
103 std::cout << signo << std::endl;
107 PrimitiveSTOP_Afma6();
108 std::cout <<
"Robot was stopped\n";
113 fprintf(stdout,
"Application ");
115 kill(getpid(), SIGKILL);
181 signal(SIGINT, emergencyStopAfma6);
182 signal(SIGBUS, emergencyStopAfma6);
183 signal(SIGSEGV, emergencyStopAfma6);
184 signal(SIGKILL, emergencyStopAfma6);
185 signal(SIGQUIT, emergencyStopAfma6);
189 std::cout <<
"Open communication with MotionBlox.\n";
199 vpRobotAfma6::robotAlreadyCreated =
true;
234 time_prev_getvel = 0;
235 first_time_getvel =
true;
240 first_time_getdis =
true;
243 Try(InitializeConnection(
verbose_));
246 Try(InitializeNode_Afma6());
248 Try(PrimitiveRESET_Afma6());
254 UInt32 HIPowerStatus;
256 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
261 std::cout <<
"Robot status: ";
262 switch (EStopStatus) {
265 if (HIPowerStatus == 0)
266 std::cout <<
"Power is OFF" << std::endl;
268 std::cout <<
"Power is ON" << std::endl;
270 case ESTOP_ACTIVATED:
271 std::cout <<
"Emergency stop is activated" << std::endl;
274 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
275 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
278 std::cout << std::endl;
292 if (TryStt == -20001)
293 printf(
"No connection detected. Check if the robot is powered on \n" 294 "and if the firewire link exist between the MotionBlox and this " 296 else if (TryStt == -675)
297 printf(
" Timeout enabling power...\n");
301 PrimitivePOWEROFF_Afma6();
303 ShutDownConnection();
305 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
360 for (
unsigned int i = 0; i < 3; i++) {
361 eMc_pose[i] =
_etc[i];
362 eMc_pose[i + 3] =
_erc[i];
365 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
399 for (
unsigned int i = 0; i < 3; i++) {
400 eMc_pose[i] =
_etc[i];
401 eMc_pose[i + 3] =
_erc[i];
404 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
457 for (
unsigned int i = 0; i < 3; i++) {
458 eMc_pose[i] =
_etc[i];
459 eMc_pose[i + 3] =
_erc[i];
462 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
535 for (
unsigned int i = 0; i < 3; i++) {
536 eMc_pose[i] =
_etc[i];
537 eMc_pose[i + 3] =
_erc[i];
540 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
567 UInt32 HIPowerStatus;
568 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
579 ShutDownConnection();
581 vpRobotAfma6::robotAlreadyCreated =
false;
600 Try(PrimitiveSTOP_Afma6());
606 std::cout <<
"Change the control mode from velocity to position control.\n";
607 Try(PrimitiveSTOP_Afma6());
617 std::cout <<
"Change the control mode from stop to velocity control.\n";
645 Try(PrimitiveSTOP_Afma6());
669 UInt32 HIPowerStatus;
671 bool firsttime =
true;
672 unsigned int nitermax = 10;
674 for (
unsigned int i = 0; i < nitermax; i++) {
675 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
676 if (EStopStatus == ESTOP_AUTO) {
678 }
else if (EStopStatus == ESTOP_MANUAL) {
680 }
else if (EStopStatus == ESTOP_ACTIVATED) {
682 std::cout <<
"Emergency stop is activated! \n" 683 <<
"Check the emergency stop button and push the yellow " 684 "button before continuing." 688 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
692 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
693 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
695 ShutDownConnection();
700 if (EStopStatus == ESTOP_ACTIVATED)
701 std::cout << std::endl;
703 if (EStopStatus == ESTOP_ACTIVATED) {
704 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
708 if (HIPowerStatus == 0) {
709 fprintf(stdout,
"Power ON the Afma6 robot\n");
712 Try(PrimitivePOWERON_Afma6());
736 UInt32 HIPowerStatus;
737 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
740 if (HIPowerStatus == 1) {
741 fprintf(stdout,
"Power OFF the Afma6 robot\n");
744 Try(PrimitivePOWEROFF_Afma6());
770 UInt32 HIPowerStatus;
771 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
774 if (HIPowerStatus == 1) {
832 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
836 for (
unsigned int i = 0; i <
njoint; i++)
876 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
880 for (
unsigned int i = 0; i <
njoint; i++)
1013 for (
unsigned int i = 0; i < 3; i++) {
1014 position[i] = pose[i];
1015 position[i + 3] = rxyz[i];
1019 "Joint frame not implemented for pose positionning.");
1111 "Modification of the robot state");
1115 double _destination[6];
1123 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1126 for (
unsigned int i = 0; i <
njoint; i++)
1136 for (
unsigned int i = 0; i < 3; i++) {
1137 txyz[i] = position[i];
1138 rxyz[i] = position[i + 3];
1149 bool nearest =
true;
1152 for (
unsigned int i = 0; i <
njoint; i++) {
1153 _destination[i] = q[i];
1155 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1156 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1165 for (
unsigned int i = 0; i <
njoint; i++) {
1166 _destination[i] = position[i];
1168 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1169 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1176 for (
unsigned int i = 0; i < 3; i++) {
1177 txyz[i] = position[i];
1178 rxyz[i] = position[i + 3];
1186 bool nearest =
true;
1189 for (
unsigned int i = 0; i <
njoint; i++) {
1190 _destination[i] = q[i];
1192 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1193 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1203 "Mixt frame not implemented.");
1207 "end-effector frame not implemented.");
1212 if (TryStt == InvalidPosition || TryStt == -1023)
1213 std::cout <<
" : Position out of range.\n";
1214 else if (TryStt < 0)
1215 std::cout <<
" : Unknown error (see Fabien).\n";
1216 else if (error == -1)
1217 std::cout <<
"Position out of range.\n";
1219 if (TryStt < 0 || error < 0) {
1295 const double pos3,
const double pos4,
const double pos5,
const double pos6)
1437 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1438 for (
unsigned int i = 0; i <
njoint; i++) {
1439 position[i] = _q[i];
1446 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1449 for (
unsigned int i = 0; i <
njoint; i++)
1462 for (
unsigned int i = 0; i < 3; i++) {
1463 position[i] = fMc[i][3];
1464 position[i + 3] = rxyz[i];
1493 PrimitiveACQ_TIME_Afma6(×tamp);
1528 for (
unsigned int j = 0; j < 3; j++)
1529 RxyzVect[j] = posRxyz[j + 3];
1534 for (
unsigned int j = 0; j < 3; j++) {
1535 position[j] = posRxyz[j];
1536 position[j + 3] = RtuVect[j];
1623 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1625 "Cannot send a velocity to the robot " 1626 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1631 for (
unsigned int i = 0; i < 3; i++)
1633 for (
unsigned int i = 3; i < 6; i++)
1642 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPCAM_AFMA6));
1652 Try(PrimitiveMOVESPEED_CART_Afma6(v_c.
data, REPCAM_AFMA6));
1657 Try(PrimitiveMOVESPEED_Afma6(vel_sat.
data));
1661 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPFIX_AFMA6));
1665 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPMIX_AFMA6));
1670 "Case not taken in account.");
1677 if (TryStt == VelStopOnJoint) {
1678 Int32 axisInJoint[
njoint];
1679 PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1680 for (
unsigned int i = 0; i <
njoint; i++) {
1682 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1685 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1686 if (TryString != NULL) {
1688 printf(
" Error sentence %s\n", TryString);
1765 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
1766 time_cur = timestamp;
1767 for (
unsigned int i = 0; i <
njoint; i++) {
1774 if (!first_time_getvel) {
1779 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1788 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1794 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1809 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1817 for (
unsigned int i = 0; i < 3; i++) {
1819 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1821 velocity[i + 3] = thetaU[i];
1825 velocity /= (time_cur - time_prev_getvel);
1830 "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1834 first_time_getvel =
false;
1838 fMc_prev_getvel = fMc_cur;
1841 q_prev_getvel = q_cur;
1844 time_prev_getvel = time_cur;
1984 std::ifstream fd(filename.c_str(), std::ios::in);
1986 if (!fd.is_open()) {
1991 std::string key(
"R:");
1992 std::string id(
"#AFMA6 - Position");
1993 bool pos_found =
false;
1998 while (std::getline(fd, line)) {
2001 if (!(line.compare(0,
id.size(), id) == 0)) {
2002 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
2006 if ((line.compare(0, 1,
"#") == 0)) {
2009 if ((line.compare(0, key.size(), key) == 0)) {
2012 if (chain.size() <
njoint + 1)
2014 if (chain.size() <
njoint + 1)
2017 std::istringstream ss(line);
2020 for (
unsigned int i = 0; i <
njoint; i++)
2035 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2070 fd = fopen(filename.c_str(),
"w");
2075 #AFMA6 - Position - Version 2.01\n\ 2078 # Joint position: X, Y, Z: translations in meters\n\ 2079 # A, B, C: rotations in degrees\n\ 2142 Try(PrimitiveGripper_Afma6(1));
2143 std::cout <<
"Open the gripper..." << std::endl;
2161 Try(PrimitiveGripper_Afma6(0));
2162 std::cout <<
"Close the gripper..." << std::endl;
2201 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
2202 for (
unsigned int i = 0; i <
njoint; i++) {
2209 if (!first_time_getdis) {
2213 c_prevMc_cur = fMc_prev_getdis.
inverse() * fMc_cur;
2223 for (
unsigned int i = 0; i < 3; i++) {
2224 displacement[i] = t[i];
2225 displacement[i + 3] = rxyz[i];
2231 displacement = q_cur - q_prev_getdis;
2236 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2241 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2245 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2250 first_time_getdis =
false;
2254 q_prev_getdis = q_cur;
2257 fMc_prev_getdis = fMc_cur;
2278 Int32 axisInJoint[
njoint];
2283 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
2284 for (
unsigned int i = 0; i <
njoint; i++) {
2285 if (axisInJoint[i]) {
2286 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
2287 jointsStatus[i] = axisInJoint[i];
2290 jointsStatus[i] = 0;
2303 #elif !defined(VISP_BUILD_SHARED_LIBS) 2306 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.
void set_eMc(const vpHomogeneousMatrix &eMc)
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
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
error that can be emited by ViSP classes.
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.
vpCameraParameters::vpCameraParametersProjType projModel
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.
static bool readPosFile(const std::string &filename, vpColVector &q)
static bool savePosFile(const std::string &filename, const vpColVector &q)
Initialize the velocity controller.
void move(const std::string &filename)
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)
static double rad(double deg)
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
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)
Function not implemented.
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
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