ViSP  2.10.0
vpRobotAfma6.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotAfma6.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 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 #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/vpRobotAfma6.h>
57 #include <visp/vpRotationMatrix.h>
58 
59 /* ---------------------------------------------------------------------- */
60 /* --- STATIC ----------------------------------------------------------- */
61 /* ---------------------------------------------------------------------- */
62 
63 bool vpRobotAfma6::robotAlreadyCreated = false;
64 
73 
74 /* ---------------------------------------------------------------------- */
75 /* --- EMERGENCY STOP --------------------------------------------------- */
76 /* ---------------------------------------------------------------------- */
77 
85 void emergencyStopAfma6(int signo)
86 {
87  std::cout << "Stop the Afma6 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_Afma6();
106  PrimitiveSTOP_Afma6();
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 
158 vpRobotAfma6::vpRobotAfma6 (bool verbose)
159  :
160  vpAfma6 (),
161  vpRobot ()
162 {
163 
164  /*
165  #define SIGHUP 1 // hangup
166  #define SIGINT 2 // interrupt (rubout)
167  #define SIGQUIT 3 // quit (ASCII FS)
168  #define SIGILL 4 // illegal instruction (not reset when caught)
169  #define SIGTRAP 5 // trace trap (not reset when caught)
170  #define SIGIOT 6 // IOT instruction
171  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
172  #define SIGEMT 7 // EMT instruction
173  #define SIGFPE 8 // floating point exception
174  #define SIGKILL 9 // kill (cannot be caught or ignored)
175  #define SIGBUS 10 // bus error
176  #define SIGSEGV 11 // segmentation violation
177  #define SIGSYS 12 // bad argument to system call
178  #define SIGPIPE 13 // write on a pipe with no one to read it
179  #define SIGALRM 14 // alarm clock
180  #define SIGTERM 15 // software termination signal from kill
181  */
182 
183  signal(SIGINT, emergencyStopAfma6);
184  signal(SIGBUS, emergencyStopAfma6) ;
185  signal(SIGSEGV, emergencyStopAfma6) ;
186  signal(SIGKILL, emergencyStopAfma6);
187  signal(SIGQUIT, emergencyStopAfma6);
188 
189  setVerbose(verbose);
190  if (verbose_)
191  std::cout << "Open communication with MotionBlox.\n";
192  try {
193  this->init();
195  }
196  catch(...) {
197  // vpERROR_TRACE("Error caught") ;
198  throw ;
199  }
200  positioningVelocity = defaultPositioningVelocity ;
201 
202  vpRobotAfma6::robotAlreadyCreated = true;
203 
204  return ;
205 }
206 
207 
208 /* ------------------------------------------------------------------------ */
209 /* --- INITIALISATION ----------------------------------------------------- */
210 /* ------------------------------------------------------------------------ */
211 
231 void
233 {
234  InitTry;
235 
236  // Initialise private variables used to compute the measured velocities
237  q_prev_getvel.resize(6);
238  q_prev_getvel = 0;
239  time_prev_getvel = 0;
240  first_time_getvel = true;
241 
242  // Initialise private variables used to compute the measured displacement
243  q_prev_getdis.resize(6);
244  q_prev_getdis = 0;
245  first_time_getdis = true;
246 
247  // Initialize the firewire connection
248  Try( InitializeConnection(verbose_) );
249 
250  // Connect to the servoboard using the servo board GUID
251  Try( InitializeNode_Afma6() );
252 
253  Try( PrimitiveRESET_Afma6() );
254 
255  // Update the eMc matrix in the low level controller
257 
258  // Look if the power is on or off
259  UInt32 HIPowerStatus;
260  UInt32 EStopStatus;
261  Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
262  &HIPowerStatus));
263  CAL_Wait(0.1);
264 
265  // Print the robot status
266  if (verbose_) {
267  std::cout << "Robot status: ";
268  switch(EStopStatus) {
269  case ESTOP_AUTO:
270  case ESTOP_MANUAL:
271  if (HIPowerStatus == 0)
272  std::cout << "Power is OFF" << std::endl;
273  else
274  std::cout << "Power is ON" << std::endl;
275  break;
276  case ESTOP_ACTIVATED:
277  std::cout << "Emergency stop is activated" << std::endl;
278  break;
279  default:
280  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
281  std::cout << "You have to call Adept for maintenance..." << std::endl;
282  // Free allocated resources
283  }
284  std::cout << std::endl;
285  }
286  // get real joint min/max from the MotionBlox
287  Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
288 // for (unsigned int i=0; i < njoint; i++) {
289 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
290 // }
291 
292  // If an error occur in the low level controller, goto here
293  //CatchPrint();
294  Catch();
295 
296  // Test if an error occurs
297  if (TryStt == -20001)
298  printf("No connection detected. Check if the robot is powered on \n"
299  "and if the firewire link exist between the MotionBlox and this computer.\n");
300  else if (TryStt == -675)
301  printf(" Timeout enabling power...\n");
302 
303  if (TryStt < 0) {
304  // Power off the robot
305  PrimitivePOWEROFF_Afma6();
306  // Free allocated resources
307  ShutDownConnection();
308 
309  std::cout << "Cannot open connexion with the motionblox..." << std::endl;
311  "Cannot open connexion with the motionblox");
312  }
313  return ;
314 }
315 
352 void
355 {
356 
357  InitTry;
358  // Read the robot constants from files
359  // - joint [min,max], coupl_56, long_56
360  // - camera extrinsic parameters relative to eMc
361  vpAfma6::init(tool, projModel);
362 
363  // Set the robot constant (coupl_56, long_56) in the MotionBlox
364  Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
365 
366  // Set the camera constant (eMc pose) in the MotionBlox
367  double eMc_pose[6];
368  for (unsigned int i=0; i < 3; i ++) {
369  eMc_pose[i] = _etc[i]; // translation in meters
370  eMc_pose[i+3] = _erc[i]; // rotation in rad
371  }
372  // Update the eMc pose in the low level controller
373  Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
374 
375  // get real joint min/max from the MotionBlox
376  Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
377 // for (unsigned int i=0; i < njoint; i++) {
378 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
379 // }
380 
381  setToolType(tool);
382 
383  CatchPrint();
384  return ;
385 }
386 
387 /* ------------------------------------------------------------------------ */
388 /* --- DESTRUCTOR --------------------------------------------------------- */
389 /* ------------------------------------------------------------------------ */
390 
398 {
399  InitTry;
400 
402 
403  // Look if the power is on or off
404  UInt32 HIPowerStatus;
405  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
406  &HIPowerStatus));
407  CAL_Wait(0.1);
408 
409 // if (HIPowerStatus == 1) {
410 // fprintf(stdout, "Power OFF the robot\n");
411 // fflush(stdout);
412 
413 // Try( PrimitivePOWEROFF_Afma6() );
414 // }
415 
416  // Free allocated resources
417  ShutDownConnection();
418 
419  vpRobotAfma6::robotAlreadyCreated = false;
420 
421  CatchPrint();
422  return;
423 }
424 
425 
426 
427 
436 {
437  InitTry;
438 
439  switch (newState) {
440  case vpRobot::STATE_STOP: {
442  Try( PrimitiveSTOP_Afma6() );
443  }
444  break;
445  }
448  std::cout << "Change the control mode from velocity to position control.\n";
449  Try( PrimitiveSTOP_Afma6() );
450  }
451  else {
452  //std::cout << "Change the control mode from stop to position control.\n";
453  }
454  this->powerOn();
455  break;
456  }
459  std::cout << "Change the control mode from stop to velocity control.\n";
460  }
461  this->powerOn();
462  break;
463  }
464  default:
465  break ;
466  }
467 
468  CatchPrint();
469 
470  return vpRobot::setRobotState (newState);
471 }
472 
473 
474 /* ------------------------------------------------------------------------ */
475 /* --- STOP --------------------------------------------------------------- */
476 /* ------------------------------------------------------------------------ */
477 
485 void
487 {
488  InitTry;
489  Try( PrimitiveSTOP_Afma6() );
491 
492  CatchPrint();
493  if (TryStt < 0) {
494  vpERROR_TRACE ("Cannot stop robot motion");
496  "Cannot stop robot motion.");
497  }
498 }
499 
509 void
511 {
512  InitTry;
513 
514  // Look if the power is on or off
515  UInt32 HIPowerStatus;
516  UInt32 EStopStatus;
517  bool firsttime = true;
518  unsigned int nitermax = 10;
519 
520  for (unsigned int i=0; i<nitermax; i++) {
521  Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
522  &HIPowerStatus));
523  if (EStopStatus == ESTOP_AUTO) {
524  break; // exit for loop
525  }
526  else if (EStopStatus == ESTOP_MANUAL) {
527  break; // exit for loop
528  }
529  else if (EStopStatus == ESTOP_ACTIVATED) {
530  if (firsttime) {
531  std::cout << "Emergency stop is activated! \n"
532  << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
533  firsttime = false;
534  }
535  fprintf(stdout, "Remaining time %ds \r", nitermax-i);
536  fflush(stdout);
537  CAL_Wait(1);
538  }
539  else {
540  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
541  std::cout << "You have to call Adept for maintenance..." << std::endl;
542  // Free allocated resources
543  ShutDownConnection();
544  exit(0);
545  }
546  }
547 
548  if (EStopStatus == ESTOP_ACTIVATED)
549  std::cout << std::endl;
550 
551  if (EStopStatus == ESTOP_ACTIVATED) {
552  std::cout << "Sorry, cannot power on the robot." << std::endl;
554  "Cannot power on the robot.");
555  }
556 
557  if (HIPowerStatus == 0) {
558  fprintf(stdout, "Power ON the Afma6 robot\n");
559  fflush(stdout);
560 
561  Try( PrimitivePOWERON_Afma6() );
562  }
563 
564  CatchPrint();
565  if (TryStt < 0) {
566  vpERROR_TRACE ("Cannot power on the robot");
568  "Cannot power off the robot.");
569  }
570 }
571 
581 void
583 {
584  InitTry;
585 
586  // Look if the power is on or off
587  UInt32 HIPowerStatus;
588  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
589  &HIPowerStatus));
590  CAL_Wait(0.1);
591 
592  if (HIPowerStatus == 1) {
593  fprintf(stdout, "Power OFF the Afma6 robot\n");
594  fflush(stdout);
595 
596  Try( PrimitivePOWEROFF_Afma6() );
597  }
598 
599  CatchPrint();
600  if (TryStt < 0) {
601  vpERROR_TRACE ("Cannot power off the robot");
603  "Cannot power off the robot.");
604  }
605 }
606 
618 bool
620 {
621  InitTry;
622  bool status = false;
623  // Look if the power is on or off
624  UInt32 HIPowerStatus;
625  Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
626  &HIPowerStatus));
627  CAL_Wait(0.1);
628 
629  if (HIPowerStatus == 1) {
630  status = true;
631  }
632 
633  CatchPrint();
634  if (TryStt < 0) {
635  vpERROR_TRACE ("Cannot get the power status");
637  "Cannot get the power status.");
638  }
639  return status;
640 }
641 
651 void
653 {
654  vpHomogeneousMatrix cMe ;
655  vpAfma6::get_cMe(cMe) ;
656 
657  cVe.buildFrom(cMe) ;
658 }
659 
670 void
672 {
673  vpAfma6::get_cMe(cMe) ;
674 }
675 
676 
687 void
689 {
690 
691  double position[6];
692  double timestamp;
693 
694  InitTry;
695  Try( PrimitiveACQ_POS_Afma6(position, &timestamp) );
696  CatchPrint();
697 
698  vpColVector q(6);
699  for (unsigned int i=0; i < njoint; i++)
700  q[i] = position[i];
701 
702  try
703  {
704  vpAfma6::get_eJe(q, eJe) ;
705  }
706  catch(...)
707  {
708  vpERROR_TRACE("catch exception ") ;
709  throw ;
710  }
711 }
735  void
737 {
738 
739  double position[6];
740  double timestamp;
741 
742  InitTry;
743  Try( PrimitiveACQ_POS_Afma6(position, &timestamp) );
744  CatchPrint();
745 
746  vpColVector q(6);
747  for (unsigned int i=0; i < njoint; i++)
748  q[i] = position[i];
749 
750  try
751  {
752  vpAfma6::get_fJe(q, fJe) ;
753  }
754  catch(...)
755  {
756  vpERROR_TRACE("Error caught");
757  throw ;
758  }
759 }
760 
789 void
791 {
792  positioningVelocity = velocity;
793 }
794 
800 double
802 {
803  return positioningVelocity;
804 }
805 
880 void
882  const vpPoseVector & pose )
883 
884 {
885  vpColVector position(6);
886  vpRxyzVector rxyz;
888 
889  R.buildFrom(pose[3], pose[4], pose[5]); //thetau
890  rxyz.buildFrom(R);
891 
892  for (unsigned int i=0; i < 3; i++) {
893  position[i] = pose[i];
894  position[i+3] = rxyz[i];
895  }
896  if (frame == vpRobot::ARTICULAR_FRAME) {
898  "Positionning error: "
899  "Joint frame not implemented for pose positionning.");
900  }
901  setPosition (frame, position);
902 }
985 void
987  const vpColVector & position )
988 {
989 
991  {
992  vpERROR_TRACE ("Robot was not in position-based control\n"
993  "Modification of the robot state");
995  }
996 
997  double _destination[6];
998  int error = 0;
999  double timestamp;
1000 
1001  InitTry;
1002  switch(frame) {
1003  case vpRobot::CAMERA_FRAME : {
1004  double _q[njoint];
1005  Try( PrimitiveACQ_POS_Afma6(_q, &timestamp) );
1006 
1007  vpColVector q(njoint);
1008  for (unsigned int i=0; i < njoint; i++)
1009  q[i] = _q[i];
1010 
1011  // Get fMc from the inverse kinematics
1012  vpHomogeneousMatrix fMc;
1013  vpAfma6::get_fMc(q, fMc);
1014 
1015  // Set cMc from the input position
1016  vpTranslationVector txyz;
1017  vpRxyzVector rxyz;
1018  for (unsigned int i=0; i < 3; i++) {
1019  txyz[i] = position[i];
1020  rxyz[i] = position[i+3];
1021  }
1022 
1023  // Compute cMc2
1024  vpRotationMatrix cRc2(rxyz);
1025  vpHomogeneousMatrix cMc2(txyz, cRc2);
1026 
1027  // Compute the new position to reach: fMc*cMc2
1028  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1029 
1030  // Compute the corresponding joint position from the inverse kinematics
1031  bool nearest = true;
1032  int solution = this->getInverseKinematics(fMc2, q, nearest);
1033  if (solution) { // Position is reachable
1034  for (unsigned int i=0; i < njoint; i ++) {
1035  _destination[i] = q[i];
1036  }
1037  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1038  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1039  }
1040  else {
1041  // Cartesian position is out of range
1042  error = -1;
1043  }
1044 
1045  break ;
1046  }
1047  case vpRobot::ARTICULAR_FRAME: {
1048  for (unsigned int i=0; i < njoint; i ++) {
1049  _destination[i] = position[i];
1050  }
1051  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1052  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1053  break ;
1054 
1055  }
1056  case vpRobot::REFERENCE_FRAME: {
1057  // Set fMc from the input position
1058  vpTranslationVector txyz;
1059  vpRxyzVector rxyz;
1060  for (unsigned int i=0; i < 3; i++) {
1061  txyz[i] = position[i];
1062  rxyz[i] = position[i+3];
1063  }
1064  // Compute fMc from the input position
1065  vpRotationMatrix fRc(rxyz);
1066  vpHomogeneousMatrix fMc(txyz, fRc);
1067 
1068  // Compute the corresponding joint position from the inverse kinematics
1069  vpColVector q(6);
1070  bool nearest = true;
1071  int solution = this->getInverseKinematics(fMc, q, nearest);
1072  if (solution) { // Position is reachable
1073  for (unsigned int i=0; i < njoint; i ++) {
1074  _destination[i] = q[i];
1075  }
1076  Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
1077  Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
1078  }
1079  else {
1080  // Cartesian position is out of range
1081  error = -1;
1082  }
1083 
1084  break ;
1085  }
1086  case vpRobot::MIXT_FRAME:
1087  {
1088  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1090  "Positionning error: "
1091  "Mixt frame not implemented.");
1092  break ;
1093  }
1094  }
1095 
1096  CatchPrint();
1097  if (TryStt == InvalidPosition || TryStt == -1023)
1098  std::cout << " : Position out of range.\n";
1099  else if (TryStt < 0)
1100  std::cout << " : Unknown error (see Fabien).\n";
1101  else if (error == -1)
1102  std::cout << "Position out of range.\n";
1103 
1104  if (TryStt < 0 || error < 0) {
1105  vpERROR_TRACE ("Positionning error.");
1107  "Position out of range.");
1108  }
1109 
1110  return ;
1111 }
1112 
1181  const double pos1,
1182  const double pos2,
1183  const double pos3,
1184  const double pos4,
1185  const double pos5,
1186  const double pos6)
1187 {
1188  try{
1189  vpColVector position(6) ;
1190  position[0] = pos1 ;
1191  position[1] = pos2 ;
1192  position[2] = pos3 ;
1193  position[3] = pos4 ;
1194  position[4] = pos5 ;
1195  position[5] = pos6 ;
1196 
1197  setPosition(frame, position) ;
1198  }
1199  catch(...)
1200  {
1201  vpERROR_TRACE("Error caught");
1202  throw ;
1203  }
1204 }
1205 
1247 void vpRobotAfma6::setPosition(const char *filename)
1248 {
1249  vpColVector q;
1250  bool ret;
1251 
1252  ret = this->readPosFile(filename, q);
1253 
1254  if (ret == false) {
1255  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1257  "Bad position in filename.");
1258  }
1261 }
1262 
1316 void
1318  vpColVector & position, double &timestamp)
1319 {
1320 
1321  InitTry;
1322 
1323  position.resize (6);
1324 
1325  switch (frame) {
1326  case vpRobot::CAMERA_FRAME : {
1327  position = 0;
1328  return;
1329  }
1330  case vpRobot::ARTICULAR_FRAME : {
1331  double _q[njoint];
1332  Try( PrimitiveACQ_POS_Afma6(_q, &timestamp) );
1333  for (unsigned int i=0; i < njoint; i ++) {
1334  position[i] = _q[i];
1335  }
1336 
1337  return;
1338  }
1339  case vpRobot::REFERENCE_FRAME : {
1340  double _q[njoint];
1341  Try( PrimitiveACQ_POS_Afma6(_q, &timestamp) );
1342 
1343  vpColVector q(njoint);
1344  for (unsigned int i=0; i < njoint; i++)
1345  q[i] = _q[i];
1346 
1347  // Compute fMc
1348  vpHomogeneousMatrix fMc;
1349  vpAfma6::get_fMc(q, fMc);
1350 
1351  // From fMc extract the pose
1352  vpRotationMatrix fRc;
1353  fMc.extract(fRc);
1354  vpRxyzVector rxyz;
1355  rxyz.buildFrom(fRc);
1356 
1357  for (unsigned int i=0; i < 3; i++) {
1358  position[i] = fMc[i][3]; // translation x,y,z
1359  position[i+3] = rxyz[i]; // Euler rotation x,y,z
1360  }
1361  break ;
1362  }
1363  case vpRobot::MIXT_FRAME: {
1364  vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
1366  "Cannot get position in mixt frame: "
1367  "not implemented");
1368  break ;
1369  }
1370  }
1371 
1372  CatchPrint();
1373  if (TryStt < 0) {
1374  vpERROR_TRACE ("Cannot get position.");
1376  "Cannot get position.");
1377  }
1378 
1379  return;
1380 }
1385 {
1386  double timestamp;
1387  PrimitiveACQ_TIME_Afma6(&timestamp);
1388  return timestamp;
1389 }
1390 
1401  vpColVector &position)
1402 {
1403  double timestamp;
1404  getPosition(frame, position, timestamp);
1405 }
1406 
1415 void
1417  vpPoseVector &position, double &timestamp)
1418 {
1419  vpColVector posRxyz;
1420  //recupere position en Rxyz
1421  this->getPosition(frame, posRxyz, timestamp);
1422  vpRxyzVector RxyzVect;
1423  for(unsigned int j=0;j<3;j++)
1424  RxyzVect[j]=posRxyz[j+3];
1425  //recupere le vecteur thetaU correspondant
1426  vpThetaUVector RtuVect(RxyzVect);
1427 
1428  //remplit le vpPoseVector avec translation et rotation ThetaU
1429  for(unsigned int j=0;j<3;j++)
1430  {
1431  position[j]=posRxyz[j];
1432  position[j+3]=RtuVect[j];
1433  }
1434 }
1435 
1446  vpPoseVector &position)
1447 {
1448  double timestamp;
1449  getPosition(frame, position, timestamp);
1450 }
1451 
1506 void
1508  const vpColVector & vel)
1509 {
1511  {
1512  vpERROR_TRACE ("Cannot send a velocity to the robot "
1513  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1515  "Cannot send a velocity to the robot "
1516  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1517  }
1518 
1519  vpColVector vel_max(6);
1520 
1521  for (unsigned int i=0; i<3; i++)
1522  vel_max[i] = getMaxTranslationVelocity();
1523  for (unsigned int i=3; i<6; i++)
1524  vel_max[i] = getMaxRotationVelocity();
1525 
1526  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1527 
1528  InitTry;
1529 
1530  switch(frame) {
1531  case vpRobot::CAMERA_FRAME : {
1532  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6) );
1533  break ;
1534  }
1535  case vpRobot::ARTICULAR_FRAME : {
1536  //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1537  Try( PrimitiveMOVESPEED_Afma6(vel_sat.data) );
1538  break ;
1539  }
1540  case vpRobot::REFERENCE_FRAME : {
1541  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6) );
1542  break ;
1543  }
1544  case vpRobot::MIXT_FRAME : {
1545  Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6) );
1546  break ;
1547  }
1548  default: {
1549  vpERROR_TRACE ("Error in spec of vpRobot. "
1550  "Case not taken in account.");
1551  return;
1552  }
1553  }
1554 
1555  Catch();
1556  if (TryStt < 0) {
1557  if (TryStt == VelStopOnJoint) {
1558  Int32 axisInJoint[njoint];
1559  PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1560  for (unsigned int i=0; i < njoint; i ++) {
1561  if (axisInJoint[i])
1562  std::cout << "\nWarning: Velocity control stopped: axis "
1563  << i+1 << " on joint limit!" <<std::endl;
1564  }
1565  }
1566  else {
1567  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1568  if (TryString != NULL) {
1569  // The statement is in TryString, but we need to check the validity
1570  printf(" Error sentence %s\n", TryString); // Print the TryString
1571  }
1572  else {
1573  printf("\n");
1574  }
1575  }
1576  }
1577 
1578  return;
1579 }
1580 
1581 
1582 
1583 
1584 
1585 
1586 /* ------------------------------------------------------------------------ */
1587 /* --- GET ---------------------------------------------------------------- */
1588 /* ------------------------------------------------------------------------ */
1589 
1590 
1640 void
1642  vpColVector & velocity, double &timestamp)
1643 {
1644  velocity.resize (6);
1645  velocity = 0;
1646 
1647  double q[6];
1648  vpColVector q_cur(6);
1649  vpHomogeneousMatrix fMc_cur;
1650  vpHomogeneousMatrix cMc; // camera displacement
1651  double time_cur;
1652 
1653  InitTry;
1654 
1655  // Get the current joint position
1656  Try( PrimitiveACQ_POS_Afma6(q, &timestamp) );
1657  time_cur = timestamp;
1658  for (unsigned int i=0; i < njoint; i ++) {
1659  q_cur[i] = q[i];
1660  }
1661 
1662  // Get the camera pose from the direct kinematics
1663  vpAfma6::get_fMc(q_cur, fMc_cur);
1664 
1665  if ( ! first_time_getvel ) {
1666 
1667  switch (frame) {
1668  case vpRobot::CAMERA_FRAME: {
1669  // Compute the displacement of the camera since the previous call
1670  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1671 
1672  // Compute the velocity of the camera from this displacement
1673  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1674 
1675  break ;
1676  }
1677 
1678  case vpRobot::ARTICULAR_FRAME: {
1679  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1680  break ;
1681  }
1682 
1683  case vpRobot::REFERENCE_FRAME: {
1684  // Compute the displacement of the camera since the previous call
1685  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1686 
1687  // Compute the velocity of the camera from this displacement
1688  vpColVector v;
1689  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1690 
1691  // Express this velocity in the reference frame
1692  vpVelocityTwistMatrix fVc(fMc_cur);
1693  velocity = fVc * v;
1694 
1695  break ;
1696  }
1697 
1698  case vpRobot::MIXT_FRAME: {
1699  // Compute the displacement of the camera since the previous call
1700  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1701 
1702  // Compute the ThetaU representation for the rotation
1703  vpRotationMatrix cRc;
1704  cMc.extract(cRc);
1705  vpThetaUVector thetaU;
1706  thetaU.buildFrom(cRc);
1707 
1708  for (unsigned int i=0; i < 3; i++) {
1709  // Compute the translation displacement in the reference frame
1710  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1711  // Update the rotation displacement in the camera frame
1712  velocity[i+3] = thetaU[i];
1713  }
1714 
1715  // Compute the velocity
1716  velocity /= (time_cur - time_prev_getvel);
1717  break ;
1718  }
1719  }
1720  }
1721  else {
1722  first_time_getvel = false;
1723  }
1724 
1725  // Memorize the camera pose for the next call
1726  fMc_prev_getvel = fMc_cur;
1727 
1728  // Memorize the joint position for the next call
1729  q_prev_getvel = q_cur;
1730 
1731  // Memorize the time associated to the joint position for the next call
1732  time_prev_getvel = time_cur;
1733 
1734 
1735  CatchPrint();
1736  if (TryStt < 0) {
1737  vpERROR_TRACE ("Cannot get velocity.");
1739  "Cannot get velocity.");
1740  }
1741 }
1742 
1752  vpColVector & velocity)
1753 {
1754  double timestamp;
1755  getVelocity(frame, velocity, timestamp);
1756 }
1757 
1758 
1802 {
1803  vpColVector velocity;
1804  getVelocity (frame, velocity, timestamp);
1805 
1806  return velocity;
1807 }
1808 
1818 {
1819  vpColVector velocity;
1820  double timestamp;
1821  getVelocity (frame, velocity, timestamp);
1822 
1823  return velocity;
1824 }
1825 
1874 bool
1875 vpRobotAfma6::readPosFile(const char *filename, vpColVector &q)
1876 {
1877 
1878  FILE * fd ;
1879  fd = fopen(filename, "r") ;
1880  if (fd == NULL)
1881  return false;
1882 
1883  char line[FILENAME_MAX];
1884  char dummy[FILENAME_MAX];
1885  char head[] = "R:";
1886  bool sortie = false;
1887 
1888  do {
1889  // Saut des lignes commencant par #
1890  if (fgets (line, FILENAME_MAX, fd) != NULL) {
1891  if ( strncmp (line, "#", 1) != 0) {
1892  // La ligne n'est pas un commentaire
1893  if ( strncmp (line, head, sizeof(head)-1) == 0) {
1894  sortie = true; // Position robot trouvee.
1895  }
1896 // else
1897 // return (false); // fin fichier sans position robot.
1898  }
1899  }
1900  else {
1901  return (false); /* fin fichier */
1902  }
1903 
1904  }
1905  while ( sortie != true );
1906 
1907  // Lecture des positions
1908  q.resize(njoint);
1909  sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
1910  dummy,
1911  &q[0], &q[1], &q[2],
1912  &q[3], &q[4], &q[5]);
1913 
1914  // converts rotations from degrees into radians
1915  for (unsigned int i=3; i < njoint; i ++)
1916  q[i] = vpMath::rad(q[i]) ;
1917 
1918  fclose(fd) ;
1919  return (true);
1920 }
1921 
1946 bool
1947 vpRobotAfma6::savePosFile(const char *filename, const vpColVector &q)
1948 {
1949 
1950  FILE * fd ;
1951  fd = fopen(filename, "w") ;
1952  if (fd == NULL)
1953  return false;
1954 
1955  fprintf(fd, "\
1956 #AFMA6 - Position - Version 2.01\n\
1957 #\n\
1958 # R: X Y Z A B C\n\
1959 # Joint position: X, Y, Z: translations in meters\n\
1960 # A, B, C: rotations in degrees\n\
1961 #\n\
1962 #\n\n");
1963 
1964  // Save positions in mm and deg
1965  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
1966  q[0],
1967  q[1],
1968  q[2],
1969  vpMath::deg(q[3]),
1970  vpMath::deg(q[4]),
1971  vpMath::deg(q[5]));
1972 
1973  fclose(fd) ;
1974  return (true);
1975 }
1976 
1987 void
1988 vpRobotAfma6::move(const char *filename)
1989 {
1990  vpColVector q;
1991 
1992  this->readPosFile(filename, q) ;
1994  this->setPositioningVelocity(10);
1996 }
1997 
2010 void
2011 vpRobotAfma6::move(const char *filename, const double velocity)
2012 {
2013  vpColVector q;
2014 
2015  this->readPosFile(filename, q) ;
2017  this->setPositioningVelocity(velocity);
2019 }
2020 
2027 void
2029 {
2030  InitTry;
2031  Try( PrimitiveGripper_Afma6(1) );
2032  std::cout << "Open the gripper..." << std::endl;
2033  CatchPrint();
2034  if (TryStt < 0) {
2035  vpERROR_TRACE ("Cannot open the gripper");
2037  "Cannot open the gripper.");
2038  }
2039 }
2040 
2048 void
2050 {
2051  InitTry;
2052  Try( PrimitiveGripper_Afma6(0) );
2053  std::cout << "Close the gripper..." << std::endl;
2054  CatchPrint();
2055  if (TryStt < 0) {
2056  vpERROR_TRACE ("Cannot close the gripper");
2058  "Cannot close the gripper.");
2059  }
2060 }
2061 
2075 void
2076 vpRobotAfma6::getCameraDisplacement(vpColVector &displacement)
2077 {
2078  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
2079 }
2091 void
2092 vpRobotAfma6::getArticularDisplacement(vpColVector &displacement)
2093 {
2095 }
2096 
2117 void
2119  vpColVector &displacement)
2120 {
2121  displacement.resize (6);
2122  displacement = 0;
2123 
2124  double q[6];
2125  vpColVector q_cur(6);
2126  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2127  double timestamp;
2128 
2129  InitTry;
2130 
2131  // Get the current joint position
2132  Try( PrimitiveACQ_POS_Afma6(q, &timestamp) );
2133  for (unsigned int i=0; i < njoint; i ++) {
2134  q_cur[i] = q[i];
2135  }
2136 
2137  // Compute the camera pose in the reference frame
2138  fMc_cur = get_fMc(q_cur);
2139 
2140  if ( ! first_time_getdis ) {
2141  switch (frame) {
2142  case vpRobot::CAMERA_FRAME: {
2143  // Compute the camera dispacement from the previous pose
2144  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2145 
2147  vpRotationMatrix R;
2148  c_prevMc_cur.extract(t);
2149  c_prevMc_cur.extract(R);
2150 
2151  vpRxyzVector rxyz;
2152  rxyz.buildFrom(R);
2153 
2154  for (unsigned int i=0; i<3; i++) {
2155  displacement[i] = t[i];
2156  displacement[i+3] = rxyz[i];
2157  }
2158  break ;
2159  }
2160 
2161  case vpRobot::ARTICULAR_FRAME: {
2162  displacement = q_cur - q_prev_getdis;
2163  break ;
2164  }
2165 
2166  case vpRobot::REFERENCE_FRAME: {
2167  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2168  return;
2169  break ;
2170  }
2171 
2172  case vpRobot::MIXT_FRAME: {
2173  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2174  return;
2175  break ;
2176  }
2177  }
2178  }
2179  else {
2180  first_time_getdis = false;
2181  }
2182 
2183  // Memorize the joint position for the next call
2184  q_prev_getdis = q_cur;
2185 
2186  // Memorize the pose for the next call
2187  fMc_prev_getdis = fMc_cur;
2188 
2189  CatchPrint();
2190  if (TryStt < 0) {
2191  vpERROR_TRACE ("Cannot get velocity.");
2193  "Cannot get velocity.");
2194  }
2195 }
2196 
2207 bool
2209 {
2210  Int32 axisInJoint[njoint];
2211  bool status = true;
2212  jointsStatus.resize(6);
2213  InitTry;
2214 
2215  Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
2216 NULL));
2217  for (unsigned int i=0; i < njoint; i ++) {
2218  if (axisInJoint[i]){
2219  std::cout << "\nWarning: Velocity control stopped: axis "
2220  << i+1 << " on joint limit!" <<std::endl;
2221  jointsStatus[i] = axisInJoint[i];
2222  status = false;
2223  }
2224  else{
2225  jointsStatus[i] = 0;
2226  }
2227  }
2228 
2229  Catch();
2230  if (TryStt < 0) {
2231  vpERROR_TRACE ("Cannot check joint limits.");
2233  "Cannot check joint limits.");
2234  }
2235 
2236  return status;
2237 
2238 }
2239 /*
2240  * Local variables:
2241  * c-basic-offset: 2
2242  * End:
2243  */
2244 #endif
2245 
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:98
vpRxyzVector _erc
Definition: vpAfma6.h:190
bool checkJointLimits(vpColVector &jointsStatus)
vpTranslationVector _etc
Definition: vpAfma6.h:189
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:176
void get_eJe(vpMatrix &_eJe)
Error that can be emited by the vpRobot class and its derivates.
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:564
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
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:745
static bool savePosFile(const char *filename, const vpColVector &q)
double _coupl_56
Definition: vpAfma6.h:184
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:254
Initialize the position controller.
Definition: vpRobot.h:71
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:899
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:167
vpControlFrameType
Definition: vpRobot.h:78
double getPositioningVelocity(void)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:279
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:205
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:174
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:118
void get_cMe(vpHomogeneousMatrix &_cMe) const
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:117
Initialize the velocity controller.
Definition: vpRobot.h:70
vpRobotStateType
Definition: vpRobot.h:66
bool verbose_
Definition: vpRobot.h:115
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Build a rotation matrix from an homogeneous matrix.
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:861
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...
static double rad(double deg)
Definition: vpMath.h:100
double _long_56
Definition: vpAfma6.h:185
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
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:140
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:199
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double _joint_max[6]
Definition: vpAfma6.h:186
static bool readPosFile(const char *filename, vpColVector &q)
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:184
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:968
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:187
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:262
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getTime() const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98
void get_cVe(vpVelocityTwistMatrix &_cVe) const