Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
vpRobotAfma4.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Afma4 robot.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_AFMA4
39 
40 #include <signal.h>
41 #include <stdlib.h>
42 #include <sys/types.h>
43 #include <unistd.h>
44 
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>
52 
53 BEGIN_VISP_NAMESPACE
54 /* ---------------------------------------------------------------------- */
55 /* --- STATIC ----------------------------------------------------------- */
56 /* ---------------------------------------------------------------------- */
57 
58 bool vpRobotAfma4::robotAlreadyCreated = false;
59 
68 
69 /* ---------------------------------------------------------------------- */
70 /* --- EMERGENCY STOP --------------------------------------------------- */
71 /* ---------------------------------------------------------------------- */
72 
80 void emergencyStopAfma4(int signo)
81 {
82  std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
83  switch (signo) {
84  case SIGINT:
85  std::cout << "SIGINT (stop by ^C) " << std::endl;
86  break;
87  case SIGBUS:
88  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
89  break;
90  case SIGSEGV:
91  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
92  break;
93  case SIGKILL:
94  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
95  break;
96  case SIGQUIT:
97  std::cout << "SIGQUIT " << std::endl;
98  break;
99  default:
100  std::cout << signo << std::endl;
101  }
102  // std::cout << "Emergency stop called\n";
103  // PrimitiveESTOP_Afma4();
104  PrimitiveSTOP_Afma4();
105  std::cout << "Robot was stopped\n";
106 
107  // Free allocated resources
108  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109 
110  fprintf(stdout, "Application ");
111  fflush(stdout);
112  kill(getpid(), SIGKILL);
113  exit(1);
114 }
115 
116 /* ---------------------------------------------------------------------- */
117 /* --- CONSTRUCTOR ------------------------------------------------------ */
118 /* ---------------------------------------------------------------------- */
119 
131 vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
132 {
133 
134  /*
135  #define SIGHUP 1 // hangup
136  #define SIGINT 2 // interrupt (rubout)
137  #define SIGQUIT 3 // quit (ASCII FS)
138  #define SIGILL 4 // illegal instruction (not reset when caught)
139  #define SIGTRAP 5 // trace trap (not reset when caught)
140  #define SIGIOT 6 // IOT instruction
141  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
142  #define SIGEMT 7 // EMT instruction
143  #define SIGFPE 8 // floating point exception
144  #define SIGKILL 9 // kill (cannot be caught or ignored)
145  #define SIGBUS 10 // bus error
146  #define SIGSEGV 11 // segmentation violation
147  #define SIGSYS 12 // bad argument to system call
148  #define SIGPIPE 13 // write on a pipe with no one to read it
149  #define SIGALRM 14 // alarm clock
150  #define SIGTERM 15 // software termination signal from kill
151  */
152 
153  signal(SIGINT, emergencyStopAfma4);
154  signal(SIGBUS, emergencyStopAfma4);
155  signal(SIGSEGV, emergencyStopAfma4);
156  signal(SIGKILL, emergencyStopAfma4);
157  signal(SIGQUIT, emergencyStopAfma4);
158 
159  setVerbose(verbose);
160  if (verbose_)
161  std::cout << "Open communication with MotionBlox.\n";
162  try {
163  this->init();
165  }
166  catch (...) {
167  // vpERROR_TRACE("Error caught") ;
168  throw;
169  }
170  positioningVelocity = defaultPositioningVelocity;
171 
172  vpRobotAfma4::robotAlreadyCreated = true;
173 
174  return;
175 }
176 
177 /* ------------------------------------------------------------------------ */
178 /* --- INITIALISATION ----------------------------------------------------- */
179 /* ------------------------------------------------------------------------ */
180 
190 {
191  int stt;
192  InitTry;
193 
194  // Initialise private variables used to compute the measured velocities
195  q_prev_getvel.resize(4);
196  q_prev_getvel = 0;
197  time_prev_getvel = 0;
198  first_time_getvel = true;
199 
200  // Initialise private variables used to compute the measured displacement
201  q_prev_getdis.resize(4);
202  q_prev_getdis = 0;
203  first_time_getdis = true;
204 
205  // Initialize the firewire connection
206  Try(stt = InitializeConnection(verbose_));
207 
208  if (stt != SUCCESS) {
209  vpERROR_TRACE("Cannot open connection with the motionblox.");
210  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
211  }
212 
213  // Connect to the servoboard using the servo board GUID
214  Try(stt = InitializeNode_Afma4());
215 
216  if (stt != SUCCESS) {
217  vpERROR_TRACE("Cannot open connection with the motionblox.");
218  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
219  }
220  Try(PrimitiveRESET_Afma4());
221 
222  // Look if the power is on or off
223  UInt32 HIPowerStatus;
224  UInt32 EStopStatus;
225  Try(PrimitiveSTATUS_Afma4(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
226  CAL_Wait(0.1);
227 
228  // Print the robot status
229  if (verbose_) {
230  std::cout << "Robot status: ";
231  switch (EStopStatus) {
232  case ESTOP_AUTO:
233  case ESTOP_MANUAL:
234  if (HIPowerStatus == 0)
235  std::cout << "Power is OFF" << std::endl;
236  else
237  std::cout << "Power is ON" << std::endl;
238  break;
239  case ESTOP_ACTIVATED:
240  std::cout << "Emergency stop is activated" << std::endl;
241  break;
242  default:
243  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
244  std::cout << "You have to call Adept for maintenance..." << std::endl;
245  // Free allocated resources
246  }
247  std::cout << std::endl;
248  }
249  // get real joint min/max from the MotionBlox
250  Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
251  // for (unsigned int i=0; i < njoint; i++) {
252  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
253  // _joint_max[i]);
254  // }
255 
256  // If an error occur in the low level controller, goto here
257  // CatchPrint();
258  Catch();
259 
260  // Test if an error occurs
261  if (TryStt == -20001)
262  printf("No connection detected. Check if the robot is powered on \n"
263  "and if the firewire link exist between the MotionBlox and this "
264  "computer.\n");
265  else if (TryStt == -675)
266  printf(" Timeout enabling power...\n");
267 
268  if (TryStt < 0) {
269  // Power off the robot
270  PrimitivePOWEROFF_Afma4();
271  // Free allocated resources
272  ShutDownConnection();
273 
274  std::cout << "Cannot open connection with the motionblox..." << std::endl;
275  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
276  }
277  return;
278 }
279 
280 /* ------------------------------------------------------------------------ */
281 /* --- DESTRUCTOR --------------------------------------------------------- */
282 /* ------------------------------------------------------------------------ */
283 
291 {
292  InitTry;
293 
295 
296  // Look if the power is on or off
297  UInt32 HIPowerStatus;
298  Try(PrimitiveSTATUS_Afma4(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
299  CAL_Wait(0.1);
300 
301  // if (HIPowerStatus == 1) {
302  // fprintf(stdout, "Power OFF the robot\n");
303  // fflush(stdout);
304 
305  // Try( PrimitivePOWEROFF_Afma4() );
306  // }
307 
308  // Free allocated resources
309  ShutDownConnection();
310 
311  vpRobotAfma4::robotAlreadyCreated = false;
312 
313  CatchPrint();
314  return;
315 }
316 
324 {
325  InitTry;
326 
327  switch (newState) {
328  case vpRobot::STATE_STOP: {
330  Try(PrimitiveSTOP_Afma4());
331  }
332  break;
333  }
336  std::cout << "Change the control mode from velocity to position control.\n";
337  Try(PrimitiveSTOP_Afma4());
338  }
339  else {
340  // std::cout << "Change the control mode from stop to position
341  // control.\n";
342  }
343  this->powerOn();
344  break;
345  }
348  std::cout << "Change the control mode from stop to velocity control.\n";
349  }
350  this->powerOn();
351  break;
352  }
353  default:
354  break;
355  }
356 
357  CatchPrint();
358 
359  return vpRobot::setRobotState(newState);
360 }
361 
362 /* ------------------------------------------------------------------------ */
363 /* --- STOP --------------------------------------------------------------- */
364 /* ------------------------------------------------------------------------ */
365 
374 {
375  InitTry;
376  Try(PrimitiveSTOP_Afma4());
378 
379  CatchPrint();
380  if (TryStt < 0) {
381  vpERROR_TRACE("Cannot stop robot motion");
382  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
383  }
384 }
385 
396 {
397  InitTry;
398 
399  // Look if the power is on or off
400  UInt32 HIPowerStatus;
401  UInt32 EStopStatus;
402  bool firsttime = true;
403  unsigned int nitermax = 10;
404 
405  for (unsigned int i = 0; i < nitermax; i++) {
406  Try(PrimitiveSTATUS_Afma4(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
407  if (EStopStatus == ESTOP_AUTO) {
408  break; // exit for loop
409  }
410  else if (EStopStatus == ESTOP_MANUAL) {
411  break; // exit for loop
412  }
413  else if (EStopStatus == ESTOP_ACTIVATED) {
414  if (firsttime) {
415  std::cout << "Emergency stop is activated! \n"
416  << "Check the emergency stop button and push the yellow "
417  "button before continuing."
418  << std::endl;
419  firsttime = false;
420  }
421  fprintf(stdout, "Remaining time %us \r", nitermax - i);
422  fflush(stdout);
423  CAL_Wait(1);
424  }
425  else {
426  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
427  std::cout << "You have to call Adept for maintenance..." << std::endl;
428  // Free allocated resources
429  ShutDownConnection();
430  throw(vpRobotException(vpRobotException::lowLevelError, "Error in the emergency chain"));
431  }
432  }
433 
434  if (EStopStatus == ESTOP_ACTIVATED)
435  std::cout << std::endl;
436 
437  if (EStopStatus == ESTOP_ACTIVATED) {
438  std::cout << "Sorry, cannot power on the robot." << std::endl;
439  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
440  }
441 
442  if (HIPowerStatus == 0) {
443  fprintf(stdout, "Power ON the Afma4 robot\n");
444  fflush(stdout);
445 
446  Try(PrimitivePOWERON_Afma4());
447  }
448 
449  CatchPrint();
450  if (TryStt < 0) {
451  vpERROR_TRACE("Cannot power on the robot");
452  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
453  }
454 }
455 
466 {
467  InitTry;
468 
469  // Look if the power is on or off
470  UInt32 HIPowerStatus;
471  Try(PrimitiveSTATUS_Afma4(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
472  CAL_Wait(0.1);
473 
474  if (HIPowerStatus == 1) {
475  fprintf(stdout, "Power OFF the Afma4 robot\n");
476  fflush(stdout);
477 
478  Try(PrimitivePOWEROFF_Afma4());
479  }
480 
481  CatchPrint();
482  if (TryStt < 0) {
483  vpERROR_TRACE("Cannot power off the robot");
484  throw vpRobotException(vpRobotException::communicationError, "Cannot power off the robot.");
485  }
486 }
487 
500 {
501  InitTry;
502  bool status = false;
503  // Look if the power is on or off
504  UInt32 HIPowerStatus;
505  Try(PrimitiveSTATUS_Afma4(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
506  CAL_Wait(0.1);
507 
508  if (HIPowerStatus == 1) {
509  status = true;
510  }
511 
512  CatchPrint();
513  if (TryStt < 0) {
514  vpERROR_TRACE("Cannot get the power status");
515  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
516  }
517  return status;
518 }
519 
530 {
532  vpAfma4::get_cMe(cMe);
533 
534  cVe.buildFrom(cMe);
535 }
536 
547 {
548  double position[this->njoint];
549  double timestamp;
550 
551  InitTry;
552  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
553  CatchPrint();
554 
555  vpColVector q(this->njoint);
556  for (unsigned int i = 0; i < njoint; i++)
557  q[i] = position[i];
558 
559  try {
560  vpAfma4::get_cVf(q, cVf);
561  }
562  catch (...) {
563  vpERROR_TRACE("catch exception ");
564  throw;
565  }
566 }
567 
579 
593 {
594 
595  double position[this->njoint];
596  double timestamp;
597 
598  InitTry;
599  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
600  CatchPrint();
601 
602  vpColVector q(this->njoint);
603  for (unsigned int i = 0; i < njoint; i++)
604  q[i] = position[i];
605 
606  try {
607  vpAfma4::get_eJe(q, eJe);
608  }
609  catch (...) {
610  vpERROR_TRACE("catch exception ");
611  throw;
612  }
613 }
614 
630 {
631  double position[6];
632  double timestamp;
633 
634  InitTry;
635  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
636  CatchPrint();
637 
638  vpColVector q(6);
639  for (unsigned int i = 0; i < njoint; i++)
640  q[i] = position[i];
641 
642  try {
643  vpAfma4::get_fJe(q, fJe);
644  }
645  catch (...) {
646  vpERROR_TRACE("Error caught");
647  throw;
648  }
649 }
678 void vpRobotAfma4::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
679 
685 double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; }
686 
760 {
761 
763  vpERROR_TRACE("Robot was not in position-based control\n"
764  "Modification of the robot state");
766  }
767 
768  int error = 0;
769 
770  switch (frame) {
772  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
773  "Reference frame not implemented.");
774  break;
776  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
777  "Camera frame not implemented.");
778  break;
779  case vpRobot::MIXT_FRAME:
780  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
781  "Mixt frame not implemented.");
782  break;
784  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
785  "End-effector frame not implemented.");
786  break;
787 
789  break;
790  }
791  }
792  if (position.getRows() != this->njoint) {
793  vpERROR_TRACE("Positioning error: bad vector dimension.");
794  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positioning error: bad vector dimension.");
795  }
796 
797  InitTry;
798 
799  Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
800  Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
801 
802  CatchPrint();
803  if (TryStt == InvalidPosition || TryStt == -1023)
804  std::cout << " : Position out of range.\n";
805  else if (TryStt < 0)
806  std::cout << " : Unknown error (see Fabien).\n";
807  else if (error == -1)
808  std::cout << "Position out of range.\n";
809 
810  if (TryStt < 0 || error < 0) {
811  vpERROR_TRACE("Positioning error.");
812  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
813  }
814 
815  return;
816 }
817 
868 void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
869  const double q4, const double q5)
870 {
871  try {
872  vpColVector position(this->njoint);
873  position[0] = q1;
874  position[1] = q2;
875  position[2] = q4;
876  position[3] = q5;
877 
878  setPosition(frame, position);
879  }
880  catch (...) {
881  vpERROR_TRACE("Error caught");
882  throw;
883  }
884 }
885 
915 void vpRobotAfma4::setPosition(const char *filename)
916 {
917  vpColVector q;
918  bool ret;
919 
920  ret = this->readPosFile(filename, q);
921 
922  if (ret == false) {
923  vpERROR_TRACE("Bad position in \"%s\"", filename);
924  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
925  }
928 }
929 
934 double vpRobotAfma4::getTime() const
935 {
936  double timestamp;
937  PrimitiveACQ_TIME_Afma4(&timestamp);
938  return timestamp;
939 }
940 
1001 void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1002 {
1003 
1004  InitTry;
1005 
1006  position.resize(this->njoint);
1007 
1008  switch (frame) {
1009  case vpRobot::CAMERA_FRAME: {
1010  position = 0;
1011  return;
1012  }
1013  case vpRobot::ARTICULAR_FRAME: {
1014  double _q[njoint];
1015  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1016  for (unsigned int i = 0; i < this->njoint; i++) {
1017  position[i] = _q[i];
1018  }
1019 
1020  return;
1021  }
1022  case vpRobot::REFERENCE_FRAME: {
1023  double _q[njoint];
1024  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1025 
1026  vpColVector q(this->njoint);
1027  for (unsigned int i = 0; i < this->njoint; i++)
1028  q[i] = _q[i];
1029 
1030  // Compute fMc
1031  vpHomogeneousMatrix fMc;
1032  vpAfma4::get_fMc(q, fMc);
1033 
1034  // From fMc extract the pose
1035  vpRotationMatrix fRc;
1036  fMc.extract(fRc);
1037  vpRxyzVector rxyz;
1038  rxyz.buildFrom(fRc);
1039 
1040  for (unsigned int i = 0; i < 3; i++) {
1041  position[i] = fMc[i][3]; // translation x,y,z
1042  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1043  }
1044  break;
1045  }
1046  case vpRobot::MIXT_FRAME: {
1047  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1048  "not implemented");
1049  }
1051  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1052  "not implemented");
1053  }
1054  }
1055 
1056  CatchPrint();
1057  if (TryStt < 0) {
1058  vpERROR_TRACE("Cannot get position.");
1059  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1060  }
1061 
1062  return;
1063 }
1064 
1076 {
1077  double timestamp;
1078  getPosition(frame, position, timestamp);
1079 }
1080 
1135 {
1136 
1138  vpERROR_TRACE("Cannot send a velocity to the robot "
1139  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1141  "Cannot send a velocity to the robot "
1142  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1143  }
1144 
1145  // Check the dimension of the velocity vector to see if it is
1146  // compatible with the requested frame
1147  switch (frame) {
1148  case vpRobot::CAMERA_FRAME: {
1149  // if (vel.getRows() != 2) {
1150  if (vel.getRows() != 6) {
1151  vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1152  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1153  "in camera frame");
1154  }
1155  break;
1156  }
1157  case vpRobot::ARTICULAR_FRAME: {
1158  if (vel.getRows() != this->njoint) {
1159  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1160  "velocity vector ");
1161  }
1162  break;
1163  }
1164  case vpRobot::REFERENCE_FRAME: {
1165  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1166  "in the reference frame:"
1167  "functionality not implemented");
1168  }
1169  case vpRobot::MIXT_FRAME: {
1170  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1171  "in the mixt frame:"
1172  "functionality not implemented");
1173  }
1175  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1176  "in the end-effector frame:"
1177  "functionality not implemented");
1178  }
1179  default: {
1180  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
1181  }
1182  }
1183 
1184  //
1185  // Velocities saturation with normalization
1186  //
1187  vpColVector joint_vel(this->njoint);
1188 
1189  // Case of the camera frame where we control only 2 dof
1190  if (frame == vpRobot::CAMERA_FRAME) {
1191  vpColVector vel_max(6);
1192 
1193  for (unsigned int i = 0; i < 3; i++) {
1194  vel_max[i] = getMaxTranslationVelocity();
1195  vel_max[i + 3] = getMaxRotationVelocity();
1196  }
1197 
1198  vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1199 
1200 #if 0 // ok
1201  vpMatrix eJe(4, 6);
1202  eJe = 0;
1203  eJe[2][4] = -1;
1204  eJe[3][3] = 1;
1205 
1206  joint_vel = eJe * velocity; // Compute the articular velocity
1207 #endif
1208  vpColVector q;
1210  vpMatrix fJe_inverse;
1211  get_fJe_inverse(q, fJe_inverse);
1212  vpHomogeneousMatrix fMe;
1213  get_fMe(q, fMe);
1215  t = 0;
1216  vpRotationMatrix fRe;
1217  fMe.extract(fRe);
1218  vpVelocityTwistMatrix fVe(t, fRe);
1219  // compute the inverse jacobian in the end-effector frame
1220  vpMatrix eJe_inverse = fJe_inverse * fVe;
1221 
1222  // Transform the velocities from camera to end-effector frame
1224  eVc.buildFrom(this->_eMc);
1225  joint_vel = eJe_inverse * eVc * velocity;
1226 
1227  // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1228  // joint_vel[2], joint_vel[3]);
1229  }
1230 
1231  // Case of the joint control where we control all the joints
1232  else if (frame == vpRobot::ARTICULAR_FRAME) {
1233 
1234  vpColVector vel_max(4);
1235 
1236  vel_max[0] = getMaxRotationVelocity();
1237  vel_max[1] = getMaxTranslationVelocity();
1238  vel_max[2] = getMaxRotationVelocity();
1239  vel_max[3] = getMaxRotationVelocity();
1240 
1241  joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1242  }
1243 
1244  InitTry;
1245 
1246  // Send a joint velocity to the low level controller
1247  Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
1248 
1249  Catch();
1250  if (TryStt < 0) {
1251  if (TryStt == VelStopOnJoint) {
1252  UInt32 axisInJoint[njoint];
1253  PrimitiveSTATUS_Afma4(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
1254  for (unsigned int i = 0; i < njoint; i++) {
1255  if (axisInJoint[i])
1256  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1257  }
1258  }
1259  else {
1260  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1261  if (TryString != nullptr) {
1262  // The statement is in TryString, but we need to check the validity
1263  printf(" Error sentence %s\n", TryString); // Print the TryString
1264  }
1265  else {
1266  printf("\n");
1267  }
1268  }
1269  }
1270 
1271  return;
1272 }
1273 
1274 /* ------------------------------------------------------------------------ */
1275 /* --- GET ---------------------------------------------------------------- */
1276 /* ------------------------------------------------------------------------ */
1277 
1325 void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1326 {
1327 
1328  switch (frame) {
1330  velocity.resize(this->njoint);
1331  break;
1332  default:
1333  velocity.resize(6);
1334  }
1335 
1336  velocity = 0;
1337 
1338  double q[4];
1339  vpColVector q_cur(4);
1340  vpHomogeneousMatrix fMc_cur;
1341  vpHomogeneousMatrix cMc; // camera displacement
1342  double time_cur;
1343 
1344  InitTry;
1345 
1346  // Get the current joint position
1347  Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1348  time_cur = timestamp;
1349 
1350  for (unsigned int i = 0; i < this->njoint; i++) {
1351  q_cur[i] = q[i];
1352  }
1353 
1354  // Get the camera pose from the direct kinematics
1355  vpAfma4::get_fMc(q_cur, fMc_cur);
1356 
1357  if (!first_time_getvel) {
1358 
1359  switch (frame) {
1360  case vpRobot::CAMERA_FRAME: {
1361  // Compute the displacement of the camera since the previous call
1362  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1363 
1364  // Compute the velocity of the camera from this displacement
1365  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1366 
1367  break;
1368  }
1369 
1370  case vpRobot::ARTICULAR_FRAME: {
1371  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1372  break;
1373  }
1374 
1375  case vpRobot::REFERENCE_FRAME: {
1376  // Compute the displacement of the camera since the previous call
1377  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1378 
1379  // Compute the velocity of the camera from this displacement
1380  vpColVector v;
1381  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1382 
1383  // Express this velocity in the reference frame
1384  vpVelocityTwistMatrix fVc(fMc_cur);
1385  velocity = fVc * v;
1386 
1387  break;
1388  }
1389 
1390  case vpRobot::MIXT_FRAME: {
1391  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1392  "functionality not implemented");
1393 
1394  break;
1395  }
1397  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
1398  "functionality not implemented");
1399 
1400  break;
1401  }
1402  }
1403  }
1404  else {
1405  first_time_getvel = false;
1406  }
1407 
1408  // Memorize the camera pose for the next call
1409  fMc_prev_getvel = fMc_cur;
1410 
1411  // Memorize the joint position for the next call
1412  q_prev_getvel = q_cur;
1413 
1414  // Memorize the time associated to the joint position for the next call
1415  time_prev_getvel = time_cur;
1416 
1417  CatchPrint();
1418  if (TryStt < 0) {
1419  vpERROR_TRACE("Cannot get velocity.");
1420  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1421  }
1422 }
1423 
1433 {
1434  double timestamp;
1435  getVelocity(frame, velocity, timestamp);
1436 }
1437 
1478 {
1479  vpColVector velocity;
1480  getVelocity(frame, velocity, timestamp);
1481 
1482  return velocity;
1483 }
1484 
1494 {
1495  vpColVector velocity;
1496  double timestamp;
1497  getVelocity(frame, velocity, timestamp);
1498 
1499  return velocity;
1500 }
1501 
1553 bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1554 {
1555  std::ifstream fd(filename.c_str(), std::ios::in);
1556 
1557  if (!fd.is_open()) {
1558  return false;
1559  }
1560 
1561  std::string line;
1562  std::string key("R:");
1563  std::string id("#AFMA4 - Position");
1564  bool pos_found = false;
1565  int lineNum = 0;
1566 
1567  q.resize(njoint);
1568 
1569  while (std::getline(fd, line)) {
1570  lineNum++;
1571  if (lineNum == 1) {
1572  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1573  std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1574  return false;
1575  }
1576  }
1577  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1578  continue;
1579  }
1580  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1581  // check if there are at least njoint values in the line
1582  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1583  if (chain.size() < njoint + 1) // try to split with tab separator
1584  chain = vpIoTools::splitChain(line, std::string("\t"));
1585  if (chain.size() < njoint + 1)
1586  continue;
1587 
1588  std::istringstream ss(line);
1589  std::string key_;
1590  ss >> key_;
1591  for (unsigned int i = 0; i < njoint; i++)
1592  ss >> q[i];
1593  pos_found = true;
1594  break;
1595  }
1596  }
1597 
1598  // converts rotations from degrees into radians
1599  q[0] = vpMath::rad(q[0]);
1600  q[2] = vpMath::rad(q[2]);
1601  q[3] = vpMath::rad(q[3]);
1602 
1603  fd.close();
1604 
1605  if (!pos_found) {
1606  std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1607  return false;
1608  }
1609 
1610  return true;
1611 }
1612 
1636 bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1637 {
1638 
1639  FILE *fd;
1640  fd = fopen(filename.c_str(), "w");
1641  if (fd == nullptr)
1642  return false;
1643 
1644  fprintf(fd, "\
1645 #AFMA4 - Position - Version 2.01\n\
1646 #\n\
1647 # R: X Y A B\n\
1648 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1649 # Y : vertical translation in meters (joint 2)\n\
1650 # A : pan rotation of the camera in degrees (joint 4)\n\
1651 # B : tilt rotation of the camera in degrees (joint 5)\n\
1652 #\n\n");
1653 
1654  // Save positions in mm and deg
1655  fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1656 
1657  fclose(fd);
1658  return (true);
1659 }
1660 
1671 void vpRobotAfma4::move(const char *filename)
1672 {
1673  vpColVector q;
1674 
1675  this->readPosFile(filename, q);
1677  this->setPositioningVelocity(10);
1679 }
1680 
1700 {
1701  displacement.resize(6);
1702  displacement = 0;
1703 
1704  double q[6];
1705  vpColVector q_cur(6);
1706  double timestamp;
1707 
1708  InitTry;
1709 
1710  // Get the current joint position
1711  Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1712  for (unsigned int i = 0; i < njoint; i++) {
1713  q_cur[i] = q[i];
1714  }
1715 
1716  if (!first_time_getdis) {
1717  switch (frame) {
1718  case vpRobot::CAMERA_FRAME: {
1719  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1720  return;
1721  }
1722 
1723  case vpRobot::ARTICULAR_FRAME: {
1724  displacement = q_cur - q_prev_getdis;
1725  break;
1726  }
1727 
1728  case vpRobot::REFERENCE_FRAME: {
1729  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1730  return;
1731  }
1732 
1733  case vpRobot::MIXT_FRAME: {
1734  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1735  return;
1736  }
1738  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1739  return;
1740  }
1741  }
1742  }
1743  else {
1744  first_time_getdis = false;
1745  }
1746 
1747  // Memorize the joint position for the next call
1748  q_prev_getdis = q_cur;
1749 
1750  CatchPrint();
1751  if (TryStt < 0) {
1752  vpERROR_TRACE("Cannot get velocity.");
1753  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1754  }
1755 }
1756 END_VISP_NAMESPACE
1757 #elif !defined(VISP_BUILD_SHARED_LIBS)
1758 // Work around to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no symbols
1759 void dummy_vpRobotAfma4() { };
1760 #endif
Modelization of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:110
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:448
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:174
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:307
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:393
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:504
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:346
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:139
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:153
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:257
double _joint_min[4]
Definition: vpAfma4.h:147
double _joint_max[4]
Definition: vpAfma4.h:146
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
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 std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1661
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) VP_OVERRIDE
bool getPowerState()
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void move(const char *filename)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) VP_OVERRIDE
static const double defaultPositioningVelocity
Definition: vpRobotAfma4.h:216
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void init(void)
double getTime() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) VP_OVERRIDE
void get_fJe(vpMatrix &fJe) VP_OVERRIDE
static bool savePosFile(const std::string &filename, const vpColVector &q)
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.
Definition: vpRobot.h:59
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:106
void setVerbose(bool verbose)
Definition: vpRobot.h:170
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:155
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:164
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ ARTICULAR_FRAME
Definition: vpRobot.h:80
@ MIXT_FRAME
Definition: vpRobot.h:88
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:274
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:110
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252
bool verbose_
Definition: vpRobot.h:118
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
vpRxyzVector & buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)