Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpRobotAfma4.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_AFMA4
42 
43 #include <signal.h>
44 #include <stdlib.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/robot/vpRobotAfma4.h>
54 #include <visp3/robot/vpRobotException.h>
55 
56 /* ---------------------------------------------------------------------- */
57 /* --- STATIC ----------------------------------------------------------- */
58 /* ---------------------------------------------------------------------- */
59 
60 bool vpRobotAfma4::robotAlreadyCreated = false;
61 
70 
71 /* ---------------------------------------------------------------------- */
72 /* --- EMERGENCY STOP --------------------------------------------------- */
73 /* ---------------------------------------------------------------------- */
74 
82 void emergencyStopAfma4(int signo)
83 {
84  std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
85  switch (signo) {
86  case SIGINT:
87  std::cout << "SIGINT (stop by ^C) " << std::endl;
88  break;
89  case SIGBUS:
90  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
91  break;
92  case SIGSEGV:
93  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
94  break;
95  case SIGKILL:
96  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
97  break;
98  case SIGQUIT:
99  std::cout << "SIGQUIT " << std::endl;
100  break;
101  default:
102  std::cout << signo << std::endl;
103  }
104  // std::cout << "Emergency stop called\n";
105  // PrimitiveESTOP_Afma4();
106  PrimitiveSTOP_Afma4();
107  std::cout << "Robot was stopped\n";
108 
109  // Free allocated resources
110  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
111 
112  fprintf(stdout, "Application ");
113  fflush(stdout);
114  kill(getpid(), SIGKILL);
115  exit(1);
116 }
117 
118 /* ---------------------------------------------------------------------- */
119 /* --- CONSTRUCTOR ------------------------------------------------------ */
120 /* ---------------------------------------------------------------------- */
121 
133 vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
134 {
135 
136  /*
137  #define SIGHUP 1 // hangup
138  #define SIGINT 2 // interrupt (rubout)
139  #define SIGQUIT 3 // quit (ASCII FS)
140  #define SIGILL 4 // illegal instruction (not reset when caught)
141  #define SIGTRAP 5 // trace trap (not reset when caught)
142  #define SIGIOT 6 // IOT instruction
143  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
144  #define SIGEMT 7 // EMT instruction
145  #define SIGFPE 8 // floating point exception
146  #define SIGKILL 9 // kill (cannot be caught or ignored)
147  #define SIGBUS 10 // bus error
148  #define SIGSEGV 11 // segmentation violation
149  #define SIGSYS 12 // bad argument to system call
150  #define SIGPIPE 13 // write on a pipe with no one to read it
151  #define SIGALRM 14 // alarm clock
152  #define SIGTERM 15 // software termination signal from kill
153  */
154 
155  signal(SIGINT, emergencyStopAfma4);
156  signal(SIGBUS, emergencyStopAfma4);
157  signal(SIGSEGV, emergencyStopAfma4);
158  signal(SIGKILL, emergencyStopAfma4);
159  signal(SIGQUIT, emergencyStopAfma4);
160 
161  setVerbose(verbose);
162  if (verbose_)
163  std::cout << "Open communication with MotionBlox.\n";
164  try {
165  this->init();
167  } catch (...) {
168  // vpERROR_TRACE("Error caught") ;
169  throw;
170  }
171  positioningVelocity = defaultPositioningVelocity;
172 
173  vpRobotAfma4::robotAlreadyCreated = true;
174 
175  return;
176 }
177 
178 /* ------------------------------------------------------------------------ */
179 /* --- INITIALISATION ----------------------------------------------------- */
180 /* ------------------------------------------------------------------------ */
181 
191 {
192  int stt;
193  InitTry;
194 
195  // Initialise private variables used to compute the measured velocities
196  q_prev_getvel.resize(4);
197  q_prev_getvel = 0;
198  time_prev_getvel = 0;
199  first_time_getvel = true;
200 
201  // Initialise private variables used to compute the measured displacement
202  q_prev_getdis.resize(4);
203  q_prev_getdis = 0;
204  first_time_getdis = true;
205 
206  // Initialize the firewire connection
207  Try(stt = InitializeConnection(verbose_));
208 
209  if (stt != SUCCESS) {
210  vpERROR_TRACE("Cannot open connection with the motionblox.");
211  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
212  }
213 
214  // Connect to the servoboard using the servo board GUID
215  Try(stt = InitializeNode_Afma4());
216 
217  if (stt != SUCCESS) {
218  vpERROR_TRACE("Cannot open connection with the motionblox.");
219  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
220  }
221  Try(PrimitiveRESET_Afma4());
222 
223  // Look if the power is on or off
224  UInt32 HIPowerStatus;
225  UInt32 EStopStatus;
226  Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
227  CAL_Wait(0.1);
228 
229  // Print the robot status
230  if (verbose_) {
231  std::cout << "Robot status: ";
232  switch (EStopStatus) {
233  case ESTOP_AUTO:
234  case ESTOP_MANUAL:
235  if (HIPowerStatus == 0)
236  std::cout << "Power is OFF" << std::endl;
237  else
238  std::cout << "Power is ON" << std::endl;
239  break;
240  case ESTOP_ACTIVATED:
241  std::cout << "Emergency stop is activated" << std::endl;
242  break;
243  default:
244  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
245  std::cout << "You have to call Adept for maintenance..." << std::endl;
246  // Free allocated resources
247  }
248  std::cout << std::endl;
249  }
250  // get real joint min/max from the MotionBlox
251  Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
252  // for (unsigned int i=0; i < njoint; i++) {
253  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
254  // _joint_max[i]);
255  // }
256 
257  // If an error occur in the low level controller, goto here
258  // CatchPrint();
259  Catch();
260 
261  // Test if an error occurs
262  if (TryStt == -20001)
263  printf("No connection detected. Check if the robot is powered on \n"
264  "and if the firewire link exist between the MotionBlox and this "
265  "computer.\n");
266  else if (TryStt == -675)
267  printf(" Timeout enabling power...\n");
268 
269  if (TryStt < 0) {
270  // Power off the robot
271  PrimitivePOWEROFF_Afma4();
272  // Free allocated resources
273  ShutDownConnection();
274 
275  std::cout << "Cannot open connection with the motionblox..." << std::endl;
276  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
277  }
278  return;
279 }
280 
281 /* ------------------------------------------------------------------------ */
282 /* --- DESTRUCTOR --------------------------------------------------------- */
283 /* ------------------------------------------------------------------------ */
284 
292 {
293  InitTry;
294 
296 
297  // Look if the power is on or off
298  UInt32 HIPowerStatus;
299  Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
300  CAL_Wait(0.1);
301 
302  // if (HIPowerStatus == 1) {
303  // fprintf(stdout, "Power OFF the robot\n");
304  // fflush(stdout);
305 
306  // Try( PrimitivePOWEROFF_Afma4() );
307  // }
308 
309  // Free allocated resources
310  ShutDownConnection();
311 
312  vpRobotAfma4::robotAlreadyCreated = false;
313 
314  CatchPrint();
315  return;
316 }
317 
325 {
326  InitTry;
327 
328  switch (newState) {
329  case vpRobot::STATE_STOP: {
331  Try(PrimitiveSTOP_Afma4());
332  }
333  break;
334  }
337  std::cout << "Change the control mode from velocity to position control.\n";
338  Try(PrimitiveSTOP_Afma4());
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(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
407  if (EStopStatus == ESTOP_AUTO) {
408  break; // exit for loop
409  } else if (EStopStatus == ESTOP_MANUAL) {
410  break; // exit for loop
411  } else if (EStopStatus == ESTOP_ACTIVATED) {
412  if (firsttime) {
413  std::cout << "Emergency stop is activated! \n"
414  << "Check the emergency stop button and push the yellow "
415  "button before continuing."
416  << std::endl;
417  firsttime = false;
418  }
419  fprintf(stdout, "Remaining time %us \r", nitermax - i);
420  fflush(stdout);
421  CAL_Wait(1);
422  } else {
423  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
424  std::cout << "You have to call Adept for maintenance..." << std::endl;
425  // Free allocated resources
426  ShutDownConnection();
427  exit(0);
428  }
429  }
430 
431  if (EStopStatus == ESTOP_ACTIVATED)
432  std::cout << std::endl;
433 
434  if (EStopStatus == ESTOP_ACTIVATED) {
435  std::cout << "Sorry, cannot power on the robot." << std::endl;
436  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot.");
437  }
438 
439  if (HIPowerStatus == 0) {
440  fprintf(stdout, "Power ON the Afma4 robot\n");
441  fflush(stdout);
442 
443  Try(PrimitivePOWERON_Afma4());
444  }
445 
446  CatchPrint();
447  if (TryStt < 0) {
448  vpERROR_TRACE("Cannot power on the robot");
449  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
450  }
451 }
452 
463 {
464  InitTry;
465 
466  // Look if the power is on or off
467  UInt32 HIPowerStatus;
468  Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
469  CAL_Wait(0.1);
470 
471  if (HIPowerStatus == 1) {
472  fprintf(stdout, "Power OFF the Afma4 robot\n");
473  fflush(stdout);
474 
475  Try(PrimitivePOWEROFF_Afma4());
476  }
477 
478  CatchPrint();
479  if (TryStt < 0) {
480  vpERROR_TRACE("Cannot power off the robot");
481  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
482  }
483 }
484 
497 {
498  InitTry;
499  bool status = false;
500  // Look if the power is on or off
501  UInt32 HIPowerStatus;
502  Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
503  CAL_Wait(0.1);
504 
505  if (HIPowerStatus == 1) {
506  status = true;
507  }
508 
509  CatchPrint();
510  if (TryStt < 0) {
511  vpERROR_TRACE("Cannot get the power status");
512  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
513  }
514  return status;
515 }
516 
527 {
529  vpAfma4::get_cMe(cMe);
530 
531  cVe.buildFrom(cMe);
532 }
533 
544 {
545  double position[this->njoint];
546  double timestamp;
547 
548  InitTry;
549  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
550  CatchPrint();
551 
552  vpColVector q(this->njoint);
553  for (unsigned int i = 0; i < njoint; i++)
554  q[i] = position[i];
555 
556  try {
557  vpAfma4::get_cVf(q, cVf);
558  } catch (...) {
559  vpERROR_TRACE("catch exception ");
560  throw;
561  }
562 }
563 
575 
589 {
590 
591  double position[this->njoint];
592  double timestamp;
593 
594  InitTry;
595  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
596  CatchPrint();
597 
598  vpColVector q(this->njoint);
599  for (unsigned int i = 0; i < njoint; i++)
600  q[i] = position[i];
601 
602  try {
603  vpAfma4::get_eJe(q, eJe);
604  } catch (...) {
605  vpERROR_TRACE("catch exception ");
606  throw;
607  }
608 }
609 
625 {
626  double position[6];
627  double timestamp;
628 
629  InitTry;
630  Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
631  CatchPrint();
632 
633  vpColVector q(6);
634  for (unsigned int i = 0; i < njoint; i++)
635  q[i] = position[i];
636 
637  try {
638  vpAfma4::get_fJe(q, fJe);
639  } catch (...) {
640  vpERROR_TRACE("Error caught");
641  throw;
642  }
643 }
672 void vpRobotAfma4::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
673 
679 double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; }
680 
754 {
755 
757  vpERROR_TRACE("Robot was not in position-based control\n"
758  "Modification of the robot state");
760  }
761 
762  int error = 0;
763 
764  switch (frame) {
766  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
767  "Reference frame not implemented.");
768  break;
770  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
771  "Camera frame not implemented.");
772  break;
773  case vpRobot::MIXT_FRAME:
774  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
775  "Mixt frame not implemented.");
776  break;
778  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
779  "End-effector frame not implemented.");
780  break;
781 
783  break;
784  }
785  }
786  if (position.getRows() != this->njoint) {
787  vpERROR_TRACE("Positionning error: bad vector dimension.");
788  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension.");
789  }
790 
791  InitTry;
792 
793  Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
794  Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
795 
796  CatchPrint();
797  if (TryStt == InvalidPosition || TryStt == -1023)
798  std::cout << " : Position out of range.\n";
799  else if (TryStt < 0)
800  std::cout << " : Unknown error (see Fabien).\n";
801  else if (error == -1)
802  std::cout << "Position out of range.\n";
803 
804  if (TryStt < 0 || error < 0) {
805  vpERROR_TRACE("Positionning error.");
806  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
807  }
808 
809  return;
810 }
811 
862 void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
863  const double q4, const double q5)
864 {
865  try {
866  vpColVector position(this->njoint);
867  position[0] = q1;
868  position[1] = q2;
869  position[2] = q4;
870  position[3] = q5;
871 
872  setPosition(frame, position);
873  } catch (...) {
874  vpERROR_TRACE("Error caught");
875  throw;
876  }
877 }
878 
908 void vpRobotAfma4::setPosition(const char *filename)
909 {
910  vpColVector q;
911  bool ret;
912 
913  ret = this->readPosFile(filename, q);
914 
915  if (ret == false) {
916  vpERROR_TRACE("Bad position in \"%s\"", filename);
917  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
918  }
921 }
922 
927 double vpRobotAfma4::getTime() const
928 {
929  double timestamp;
930  PrimitiveACQ_TIME_Afma4(&timestamp);
931  return timestamp;
932 }
933 
994 void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
995 {
996 
997  InitTry;
998 
999  position.resize(this->njoint);
1000 
1001  switch (frame) {
1002  case vpRobot::CAMERA_FRAME: {
1003  position = 0;
1004  return;
1005  }
1006  case vpRobot::ARTICULAR_FRAME: {
1007  double _q[njoint];
1008  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1009  for (unsigned int i = 0; i < this->njoint; i++) {
1010  position[i] = _q[i];
1011  }
1012 
1013  return;
1014  }
1015  case vpRobot::REFERENCE_FRAME: {
1016  double _q[njoint];
1017  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1018 
1019  vpColVector q(this->njoint);
1020  for (unsigned int i = 0; i < this->njoint; i++)
1021  q[i] = _q[i];
1022 
1023  // Compute fMc
1024  vpHomogeneousMatrix fMc;
1025  vpAfma4::get_fMc(q, fMc);
1026 
1027  // From fMc extract the pose
1028  vpRotationMatrix fRc;
1029  fMc.extract(fRc);
1030  vpRxyzVector rxyz;
1031  rxyz.buildFrom(fRc);
1032 
1033  for (unsigned int i = 0; i < 3; i++) {
1034  position[i] = fMc[i][3]; // translation x,y,z
1035  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1036  }
1037  break;
1038  }
1039  case vpRobot::MIXT_FRAME: {
1040  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1041  "not implemented");
1042  }
1044  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1045  "not implemented");
1046  }
1047  }
1048 
1049  CatchPrint();
1050  if (TryStt < 0) {
1051  vpERROR_TRACE("Cannot get position.");
1052  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1053  }
1054 
1055  return;
1056 }
1057 
1069 {
1070  double timestamp;
1071  getPosition(frame, position, timestamp);
1072 }
1073 
1128 {
1129 
1131  vpERROR_TRACE("Cannot send a velocity to the robot "
1132  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1134  "Cannot send a velocity to the robot "
1135  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1136  }
1137 
1138  // Check the dimension of the velocity vector to see if it is
1139  // compatible with the requested frame
1140  switch (frame) {
1141  case vpRobot::CAMERA_FRAME: {
1142  // if (vel.getRows() != 2) {
1143  if (vel.getRows() != 6) {
1144  vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1145  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1146  "in camera frame");
1147  }
1148  break;
1149  }
1150  case vpRobot::ARTICULAR_FRAME: {
1151  if (vel.getRows() != this->njoint) {
1152  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1153  "velocity vector ");
1154  }
1155  break;
1156  }
1157  case vpRobot::REFERENCE_FRAME: {
1158  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1159  "in the reference frame:"
1160  "functionality not implemented");
1161  }
1162  case vpRobot::MIXT_FRAME: {
1163  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1164  "in the mixt frame:"
1165  "functionality not implemented");
1166  }
1168  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1169  "in the end-effector frame:"
1170  "functionality not implemented");
1171  }
1172  default: {
1173  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
1174  }
1175  }
1176 
1177  //
1178  // Velocities saturation with normalization
1179  //
1180  vpColVector joint_vel(this->njoint);
1181 
1182  // Case of the camera frame where we control only 2 dof
1183  if (frame == vpRobot::CAMERA_FRAME) {
1184  vpColVector vel_max(6);
1185 
1186  for (unsigned int i = 0; i < 3; i++) {
1187  vel_max[i] = getMaxTranslationVelocity();
1188  vel_max[i + 3] = getMaxRotationVelocity();
1189  }
1190 
1191  vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1192 
1193 #if 0 // ok
1194  vpMatrix eJe(4,6);
1195  eJe = 0;
1196  eJe[2][4] = -1;
1197  eJe[3][3] = 1;
1198 
1199  joint_vel = eJe * velocity; // Compute the articular velocity
1200 #endif
1201  vpColVector q;
1203  vpMatrix fJe_inverse;
1204  get_fJe_inverse(q, fJe_inverse);
1205  vpHomogeneousMatrix fMe;
1206  get_fMe(q, fMe);
1208  t = 0;
1209  vpRotationMatrix fRe;
1210  fMe.extract(fRe);
1211  vpVelocityTwistMatrix fVe(t, fRe);
1212  // compute the inverse jacobian in the end-effector frame
1213  vpMatrix eJe_inverse = fJe_inverse * fVe;
1214 
1215  // Transform the velocities from camera to end-effector frame
1217  eVc.buildFrom(this->_eMc);
1218  joint_vel = eJe_inverse * eVc * velocity;
1219 
1220  // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1221  // joint_vel[2], joint_vel[3]);
1222  }
1223 
1224  // Case of the joint control where we control all the joints
1225  else if (frame == vpRobot::ARTICULAR_FRAME) {
1226 
1227  vpColVector vel_max(4);
1228 
1229  vel_max[0] = getMaxRotationVelocity();
1230  vel_max[1] = getMaxTranslationVelocity();
1231  vel_max[2] = getMaxRotationVelocity();
1232  vel_max[3] = getMaxRotationVelocity();
1233 
1234  joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1235  }
1236 
1237  InitTry;
1238 
1239  // Send a joint velocity to the low level controller
1240  Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
1241 
1242  Catch();
1243  if (TryStt < 0) {
1244  if (TryStt == VelStopOnJoint) {
1245  UInt32 axisInJoint[njoint];
1246  PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1247  for (unsigned int i = 0; i < njoint; i++) {
1248  if (axisInJoint[i])
1249  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1250  }
1251  } else {
1252  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1253  if (TryString != NULL) {
1254  // The statement is in TryString, but we need to check the validity
1255  printf(" Error sentence %s\n", TryString); // Print the TryString
1256  } else {
1257  printf("\n");
1258  }
1259  }
1260  }
1261 
1262  return;
1263 }
1264 
1265 /* ------------------------------------------------------------------------ */
1266 /* --- GET ---------------------------------------------------------------- */
1267 /* ------------------------------------------------------------------------ */
1268 
1316 void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1317 {
1318 
1319  switch (frame) {
1321  velocity.resize(this->njoint);
1322  break;
1323  default:
1324  velocity.resize(6);
1325  }
1326 
1327  velocity = 0;
1328 
1329  double q[4];
1330  vpColVector q_cur(4);
1331  vpHomogeneousMatrix fMc_cur;
1332  vpHomogeneousMatrix cMc; // camera displacement
1333  double time_cur;
1334 
1335  InitTry;
1336 
1337  // Get the current joint position
1338  Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1339  time_cur = timestamp;
1340 
1341  for (unsigned int i = 0; i < this->njoint; i++) {
1342  q_cur[i] = q[i];
1343  }
1344 
1345  // Get the camera pose from the direct kinematics
1346  vpAfma4::get_fMc(q_cur, fMc_cur);
1347 
1348  if (!first_time_getvel) {
1349 
1350  switch (frame) {
1351  case vpRobot::CAMERA_FRAME: {
1352  // Compute the displacement of the camera since the previous call
1353  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1354 
1355  // Compute the velocity of the camera from this displacement
1356  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1357 
1358  break;
1359  }
1360 
1361  case vpRobot::ARTICULAR_FRAME: {
1362  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1363  break;
1364  }
1365 
1366  case vpRobot::REFERENCE_FRAME: {
1367  // Compute the displacement of the camera since the previous call
1368  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1369 
1370  // Compute the velocity of the camera from this displacement
1371  vpColVector v;
1372  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1373 
1374  // Express this velocity in the reference frame
1375  vpVelocityTwistMatrix fVc(fMc_cur);
1376  velocity = fVc * v;
1377 
1378  break;
1379  }
1380 
1381  case vpRobot::MIXT_FRAME: {
1382  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1383  "functionality not implemented");
1384 
1385  break;
1386  }
1388  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
1389  "functionality not implemented");
1390 
1391  break;
1392  }
1393  }
1394  } else {
1395  first_time_getvel = false;
1396  }
1397 
1398  // Memorize the camera pose for the next call
1399  fMc_prev_getvel = fMc_cur;
1400 
1401  // Memorize the joint position for the next call
1402  q_prev_getvel = q_cur;
1403 
1404  // Memorize the time associated to the joint position for the next call
1405  time_prev_getvel = time_cur;
1406 
1407  CatchPrint();
1408  if (TryStt < 0) {
1409  vpERROR_TRACE("Cannot get velocity.");
1410  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1411  }
1412 }
1413 
1423 {
1424  double timestamp;
1425  getVelocity(frame, velocity, timestamp);
1426 }
1427 
1468 {
1469  vpColVector velocity;
1470  getVelocity(frame, velocity, timestamp);
1471 
1472  return velocity;
1473 }
1474 
1484 {
1485  vpColVector velocity;
1486  double timestamp;
1487  getVelocity(frame, velocity, timestamp);
1488 
1489  return velocity;
1490 }
1491 
1543 bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1544 {
1545  std::ifstream fd(filename.c_str(), std::ios::in);
1546 
1547  if (!fd.is_open()) {
1548  return false;
1549  }
1550 
1551  std::string line;
1552  std::string key("R:");
1553  std::string id("#AFMA4 - Position");
1554  bool pos_found = false;
1555  int lineNum = 0;
1556 
1557  q.resize(njoint);
1558 
1559  while (std::getline(fd, line)) {
1560  lineNum++;
1561  if (lineNum == 1) {
1562  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1563  std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1564  return false;
1565  }
1566  }
1567  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1568  continue;
1569  }
1570  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1571  // check if there are at least njoint values in the line
1572  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1573  if (chain.size() < njoint + 1) // try to split with tab separator
1574  chain = vpIoTools::splitChain(line, std::string("\t"));
1575  if (chain.size() < njoint + 1)
1576  continue;
1577 
1578  std::istringstream ss(line);
1579  std::string key_;
1580  ss >> key_;
1581  for (unsigned int i = 0; i < njoint; i++)
1582  ss >> q[i];
1583  pos_found = true;
1584  break;
1585  }
1586  }
1587 
1588  // converts rotations from degrees into radians
1589  q[0] = vpMath::rad(q[0]);
1590  q[2] = vpMath::rad(q[2]);
1591  q[3] = vpMath::rad(q[3]);
1592 
1593  fd.close();
1594 
1595  if (!pos_found) {
1596  std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1597  return false;
1598  }
1599 
1600  return true;
1601 }
1602 
1626 bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1627 {
1628 
1629  FILE *fd;
1630  fd = fopen(filename.c_str(), "w");
1631  if (fd == NULL)
1632  return false;
1633 
1634  fprintf(fd, "\
1635 #AFMA4 - Position - Version 2.01\n\
1636 #\n\
1637 # R: X Y A B\n\
1638 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1639 # Y : vertical translation in meters (joint 2)\n\
1640 # A : pan rotation of the camera in degrees (joint 4)\n\
1641 # B : tilt rotation of the camera in degrees (joint 5)\n\
1642 #\n\n");
1643 
1644  // Save positions in mm and deg
1645  fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1646 
1647  fclose(fd);
1648  return (true);
1649 }
1650 
1661 void vpRobotAfma4::move(const char *filename)
1662 {
1663  vpColVector q;
1664 
1665  this->readPosFile(filename, q);
1667  this->setPositioningVelocity(10);
1669 }
1670 
1690 {
1691  displacement.resize(6);
1692  displacement = 0;
1693 
1694  double q[6];
1695  vpColVector q_cur(6);
1696  double timestamp;
1697 
1698  InitTry;
1699 
1700  // Get the current joint position
1701  Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1702  for (unsigned int i = 0; i < njoint; i++) {
1703  q_cur[i] = q[i];
1704  }
1705 
1706  if (!first_time_getdis) {
1707  switch (frame) {
1708  case vpRobot::CAMERA_FRAME: {
1709  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1710  return;
1711  }
1712 
1713  case vpRobot::ARTICULAR_FRAME: {
1714  displacement = q_cur - q_prev_getdis;
1715  break;
1716  }
1717 
1718  case vpRobot::REFERENCE_FRAME: {
1719  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1720  return;
1721  }
1722 
1723  case vpRobot::MIXT_FRAME: {
1724  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1725  return;
1726  }
1728  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1729  return;
1730  }
1731  }
1732  } else {
1733  first_time_getdis = false;
1734  }
1735 
1736  // Memorize the joint position for the next call
1737  q_prev_getdis = q_cur;
1738 
1739  CatchPrint();
1740  if (TryStt < 0) {
1741  vpERROR_TRACE("Cannot get velocity.");
1742  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1743  }
1744 }
1745 
1746 #elif !defined(VISP_BUILD_SHARED_LIBS)
1747 // Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
1748 // symbols
1749 void dummy_vpRobotAfma4(){};
1750 #endif
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:176
Error that can be emited by the vpRobot class and its derivates.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:259
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:309
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:159
virtual ~vpRobotAfma4(void)
#define vpERROR_TRACE
Definition: vpDebug.h:393
void init(void)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
Initialize the position controller.
Definition: vpRobot.h:67
double getTime() const
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpControlFrameType
Definition: vpRobot.h:75
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:348
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void move(const char *filename)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:395
Implementation of a rotation matrix and operations on such kind of matrices.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void get_cMe(vpHomogeneousMatrix &cMe) const
double _joint_max[4]
Definition: vpAfma4.h:149
Initialize the velocity controller.
Definition: vpRobot.h:66
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:116
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setPositioningVelocity(const double velocity)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double _joint_min[4]
Definition: vpAfma4.h:150
static bool readPosFile(const std::string &filename, vpColVector &q)
static const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
Modelisation of Irisa&#39;s cylindrical robot named Afma4.
Definition: vpAfma4.h:110
void extract(vpRotationMatrix &R) const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1804
unsigned int getRows() const
Definition: vpArray2D.h:289
static bool savePosFile(const std::string &filename, const vpColVector &q)
static double rad(double deg)
Definition: vpMath.h:108
void get_eJe(vpMatrix &eJe)
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static double deg(double rad)
Definition: vpMath.h:101
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpHomogeneousMatrix inverse() const
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:506
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
void get_fJe(vpMatrix &fJe)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
bool getPowerState()
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:450
double getPositioningVelocity(void)
void get_cVf(vpVelocityTwistMatrix &cVf) const
void get_cVe(vpVelocityTwistMatrix &cVe) const
static const double defaultPositioningVelocity
Definition: vpRobotAfma4.h:218
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
Class that consider the case of a translation vector.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310