ViSP  2.10.0
vpRobotAfma4.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotAfma4.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Irisa's Afma4 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 
44 #ifdef VISP_HAVE_AFMA4
45 
46 #include <signal.h>
47 #include <stdlib.h>
48 #include <sys/types.h>
49 #include <unistd.h>
50 
51 #include <visp/vpRobotException.h>
52 #include <visp/vpExponentialMap.h>
53 #include <visp/vpDebug.h>
54 #include <visp/vpVelocityTwistMatrix.h>
55 #include <visp/vpThetaUVector.h>
56 #include <visp/vpRobotAfma4.h>
57 
58 /* ---------------------------------------------------------------------- */
59 /* --- STATIC ----------------------------------------------------------- */
60 /* ---------------------------------------------------------------------- */
61 
62 bool vpRobotAfma4::robotAlreadyCreated = false;
63 
72 
73 
74 /* ---------------------------------------------------------------------- */
75 /* --- EMERGENCY STOP --------------------------------------------------- */
76 /* ---------------------------------------------------------------------- */
77 
85 void emergencyStopAfma4(int signo)
86 {
87  std::cout << "Stop the Afma4 application by signal ("
88  << signo << "): " << (char)7 ;
89  switch(signo)
90  {
91  case SIGINT:
92  std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
93  case SIGBUS:
94  std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
95  case SIGSEGV:
96  std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
97  case SIGKILL:
98  std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
99  case SIGQUIT:
100  std::cout <<"SIGQUIT " << std::endl ; 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 /* ---------------------------------------------------------------------- */
120 /* --- CONSTRUCTOR ------------------------------------------------------ */
121 /* ---------------------------------------------------------------------- */
122 
134 vpRobotAfma4::vpRobotAfma4 (bool verbose)
135  :
136  vpAfma4 (),
137  vpRobot ()
138 {
139 
140  /*
141  #define SIGHUP 1 // hangup
142  #define SIGINT 2 // interrupt (rubout)
143  #define SIGQUIT 3 // quit (ASCII FS)
144  #define SIGILL 4 // illegal instruction (not reset when caught)
145  #define SIGTRAP 5 // trace trap (not reset when caught)
146  #define SIGIOT 6 // IOT instruction
147  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
148  #define SIGEMT 7 // EMT instruction
149  #define SIGFPE 8 // floating point exception
150  #define SIGKILL 9 // kill (cannot be caught or ignored)
151  #define SIGBUS 10 // bus error
152  #define SIGSEGV 11 // segmentation violation
153  #define SIGSYS 12 // bad argument to system call
154  #define SIGPIPE 13 // write on a pipe with no one to read it
155  #define SIGALRM 14 // alarm clock
156  #define SIGTERM 15 // software termination signal from kill
157  */
158 
159  signal(SIGINT, emergencyStopAfma4);
160  signal(SIGBUS, emergencyStopAfma4) ;
161  signal(SIGSEGV, emergencyStopAfma4) ;
162  signal(SIGKILL, emergencyStopAfma4);
163  signal(SIGQUIT, emergencyStopAfma4);
164 
165  setVerbose(verbose);
166  if (verbose_)
167  std::cout << "Open communication with MotionBlox.\n";
168  try {
169  this->init();
171  }
172  catch(...) {
173  // vpERROR_TRACE("Error caught") ;
174  throw ;
175  }
176  positioningVelocity = defaultPositioningVelocity ;
177 
178  vpRobotAfma4::robotAlreadyCreated = true;
179 
180  return ;
181 }
182 
183 
184 /* ------------------------------------------------------------------------ */
185 /* --- INITIALISATION ----------------------------------------------------- */
186 /* ------------------------------------------------------------------------ */
187 
196 void
198 {
199  int stt;
200  InitTry;
201 
202  // Initialise private variables used to compute the measured velocities
203  q_prev_getvel.resize(4);
204  q_prev_getvel = 0;
205  time_prev_getvel = 0;
206  first_time_getvel = true;
207 
208  // Initialise private variables used to compute the measured displacement
209  q_prev_getdis.resize(4);
210  q_prev_getdis = 0;
211  first_time_getdis = true;
212 
213  // Initialize the firewire connection
214  Try( stt = InitializeConnection(verbose_) );
215 
216  if (stt != SUCCESS) {
217  vpERROR_TRACE ("Cannot open connexion with the motionblox.");
219  "Cannot open connexion with the motionblox");
220  }
221 
222  // Connect to the servoboard using the servo board GUID
223  Try( stt = InitializeNode_Afma4() );
224 
225  if (stt != SUCCESS) {
226  vpERROR_TRACE ("Cannot open connexion with the motionblox.");
228  "Cannot open connexion with the motionblox");
229  }
230  Try( PrimitiveRESET_Afma4() );
231 
232  // Look if the power is on or off
233  UInt32 HIPowerStatus;
234  UInt32 EStopStatus;
235  Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
236  &HIPowerStatus));
237  CAL_Wait(0.1);
238 
239  // Print the robot status
240  if (verbose_) {
241  std::cout << "Robot status: ";
242  switch(EStopStatus) {
243  case ESTOP_AUTO:
244  case ESTOP_MANUAL:
245  if (HIPowerStatus == 0)
246  std::cout << "Power is OFF" << std::endl;
247  else
248  std::cout << "Power is ON" << std::endl;
249  break;
250  case ESTOP_ACTIVATED:
251  std::cout << "Emergency stop is activated" << std::endl;
252  break;
253  default:
254  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
255  std::cout << "You have to call Adept for maintenance..." << std::endl;
256  // Free allocated resources
257  }
258  std::cout << std::endl;
259  }
260  // get real joint min/max from the MotionBlox
261  Try( PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max) );
262 // for (unsigned int i=0; i < njoint; i++) {
263 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
264 // }
265 
266  // If an error occur in the low level controller, goto here
267  // CatchPrint();
268  Catch();
269 
270  // Test if an error occurs
271  if (TryStt == -20001)
272  printf("No connection detected. Check if the robot is powered on \n"
273  "and if the firewire link exist between the MotionBlox and this computer.\n");
274  else if (TryStt == -675)
275  printf(" Timeout enabling power...\n");
276 
277  if (TryStt < 0) {
278  // Power off the robot
279  PrimitivePOWEROFF_Afma4();
280  // Free allocated resources
281  ShutDownConnection();
282 
283  std::cout << "Cannot open connexion with the motionblox..." << std::endl;
285  "Cannot open connexion with the motionblox");
286  }
287  return ;
288 }
289 
290 
291 /* ------------------------------------------------------------------------ */
292 /* --- DESTRUCTOR --------------------------------------------------------- */
293 /* ------------------------------------------------------------------------ */
294 
302 {
303  InitTry;
304 
306 
307  // Look if the power is on or off
308  UInt32 HIPowerStatus;
309  Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
310  &HIPowerStatus));
311  CAL_Wait(0.1);
312 
313 // if (HIPowerStatus == 1) {
314 // fprintf(stdout, "Power OFF the robot\n");
315 // fflush(stdout);
316 
317 // Try( PrimitivePOWEROFF_Afma4() );
318 // }
319 
320  // Free allocated resources
321  ShutDownConnection();
322 
323  vpRobotAfma4::robotAlreadyCreated = false;
324 
325  CatchPrint();
326  return;
327 }
328 
329 
330 
331 
332 
341 {
342  InitTry;
343 
344  switch (newState) {
345  case vpRobot::STATE_STOP: {
347  Try( PrimitiveSTOP_Afma4() );
348  }
349  break;
350  }
353  std::cout << "Change the control mode from velocity to position control.\n";
354  Try( PrimitiveSTOP_Afma4() );
355  }
356  else {
357  //std::cout << "Change the control mode from stop to position control.\n";
358  }
359  this->powerOn();
360  break;
361  }
364  std::cout << "Change the control mode from stop to velocity control.\n";
365  }
366  this->powerOn();
367  break;
368  }
369  default:
370  break ;
371  }
372 
373  CatchPrint();
374 
375  return vpRobot::setRobotState (newState);
376 }
377 
378 /* ------------------------------------------------------------------------ */
379 /* --- STOP --------------------------------------------------------------- */
380 /* ------------------------------------------------------------------------ */
381 
389 void
391 {
392  InitTry;
393  Try( PrimitiveSTOP_Afma4() );
395 
396  CatchPrint();
397  if (TryStt < 0) {
398  vpERROR_TRACE ("Cannot stop robot motion");
400  "Cannot stop robot motion.");
401  }
402 }
403 
404 
414 void
416 {
417  InitTry;
418 
419  // Look if the power is on or off
420  UInt32 HIPowerStatus;
421  UInt32 EStopStatus;
422  bool firsttime = true;
423  unsigned int nitermax = 10;
424 
425  for (unsigned int i=0; i<nitermax; i++) {
426  Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
427  &HIPowerStatus));
428  if (EStopStatus == ESTOP_AUTO) {
429  break; // exit for loop
430  }
431  else if (EStopStatus == ESTOP_MANUAL) {
432  break; // exit for loop
433  }
434  else if (EStopStatus == ESTOP_ACTIVATED) {
435  if (firsttime) {
436  std::cout << "Emergency stop is activated! \n"
437  << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
438  firsttime = false;
439  }
440  fprintf(stdout, "Remaining time %ds \r", nitermax-i);
441  fflush(stdout);
442  CAL_Wait(1);
443  }
444  else {
445  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
446  std::cout << "You have to call Adept for maintenance..." << std::endl;
447  // Free allocated resources
448  ShutDownConnection();
449  exit(0);
450  }
451  }
452 
453  if (EStopStatus == ESTOP_ACTIVATED)
454  std::cout << std::endl;
455 
456  if (EStopStatus == ESTOP_ACTIVATED) {
457  std::cout << "Sorry, cannot power on the robot." << std::endl;
459  "Cannot power on the robot.");
460  }
461 
462  if (HIPowerStatus == 0) {
463  fprintf(stdout, "Power ON the Afma4 robot\n");
464  fflush(stdout);
465 
466  Try( PrimitivePOWERON_Afma4() );
467  }
468 
469  CatchPrint();
470  if (TryStt < 0) {
471  vpERROR_TRACE ("Cannot power on the robot");
473  "Cannot power off the robot.");
474  }
475 }
476 
486 void
488 {
489  InitTry;
490 
491  // Look if the power is on or off
492  UInt32 HIPowerStatus;
493  Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
494  &HIPowerStatus));
495  CAL_Wait(0.1);
496 
497  if (HIPowerStatus == 1) {
498  fprintf(stdout, "Power OFF the Afma4 robot\n");
499  fflush(stdout);
500 
501  Try( PrimitivePOWEROFF_Afma4() );
502  }
503 
504  CatchPrint();
505  if (TryStt < 0) {
506  vpERROR_TRACE ("Cannot power off the robot");
508  "Cannot power off the robot.");
509  }
510 }
511 
523 bool
525 {
526  InitTry;
527  bool status = false;
528  // Look if the power is on or off
529  UInt32 HIPowerStatus;
530  Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
531  &HIPowerStatus));
532  CAL_Wait(0.1);
533 
534  if (HIPowerStatus == 1) {
535  status = true;
536  }
537 
538  CatchPrint();
539  if (TryStt < 0) {
540  vpERROR_TRACE ("Cannot get the power status");
542  "Cannot get the power status.");
543  }
544  return status;
545 }
546 
556 void
558 {
559  vpHomogeneousMatrix cMe ;
560  vpAfma4::get_cMe(cMe) ;
561 
562  cVe.buildFrom(cMe) ;
563 }
564 
574 void
576 {
577  double position[this->njoint];
578  double timestamp;
579 
580  InitTry;
581  Try( PrimitiveACQ_POS_Afma4(position, &timestamp) );
582  CatchPrint();
583 
584  vpColVector q(this->njoint);
585  for (unsigned int i=0; i < njoint; i++)
586  q[i] = position[i];
587 
588  try {
589  vpAfma4::get_cVf(q, cVf) ;
590  }
591  catch(...) {
592  vpERROR_TRACE("catch exception ") ;
593  throw ;
594  }}
595 
606 void
608 {
609  vpAfma4::get_cMe(cMe) ;
610 }
611 
624 void
626 {
627 
628  double position[this->njoint];
629  double timestamp;
630 
631  InitTry;
632  Try( PrimitiveACQ_POS_Afma4(position, &timestamp) );
633  CatchPrint();
634 
635  vpColVector q(this->njoint);
636  for (unsigned int i=0; i < njoint; i++)
637  q[i] = position[i];
638 
639  try {
640  vpAfma4::get_eJe(q, eJe) ;
641  }
642  catch(...) {
643  vpERROR_TRACE("catch exception ") ;
644  throw ;
645  }
646 }
647 
663 {
664  double position[6];
665  double timestamp;
666 
667  InitTry;
668  Try( PrimitiveACQ_POS_Afma4(position, &timestamp) );
669  CatchPrint();
670 
671  vpColVector q(6);
672  for (unsigned int i=0; i < njoint; i++)
673  q[i] = position[i];
674 
675  try {
676  vpAfma4::get_fJe(q, fJe) ;
677  }
678  catch(...) {
679  vpERROR_TRACE("Error caught");
680  throw ;
681  }
682 }
711 void
713 {
714  positioningVelocity = velocity;
715 }
716 
722 double
724 {
725  return positioningVelocity;
726 }
727 
728 
800 void
802  const vpColVector & position )
803 {
804 
806  vpERROR_TRACE ("Robot was not in position-based control\n"
807  "Modification of the robot state");
809  }
810 
811  int error = 0;
812 
813  switch(frame) {
815  vpERROR_TRACE ("Positionning error. Reference frame not implemented");
817  "Positionning error: "
818  "Reference frame not implemented.");
819  break ;
820  case vpRobot::CAMERA_FRAME :
821  vpERROR_TRACE ("Positionning error. Camera frame not implemented");
823  "Positionning error: "
824  "Camera frame not implemented.");
825  break ;
826  case vpRobot::MIXT_FRAME:
827  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
829  "Positionning error: "
830  "Mixt frame not implemented.");
831  break ;
832 
834  break ;
835 
836  }
837  }
838  if (position.getRows() != this->njoint) {
839  vpERROR_TRACE ("Positionning error: bad vector dimension.");
841  "Positionning error: bad vector dimension.");
842  }
843 
844  InitTry;
845 
846  Try( PrimitiveMOVE_Afma4(position.data, positioningVelocity) );
847  Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
848 
849  CatchPrint();
850  if (TryStt == InvalidPosition || TryStt == -1023)
851  std::cout << " : Position out of range.\n";
852  else if (TryStt < 0)
853  std::cout << " : Unknown error (see Fabien).\n";
854  else if (error == -1)
855  std::cout << "Position out of range.\n";
856 
857  if (TryStt < 0 || error < 0) {
858  vpERROR_TRACE ("Positionning error.");
860  "Position out of range.");
861  }
862 
863  return ;
864 }
865 
866 
918  const double q1,
919  const double q2,
920  const double q4,
921  const double q5)
922 {
923  try {
924  vpColVector position(this->njoint) ;
925  position[0] = q1 ;
926  position[1] = q2 ;
927  position[2] = q4 ;
928  position[3] = q5 ;
929 
930  setPosition(frame, position) ;
931  }
932  catch(...) {
933  vpERROR_TRACE("Error caught");
934  throw ;
935  }
936 }
937 
938 
939 
940 
970 void vpRobotAfma4::setPosition(const char *filename)
971 {
972  vpColVector q;
973  bool ret;
974 
975  ret = this->readPosFile(filename, q);
976 
977  if (ret == false) {
978  vpERROR_TRACE ("Bad position in \"%s\"", filename);
980  "Bad position in filename.");
981  }
984 }
985 
989 double vpRobotAfma4::getTime() const
990 {
991  double timestamp;
992  PrimitiveACQ_TIME_Afma4(&timestamp);
993  return timestamp;
994 }
995 
1055 void
1057  vpColVector & position, double &timestamp)
1058 {
1059 
1060  InitTry;
1061 
1062  position.resize (this->njoint);
1063 
1064  switch (frame) {
1065  case vpRobot::CAMERA_FRAME : {
1066  position = 0;
1067  return;
1068  }
1069  case vpRobot::ARTICULAR_FRAME : {
1070  double _q[njoint];
1071  Try( PrimitiveACQ_POS_Afma4(_q, &timestamp) );
1072  for (unsigned int i=0; i < this->njoint; i ++) {
1073  position[i] = _q[i];
1074  }
1075 
1076  return;
1077  }
1078  case vpRobot::REFERENCE_FRAME : {
1079  double _q[njoint];
1080  Try( PrimitiveACQ_POS_Afma4(_q, &timestamp) );
1081 
1082  vpColVector q(this->njoint);
1083  for (unsigned int i=0; i < this->njoint; i++)
1084  q[i] = _q[i];
1085 
1086  // Compute fMc
1087  vpHomogeneousMatrix fMc;
1088  vpAfma4::get_fMc(q, fMc);
1089 
1090  // From fMc extract the pose
1091  vpRotationMatrix fRc;
1092  fMc.extract(fRc);
1093  vpRxyzVector rxyz;
1094  rxyz.buildFrom(fRc);
1095 
1096  for (unsigned int i=0; i < 3; i++) {
1097  position[i] = fMc[i][3]; // translation x,y,z
1098  position[i+3] = rxyz[i]; // Euler rotation x,y,z
1099  }
1100  break ;
1101  }
1102  case vpRobot::MIXT_FRAME: {
1103  vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
1105  "Cannot get position in mixt frame: "
1106  "not implemented");
1107  break ;
1108  }
1109  }
1110 
1111  CatchPrint();
1112  if (TryStt < 0) {
1113  vpERROR_TRACE ("Cannot get position.");
1115  "Cannot get position.");
1116  }
1117 
1118  return;
1119 }
1120 
1131  vpColVector &position)
1132 {
1133  double timestamp;
1134  getPosition(frame, position, timestamp);
1135 }
1136 
1183 void
1185  const vpColVector & vel)
1186 {
1187 
1189  {
1190  vpERROR_TRACE ("Cannot send a velocity to the robot "
1191  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1193  "Cannot send a velocity to the robot "
1194  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1195  }
1196 
1197  // Check the dimension of the velocity vector to see if it is
1198  // compatible with the requested frame
1199  switch(frame) {
1200  case vpRobot::CAMERA_FRAME : {
1201  //if (vel.getRows() != 2) {
1202  if (vel.getRows() != 6) {
1203  vpERROR_TRACE ("Bad dimension of the velocity vector in camera frame");
1205  "Bad dimension of the velocity vector "
1206  "in camera frame");
1207  }
1208  break ;
1209  }
1210  case vpRobot::ARTICULAR_FRAME : {
1211  if (vel.getRows() != this->njoint) {
1212  vpERROR_TRACE ("Bad dimension of the articular velocity vector");
1214  "Bad dimension of the articular "
1215  "velocity vector ");
1216  }
1217  break ;
1218  }
1219  case vpRobot::REFERENCE_FRAME : {
1220  vpERROR_TRACE ("Cannot send a velocity to the robot "
1221  "in the reference frame: "
1222  "functionality not implemented");
1224  "Cannot send a velocity to the robot "
1225  "in the reference frame:"
1226  "functionality not implemented");
1227  break ;
1228  }
1229  case vpRobot::MIXT_FRAME : {
1230  vpERROR_TRACE ("Cannot send a velocity to the robot "
1231  "in the mixt frame: "
1232  "functionality not implemented");
1234  "Cannot send a velocity to the robot "
1235  "in the mixt frame:"
1236  "functionality not implemented");
1237  break ;
1238  }
1239  default: {
1240  vpERROR_TRACE ("Error in spec of vpRobot. "
1241  "Case not taken in account.");
1243  "Cannot send a velocity to the robot ");
1244  }
1245  }
1246 
1247 
1248  //
1249  // Velocities saturation with normalization
1250  //
1251  vpColVector joint_vel(this->njoint);
1252 
1253  // Case of the camera frame where we control only 2 dof
1254  if (frame == vpRobot::CAMERA_FRAME) {
1255  vpColVector vel_max(6);
1256 
1257  for (unsigned int i=0; i<3; i++) {
1258  vel_max[i] = getMaxTranslationVelocity();
1259  vel_max[i+3] = getMaxRotationVelocity();
1260  }
1261 
1262  vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1263 
1264 #if 0 // ok
1265  vpMatrix eJe(4,6);
1266  eJe = 0;
1267  eJe[2][4] = -1;
1268  eJe[3][3] = 1;
1269 
1270  joint_vel = eJe * velocity; // Compute the articular velocity
1271 #endif
1272  vpColVector q;
1274  vpMatrix fJe_inverse;
1275  get_fJe_inverse(q, fJe_inverse);
1276  vpHomogeneousMatrix fMe;
1277  get_fMe(q, fMe);
1279  t=0;
1280  vpRotationMatrix fRe;
1281  fMe.extract(fRe);
1282  vpVelocityTwistMatrix fVe(t, fRe);
1283  // compute the inverse jacobian in the end-effector frame
1284  vpMatrix eJe_inverse = fJe_inverse * fVe;
1285 
1286  // Transform the velocities from camera to end-effector frame
1288  eVc.buildFrom(this->_eMc);
1289  joint_vel = eJe_inverse * eVc * velocity;
1290 
1291  // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1292  // joint_vel[2], joint_vel[3]);
1293  }
1294 
1295  // Case of the joint control where we control all the joints
1296  else if (frame == vpRobot::ARTICULAR_FRAME) {
1297 
1298  vpColVector vel_max(4);
1299 
1300  vel_max[0] = getMaxRotationVelocity();
1301  vel_max[1] = getMaxTranslationVelocity();
1302  vel_max[2] = getMaxRotationVelocity();
1303  vel_max[3] = getMaxRotationVelocity();
1304 
1305  joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1306  }
1307 
1308  InitTry;
1309 
1310  // Send a joint velocity to the low level controller
1311  Try( PrimitiveMOVESPEED_Afma4(joint_vel.data) );
1312 
1313  Catch();
1314  if (TryStt < 0) {
1315  if (TryStt == VelStopOnJoint) {
1316  UInt32 axisInJoint[njoint];
1317  PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1318  for (unsigned int i=0; i < njoint; i ++) {
1319  if (axisInJoint[i])
1320  std::cout << "\nWarning: Velocity control stopped: axis "
1321  << i+1 << " on joint limit!" <<std::endl;
1322  }
1323  }
1324  else {
1325  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1326  if (TryString != NULL) {
1327  // The statement is in TryString, but we need to check the validity
1328  printf(" Error sentence %s\n", TryString); // Print the TryString
1329  }
1330  else {
1331  printf("\n");
1332  }
1333  }
1334  }
1335 
1336  return;
1337 }
1338 
1339 /* ------------------------------------------------------------------------ */
1340 /* --- GET ---------------------------------------------------------------- */
1341 /* ------------------------------------------------------------------------ */
1342 
1343 
1391 void
1393  vpColVector & velocity, double &timestamp)
1394 {
1395 
1396  switch (frame) {
1398  velocity.resize (this->njoint);
1399  default:
1400  velocity.resize (6);
1401  }
1402 
1403  velocity = 0;
1404 
1405  double q[4];
1406  vpColVector q_cur(4);
1407  vpHomogeneousMatrix fMc_cur;
1408  vpHomogeneousMatrix cMc; // camera displacement
1409  double time_cur;
1410 
1411  InitTry;
1412 
1413  // Get the current joint position
1414  Try( PrimitiveACQ_POS_Afma4(q, &timestamp) );
1415  time_cur = timestamp;
1416 
1417  for (unsigned int i=0; i < this->njoint; i ++) {
1418  q_cur[i] = q[i];
1419  }
1420 
1421  // Get the camera pose from the direct kinematics
1422  vpAfma4::get_fMc(q_cur, fMc_cur);
1423 
1424  if ( ! first_time_getvel ) {
1425 
1426  switch (frame) {
1427  case vpRobot::CAMERA_FRAME: {
1428  // Compute the displacement of the camera since the previous call
1429  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1430 
1431  // Compute the velocity of the camera from this displacement
1432  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1433 
1434  break ;
1435  }
1436 
1437  case vpRobot::ARTICULAR_FRAME: {
1438  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1439  break ;
1440  }
1441 
1442  case vpRobot::REFERENCE_FRAME: {
1443  // Compute the displacement of the camera since the previous call
1444  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1445 
1446  // Compute the velocity of the camera from this displacement
1447  vpColVector v;
1448  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1449 
1450  // Express this velocity in the reference frame
1451  vpVelocityTwistMatrix fVc(fMc_cur);
1452  velocity = fVc * v;
1453 
1454  break ;
1455  }
1456 
1457  case vpRobot::MIXT_FRAME: {
1458  vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
1459  "functionality not implemented");
1461  "Cannot get a displacement in the mixt frame:"
1462  "functionality not implemented");
1463 
1464  break ;
1465  }
1466  }
1467  }
1468  else {
1469  first_time_getvel = false;
1470  }
1471 
1472  // Memorize the camera pose for the next call
1473  fMc_prev_getvel = fMc_cur;
1474 
1475  // Memorize the joint position for the next call
1476  q_prev_getvel = q_cur;
1477 
1478  // Memorize the time associated to the joint position for the next call
1479  time_prev_getvel = time_cur;
1480 
1481 
1482  CatchPrint();
1483  if (TryStt < 0) {
1484  vpERROR_TRACE ("Cannot get velocity.");
1486  "Cannot get velocity.");
1487  }
1488 }
1489 
1499  vpColVector & velocity)
1500 {
1501  double timestamp;
1502  getVelocity(frame, velocity, timestamp);
1503 }
1504 
1505 
1506 
1548 {
1549  vpColVector velocity;
1550  getVelocity (frame, velocity, timestamp);
1551 
1552  return velocity;
1553 }
1554 
1564 {
1565  vpColVector velocity;
1566  double timestamp;
1567  getVelocity (frame, velocity, timestamp);
1568 
1569  return velocity;
1570 }
1571 
1622 bool
1623 vpRobotAfma4::readPosFile(const char *filename, vpColVector &q)
1624 {
1625 
1626  FILE * fd ;
1627  fd = fopen(filename, "r") ;
1628  if (fd == NULL)
1629  return false;
1630 
1631  char line[FILENAME_MAX];
1632  char dummy[FILENAME_MAX];
1633  char head[] = "R:";
1634  bool sortie = false;
1635 
1636  do {
1637  // Saut des lignes commencant par #
1638  if (fgets (line, FILENAME_MAX, fd) != NULL) {
1639  if ( strncmp (line, "#", 1) != 0) {
1640  // La ligne n'est pas un commentaire
1641  if ( strncmp (line, head, sizeof(head)-1) == 0) {
1642  sortie = true; // Position robot trouvee.
1643  }
1644 // else
1645 // return (false); // fin fichier sans position robot.
1646  }
1647  }
1648  else {
1649  return (false); /* fin fichier */
1650  }
1651 
1652  }
1653  while ( sortie != true );
1654 
1655  // Lecture des positions
1656  q.resize(4);
1657  sscanf(line, "%s %lf %lf %lf %lf",
1658  dummy,
1659  &q[0], &q[1], &q[2], &q[3]);
1660 
1661  // converts rotations from degrees into radians
1662  q[0] = vpMath::rad(q[0]);
1663  q[2] = vpMath::rad(q[2]);
1664  q[3] = vpMath::rad(q[3]);
1665 
1666 
1667  fclose(fd) ;
1668  return (true);
1669 }
1670 
1694 bool
1695 vpRobotAfma4::savePosFile(const char *filename, const vpColVector &q)
1696 {
1697 
1698  FILE * fd ;
1699  fd = fopen(filename, "w") ;
1700  if (fd == NULL)
1701  return false;
1702 
1703  fprintf(fd, "\
1704 #AFMA4 - Position - Version 2.01\n\
1705 #\n\
1706 # R: X Y A B\n\
1707 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1708 # Y : vertical translation in meters (joint 2)\n\
1709 # A : pan rotation of the camera in degrees (joint 4)\n\
1710 # B : tilt rotation of the camera in degrees (joint 5)\n\
1711 #\n\n");
1712 
1713  // Save positions in mm and deg
1714  fprintf(fd, "R: %lf %lf %lf %lf\n",
1715  vpMath::deg(q[0]),
1716  q[1],
1717  vpMath::deg(q[2]),
1718  vpMath::deg(q[3]));
1719 
1720  fclose(fd) ;
1721  return (true);
1722 }
1723 
1734 void
1735 vpRobotAfma4::move(const char *filename)
1736 {
1737  vpColVector q;
1738 
1739  this->readPosFile(filename, q) ;
1741  this->setPositioningVelocity(10);
1743 }
1744 
1758 void
1759 vpRobotAfma4::getCameraDisplacement(vpColVector &displacement)
1760 {
1761  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1762 }
1774 void
1775 vpRobotAfma4::getArticularDisplacement(vpColVector &displacement)
1776 {
1778 }
1779 
1800 void
1802  vpColVector &displacement)
1803 {
1804  displacement.resize (6);
1805  displacement = 0;
1806 
1807  double q[6];
1808  vpColVector q_cur(6);
1809  double timestamp;
1810 
1811  InitTry;
1812 
1813  // Get the current joint position
1814  Try( PrimitiveACQ_POS_Afma4(q, &timestamp) );
1815  for (unsigned int i=0; i < njoint; i ++) {
1816  q_cur[i] = q[i];
1817  }
1818 
1819  if ( ! first_time_getdis ) {
1820  switch (frame) {
1821  case vpRobot::CAMERA_FRAME: {
1822  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1823  return;
1824  break ;
1825  }
1826 
1827  case vpRobot::ARTICULAR_FRAME: {
1828  displacement = q_cur - q_prev_getdis;
1829  break ;
1830  }
1831 
1832  case vpRobot::REFERENCE_FRAME: {
1833  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1834  return;
1835  break ;
1836  }
1837 
1838  case vpRobot::MIXT_FRAME: {
1839  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1840  return;
1841  break ;
1842  }
1843  }
1844  }
1845  else {
1846  first_time_getdis = false;
1847  }
1848 
1849  // Memorize the joint position for the next call
1850  q_prev_getdis = q_cur;
1851 
1852  CatchPrint();
1853  if (TryStt < 0) {
1854  vpERROR_TRACE ("Cannot get velocity.");
1856  "Cannot get velocity.");
1857  }
1858 }
1859 
1860 
1861 /*
1862  * Local variables:
1863  * c-basic-offset: 2
1864  * End:
1865  */
1866 
1867 #endif
1868 
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma4.cpp:190
Error that can be emited by the vpRobot class and its derivates.
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma4.cpp:273
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma4.cpp:324
static bool savePosFile(const char *filename, const vpColVector &q)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
void setVerbose(bool verbose)
Definition: vpRobot.h:158
virtual ~vpRobotAfma4(void)
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:254
Initialize the position controller.
Definition: vpRobot.h:71
double getTime() const
class that defines a generic virtual robot
Definition: vpRobot.h:60
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:167
vpControlFrameType
Definition: vpRobot.h:78
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition: vpAfma4.cpp:368
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:279
void move(const char *filename)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma4.cpp:416
The vpRotationMatrix considers the particular case of a rotation matrix.
double * data
address of the first element of the data array
Definition: vpMatrix.h:118
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:70
vpRobotStateType
Definition: vpRobot.h:66
bool verbose_
Definition: vpRobot.h:115
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 const unsigned int njoint
Number of joint.
Definition: vpAfma4.h:142
Modelisation of Irisa's cylindrical robot named Afma4.
Definition: vpAfma4.h:112
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
static double rad(double deg)
Definition: vpMath.h:100
void get_eJe(vpMatrix &eJe)
static bool readPosFile(const char *filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static double deg(double rad)
Definition: vpMath.h:93
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:103
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
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:526
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:140
void get_fJe(vpMatrix &fJe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
bool getPowerState()
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma4.cpp:470
double getPositioningVelocity(void)
void get_cVf(vpVelocityTwistMatrix &cVf) const
void get_cVe(vpVelocityTwistMatrix &cVe) const
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
static const double defaultPositioningVelocity
Definition: vpRobotAfma4.h:227
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:184
Class that consider the case of a translation vector.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98