ViSP  2.6.2
vpRobotAfma6.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotAfma6.cpp 3697 2012-05-03 08:41:33Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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 Afma6 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 
44 #ifdef VISP_HAVE_AFMA6
45 
46 #include <signal.h>
47 #include <stdlib.h>
48 
49 #include <visp/vpRobotException.h>
50 #include <visp/vpExponentialMap.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpVelocityTwistMatrix.h>
53 #include <visp/vpThetaUVector.h>
54 #include <visp/vpRobotAfma6.h>
55 #include <visp/vpRotationMatrix.h>
56 
57 /* ---------------------------------------------------------------------- */
58 /* --- STATIC ----------------------------------------------------------- */
59 /* ---------------------------------------------------------------------- */
60 
61 bool vpRobotAfma6::robotAlreadyCreated = false;
62 
71 
72 /* ---------------------------------------------------------------------- */
73 /* --- EMERGENCY STOP --------------------------------------------------- */
74 /* ---------------------------------------------------------------------- */
75 
83 void emergencyStopAfma6(int signo)
84 {
85  std::cout << "Stop the Afma6 application by signal ("
86  << signo << "): " << (char)7 ;
87  switch(signo)
88  {
89  case SIGINT:
90  std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
91  case SIGBUS:
92  std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
93  case SIGSEGV:
94  std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
95  case SIGKILL:
96  std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
97  case SIGQUIT:
98  std::cout <<"SIGQUIT " << std::endl ; break ;
99  default :
100  std::cout << signo << std::endl ;
101  }
102  //std::cout << "Emergency stop called\n";
103  // PrimitiveESTOP_Afma6();
104  PrimitiveSTOP_Afma6();
105  std::cout << "Robot was stopped\n";
106 
107  // Free allocated ressources
108  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109 
110  fprintf(stdout, "Application ");
111  fflush(stdout);
112  kill(getpid(), SIGKILL);
113  exit(1) ;
114 }
115 
116 /* ---------------------------------------------------------------------- */
117 /* --- CONSTRUCTOR ------------------------------------------------------ */
118 /* ---------------------------------------------------------------------- */
119 
157  :
158  vpAfma6 (),
159  vpRobot ()
160 {
161 
162  /*
163  #define SIGHUP 1 // hangup
164  #define SIGINT 2 // interrupt (rubout)
165  #define SIGQUIT 3 // quit (ASCII FS)
166  #define SIGILL 4 // illegal instruction (not reset when caught)
167  #define SIGTRAP 5 // trace trap (not reset when caught)
168  #define SIGIOT 6 // IOT instruction
169  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
170  #define SIGEMT 7 // EMT instruction
171  #define SIGFPE 8 // floating point exception
172  #define SIGKILL 9 // kill (cannot be caught or ignored)
173  #define SIGBUS 10 // bus error
174  #define SIGSEGV 11 // segmentation violation
175  #define SIGSYS 12 // bad argument to system call
176  #define SIGPIPE 13 // write on a pipe with no one to read it
177  #define SIGALRM 14 // alarm clock
178  #define SIGTERM 15 // software termination signal from kill
179  */
180 
181  signal(SIGINT, emergencyStopAfma6);
182  signal(SIGBUS, emergencyStopAfma6) ;
183  signal(SIGSEGV, emergencyStopAfma6) ;
184  signal(SIGKILL, emergencyStopAfma6);
185  signal(SIGQUIT, emergencyStopAfma6);
186 
187  std::cout << "Open communication with MotionBlox.\n";
188  try {
189  this->init();
191  }
192  catch(...) {
193  // vpERROR_TRACE("Error caught") ;
194  throw ;
195  }
196  positioningVelocity = defaultPositioningVelocity ;
197 
198  vpRobotAfma6::robotAlreadyCreated = true;
199 
200  return ;
201 }
202 
203 
204 /* ------------------------------------------------------------------------ */
205 /* --- INITIALISATION ----------------------------------------------------- */
206 /* ------------------------------------------------------------------------ */
207 
227 void
229 {
230  InitTry;
231 
232  // Initialise private variables used to compute the measured velocities
233  q_prev_getvel.resize(6);
234  q_prev_getvel = 0;
235  time_prev_getvel = 0;
236  first_time_getvel = true;
237 
238  // Initialise private variables used to compute the measured displacement
239  q_prev_getdis.resize(6);
240  q_prev_getdis = 0;
241  first_time_getdis = true;
242 
243 
244  // Initialize the firewire connection
245  Try( InitializeConnection() );
246 
247  // Connect to the servoboard using the servo board GUID
248  Try( InitializeNode_Afma6() );
249 
250  Try( PrimitiveRESET_Afma6() );
251 
252  // Update the eMc matrix in the low level controller
254 
255  // Look if the power is on or off
256  UInt32 HIPowerStatus;
257  UInt32 EStopStatus;
258  Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
259  &HIPowerStatus));
260  CAL_Wait(0.1);
261 
262  // Print the robot status
263  std::cout << "Robot status: ";
264  switch(EStopStatus) {
265  case ESTOP_AUTO:
266  case ESTOP_MANUAL:
267  if (HIPowerStatus == 0)
268  std::cout << "Power is OFF" << std::endl;
269  else
270  std::cout << "Power is ON" << std::endl;
271  break;
272  case ESTOP_ACTIVATED:
273  std::cout << "Emergency stop is activated" << std::endl;
274  break;
275  default:
276  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
277  std::cout << "You have to call Adept for maintenance..." << std::endl;
278  // Free allocated ressources
279  }
280  std::cout << std::endl;
281 
282  // get real joint min/max from the MotionBlox
283  Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
284 // for (unsigned int i=0; i < njoint; i++) {
285 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
286 // }
287 
288  // If an error occur in the low level controller, goto here
289  //CatchPrint();
290  Catch();
291 
292  // Test if an error occurs
293  if (TryStt == -20001)
294  printf("No connection detected. Check if the robot is powered on \n"
295  "and if the firewire link exist between the MotionBlox and this computer.\n");
296  else if (TryStt == -675)
297  printf(" Timeout enabling power...\n");
298 
299  if (TryStt < 0) {
300  // Power off the robot
301  PrimitivePOWEROFF_Afma6();
302  // Free allocated ressources
303  ShutDownConnection();
304 
305  std::cout << "Cannot open connexion with the motionblox..." << std::endl;
307  "Cannot open connexion with the motionblox");
308  }
309  return ;
310 }
311 
348 void
351 {
352 
353  InitTry;
354  // Read the robot constants from files
355  // - joint [min,max], coupl_56, long_56
356  // - camera extrinsic parameters relative to eMc
357  vpAfma6::init(tool, projModel);
358 
359  // Set the robot constant (coupl_56, long_56) in the MotionBlox
360  Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
361 
362  // Set the camera constant (eMc pose) in the MotionBlox
363  double eMc_pose[6];
364  for (unsigned int i=0; i < 3; i ++) {
365  eMc_pose[i] = _etc[i]; // translation in meters
366  eMc_pose[i+3] = _erc[i]; // rotation in rad
367  }
368  // Update the eMc pose in the low level controller
369  Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
370 
371  // get real joint min/max from the MotionBlox
372  Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
373 // for (unsigned int i=0; i < njoint; i++) {
374 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
375 // }
376 
377  setToolType(tool);
378 
379  CatchPrint();
380  return ;
381 }
382 
383 /* ------------------------------------------------------------------------ */
384 /* --- DESTRUCTOR --------------------------------------------------------- */
385 /* ------------------------------------------------------------------------ */
386 
394 {
395  InitTry;
396 
398 
399  // Look if the power is on or off
400  UInt32 HIPowerStatus;
401  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
402  &HIPowerStatus));
403  CAL_Wait(0.1);
404 
405 // if (HIPowerStatus == 1) {
406 // fprintf(stdout, "Power OFF the robot\n");
407 // fflush(stdout);
408 
409 // Try( PrimitivePOWEROFF_Afma6() );
410 // }
411 
412  // Free allocated ressources
413  ShutDownConnection();
414 
415  vpRobotAfma6::robotAlreadyCreated = false;
416 
417  CatchPrint();
418  return;
419 }
420 
421 
422 
423 
432 {
433  InitTry;
434 
435  switch (newState) {
436  case vpRobot::STATE_STOP: {
438  Try( PrimitiveSTOP_Afma6() );
439  }
440  break;
441  }
444  std::cout << "Change the control mode from velocity to position control.\n";
445  Try( PrimitiveSTOP_Afma6() );
446  }
447  else {
448  //std::cout << "Change the control mode from stop to position control.\n";
449  }
450  this->powerOn();
451  break;
452  }
455  std::cout << "Change the control mode from stop to velocity control.\n";
456  }
457  this->powerOn();
458  break;
459  }
460  default:
461  break ;
462  }
463 
464  CatchPrint();
465 
466  return vpRobot::setRobotState (newState);
467 }
468 
469 
470 /* ------------------------------------------------------------------------ */
471 /* --- STOP --------------------------------------------------------------- */
472 /* ------------------------------------------------------------------------ */
473 
481 void
483 {
484  InitTry;
485  Try( PrimitiveSTOP_Afma6() );
487 
488  CatchPrint();
489  if (TryStt < 0) {
490  vpERROR_TRACE ("Cannot stop robot motion");
492  "Cannot stop robot motion.");
493  }
494 }
495 
505 void
507 {
508  InitTry;
509 
510  // Look if the power is on or off
511  UInt32 HIPowerStatus;
512  UInt32 EStopStatus;
513  bool firsttime = true;
514  unsigned int nitermax = 10;
515 
516  for (unsigned int i=0; i<nitermax; i++) {
517  Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
518  &HIPowerStatus));
519  switch(EStopStatus) {
520  case ESTOP_AUTO: break;
521  case ESTOP_MANUAL: break;
522  case ESTOP_ACTIVATED:
523  if (firsttime) {
524  std::cout << "Emergency stop is activated! \n"
525  << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
526  firsttime = false;
527  }
528  fprintf(stdout, "Remaining time %ds \r", nitermax-i);
529  fflush(stdout);
530  CAL_Wait(1);
531  break;
532  default:
533  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
534  std::cout << "You have to call Adept for maintenance..." << std::endl;
535  // Free allocated ressources
536  ShutDownConnection();
537  exit(0);
538  }
539  }
540 
541  std::cout << std::endl;
542 
543  if (EStopStatus == ESTOP_ACTIVATED) {
544  std::cout << "Sorry, cannot power on the robot." << std::endl;
546  "Cannot power on the robot.");
547  }
548 
549  if (HIPowerStatus == 0) {
550  fprintf(stdout, "Power ON the Afma6 robot\n");
551  fflush(stdout);
552 
553  Try( PrimitivePOWERON_Afma6() );
554  }
555 
556  CatchPrint();
557  if (TryStt < 0) {
558  vpERROR_TRACE ("Cannot power on the robot");
560  "Cannot power off the robot.");
561  }
562 }
563 
573 void
575 {
576  InitTry;
577 
578  // Look if the power is on or off
579  UInt32 HIPowerStatus;
580  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
581  &HIPowerStatus));
582  CAL_Wait(0.1);
583 
584  if (HIPowerStatus == 1) {
585  fprintf(stdout, "Power OFF the Afma6 robot\n");
586  fflush(stdout);
587 
588  Try( PrimitivePOWEROFF_Afma6() );
589  }
590 
591  CatchPrint();
592  if (TryStt < 0) {
593  vpERROR_TRACE ("Cannot power off the robot");
595  "Cannot power off the robot.");
596  }
597 }
598 
610 bool
612 {
613  InitTry;
614  bool status = false;
615  // Look if the power is on or off
616  UInt32 HIPowerStatus;
617  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
618  &HIPowerStatus));
619  CAL_Wait(0.1);
620 
621  if (HIPowerStatus == 1) {
622  status = true;
623  }
624 
625  CatchPrint();
626  if (TryStt < 0) {
627  vpERROR_TRACE ("Cannot get the power status");
629  "Cannot get the power status.");
630  }
631  return status;
632 }
633 
643 void
645 {
646  vpHomogeneousMatrix cMe ;
647  vpAfma6::get_cMe(cMe) ;
648 
649  cVe.buildFrom(cMe) ;
650 }
651 
662 void
664 {
665  vpAfma6::get_cMe(cMe) ;
666 }
667 
668 
679 void
681 {
682 
683  double position[6];
684 
685  InitTry;
686  Try( PrimitiveACQ_POS_Afma6(position) );
687  CatchPrint();
688 
689  vpColVector q(6);
690  for (unsigned int i=0; i < njoint; i++)
691  q[i] = position[i];
692 
693  try
694  {
695  vpAfma6::get_eJe(q, eJe) ;
696  }
697  catch(...)
698  {
699  vpERROR_TRACE("catch exception ") ;
700  throw ;
701  }
702 }
726  void
728 {
729 
730  double position[6];
731 
732  InitTry;
733  Try( PrimitiveACQ_POS_Afma6(position) );
734  CatchPrint();
735 
736  vpColVector q(6);
737  for (unsigned int i=0; i < njoint; i++)
738  q[i] = position[i];
739 
740  try
741  {
742  vpAfma6::get_fJe(q, fJe) ;
743  }
744  catch(...)
745  {
746  vpERROR_TRACE("Error caught");
747  throw ;
748  }
749 }
750 
779 void
781 {
782  positioningVelocity = velocity;
783 }
784 
790 double
792 {
793  return positioningVelocity;
794 }
795 
870 void
872  const vpPoseVector & pose )
873 
874 {
875  vpColVector position(6);
876  vpRxyzVector rxyz;
878 
879  R.buildFrom(pose[3], pose[4], pose[5]); //thetau
880  rxyz.buildFrom(R);
881 
882  for (unsigned int i=0; i < 3; i++) {
883  position[i] = pose[i];
884  position[i+3] = rxyz[i];
885  }
886  if (frame == vpRobot::ARTICULAR_FRAME) {
888  "Positionning error: "
889  "Joint frame not implemented for pose positionning.");
890  }
891  setPosition (frame, position);
892 }
975 void
977  const vpColVector & position )
978 {
979 
981  {
982  vpERROR_TRACE ("Robot was not in position-based control\n"
983  "Modification of the robot state");
985  }
986 
987  double _destination[6];
988  int error = 0;
989 
990  InitTry;
991  switch(frame) {
992  case vpRobot::CAMERA_FRAME : {
993  double _q[njoint];
994  Try( PrimitiveACQ_POS_Afma6(_q) );
995 
996  vpColVector q(njoint);
997  for (unsigned int i=0; i < njoint; i++)
998  q[i] = _q[i];
999 
1000  // Get fMc from the inverse kinematics
1001  vpHomogeneousMatrix fMc;
1002  vpAfma6::get_fMc(q, fMc);
1003 
1004  // Set cMc from the input position
1005  vpTranslationVector txyz;
1006  vpRxyzVector rxyz;
1007  for (unsigned int i=0; i < 3; i++) {
1008  txyz[i] = position[i];
1009  rxyz[i] = position[i+3];
1010  }
1011 
1012  // Compute cMc2
1013  vpRotationMatrix cRc2(rxyz);
1014  vpHomogeneousMatrix cMc2(txyz, cRc2);
1015 
1016  // Compute the new position to reach: fMc*cMc2
1017  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1018 
1019  // Compute the corresponding joint position from the inverse kinematics
1020  bool nearest = true;
1021  int solution = this->getInverseKinematics(fMc2, q, nearest);
1022  if (solution) { // Position is reachable
1023  for (unsigned int i=0; i < njoint; i ++) {
1024  _destination[i] = q[i];
1025  }
1026  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1027  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1028  }
1029  else {
1030  // Cartesian position is out of range
1031  error = -1;
1032  }
1033 
1034  break ;
1035  }
1036  case vpRobot::ARTICULAR_FRAME: {
1037  for (unsigned int i=0; i < njoint; i ++) {
1038  _destination[i] = position[i];
1039  }
1040  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1041  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1042  break ;
1043 
1044  }
1045  case vpRobot::REFERENCE_FRAME: {
1046  // Set fMc from the input position
1047  vpTranslationVector txyz;
1048  vpRxyzVector rxyz;
1049  for (unsigned int i=0; i < 3; i++) {
1050  txyz[i] = position[i];
1051  rxyz[i] = position[i+3];
1052  }
1053  // Compute fMc from the input position
1054  vpRotationMatrix fRc(rxyz);
1055  vpHomogeneousMatrix fMc(txyz, fRc);
1056 
1057  // Compute the corresponding joint position from the inverse kinematics
1058  vpColVector q(6);
1059  bool nearest = true;
1060  int solution = this->getInverseKinematics(fMc, q, nearest);
1061  if (solution) { // Position is reachable
1062  for (unsigned int i=0; i < njoint; i ++) {
1063  _destination[i] = q[i];
1064  }
1065  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1066  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1067  }
1068  else {
1069  // Cartesian position is out of range
1070  error = -1;
1071  }
1072 
1073  break ;
1074  }
1075  case vpRobot::MIXT_FRAME:
1076  {
1077  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1079  "Positionning error: "
1080  "Mixt frame not implemented.");
1081  break ;
1082  }
1083  }
1084 
1085  CatchPrint();
1086  if (TryStt == InvalidPosition || TryStt == -1023)
1087  std::cout << " : Position out of range.\n";
1088  else if (TryStt < 0)
1089  std::cout << " : Unknown error (see Fabien).\n";
1090  else if (error == -1)
1091  std::cout << "Position out of range.\n";
1092 
1093  if (TryStt < 0 || error < 0) {
1094  vpERROR_TRACE ("Positionning error.");
1096  "Position out of range.");
1097  }
1098 
1099  return ;
1100 }
1101 
1170  const double pos1,
1171  const double pos2,
1172  const double pos3,
1173  const double pos4,
1174  const double pos5,
1175  const double pos6)
1176 {
1177  try{
1178  vpColVector position(6) ;
1179  position[0] = pos1 ;
1180  position[1] = pos2 ;
1181  position[2] = pos3 ;
1182  position[3] = pos4 ;
1183  position[4] = pos5 ;
1184  position[5] = pos6 ;
1185 
1186  setPosition(frame, position) ;
1187  }
1188  catch(...)
1189  {
1190  vpERROR_TRACE("Error caught");
1191  throw ;
1192  }
1193 }
1194 
1236 void vpRobotAfma6::setPosition(const char *filename)
1237 {
1238  vpColVector q;
1239  bool ret;
1240 
1241  ret = this->readPosFile(filename, q);
1242 
1243  if (ret == false) {
1244  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1246  "Bad position in filename.");
1247  }
1250 }
1251 
1303 void
1305  vpColVector & position)
1306 {
1307 
1308  InitTry;
1309 
1310  position.resize (6);
1311 
1312  switch (frame) {
1313  case vpRobot::CAMERA_FRAME : {
1314  position = 0;
1315  return;
1316  }
1317  case vpRobot::ARTICULAR_FRAME : {
1318  double _q[njoint];
1319  Try( PrimitiveACQ_POS_Afma6(_q) );
1320  for (unsigned int i=0; i < njoint; i ++) {
1321  position[i] = _q[i];
1322  }
1323 
1324  return;
1325  }
1326  case vpRobot::REFERENCE_FRAME : {
1327  double _q[njoint];
1328  Try( PrimitiveACQ_POS_Afma6(_q) );
1329 
1330  vpColVector q(njoint);
1331  for (unsigned int i=0; i < njoint; i++)
1332  q[i] = _q[i];
1333 
1334  // Compute fMc
1335  vpHomogeneousMatrix fMc;
1336  vpAfma6::get_fMc(q, fMc);
1337 
1338  // From fMc extract the pose
1339  vpRotationMatrix fRc;
1340  fMc.extract(fRc);
1341  vpRxyzVector rxyz;
1342  rxyz.buildFrom(fRc);
1343 
1344  for (unsigned int i=0; i < 3; i++) {
1345  position[i] = fMc[i][3]; // translation x,y,z
1346  position[i+3] = rxyz[i]; // Euler rotation x,y,z
1347  }
1348  break ;
1349  }
1350  case vpRobot::MIXT_FRAME: {
1351  vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
1353  "Cannot get position in mixt frame: "
1354  "not implemented");
1355  break ;
1356  }
1357  }
1358 
1359  CatchPrint();
1360  if (TryStt < 0) {
1361  vpERROR_TRACE ("Cannot get position.");
1363  "Cannot get position.");
1364  }
1365 
1366  return;
1367 }
1378 void
1380  vpPoseVector &position)
1381 {
1382  vpColVector posRxyz;
1383  //recupere position en Rxyz
1384  this->getPosition(frame,posRxyz);
1385  vpRxyzVector RxyzVect;
1386  for(unsigned int j=0;j<3;j++)
1387  RxyzVect[j]=posRxyz[j+3];
1388  //recupere le vecteur thetaU correspondant
1389  vpThetaUVector RtuVect(RxyzVect);
1390 
1391  //remplit le vpPoseVector avec translation et rotation ThetaU
1392  for(unsigned int j=0;j<3;j++)
1393  {
1394  position[j]=posRxyz[j];
1395  position[j+3]=RtuVect[j];
1396  }
1397 }
1398 
1453 void
1455  const vpColVector & vel)
1456 {
1458  {
1459  vpERROR_TRACE ("Cannot send a velocity to the robot "
1460  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1462  "Cannot send a velocity to the robot "
1463  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1464  }
1465 
1466  vpColVector vel_max(6);
1467 
1468  for (int i=0; i<3; i++)
1469  vel_max[i] = getMaxTranslationVelocity();
1470  for (int i=3; i<6; i++)
1471  vel_max[i] = getMaxRotationVelocity();
1472 
1473  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1474 
1475  InitTry;
1476 
1477  switch(frame) {
1478  case vpRobot::CAMERA_FRAME : {
1479  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM) );
1480  break ;
1481  }
1482  case vpRobot::ARTICULAR_FRAME : {
1483  //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART) );
1484  Try( PrimitiveMOVESPEED_Afma6(vel_sat.data) );
1485  break ;
1486  }
1487  case vpRobot::REFERENCE_FRAME : {
1488  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX) );
1489  break ;
1490  }
1491  case vpRobot::MIXT_FRAME : {
1492  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX) );
1493  break ;
1494  }
1495  default: {
1496  vpERROR_TRACE ("Error in spec of vpRobot. "
1497  "Case not taken in account.");
1498  return;
1499  }
1500  }
1501 
1502  Catch();
1503  if (TryStt < 0) {
1504  if (TryStt == VelStopOnJoint) {
1505  Int32 axisInJoint[njoint];
1506  PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1507  for (unsigned int i=0; i < njoint; i ++) {
1508  if (axisInJoint[i])
1509  std::cout << "\nWarning: Velocity control stopped: axis "
1510  << i+1 << " on joint limit!" <<std::endl;
1511  }
1512  }
1513  else {
1514  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1515  if (TryString != NULL) {
1516  // The statement is in TryString, but we need to check the validity
1517  printf(" Error sentence %s\n", TryString); // Print the TryString
1518  }
1519  else {
1520  printf("\n");
1521  }
1522  }
1523  }
1524 
1525  return;
1526 }
1527 
1528 
1529 
1530 
1531 
1532 
1533 /* ------------------------------------------------------------------------ */
1534 /* --- GET ---------------------------------------------------------------- */
1535 /* ------------------------------------------------------------------------ */
1536 
1537 
1584 void
1586  vpColVector & velocity)
1587 {
1588 
1589  velocity.resize (6);
1590  velocity = 0;
1591 
1592 
1593  double q[6];
1594  vpColVector q_cur(6);
1595  vpHomogeneousMatrix fMc_cur;
1596  vpHomogeneousMatrix cMc; // camera displacement
1597 
1598 
1599  InitTry;
1600 
1601  // Get the actual time
1602  double time_cur = vpTime::measureTimeSecond();
1603 
1604  // Get the current joint position
1605  Try( PrimitiveACQ_POS_Afma6(q) );
1606  for (unsigned int i=0; i < njoint; i ++) {
1607  q_cur[i] = q[i];
1608  }
1609 
1610  // Get the camera pose from the direct kinematics
1611  vpAfma6::get_fMc(q_cur, fMc_cur);
1612 
1613  if ( ! first_time_getvel ) {
1614 
1615  switch (frame) {
1616  case vpRobot::CAMERA_FRAME: {
1617  // Compute the displacement of the camera since the previous call
1618  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1619 
1620  // Compute the velocity of the camera from this displacement
1621  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1622 
1623  break ;
1624  }
1625 
1626  case vpRobot::ARTICULAR_FRAME: {
1627  velocity = (q_cur - q_prev_getvel)
1628  / (time_cur - time_prev_getvel);
1629  break ;
1630  }
1631 
1632  case vpRobot::REFERENCE_FRAME: {
1633  // Compute the displacement of the camera since the previous call
1634  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1635 
1636  // Compute the velocity of the camera from this displacement
1637  vpColVector v;
1638  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1639 
1640  // Express this velocity in the reference frame
1641  vpVelocityTwistMatrix fVc(fMc_cur);
1642  velocity = fVc * v;
1643 
1644  break ;
1645  }
1646 
1647  case vpRobot::MIXT_FRAME: {
1648  // Compute the displacement of the camera since the previous call
1649  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1650 
1651  // Compute the ThetaU representation for the rotation
1652  vpRotationMatrix cRc;
1653  cMc.extract(cRc);
1654  vpThetaUVector thetaU;
1655  thetaU.buildFrom(cRc);
1656 
1657  for (unsigned int i=0; i < 3; i++) {
1658  // Compute the translation displacement in the reference frame
1659  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1660  // Update the rotation displacement in the camera frame
1661  velocity[i+3] = thetaU[i];
1662  }
1663 
1664  // Compute the velocity
1665  velocity /= (time_cur - time_prev_getvel);
1666  break ;
1667  }
1668  }
1669  }
1670  else {
1671  first_time_getvel = false;
1672  }
1673 
1674  // Memorize the camera pose for the next call
1675  fMc_prev_getvel = fMc_cur;
1676 
1677  // Memorize the joint position for the next call
1678  q_prev_getvel = q_cur;
1679 
1680  // Memorize the time associated to the joint position for the next call
1681  time_prev_getvel = time_cur;
1682 
1683 
1684  CatchPrint();
1685  if (TryStt < 0) {
1686  vpERROR_TRACE ("Cannot get velocity.");
1688  "Cannot get velocity.");
1689  }
1690 }
1691 
1692 
1693 
1694 
1735 {
1736  vpColVector velocity;
1737  getVelocity (frame, velocity);
1738 
1739  return velocity;
1740 }
1741 
1790 bool
1791 vpRobotAfma6::readPosFile(const char *filename, vpColVector &q)
1792 {
1793 
1794  FILE * fd ;
1795  fd = fopen(filename, "r") ;
1796  if (fd == NULL)
1797  return false;
1798 
1799  char line[FILENAME_MAX];
1800  char dummy[FILENAME_MAX];
1801  char head[] = "R:";
1802  bool sortie = false;
1803 
1804  do {
1805  // Saut des lignes commencant par #
1806  if (fgets (line, FILENAME_MAX, fd) != NULL) {
1807  if ( strncmp (line, "#", 1) != 0) {
1808  // La ligne n'est pas un commentaire
1809  if ( strncmp (line, head, sizeof(head)-1) == 0) {
1810  sortie = true; // Position robot trouvee.
1811  }
1812 // else
1813 // return (false); // fin fichier sans position robot.
1814  }
1815  }
1816  else {
1817  return (false); /* fin fichier */
1818  }
1819 
1820  }
1821  while ( sortie != true );
1822 
1823  // Lecture des positions
1824  q.resize(njoint);
1825  sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
1826  dummy,
1827  &q[0], &q[1], &q[2],
1828  &q[3], &q[4], &q[5]);
1829 
1830  // converts rotations from degrees into radians
1831  for (unsigned int i=3; i < njoint; i ++)
1832  q[i] = vpMath::rad(q[i]) ;
1833 
1834  fclose(fd) ;
1835  return (true);
1836 }
1837 
1862 bool
1863 vpRobotAfma6::savePosFile(const char *filename, const vpColVector &q)
1864 {
1865 
1866  FILE * fd ;
1867  fd = fopen(filename, "w") ;
1868  if (fd == NULL)
1869  return false;
1870 
1871  fprintf(fd, "\
1872 #AFMA6 - Position - Version 2.01\n\
1873 #\n\
1874 # R: X Y Z A B C\n\
1875 # Joint position: X, Y, Z: translations in meters\n\
1876 # A, B, C: rotations in degrees\n\
1877 #\n\
1878 #\n\n");
1879 
1880  // Save positions in mm and deg
1881  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
1882  q[0],
1883  q[1],
1884  q[2],
1885  vpMath::deg(q[3]),
1886  vpMath::deg(q[4]),
1887  vpMath::deg(q[5]));
1888 
1889  fclose(fd) ;
1890  return (true);
1891 }
1892 
1903 void
1904 vpRobotAfma6::move(const char *filename)
1905 {
1906  vpColVector q;
1907 
1908  this->readPosFile(filename, q) ;
1910  this->setPositioningVelocity(10);
1912 }
1913 
1926 void
1927 vpRobotAfma6::move(const char *filename, const double velocity)
1928 {
1929  vpColVector q;
1930 
1931  this->readPosFile(filename, q) ;
1933  this->setPositioningVelocity(velocity);
1935 }
1936 
1943 void
1945 {
1946  InitTry;
1947  Try( PrimitiveGripper_Afma6(1) );
1948  std::cout << "Open the gripper..." << std::endl;
1949  CatchPrint();
1950  if (TryStt < 0) {
1951  vpERROR_TRACE ("Cannot open the gripper");
1953  "Cannot open the gripper.");
1954  }
1955 }
1956 
1964 void
1966 {
1967  InitTry;
1968  Try( PrimitiveGripper_Afma6(0) );
1969  std::cout << "Close the gripper..." << std::endl;
1970  CatchPrint();
1971  if (TryStt < 0) {
1972  vpERROR_TRACE ("Cannot close the gripper");
1974  "Cannot close the gripper.");
1975  }
1976 }
1977 
1991 void
1993 {
1994  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1995 }
2007 void
2009 {
2011 }
2012 
2033 void
2035  vpColVector &displacement)
2036 {
2037  displacement.resize (6);
2038  displacement = 0;
2039 
2040  double q[6];
2041  vpColVector q_cur(6);
2042  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2043 
2044  InitTry;
2045 
2046  // Get the current joint position
2047  Try( PrimitiveACQ_POS_Afma6(q) );
2048  for (unsigned int i=0; i < njoint; i ++) {
2049  q_cur[i] = q[i];
2050  }
2051 
2052  // Compute the camera pose in the reference frame
2053  fMc_cur = get_fMc(q_cur);
2054 
2055  if ( ! first_time_getdis ) {
2056  switch (frame) {
2057  case vpRobot::CAMERA_FRAME: {
2058  // Compute the camera dispacement from the previous pose
2059  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2060 
2062  vpRotationMatrix R;
2063  c_prevMc_cur.extract(t);
2064  c_prevMc_cur.extract(R);
2065 
2066  vpRxyzVector rxyz;
2067  rxyz.buildFrom(R);
2068 
2069  for (unsigned int i=0; i<3; i++) {
2070  displacement[i] = t[i];
2071  displacement[i+3] = rxyz[i];
2072  }
2073  break ;
2074  }
2075 
2076  case vpRobot::ARTICULAR_FRAME: {
2077  displacement = q_cur - q_prev_getdis;
2078  break ;
2079  }
2080 
2081  case vpRobot::REFERENCE_FRAME: {
2082  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2083  return;
2084  break ;
2085  }
2086 
2087  case vpRobot::MIXT_FRAME: {
2088  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2089  return;
2090  break ;
2091  }
2092  }
2093  }
2094  else {
2095  first_time_getdis = false;
2096  }
2097 
2098  // Memorize the joint position for the next call
2099  q_prev_getdis = q_cur;
2100 
2101  // Memorize the pose for the next call
2102  fMc_prev_getdis = fMc_cur;
2103 
2104  CatchPrint();
2105  if (TryStt < 0) {
2106  vpERROR_TRACE ("Cannot get velocity.");
2108  "Cannot get velocity.");
2109  }
2110 }
2111 
2122 bool
2124 {
2125  Int32 axisInJoint[njoint];
2126  bool status = true;
2127  jointsStatus.resize(6);
2128  InitTry;
2129 
2130  Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
2131 NULL));
2132  for (unsigned int i=0; i < njoint; i ++) {
2133  if (axisInJoint[i]){
2134  std::cout << "\nWarning: Velocity control stopped: axis "
2135  << i+1 << " on joint limit!" <<std::endl;
2136  jointsStatus[i] = axisInJoint[i];
2137  status = false;
2138  }
2139  else{
2140  jointsStatus[i] = 0;
2141  }
2142  }
2143 
2144  Catch();
2145  if (TryStt < 0) {
2146  vpERROR_TRACE ("Cannot check joint limits.");
2148  "Cannot check joint limits.");
2149  }
2150 
2151  return status;
2152 
2153 }
2154 /*
2155  * Local variables:
2156  * c-basic-offset: 2
2157  * End:
2158  */
2159 #endif
2160 
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:69
static vpColVector inverse(const vpHomogeneousMatrix &M)
bool getPowerState()
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpRxyzVector _erc
Definition: vpAfma6.h:188
bool checkJointLimits(vpColVector &jointsStatus)
vpTranslationVector _etc
Definition: vpAfma6.h:187
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:174
void get_eJe(vpMatrix &_eJe)
void getCameraDisplacement(vpColVector &displacement)
Error that can be emited by the vpRobot class and its derivates.
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:379
static bool savePosFile(const char *filename, const vpColVector &q)
vpHomogeneousMatrix get_fMc(const vpColVector &q)
Definition: vpAfma6.cpp:729
virtual vpRobotStateType getRobotState(void)
Definition: vpRobot.h:148
double _coupl_56
Definition: vpAfma6.h:182
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:208
Initialize the position controller.
Definition: vpRobot.h:71
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:108
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:114
void getArticularDisplacement(vpColVector &displacement)
vpControlFrameType
Definition: vpRobot.h:83
double getPositioningVelocity(void)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:233
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual ~vpRobotAfma6(void)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setPositioningVelocity(const double velocity)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:152
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:172
The vpRotationMatrix considers the particular case of a rotation matrix.
void init(void)
double * data
address of the first element of the data array
Definition: vpMatrix.h:116
void get_cMe(vpHomogeneousMatrix &cMe)
Definition: vpAfma6.cpp:845
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:117
Initialize the velocity controller.
Definition: vpRobot.h:70
void get_eJe(const vpColVector &q, vpMatrix &eJe)
Definition: vpAfma6.cpp:883
vpRobotStateType
Definition: vpRobot.h:66
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_fJe(vpMatrix &_fJe)
void extract(vpRotationMatrix &R) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void get_cVe(vpVelocityTwistMatrix &_cVe)
static double rad(double deg)
Definition: vpMath.h:100
void get_cMe(vpHomogeneousMatrix &_cMe)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true)
Definition: vpAfma6.cpp:555
double _long_56
Definition: vpAfma6.h:183
void move(const char *filename)
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
void get_fJe(const vpColVector &q, vpMatrix &fJe)
Definition: vpAfma6.cpp:950
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
void init(void)
Definition: vpAfma6.cpp:196
static double measureTimeSecond()
Definition: vpTime.cpp:225
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double _joint_max[6]
Definition: vpAfma6.h:184
static bool readPosFile(const char *filename, vpColVector &q)
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:188
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
double _joint_min[6]
Definition: vpAfma6.h:185
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:262
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94