Visual Servoing Platform  version 3.1.0
vpRobotAfma4.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  vpERROR_TRACE("Positionning error. Reference frame not implemented");
767  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
768  "Reference frame not implemented.");
769  break;
771  vpERROR_TRACE("Positionning error. Camera frame not implemented");
772  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
773  "Camera frame not implemented.");
774  break;
775  case vpRobot::MIXT_FRAME:
776  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
777  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
778  "Mixt frame not implemented.");
779  break;
780 
782  break;
783  }
784  }
785  if (position.getRows() != this->njoint) {
786  vpERROR_TRACE("Positionning error: bad vector dimension.");
787  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension.");
788  }
789 
790  InitTry;
791 
792  Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
793  Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
794 
795  CatchPrint();
796  if (TryStt == InvalidPosition || TryStt == -1023)
797  std::cout << " : Position out of range.\n";
798  else if (TryStt < 0)
799  std::cout << " : Unknown error (see Fabien).\n";
800  else if (error == -1)
801  std::cout << "Position out of range.\n";
802 
803  if (TryStt < 0 || error < 0) {
804  vpERROR_TRACE("Positionning error.");
805  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
806  }
807 
808  return;
809 }
810 
861 void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
862  const double q4, const double q5)
863 {
864  try {
865  vpColVector position(this->njoint);
866  position[0] = q1;
867  position[1] = q2;
868  position[2] = q4;
869  position[3] = q5;
870 
871  setPosition(frame, position);
872  } catch (...) {
873  vpERROR_TRACE("Error caught");
874  throw;
875  }
876 }
877 
907 void vpRobotAfma4::setPosition(const char *filename)
908 {
909  vpColVector q;
910  bool ret;
911 
912  ret = this->readPosFile(filename, q);
913 
914  if (ret == false) {
915  vpERROR_TRACE("Bad position in \"%s\"", filename);
916  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
917  }
920 }
921 
926 double vpRobotAfma4::getTime() const
927 {
928  double timestamp;
929  PrimitiveACQ_TIME_Afma4(&timestamp);
930  return timestamp;
931 }
932 
993 void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
994 {
995 
996  InitTry;
997 
998  position.resize(this->njoint);
999 
1000  switch (frame) {
1001  case vpRobot::CAMERA_FRAME: {
1002  position = 0;
1003  return;
1004  }
1005  case vpRobot::ARTICULAR_FRAME: {
1006  double _q[njoint];
1007  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1008  for (unsigned int i = 0; i < this->njoint; i++) {
1009  position[i] = _q[i];
1010  }
1011 
1012  return;
1013  }
1014  case vpRobot::REFERENCE_FRAME: {
1015  double _q[njoint];
1016  Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1017 
1018  vpColVector q(this->njoint);
1019  for (unsigned int i = 0; i < this->njoint; i++)
1020  q[i] = _q[i];
1021 
1022  // Compute fMc
1023  vpHomogeneousMatrix fMc;
1024  vpAfma4::get_fMc(q, fMc);
1025 
1026  // From fMc extract the pose
1027  vpRotationMatrix fRc;
1028  fMc.extract(fRc);
1029  vpRxyzVector rxyz;
1030  rxyz.buildFrom(fRc);
1031 
1032  for (unsigned int i = 0; i < 3; i++) {
1033  position[i] = fMc[i][3]; // translation x,y,z
1034  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1035  }
1036  break;
1037  }
1038  case vpRobot::MIXT_FRAME: {
1039  vpERROR_TRACE("Cannot get position in mixt frame: not implemented");
1040  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1041  "not implemented");
1042  }
1043  }
1044 
1045  CatchPrint();
1046  if (TryStt < 0) {
1047  vpERROR_TRACE("Cannot get position.");
1048  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1049  }
1050 
1051  return;
1052 }
1053 
1065 {
1066  double timestamp;
1067  getPosition(frame, position, timestamp);
1068 }
1069 
1124 {
1125 
1127  vpERROR_TRACE("Cannot send a velocity to the robot "
1128  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1130  "Cannot send a velocity to the robot "
1131  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1132  }
1133 
1134  // Check the dimension of the velocity vector to see if it is
1135  // compatible with the requested frame
1136  switch (frame) {
1137  case vpRobot::CAMERA_FRAME: {
1138  // if (vel.getRows() != 2) {
1139  if (vel.getRows() != 6) {
1140  vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1141  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1142  "in camera frame");
1143  }
1144  break;
1145  }
1146  case vpRobot::ARTICULAR_FRAME: {
1147  if (vel.getRows() != this->njoint) {
1148  vpERROR_TRACE("Bad dimension of the articular velocity vector");
1149  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1150  "velocity vector ");
1151  }
1152  break;
1153  }
1154  case vpRobot::REFERENCE_FRAME: {
1155  vpERROR_TRACE("Cannot send a velocity to the robot "
1156  "in the reference frame: "
1157  "functionality not implemented");
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  vpERROR_TRACE("Cannot send a velocity to the robot "
1164  "in the mixt frame: "
1165  "functionality not implemented");
1166  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1167  "in the mixt frame:"
1168  "functionality not implemented");
1169  }
1170  default: {
1171  vpERROR_TRACE("Error in spec of vpRobot. "
1172  "Case not taken in account.");
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  vpERROR_TRACE("Cannot get a velocity in the mixt frame: "
1383  "functionality not implemented");
1384  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1385  "functionality not implemented");
1386 
1387  break;
1388  }
1389  }
1390  } else {
1391  first_time_getvel = false;
1392  }
1393 
1394  // Memorize the camera pose for the next call
1395  fMc_prev_getvel = fMc_cur;
1396 
1397  // Memorize the joint position for the next call
1398  q_prev_getvel = q_cur;
1399 
1400  // Memorize the time associated to the joint position for the next call
1401  time_prev_getvel = time_cur;
1402 
1403  CatchPrint();
1404  if (TryStt < 0) {
1405  vpERROR_TRACE("Cannot get velocity.");
1406  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1407  }
1408 }
1409 
1419 {
1420  double timestamp;
1421  getVelocity(frame, velocity, timestamp);
1422 }
1423 
1464 {
1465  vpColVector velocity;
1466  getVelocity(frame, velocity, timestamp);
1467 
1468  return velocity;
1469 }
1470 
1480 {
1481  vpColVector velocity;
1482  double timestamp;
1483  getVelocity(frame, velocity, timestamp);
1484 
1485  return velocity;
1486 }
1487 
1539 bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1540 {
1541  std::ifstream fd(filename.c_str(), std::ios::in);
1542 
1543  if (!fd.is_open()) {
1544  return false;
1545  }
1546 
1547  std::string line;
1548  std::string key("R:");
1549  std::string id("#AFMA4 - Position");
1550  bool pos_found = false;
1551  int lineNum = 0;
1552 
1553  q.resize(njoint);
1554 
1555  while (std::getline(fd, line)) {
1556  lineNum++;
1557  if (lineNum == 1) {
1558  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1559  std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1560  return false;
1561  }
1562  }
1563  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1564  continue;
1565  }
1566  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1567  // check if there are at least njoint values in the line
1568  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1569  if (chain.size() < njoint + 1) // try to split with tab separator
1570  chain = vpIoTools::splitChain(line, std::string("\t"));
1571  if (chain.size() < njoint + 1)
1572  continue;
1573 
1574  std::istringstream ss(line);
1575  std::string key_;
1576  ss >> key_;
1577  for (unsigned int i = 0; i < njoint; i++)
1578  ss >> q[i];
1579  pos_found = true;
1580  break;
1581  }
1582  }
1583 
1584  // converts rotations from degrees into radians
1585  q[0] = vpMath::rad(q[0]);
1586  q[2] = vpMath::rad(q[2]);
1587  q[3] = vpMath::rad(q[3]);
1588 
1589  fd.close();
1590 
1591  if (!pos_found) {
1592  std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1593  return false;
1594  }
1595 
1596  return true;
1597 }
1598 
1622 bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1623 {
1624 
1625  FILE *fd;
1626  fd = fopen(filename.c_str(), "w");
1627  if (fd == NULL)
1628  return false;
1629 
1630  fprintf(fd, "\
1631 #AFMA4 - Position - Version 2.01\n\
1632 #\n\
1633 # R: X Y A B\n\
1634 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1635 # Y : vertical translation in meters (joint 2)\n\
1636 # A : pan rotation of the camera in degrees (joint 4)\n\
1637 # B : tilt rotation of the camera in degrees (joint 5)\n\
1638 #\n\n");
1639 
1640  // Save positions in mm and deg
1641  fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1642 
1643  fclose(fd);
1644  return (true);
1645 }
1646 
1657 void vpRobotAfma4::move(const char *filename)
1658 {
1659  vpColVector q;
1660 
1661  this->readPosFile(filename, q);
1663  this->setPositioningVelocity(10);
1665 }
1666 
1686 {
1687  displacement.resize(6);
1688  displacement = 0;
1689 
1690  double q[6];
1691  vpColVector q_cur(6);
1692  double timestamp;
1693 
1694  InitTry;
1695 
1696  // Get the current joint position
1697  Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1698  for (unsigned int i = 0; i < njoint; i++) {
1699  q_cur[i] = q[i];
1700  }
1701 
1702  if (!first_time_getdis) {
1703  switch (frame) {
1704  case vpRobot::CAMERA_FRAME: {
1705  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1706  return;
1707  }
1708 
1709  case vpRobot::ARTICULAR_FRAME: {
1710  displacement = q_cur - q_prev_getdis;
1711  break;
1712  }
1713 
1714  case vpRobot::REFERENCE_FRAME: {
1715  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1716  return;
1717  }
1718 
1719  case vpRobot::MIXT_FRAME: {
1720  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1721  return;
1722  }
1723  }
1724  } else {
1725  first_time_getdis = false;
1726  }
1727 
1728  // Memorize the joint position for the next call
1729  q_prev_getdis = q_cur;
1730 
1731  CatchPrint();
1732  if (TryStt < 0) {
1733  vpERROR_TRACE("Cannot get velocity.");
1734  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1735  }
1736 }
1737 
1738 #elif !defined(VISP_BUILD_SHARED_LIBS)
1739 // Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
1740 // symbols
1741 void dummy_vpRobotAfma4(){};
1742 #endif
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Error that can be emited by the vpRobot class and its derivates.
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:309
void get_cVe(vpVelocityTwistMatrix &cVe) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:154
virtual ~vpRobotAfma4(void)
#define vpERROR_TRACE
Definition: vpDebug.h:393
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void init(void)
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:259
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
vpHomogeneousMatrix _eMc
Definition: vpAfma4.h:156
Initialize the position controller.
Definition: vpRobot.h:68
unsigned int getRows() const
Definition: vpArray2D.h:156
vpHomogeneousMatrix inverse() 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:84
vpControlFrameType
Definition: vpRobot.h:75
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
void move(const char *filename)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Implementation of a rotation matrix and operations on such kind of matrices.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
double _joint_max[4]
Definition: vpAfma4.h:149
Initialize the velocity controller.
Definition: vpRobot.h:67
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:139
vpRobotStateType
Definition: vpRobot.h:64
void get_cMe(vpHomogeneousMatrix &cMe) const
bool verbose_
Definition: vpRobot.h:111
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)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:450
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
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1665
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition: vpAfma4.cpp:506
static bool savePosFile(const std::string &filename, const vpColVector &q)
static double rad(double deg)
Definition: vpMath.h:102
void get_eJe(vpMatrix &eJe)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static double deg(double rad)
Definition: vpMath.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:99
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:176
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void get_fJe(vpMatrix &fJe)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:395
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
bool getPowerState()
double getPositioningVelocity(void)
double getTime() const
static const double defaultPositioningVelocity
Definition: vpRobotAfma4.h:217
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:103
Class that consider the case of a translation vector.
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:348
void get_cVf(vpVelocityTwistMatrix &cVf) const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:241