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