36#include <visp3/core/vpConfig.h>
45#include <visp3/core/vpDebug.h>
46#include <visp3/core/vpExponentialMap.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpRotationMatrix.h>
49#include <visp3/core/vpThetaUVector.h>
50#include <visp3/core/vpVelocityTwistMatrix.h>
51#include <visp3/robot/vpRobotAfma6.h>
52#include <visp3/robot/vpRobotException.h>
58bool vpRobotAfma6::robotAlreadyCreated =
false;
80void emergencyStopAfma6(
int signo)
82 std::cout <<
"Stop the Afma6 application by signal (" << signo <<
"): " << (char)7;
85 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
88 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
91 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
94 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
97 std::cout <<
"SIGQUIT " << std::endl;
100 std::cout << signo << std::endl;
104 PrimitiveSTOP_Afma6();
105 std::cout <<
"Robot was stopped\n";
110 fprintf(stdout,
"Application ");
112 kill(getpid(), SIGKILL);
178 signal(SIGINT, emergencyStopAfma6);
179 signal(SIGBUS, emergencyStopAfma6);
180 signal(SIGSEGV, emergencyStopAfma6);
181 signal(SIGKILL, emergencyStopAfma6);
182 signal(SIGQUIT, emergencyStopAfma6);
186 std::cout <<
"Open communication with MotionBlox.\n";
196 vpRobotAfma6::robotAlreadyCreated =
true;
231 time_prev_getvel = 0;
232 first_time_getvel =
true;
237 first_time_getdis =
true;
240 Try(InitializeConnection(
verbose_));
243 Try(InitializeNode_Afma6());
245 Try(PrimitiveRESET_Afma6());
251 UInt32 HIPowerStatus;
253 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
258 std::cout <<
"Robot status: ";
259 switch (EStopStatus) {
262 if (HIPowerStatus == 0)
263 std::cout <<
"Power is OFF" << std::endl;
265 std::cout <<
"Power is ON" << std::endl;
267 case ESTOP_ACTIVATED:
268 std::cout <<
"Emergency stop is activated" << std::endl;
271 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
272 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
275 std::cout << std::endl;
289 if (TryStt == -20001)
290 printf(
"No connection detected. Check if the robot is powered on \n"
291 "and if the firewire link exist between the MotionBlox and this "
293 else if (TryStt == -675)
294 printf(
" Timeout enabling power...\n");
298 PrimitivePOWEROFF_Afma6();
300 ShutDownConnection();
302 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
357 for (
unsigned int i = 0; i < 3; i++) {
358 eMc_pose[i] =
_etc[i];
359 eMc_pose[i + 3] =
_erc[i];
362 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
396 for (
unsigned int i = 0; i < 3; i++) {
397 eMc_pose[i] =
_etc[i];
398 eMc_pose[i + 3] =
_erc[i];
401 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
454 for (
unsigned int i = 0; i < 3; i++) {
455 eMc_pose[i] =
_etc[i];
456 eMc_pose[i + 3] =
_erc[i];
459 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
532 for (
unsigned int i = 0; i < 3; i++) {
533 eMc_pose[i] =
_etc[i];
534 eMc_pose[i + 3] =
_erc[i];
537 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
564 UInt32 HIPowerStatus;
565 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
576 ShutDownConnection();
578 vpRobotAfma6::robotAlreadyCreated =
false;
597 Try(PrimitiveSTOP_Afma6());
603 std::cout <<
"Change the control mode from velocity to position control.\n";
604 Try(PrimitiveSTOP_Afma6());
614 std::cout <<
"Change the control mode from stop to velocity control.\n";
642 Try(PrimitiveSTOP_Afma6());
666 UInt32 HIPowerStatus;
668 bool firsttime =
true;
669 unsigned int nitermax = 10;
671 for (
unsigned int i = 0; i < nitermax; i++) {
672 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
673 if (EStopStatus == ESTOP_AUTO) {
675 }
else if (EStopStatus == ESTOP_MANUAL) {
677 }
else if (EStopStatus == ESTOP_ACTIVATED) {
679 std::cout <<
"Emergency stop is activated! \n"
680 <<
"Check the emergency stop button and push the yellow "
681 "button before continuing."
685 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
689 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
690 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
692 ShutDownConnection();
697 if (EStopStatus == ESTOP_ACTIVATED)
698 std::cout << std::endl;
700 if (EStopStatus == ESTOP_ACTIVATED) {
701 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
705 if (HIPowerStatus == 0) {
706 fprintf(stdout,
"Power ON the Afma6 robot\n");
709 Try(PrimitivePOWERON_Afma6());
733 UInt32 HIPowerStatus;
734 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
737 if (HIPowerStatus == 1) {
738 fprintf(stdout,
"Power OFF the Afma6 robot\n");
741 Try(PrimitivePOWEROFF_Afma6());
767 UInt32 HIPowerStatus;
768 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
771 if (HIPowerStatus == 1) {
829 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
833 for (
unsigned int i = 0; i <
njoint; i++)
873 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
877 for (
unsigned int i = 0; i <
njoint; i++)
1009 for (
unsigned int i = 0; i < 3; i++) {
1010 position[i] = pose[i];
1011 position[i + 3] = rxyz[i];
1015 "Joint frame not implemented for pose positioning.");
1106 "Modification of the robot state");
1110 double _destination[6];
1118 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1121 for (
unsigned int i = 0; i <
njoint; i++)
1131 for (
unsigned int i = 0; i < 3; i++) {
1132 txyz[i] = position[i];
1133 rxyz[i] = position[i + 3];
1144 bool nearest =
true;
1147 for (
unsigned int i = 0; i <
njoint; i++) {
1148 _destination[i] = q[i];
1150 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1151 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1160 for (
unsigned int i = 0; i <
njoint; i++) {
1161 _destination[i] = position[i];
1163 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1164 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1171 for (
unsigned int i = 0; i < 3; i++) {
1172 txyz[i] = position[i];
1173 rxyz[i] = position[i + 3];
1181 bool nearest =
true;
1184 for (
unsigned int i = 0; i <
njoint; i++) {
1185 _destination[i] = q[i];
1187 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1188 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1198 "Mixt frame not implemented.");
1202 "end-effector frame not implemented.");
1207 if (TryStt == InvalidPosition || TryStt == -1023)
1208 std::cout <<
" : Position out of range.\n";
1209 else if (TryStt < 0)
1210 std::cout <<
" : Unknown error (see Fabien).\n";
1211 else if (error == -1)
1212 std::cout <<
"Position out of range.\n";
1214 if (TryStt < 0 || error < 0) {
1289 double pos4,
double pos5,
double pos6)
1430 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1431 for (
unsigned int i = 0; i <
njoint; i++) {
1432 position[i] = _q[i];
1439 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1442 for (
unsigned int i = 0; i <
njoint; i++)
1455 for (
unsigned int i = 0; i < 3; i++) {
1456 position[i] = fMc[i][3];
1457 position[i + 3] = rxyz[i];
1486 PrimitiveACQ_TIME_Afma6(×tamp);
1521 for (
unsigned int j = 0; j < 3; j++)
1522 RxyzVect[j] = posRxyz[j + 3];
1527 for (
unsigned int j = 0; j < 3; j++) {
1528 position[j] = posRxyz[j];
1529 position[j + 3] = RtuVect[j];
1616 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1618 "Cannot send a velocity to the robot "
1619 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1624 for (
unsigned int i = 0; i < 3; i++)
1626 for (
unsigned int i = 3; i < 6; i++)
1635 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPCAM_AFMA6));
1645 Try(PrimitiveMOVESPEED_CART_Afma6(v_c.
data, REPCAM_AFMA6));
1650 Try(PrimitiveMOVESPEED_Afma6(vel_sat.
data));
1654 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPFIX_AFMA6));
1658 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPMIX_AFMA6));
1663 "Case not taken in account.");
1670 if (TryStt == VelStopOnJoint) {
1671 Int32 axisInJoint[
njoint];
1672 PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1673 for (
unsigned int i = 0; i <
njoint; i++) {
1675 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1678 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1679 if (TryString != NULL) {
1681 printf(
" Error sentence %s\n", TryString);
1758 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
1759 time_cur = timestamp;
1760 for (
unsigned int i = 0; i <
njoint; i++) {
1767 if (!first_time_getvel) {
1772 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1781 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1787 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1802 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1810 for (
unsigned int i = 0; i < 3; i++) {
1812 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1814 velocity[i + 3] = thetaU[i];
1818 velocity /= (time_cur - time_prev_getvel);
1823 "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1827 first_time_getvel =
false;
1831 fMc_prev_getvel = fMc_cur;
1834 q_prev_getvel = q_cur;
1837 time_prev_getvel = time_cur;
1977 std::ifstream fd(filename.c_str(), std::ios::in);
1979 if (!fd.is_open()) {
1984 std::string key(
"R:");
1985 std::string id(
"#AFMA6 - Position");
1986 bool pos_found =
false;
1991 while (std::getline(fd, line)) {
1994 if (!(line.compare(0,
id.size(),
id) == 0)) {
1995 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
1999 if ((line.compare(0, 1,
"#") == 0)) {
2002 if ((line.compare(0, key.size(), key) == 0)) {
2005 if (chain.size() <
njoint + 1)
2007 if (chain.size() <
njoint + 1)
2010 std::istringstream ss(line);
2013 for (
unsigned int i = 0; i <
njoint; i++)
2028 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2063 fd = fopen(filename.c_str(),
"w");
2068#AFMA6 - Position - Version 2.01\n\
2071# Joint position: X, Y, Z: translations in meters\n\
2072# A, B, C: rotations in degrees\n\
2135 Try(PrimitiveGripper_Afma6(1));
2136 std::cout <<
"Open the gripper..." << std::endl;
2154 Try(PrimitiveGripper_Afma6(0));
2155 std::cout <<
"Close the gripper..." << std::endl;
2194 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
2195 for (
unsigned int i = 0; i <
njoint; i++) {
2202 if (!first_time_getdis) {
2206 c_prevMc_cur = fMc_prev_getdis.
inverse() * fMc_cur;
2216 for (
unsigned int i = 0; i < 3; i++) {
2217 displacement[i] = t[i];
2218 displacement[i + 3] = rxyz[i];
2224 displacement = q_cur - q_prev_getdis;
2229 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2234 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2238 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2243 first_time_getdis =
false;
2247 q_prev_getdis = q_cur;
2250 fMc_prev_getdis = fMc_cur;
2271 Int32 axisInJoint[
njoint];
2276 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
2277 for (
unsigned int i = 0; i <
njoint; i++) {
2278 if (axisInJoint[i]) {
2279 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
2280 jointsStatus[i] = axisInJoint[i];
2283 jointsStatus[i] = 0;
2296#elif !defined(VISP_BUILD_SHARED_LIBS)
2299void dummy_vpRobotAfma6(){};
Modelisation of Irisa's gantry robot named Afma6.
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
static const unsigned int njoint
Number of joint.
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
void get_cMe(vpHomogeneousMatrix &cMe) const
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
vpCameraParameters::vpCameraParametersProjType projModel
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Type * data
Address of the first element of the data array.
vpCameraParametersProjType
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
@ functionNotImplementedError
Function not implemented.
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static double rad(double deg)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Implementation of a pose vector and operations on poses.
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_fJe(vpMatrix &_fJe)
bool checkJointLimits(vpColVector &jointsStatus)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
void get_eJe(vpMatrix &_eJe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
static const double defaultPositioningVelocity
void get_cMe(vpHomogeneousMatrix &_cMe) const
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getPositioningVelocity(void)
void set_eMc(const vpHomogeneousMatrix &eMc)
virtual ~vpRobotAfma6(void)
void move(const std::string &filename)
void setPositioningVelocity(double velocity)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
Class that defines a generic virtual robot.
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void setVerbose(bool verbose)
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
double getMaxRotationVelocity(void) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double getMaxTranslationVelocity(void) const
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)