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/vpThetaUVector.h>
49#include <visp3/core/vpVelocityTwistMatrix.h>
50#include <visp3/robot/vpRobotAfma4.h>
51#include <visp3/robot/vpRobotException.h>
57bool vpRobotAfma4::robotAlreadyCreated =
false;
79void emergencyStopAfma4(
int signo)
81 std::cout <<
"Stop the Afma4 application by signal (" << signo <<
"): " << (char)7;
84 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
87 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
90 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
93 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
96 std::cout <<
"SIGQUIT " << std::endl;
99 std::cout << signo << std::endl;
103 PrimitiveSTOP_Afma4();
104 std::cout <<
"Robot was stopped\n";
109 fprintf(stdout,
"Application ");
111 kill(getpid(), SIGKILL);
152 signal(SIGINT, emergencyStopAfma4);
153 signal(SIGBUS, emergencyStopAfma4);
154 signal(SIGSEGV, emergencyStopAfma4);
155 signal(SIGKILL, emergencyStopAfma4);
156 signal(SIGQUIT, emergencyStopAfma4);
160 std::cout <<
"Open communication with MotionBlox.\n";
170 vpRobotAfma4::robotAlreadyCreated =
true;
195 time_prev_getvel = 0;
196 first_time_getvel =
true;
201 first_time_getdis =
true;
204 Try(stt = InitializeConnection(
verbose_));
206 if (stt != SUCCESS) {
207 vpERROR_TRACE(
"Cannot open connection with the motionblox.");
212 Try(stt = InitializeNode_Afma4());
214 if (stt != SUCCESS) {
215 vpERROR_TRACE(
"Cannot open connection with the motionblox.");
218 Try(PrimitiveRESET_Afma4());
221 UInt32 HIPowerStatus;
223 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
228 std::cout <<
"Robot status: ";
229 switch (EStopStatus) {
232 if (HIPowerStatus == 0)
233 std::cout <<
"Power is OFF" << std::endl;
235 std::cout <<
"Power is ON" << std::endl;
237 case ESTOP_ACTIVATED:
238 std::cout <<
"Emergency stop is activated" << std::endl;
241 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
242 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
245 std::cout << std::endl;
259 if (TryStt == -20001)
260 printf(
"No connection detected. Check if the robot is powered on \n"
261 "and if the firewire link exist between the MotionBlox and this "
263 else if (TryStt == -675)
264 printf(
" Timeout enabling power...\n");
268 PrimitivePOWEROFF_Afma4();
270 ShutDownConnection();
272 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
295 UInt32 HIPowerStatus;
296 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
307 ShutDownConnection();
309 vpRobotAfma4::robotAlreadyCreated =
false;
328 Try(PrimitiveSTOP_Afma4());
334 std::cout <<
"Change the control mode from velocity to position control.\n";
335 Try(PrimitiveSTOP_Afma4());
345 std::cout <<
"Change the control mode from stop to velocity control.\n";
373 Try(PrimitiveSTOP_Afma4());
397 UInt32 HIPowerStatus;
399 bool firsttime =
true;
400 unsigned int nitermax = 10;
402 for (
unsigned int i = 0; i < nitermax; i++) {
403 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
404 if (EStopStatus == ESTOP_AUTO) {
406 }
else if (EStopStatus == ESTOP_MANUAL) {
408 }
else if (EStopStatus == ESTOP_ACTIVATED) {
410 std::cout <<
"Emergency stop is activated! \n"
411 <<
"Check the emergency stop button and push the yellow "
412 "button before continuing."
416 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
420 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
421 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
423 ShutDownConnection();
428 if (EStopStatus == ESTOP_ACTIVATED)
429 std::cout << std::endl;
431 if (EStopStatus == ESTOP_ACTIVATED) {
432 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
436 if (HIPowerStatus == 0) {
437 fprintf(stdout,
"Power ON the Afma4 robot\n");
440 Try(PrimitivePOWERON_Afma4());
464 UInt32 HIPowerStatus;
465 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
468 if (HIPowerStatus == 1) {
469 fprintf(stdout,
"Power OFF the Afma4 robot\n");
472 Try(PrimitivePOWEROFF_Afma4());
498 UInt32 HIPowerStatus;
499 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
502 if (HIPowerStatus == 1) {
542 double position[this->
njoint];
546 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
550 for (
unsigned int i = 0; i <
njoint; i++)
588 double position[this->
njoint];
592 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
596 for (
unsigned int i = 0; i <
njoint; i++)
627 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
631 for (
unsigned int i = 0; i <
njoint; i++)
755 "Modification of the robot state");
764 "Reference frame not implemented.");
768 "Camera frame not implemented.");
772 "Mixt frame not implemented.");
776 "End-effector frame not implemented.");
783 if (position.
getRows() != this->njoint) {
790 Try(PrimitiveMOVE_Afma4(position.
data, positioningVelocity));
791 Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
794 if (TryStt == InvalidPosition || TryStt == -1023)
795 std::cout <<
" : Position out of range.\n";
797 std::cout <<
" : Unknown error (see Fabien).\n";
798 else if (error == -1)
799 std::cout <<
"Position out of range.\n";
801 if (TryStt < 0 || error < 0) {
860 const double q4,
const double q5)
927 PrimitiveACQ_TIME_Afma4(×tamp);
1005 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1006 for (
unsigned int i = 0; i < this->
njoint; i++) {
1007 position[i] = _q[i];
1014 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1017 for (
unsigned int i = 0; i < this->
njoint; i++)
1030 for (
unsigned int i = 0; i < 3; i++) {
1031 position[i] = fMc[i][3];
1032 position[i + 3] = rxyz[i];
1129 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1131 "Cannot send a velocity to the robot "
1132 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1141 vpERROR_TRACE(
"Bad dimension of the velocity vector in camera frame");
1148 if (vel.
getRows() != this->njoint) {
1150 "velocity vector ");
1156 "in the reference frame:"
1157 "functionality not implemented");
1161 "in the mixt frame:"
1162 "functionality not implemented");
1166 "in the end-effector frame:"
1167 "functionality not implemented");
1183 for (
unsigned int i = 0; i < 3; i++) {
1196 joint_vel =
eJe * velocity;
1210 vpMatrix eJe_inverse = fJe_inverse * fVe;
1215 joint_vel = eJe_inverse * eVc * velocity;
1237 Try(PrimitiveMOVESPEED_Afma4(joint_vel.
data));
1241 if (TryStt == VelStopOnJoint) {
1242 UInt32 axisInJoint[
njoint];
1243 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1244 for (
unsigned int i = 0; i <
njoint; i++) {
1246 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1249 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1250 if (TryString != NULL) {
1252 printf(
" Error sentence %s\n", TryString);
1335 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1336 time_cur = timestamp;
1338 for (
unsigned int i = 0; i < this->
njoint; i++) {
1345 if (!first_time_getvel) {
1350 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1359 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1365 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1380 "functionality not implemented");
1386 "functionality not implemented");
1392 first_time_getvel =
false;
1396 fMc_prev_getvel = fMc_cur;
1399 q_prev_getvel = q_cur;
1402 time_prev_getvel = time_cur;
1542 std::ifstream fd(filename.c_str(), std::ios::in);
1544 if (!fd.is_open()) {
1549 std::string key(
"R:");
1550 std::string id(
"#AFMA4 - Position");
1551 bool pos_found =
false;
1556 while (std::getline(fd, line)) {
1559 if (!(line.compare(0,
id.size(),
id) == 0)) {
1560 std::cout <<
"Error: this position file " << filename <<
" is not for Afma4 robot" << std::endl;
1564 if ((line.compare(0, 1,
"#") == 0)) {
1567 if ((line.compare(0, key.size(), key) == 0)) {
1570 if (chain.size() <
njoint + 1)
1572 if (chain.size() <
njoint + 1)
1575 std::istringstream ss(line);
1578 for (
unsigned int i = 0; i <
njoint; i++)
1593 std::cout <<
"Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1627 fd = fopen(filename.c_str(),
"w");
1632#AFMA4 - Position - Version 2.01\n\
1635# Joint position: X : rotation of the turret in degrees (joint 1)\n\
1636# Y : vertical translation in meters (joint 2)\n\
1637# A : pan rotation of the camera in degrees (joint 4)\n\
1638# B : tilt rotation of the camera in degrees (joint 5)\n\
1698 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1699 for (
unsigned int i = 0; i <
njoint; i++) {
1703 if (!first_time_getdis) {
1706 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1711 displacement = q_cur - q_prev_getdis;
1716 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1721 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1725 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1730 first_time_getdis =
false;
1734 q_prev_getdis = q_cur;
1743#elif !defined(VISP_BUILD_SHARED_LIBS)
1746void dummy_vpRobotAfma4(){};
Modelisation of Irisa's cylindrical robot named Afma4.
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void get_cMe(vpHomogeneousMatrix &cMe) const
static const unsigned int njoint
Number of joint.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Type * data
Address of the first element of the data array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
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.
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_cVf(vpVelocityTwistMatrix &cVf) const
virtual ~vpRobotAfma4(void)
double getPositioningVelocity(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_fJe(vpMatrix &fJe)
void move(const char *filename)
static const double defaultPositioningVelocity
void get_eJe(vpMatrix &eJe)
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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.
@ communicationError
Unable to communicate.
@ 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.
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)