36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_AFMA6
42 #include <sys/types.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>
58 bool vpRobotAfma6::robotAlreadyCreated =
false;
80 void 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";
197 vpRobotAfma6::robotAlreadyCreated =
true;
232 time_prev_getvel = 0;
233 first_time_getvel =
true;
238 first_time_getdis =
true;
241 Try(InitializeConnection(
verbose_));
244 Try(InitializeNode_Afma6());
246 Try(PrimitiveRESET_Afma6());
252 UInt32 HIPowerStatus;
254 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
259 std::cout <<
"Robot status: ";
260 switch (EStopStatus) {
263 if (HIPowerStatus == 0)
264 std::cout <<
"Power is OFF" << std::endl;
266 std::cout <<
"Power is ON" << std::endl;
268 case ESTOP_ACTIVATED:
269 std::cout <<
"Emergency stop is activated" << std::endl;
272 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
273 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
276 std::cout << std::endl;
290 if (TryStt == -20001)
291 printf(
"No connection detected. Check if the robot is powered on \n"
292 "and if the firewire link exist between the MotionBlox and this "
294 else if (TryStt == -675)
295 printf(
" Timeout enabling power...\n");
299 PrimitivePOWEROFF_Afma6();
301 ShutDownConnection();
303 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
358 for (
unsigned int i = 0; i < 3; i++) {
359 eMc_pose[i] =
_etc[i];
360 eMc_pose[i + 3] =
_erc[i];
363 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
397 for (
unsigned int i = 0; i < 3; i++) {
398 eMc_pose[i] =
_etc[i];
399 eMc_pose[i + 3] =
_erc[i];
402 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
455 for (
unsigned int i = 0; i < 3; i++) {
456 eMc_pose[i] =
_etc[i];
457 eMc_pose[i + 3] =
_erc[i];
460 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
533 for (
unsigned int i = 0; i < 3; i++) {
534 eMc_pose[i] =
_etc[i];
535 eMc_pose[i + 3] =
_erc[i];
538 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
565 UInt32 HIPowerStatus;
566 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
577 ShutDownConnection();
579 vpRobotAfma6::robotAlreadyCreated =
false;
598 Try(PrimitiveSTOP_Afma6());
604 std::cout <<
"Change the control mode from velocity to position control.\n";
605 Try(PrimitiveSTOP_Afma6());
616 std::cout <<
"Change the control mode from stop to velocity control.\n";
644 Try(PrimitiveSTOP_Afma6());
668 UInt32 HIPowerStatus;
670 bool firsttime =
true;
671 unsigned int nitermax = 10;
673 for (
unsigned int i = 0; i < nitermax; i++) {
674 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
675 if (EStopStatus == ESTOP_AUTO) {
678 else if (EStopStatus == ESTOP_MANUAL) {
681 else if (EStopStatus == ESTOP_ACTIVATED) {
683 std::cout <<
"Emergency stop is activated! \n"
684 <<
"Check the emergency stop button and push the yellow "
685 "button before continuing."
689 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
694 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
695 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
697 ShutDownConnection();
702 if (EStopStatus == ESTOP_ACTIVATED)
703 std::cout << std::endl;
705 if (EStopStatus == ESTOP_ACTIVATED) {
706 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
710 if (HIPowerStatus == 0) {
711 fprintf(stdout,
"Power ON the Afma6 robot\n");
714 Try(PrimitivePOWERON_Afma6());
738 UInt32 HIPowerStatus;
739 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
742 if (HIPowerStatus == 1) {
743 fprintf(stdout,
"Power OFF the Afma6 robot\n");
746 Try(PrimitivePOWEROFF_Afma6());
772 UInt32 HIPowerStatus;
773 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
776 if (HIPowerStatus == 1) {
834 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
838 for (
unsigned int i = 0; i <
njoint; i++)
879 Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
883 for (
unsigned int i = 0; i <
njoint; i++)
1016 for (
unsigned int i = 0; i < 3; i++) {
1017 position[i] = pose[i];
1018 position[i + 3] = rxyz[i];
1022 "Joint frame not implemented for pose positioning.");
1113 "Modification of the robot state");
1117 double _destination[6];
1125 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1128 for (
unsigned int i = 0; i <
njoint; i++)
1138 for (
unsigned int i = 0; i < 3; i++) {
1139 txyz[i] = position[i];
1140 rxyz[i] = position[i + 3];
1151 bool nearest =
true;
1154 for (
unsigned int i = 0; i <
njoint; i++) {
1155 _destination[i] = q[i];
1157 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1158 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1168 for (
unsigned int i = 0; i <
njoint; i++) {
1169 _destination[i] = position[i];
1171 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1172 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1179 for (
unsigned int i = 0; i < 3; i++) {
1180 txyz[i] = position[i];
1181 rxyz[i] = position[i + 3];
1189 bool nearest =
true;
1192 for (
unsigned int i = 0; i <
njoint; i++) {
1193 _destination[i] = q[i];
1195 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1196 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1207 "Mixt frame not implemented.");
1211 "end-effector frame not implemented.");
1216 if (TryStt == InvalidPosition || TryStt == -1023)
1217 std::cout <<
" : Position out of range.\n";
1218 else if (TryStt < 0)
1219 std::cout <<
" : Unknown error (see Fabien).\n";
1220 else if (error == -1)
1221 std::cout <<
"Position out of range.\n";
1223 if (TryStt < 0 || error < 0) {
1298 double pos4,
double pos5,
double pos6)
1440 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1441 for (
unsigned int i = 0; i <
njoint; i++) {
1442 position[i] = _q[i];
1449 Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
1452 for (
unsigned int i = 0; i <
njoint; i++)
1465 for (
unsigned int i = 0; i < 3; i++) {
1466 position[i] = fMc[i][3];
1467 position[i + 3] = rxyz[i];
1496 PrimitiveACQ_TIME_Afma6(×tamp);
1531 for (
unsigned int j = 0; j < 3; j++)
1532 RxyzVect[j] = posRxyz[j + 3];
1537 for (
unsigned int j = 0; j < 3; j++) {
1538 position[j] = posRxyz[j];
1539 position[j + 3] = RtuVect[j];
1626 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1628 "Cannot send a velocity to the robot "
1629 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1634 for (
unsigned int i = 0; i < 3; i++)
1636 for (
unsigned int i = 3; i < 6; i++)
1645 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPCAM_AFMA6));
1655 Try(PrimitiveMOVESPEED_CART_Afma6(v_c.
data, REPCAM_AFMA6));
1660 Try(PrimitiveMOVESPEED_Afma6(vel_sat.
data));
1664 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPFIX_AFMA6));
1668 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.
data, REPMIX_AFMA6));
1673 "Case not taken in account.");
1680 if (TryStt == VelStopOnJoint) {
1681 Int32 axisInJoint[
njoint];
1682 PrimitiveSTATUS_Afma6(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, axisInJoint,
nullptr);
1683 for (
unsigned int i = 0; i <
njoint; i++) {
1685 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1689 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1690 if (TryString !=
nullptr) {
1692 printf(
" Error sentence %s\n", TryString);
1770 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
1771 time_cur = timestamp;
1772 for (
unsigned int i = 0; i <
njoint; i++) {
1779 if (!first_time_getvel) {
1784 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1793 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1799 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1814 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1822 for (
unsigned int i = 0; i < 3; i++) {
1824 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1826 velocity[i + 3] = thetaU[i];
1830 velocity /= (time_cur - time_prev_getvel);
1835 "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1840 first_time_getvel =
false;
1844 fMc_prev_getvel = fMc_cur;
1847 q_prev_getvel = q_cur;
1850 time_prev_getvel = time_cur;
1990 std::ifstream fd(filename.c_str(), std::ios::in);
1992 if (!fd.is_open()) {
1997 std::string key(
"R:");
1998 std::string id(
"#AFMA6 - Position");
1999 bool pos_found =
false;
2004 while (std::getline(fd, line)) {
2007 if (!(line.compare(0,
id.size(),
id) == 0)) {
2008 std::cout <<
"Error: this position file " << filename <<
" is not for Afma6 robot" << std::endl;
2012 if ((line.compare(0, 1,
"#") == 0)) {
2015 if ((line.compare(0, key.size(), key) == 0)) {
2018 if (chain.size() <
njoint + 1)
2020 if (chain.size() <
njoint + 1)
2023 std::istringstream ss(line);
2026 for (
unsigned int i = 0; i <
njoint; i++)
2041 std::cout <<
"Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2076 fd = fopen(filename.c_str(),
"w");
2081 #AFMA6 - Position - Version 2.01\n\
2084 # Joint position: X, Y, Z: translations in meters\n\
2085 # A, B, C: rotations in degrees\n\
2148 Try(PrimitiveGripper_Afma6(1));
2149 std::cout <<
"Open the gripper..." << std::endl;
2167 Try(PrimitiveGripper_Afma6(0));
2168 std::cout <<
"Close the gripper..." << std::endl;
2207 Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
2208 for (
unsigned int i = 0; i <
njoint; i++) {
2215 if (!first_time_getdis) {
2219 c_prevMc_cur = fMc_prev_getdis.
inverse() * fMc_cur;
2229 for (
unsigned int i = 0; i < 3; i++) {
2230 displacement[i] = t[i];
2231 displacement[i + 3] = rxyz[i];
2237 displacement = q_cur - q_prev_getdis;
2242 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2247 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2251 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2257 first_time_getdis =
false;
2261 q_prev_getdis = q_cur;
2264 fMc_prev_getdis = fMc_cur;
2285 Int32 axisInJoint[
njoint];
2290 Try(PrimitiveSTATUS_Afma6(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, axisInJoint,
nullptr));
2291 for (
unsigned int i = 0; i <
njoint; i++) {
2292 if (axisInJoint[i]) {
2293 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
2294 jointsStatus[i] = axisInJoint[i];
2298 jointsStatus[i] = 0;
2311 #elif !defined(VISP_BUILD_SHARED_LIBS)
2313 void dummy_vpRobotAfma6() { };
Modelization 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 get_fJe(vpMatrix &_fJe) vp_override
bool checkJointLimits(vpColVector &jointsStatus)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
static const double defaultPositioningVelocity
void get_cMe(vpHomogeneousMatrix &_cMe) const
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getPositioningVelocity(void)
void get_eJe(vpMatrix &_eJe) vp_override
void set_eMc(const vpHomogeneousMatrix &eMc)
virtual ~vpRobotAfma6(void)
void move(const std::string &filename)
void setPositioningVelocity(double velocity)
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)