36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_AFMA4
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/vpThetaUVector.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/robot/vpRobotAfma4.h>
51 #include <visp3/robot/vpRobotException.h>
57 bool vpRobotAfma4::robotAlreadyCreated =
false;
79 void 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";
171 vpRobotAfma4::robotAlreadyCreated =
true;
196 time_prev_getvel = 0;
197 first_time_getvel =
true;
202 first_time_getdis =
true;
205 Try(stt = InitializeConnection(
verbose_));
207 if (stt != SUCCESS) {
208 vpERROR_TRACE(
"Cannot open connection with the motionblox.");
213 Try(stt = InitializeNode_Afma4());
215 if (stt != SUCCESS) {
216 vpERROR_TRACE(
"Cannot open connection with the motionblox.");
219 Try(PrimitiveRESET_Afma4());
222 UInt32 HIPowerStatus;
224 Try(PrimitiveSTATUS_Afma4(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
229 std::cout <<
"Robot status: ";
230 switch (EStopStatus) {
233 if (HIPowerStatus == 0)
234 std::cout <<
"Power is OFF" << std::endl;
236 std::cout <<
"Power is ON" << std::endl;
238 case ESTOP_ACTIVATED:
239 std::cout <<
"Emergency stop is activated" << std::endl;
242 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
243 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
246 std::cout << std::endl;
260 if (TryStt == -20001)
261 printf(
"No connection detected. Check if the robot is powered on \n"
262 "and if the firewire link exist between the MotionBlox and this "
264 else if (TryStt == -675)
265 printf(
" Timeout enabling power...\n");
269 PrimitivePOWEROFF_Afma4();
271 ShutDownConnection();
273 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
296 UInt32 HIPowerStatus;
297 Try(PrimitiveSTATUS_Afma4(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
308 ShutDownConnection();
310 vpRobotAfma4::robotAlreadyCreated =
false;
329 Try(PrimitiveSTOP_Afma4());
335 std::cout <<
"Change the control mode from velocity to position control.\n";
336 Try(PrimitiveSTOP_Afma4());
347 std::cout <<
"Change the control mode from stop to velocity control.\n";
375 Try(PrimitiveSTOP_Afma4());
399 UInt32 HIPowerStatus;
401 bool firsttime =
true;
402 unsigned int nitermax = 10;
404 for (
unsigned int i = 0; i < nitermax; i++) {
405 Try(PrimitiveSTATUS_Afma4(
nullptr,
nullptr, &EStopStatus,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
406 if (EStopStatus == ESTOP_AUTO) {
409 else if (EStopStatus == ESTOP_MANUAL) {
412 else if (EStopStatus == ESTOP_ACTIVATED) {
414 std::cout <<
"Emergency stop is activated! \n"
415 <<
"Check the emergency stop button and push the yellow "
416 "button before continuing."
420 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
425 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
426 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
428 ShutDownConnection();
433 if (EStopStatus == ESTOP_ACTIVATED)
434 std::cout << std::endl;
436 if (EStopStatus == ESTOP_ACTIVATED) {
437 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
441 if (HIPowerStatus == 0) {
442 fprintf(stdout,
"Power ON the Afma4 robot\n");
445 Try(PrimitivePOWERON_Afma4());
469 UInt32 HIPowerStatus;
470 Try(PrimitiveSTATUS_Afma4(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
473 if (HIPowerStatus == 1) {
474 fprintf(stdout,
"Power OFF the Afma4 robot\n");
477 Try(PrimitivePOWEROFF_Afma4());
503 UInt32 HIPowerStatus;
504 Try(PrimitiveSTATUS_Afma4(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, &HIPowerStatus));
507 if (HIPowerStatus == 1) {
547 double position[this->
njoint];
551 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
555 for (
unsigned int i = 0; i <
njoint; i++)
594 double position[this->
njoint];
598 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
602 for (
unsigned int i = 0; i <
njoint; i++)
634 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
638 for (
unsigned int i = 0; i <
njoint; i++)
763 "Modification of the robot state");
772 "Reference frame not implemented.");
776 "Camera frame not implemented.");
780 "Mixt frame not implemented.");
784 "End-effector frame not implemented.");
791 if (position.
getRows() != this->njoint) {
798 Try(PrimitiveMOVE_Afma4(position.
data, positioningVelocity));
799 Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
802 if (TryStt == InvalidPosition || TryStt == -1023)
803 std::cout <<
" : Position out of range.\n";
805 std::cout <<
" : Unknown error (see Fabien).\n";
806 else if (error == -1)
807 std::cout <<
"Position out of range.\n";
809 if (TryStt < 0 || error < 0) {
868 const double q4,
const double q5)
936 PrimitiveACQ_TIME_Afma4(×tamp);
1014 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1015 for (
unsigned int i = 0; i < this->
njoint; i++) {
1016 position[i] = _q[i];
1023 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1026 for (
unsigned int i = 0; i < this->
njoint; i++)
1039 for (
unsigned int i = 0; i < 3; i++) {
1040 position[i] = fMc[i][3];
1041 position[i + 3] = rxyz[i];
1138 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1140 "Cannot send a velocity to the robot "
1141 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1150 vpERROR_TRACE(
"Bad dimension of the velocity vector in camera frame");
1157 if (vel.
getRows() != this->njoint) {
1159 "velocity vector ");
1165 "in the reference frame:"
1166 "functionality not implemented");
1170 "in the mixt frame:"
1171 "functionality not implemented");
1175 "in the end-effector frame:"
1176 "functionality not implemented");
1192 for (
unsigned int i = 0; i < 3; i++) {
1205 joint_vel =
eJe * velocity;
1219 vpMatrix eJe_inverse = fJe_inverse * fVe;
1224 joint_vel = eJe_inverse * eVc * velocity;
1246 Try(PrimitiveMOVESPEED_Afma4(joint_vel.
data));
1250 if (TryStt == VelStopOnJoint) {
1251 UInt32 axisInJoint[
njoint];
1252 PrimitiveSTATUS_Afma4(
nullptr,
nullptr,
nullptr,
nullptr,
nullptr, axisInJoint,
nullptr);
1253 for (
unsigned int i = 0; i <
njoint; i++) {
1255 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1259 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1260 if (TryString !=
nullptr) {
1262 printf(
" Error sentence %s\n", TryString);
1346 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1347 time_cur = timestamp;
1349 for (
unsigned int i = 0; i < this->
njoint; i++) {
1356 if (!first_time_getvel) {
1361 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1370 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1376 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1391 "functionality not implemented");
1397 "functionality not implemented");
1404 first_time_getvel =
false;
1408 fMc_prev_getvel = fMc_cur;
1411 q_prev_getvel = q_cur;
1414 time_prev_getvel = time_cur;
1554 std::ifstream fd(filename.c_str(), std::ios::in);
1556 if (!fd.is_open()) {
1561 std::string key(
"R:");
1562 std::string id(
"#AFMA4 - Position");
1563 bool pos_found =
false;
1568 while (std::getline(fd, line)) {
1571 if (!(line.compare(0,
id.size(),
id) == 0)) {
1572 std::cout <<
"Error: this position file " << filename <<
" is not for Afma4 robot" << std::endl;
1576 if ((line.compare(0, 1,
"#") == 0)) {
1579 if ((line.compare(0, key.size(), key) == 0)) {
1582 if (chain.size() <
njoint + 1)
1584 if (chain.size() <
njoint + 1)
1587 std::istringstream ss(line);
1590 for (
unsigned int i = 0; i <
njoint; i++)
1605 std::cout <<
"Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1639 fd = fopen(filename.c_str(),
"w");
1644 #AFMA4 - Position - Version 2.01\n\
1647 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1648 # Y : vertical translation in meters (joint 2)\n\
1649 # A : pan rotation of the camera in degrees (joint 4)\n\
1650 # B : tilt rotation of the camera in degrees (joint 5)\n\
1710 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1711 for (
unsigned int i = 0; i <
njoint; i++) {
1715 if (!first_time_getdis) {
1718 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
1723 displacement = q_cur - q_prev_getdis;
1728 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
1733 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
1737 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1743 first_time_getdis =
false;
1747 q_prev_getdis = q_cur;
1756 #elif !defined(VISP_BUILD_SHARED_LIBS)
1758 void dummy_vpRobotAfma4() { };
Modelization 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
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
virtual ~vpRobotAfma4(void)
void get_eJe(vpMatrix &eJe) vp_override
double getPositioningVelocity(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void move(const char *filename)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
static const double defaultPositioningVelocity
void get_fJe(vpMatrix &fJe) vp_override
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
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)