Visual Servoing Platform  version 3.2.1 under development (2019-10-23) under development (2019-10-23)
vpRobotAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Afma6 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_AFMA6
42 
43 #include <signal.h>
44 #include <stdlib.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpRotationMatrix.h>
52 #include <visp3/core/vpThetaUVector.h>
53 #include <visp3/core/vpVelocityTwistMatrix.h>
54 #include <visp3/robot/vpRobotAfma6.h>
55 #include <visp3/robot/vpRobotException.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 (" << signo << "): " << (char)7;
86  switch (signo) {
87  case SIGINT:
88  std::cout << "SIGINT (stop by ^C) " << std::endl;
89  break;
90  case SIGBUS:
91  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
92  break;
93  case SIGSEGV:
94  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
95  break;
96  case SIGKILL:
97  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
98  break;
99  case SIGQUIT:
100  std::cout << "SIGQUIT " << std::endl;
101  break;
102  default:
103  std::cout << signo << std::endl;
104  }
105  // std::cout << "Emergency stop called\n";
106  // PrimitiveESTOP_Afma6();
107  PrimitiveSTOP_Afma6();
108  std::cout << "Robot was stopped\n";
109 
110  // Free allocated resources
111  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
112 
113  fprintf(stdout, "Application ");
114  fflush(stdout);
115  kill(getpid(), SIGKILL);
116  exit(1);
117 }
118 
119 /* ---------------------------------------------------------------------- */
120 /* --- CONSTRUCTOR ------------------------------------------------------ */
121 /* ---------------------------------------------------------------------- */
122 
159 vpRobotAfma6::vpRobotAfma6(bool verbose) : vpAfma6(), 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  setVerbose(verbose);
188  if (verbose_)
189  std::cout << "Open communication with MotionBlox.\n";
190  try {
191  this->init();
193  } catch (...) {
194  // vpERROR_TRACE("Error caught") ;
195  throw;
196  }
197  positioningVelocity = defaultPositioningVelocity;
198 
199  vpRobotAfma6::robotAlreadyCreated = true;
200 
201  return;
202 }
203 
204 /* ------------------------------------------------------------------------ */
205 /* --- INITIALISATION ----------------------------------------------------- */
206 /* ------------------------------------------------------------------------ */
207 
228 {
229  InitTry;
230 
231  // Initialise private variables used to compute the measured velocities
232  q_prev_getvel.resize(6);
233  q_prev_getvel = 0;
234  time_prev_getvel = 0;
235  first_time_getvel = true;
236 
237  // Initialise private variables used to compute the measured displacement
238  q_prev_getdis.resize(6);
239  q_prev_getdis = 0;
240  first_time_getdis = true;
241 
242  // Initialize the firewire connection
243  Try(InitializeConnection(verbose_));
244 
245  // Connect to the servoboard using the servo board GUID
246  Try(InitializeNode_Afma6());
247 
248  Try(PrimitiveRESET_Afma6());
249 
250  // Update the eMc matrix in the low level controller
252 
253  // Look if the power is on or off
254  UInt32 HIPowerStatus;
255  UInt32 EStopStatus;
256  Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
257  CAL_Wait(0.1);
258 
259  // Print the robot status
260  if (verbose_) {
261  std::cout << "Robot status: ";
262  switch (EStopStatus) {
263  case ESTOP_AUTO:
264  case ESTOP_MANUAL:
265  if (HIPowerStatus == 0)
266  std::cout << "Power is OFF" << std::endl;
267  else
268  std::cout << "Power is ON" << std::endl;
269  break;
270  case ESTOP_ACTIVATED:
271  std::cout << "Emergency stop is activated" << std::endl;
272  break;
273  default:
274  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
275  std::cout << "You have to call Adept for maintenance..." << std::endl;
276  // Free allocated resources
277  }
278  std::cout << std::endl;
279  }
280  // get real joint min/max from the MotionBlox
281  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
282  // for (unsigned int i=0; i < njoint; i++) {
283  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
284  // _joint_max[i]);
285  // }
286 
287  // If an error occur in the low level controller, goto here
288  // CatchPrint();
289  Catch();
290 
291  // Test if an error occurs
292  if (TryStt == -20001)
293  printf("No connection detected. Check if the robot is powered on \n"
294  "and if the firewire link exist between the MotionBlox and this "
295  "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 resources
303  ShutDownConnection();
304 
305  std::cout << "Cannot open connection with the motionblox..." << std::endl;
306  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
307  }
308  return;
309 }
310 
348 {
349  InitTry;
350  // Read the robot constants from files
351  // - joint [min,max], coupl_56, long_56
352  // - camera extrinsic parameters relative to eMc
353  vpAfma6::init(tool, projModel);
354 
355  // Set the robot constant (coupl_56, long_56) in the MotionBlox
356  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
357 
358  // Set the camera constant (eMc pose) in the MotionBlox
359  double eMc_pose[6];
360  for (unsigned int i = 0; i < 3; i++) {
361  eMc_pose[i] = _etc[i]; // translation in meters
362  eMc_pose[i + 3] = _erc[i]; // rotation in rad
363  }
364  // Update the eMc pose in the low level controller
365  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
366 
367  // get real joint min/max from the MotionBlox
368  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
369  // for (unsigned int i=0; i < njoint; i++) {
370  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
371  // _joint_max[i]);
372  // }
373 
374  setToolType(tool);
375 
376  CatchPrint();
377  return;
378 }
379 
392 {
393  InitTry;
394  // Set camera extrinsic parameters equal to eMc
395  this->vpAfma6::set_eMc(eMc);
396 
397  // Set the camera constant (eMc pose) in the MotionBlox
398  double eMc_pose[6];
399  for (unsigned int i = 0; i < 3; i++) {
400  eMc_pose[i] = _etc[i]; // translation in meters
401  eMc_pose[i + 3] = _erc[i]; // rotation in rad
402  }
403  // Update the eMc pose in the low level controller
404  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
405 
406  CatchPrint();
407 }
408 
445 {
446  InitTry;
447  // Read the robot constants from files
448  // - joint [min,max], coupl_56, long_56
449  // ans set camera extrinsic parameters equal to eMc
450  vpAfma6::init(tool, eMc);
451 
452  // Set the robot constant (coupl_56, long_56) in the MotionBlox
453  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
454 
455  // Set the camera constant (eMc pose) in the MotionBlox
456  double eMc_pose[6];
457  for (unsigned int i = 0; i < 3; i++) {
458  eMc_pose[i] = _etc[i]; // translation in meters
459  eMc_pose[i + 3] = _erc[i]; // rotation in rad
460  }
461  // Update the eMc pose in the low level controller
462  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
463 
464  // get real joint min/max from the MotionBlox
465  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
466 
467  setToolType(tool);
468 
469  CatchPrint();
470 }
471 
522 void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
523 {
524  InitTry;
525  // Read the robot constants from files
526  // - joint [min,max], coupl_56, long_56
527  // ans set camera extrinsic parameters from file name
528  vpAfma6::init(tool, filename);
529 
530  // Set the robot constant (coupl_56, long_56) in the MotionBlox
531  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
532 
533  // Set the camera constant (eMc pose) in the MotionBlox
534  double eMc_pose[6];
535  for (unsigned int i = 0; i < 3; i++) {
536  eMc_pose[i] = _etc[i]; // translation in meters
537  eMc_pose[i + 3] = _erc[i]; // rotation in rad
538  }
539  // Update the eMc pose in the low level controller
540  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
541 
542  // get real joint min/max from the MotionBlox
543  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
544 
545  setToolType(tool);
546 
547  CatchPrint();
548 }
549 
550 /* ------------------------------------------------------------------------ */
551 /* --- DESTRUCTOR --------------------------------------------------------- */
552 /* ------------------------------------------------------------------------ */
553 
561 {
562  InitTry;
563 
565 
566  // Look if the power is on or off
567  UInt32 HIPowerStatus;
568  Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
569  CAL_Wait(0.1);
570 
571  // if (HIPowerStatus == 1) {
572  // fprintf(stdout, "Power OFF the robot\n");
573  // fflush(stdout);
574 
575  // Try( PrimitivePOWEROFF_Afma6() );
576  // }
577 
578  // Free allocated resources
579  ShutDownConnection();
580 
581  vpRobotAfma6::robotAlreadyCreated = false;
582 
583  CatchPrint();
584  return;
585 }
586 
594 {
595  InitTry;
596 
597  switch (newState) {
598  case vpRobot::STATE_STOP: {
600  Try(PrimitiveSTOP_Afma6());
601  }
602  break;
603  }
606  std::cout << "Change the control mode from velocity to position control.\n";
607  Try(PrimitiveSTOP_Afma6());
608  } else {
609  // std::cout << "Change the control mode from stop to position
610  // control.\n";
611  }
612  this->powerOn();
613  break;
614  }
617  std::cout << "Change the control mode from stop to velocity control.\n";
618  }
619  this->powerOn();
620  break;
621  }
622  default:
623  break;
624  }
625 
626  CatchPrint();
627 
628  return vpRobot::setRobotState(newState);
629 }
630 
631 /* ------------------------------------------------------------------------ */
632 /* --- STOP --------------------------------------------------------------- */
633 /* ------------------------------------------------------------------------ */
634 
643 {
644  InitTry;
645  Try(PrimitiveSTOP_Afma6());
647 
648  CatchPrint();
649  if (TryStt < 0) {
650  vpERROR_TRACE("Cannot stop robot motion");
651  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
652  }
653 }
654 
665 {
666  InitTry;
667 
668  // Look if the power is on or off
669  UInt32 HIPowerStatus;
670  UInt32 EStopStatus;
671  bool firsttime = true;
672  unsigned int nitermax = 10;
673 
674  for (unsigned int i = 0; i < nitermax; i++) {
675  Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
676  if (EStopStatus == ESTOP_AUTO) {
677  break; // exit for loop
678  } else if (EStopStatus == ESTOP_MANUAL) {
679  break; // exit for loop
680  } else if (EStopStatus == ESTOP_ACTIVATED) {
681  if (firsttime) {
682  std::cout << "Emergency stop is activated! \n"
683  << "Check the emergency stop button and push the yellow "
684  "button before continuing."
685  << std::endl;
686  firsttime = false;
687  }
688  fprintf(stdout, "Remaining time %us \r", nitermax - i);
689  fflush(stdout);
690  CAL_Wait(1);
691  } else {
692  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
693  std::cout << "You have to call Adept for maintenance..." << std::endl;
694  // Free allocated resources
695  ShutDownConnection();
696  exit(0);
697  }
698  }
699 
700  if (EStopStatus == ESTOP_ACTIVATED)
701  std::cout << std::endl;
702 
703  if (EStopStatus == ESTOP_ACTIVATED) {
704  std::cout << "Sorry, cannot power on the robot." << std::endl;
705  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot.");
706  }
707 
708  if (HIPowerStatus == 0) {
709  fprintf(stdout, "Power ON the Afma6 robot\n");
710  fflush(stdout);
711 
712  Try(PrimitivePOWERON_Afma6());
713  }
714 
715  CatchPrint();
716  if (TryStt < 0) {
717  vpERROR_TRACE("Cannot power on the robot");
718  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
719  }
720 }
721 
732 {
733  InitTry;
734 
735  // Look if the power is on or off
736  UInt32 HIPowerStatus;
737  Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
738  CAL_Wait(0.1);
739 
740  if (HIPowerStatus == 1) {
741  fprintf(stdout, "Power OFF the Afma6 robot\n");
742  fflush(stdout);
743 
744  Try(PrimitivePOWEROFF_Afma6());
745  }
746 
747  CatchPrint();
748  if (TryStt < 0) {
749  vpERROR_TRACE("Cannot power off the robot");
750  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
751  }
752 }
753 
766 {
767  InitTry;
768  bool status = false;
769  // Look if the power is on or off
770  UInt32 HIPowerStatus;
771  Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
772  CAL_Wait(0.1);
773 
774  if (HIPowerStatus == 1) {
775  status = true;
776  }
777 
778  CatchPrint();
779  if (TryStt < 0) {
780  vpERROR_TRACE("Cannot get the power status");
781  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
782  }
783  return status;
784 }
785 
796 {
798  vpAfma6::get_cMe(cMe);
799 
800  cVe.buildFrom(cMe);
801 }
802 
814 
826 {
827 
828  double position[6];
829  double timestamp;
830 
831  InitTry;
832  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
833  CatchPrint();
834 
835  vpColVector q(6);
836  for (unsigned int i = 0; i < njoint; i++)
837  q[i] = position[i];
838 
839  try {
840  vpAfma6::get_eJe(q, eJe);
841  } catch (...) {
842  vpERROR_TRACE("catch exception ");
843  throw;
844  }
845 }
870 {
871 
872  double position[6];
873  double timestamp;
874 
875  InitTry;
876  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
877  CatchPrint();
878 
879  vpColVector q(6);
880  for (unsigned int i = 0; i < njoint; i++)
881  q[i] = position[i];
882 
883  try {
884  vpAfma6::get_fJe(q, fJe);
885  } catch (...) {
886  vpERROR_TRACE("Error caught");
887  throw;
888  }
889 }
890 
919 void vpRobotAfma6::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
920 
926 double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; }
927 
1004 
1005 {
1006  vpColVector position(6);
1007  vpRxyzVector rxyz;
1008  vpRotationMatrix R;
1009 
1010  R.buildFrom(pose[3], pose[4], pose[5]); // thetau
1011  rxyz.buildFrom(R);
1012 
1013  for (unsigned int i = 0; i < 3; i++) {
1014  position[i] = pose[i];
1015  position[i + 3] = rxyz[i];
1016  }
1017  if (frame == vpRobot::ARTICULAR_FRAME) {
1018  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1019  "Joint frame not implemented for pose positionning.");
1020  }
1021  setPosition(frame, position);
1022 }
1107 {
1108 
1110  vpERROR_TRACE("Robot was not in position-based control\n"
1111  "Modification of the robot state");
1113  }
1114 
1115  double _destination[6];
1116  int error = 0;
1117  double timestamp;
1118 
1119  InitTry;
1120  switch (frame) {
1121  case vpRobot::CAMERA_FRAME: {
1122  double _q[njoint];
1123  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1124 
1125  vpColVector q(njoint);
1126  for (unsigned int i = 0; i < njoint; i++)
1127  q[i] = _q[i];
1128 
1129  // Get fMc from the inverse kinematics
1130  vpHomogeneousMatrix fMc;
1131  vpAfma6::get_fMc(q, fMc);
1132 
1133  // Set cMc from the input position
1134  vpTranslationVector txyz;
1135  vpRxyzVector rxyz;
1136  for (unsigned int i = 0; i < 3; i++) {
1137  txyz[i] = position[i];
1138  rxyz[i] = position[i + 3];
1139  }
1140 
1141  // Compute cMc2
1142  vpRotationMatrix cRc2(rxyz);
1143  vpHomogeneousMatrix cMc2(txyz, cRc2);
1144 
1145  // Compute the new position to reach: fMc*cMc2
1146  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1147 
1148  // Compute the corresponding joint position from the inverse kinematics
1149  bool nearest = true;
1150  int solution = this->getInverseKinematics(fMc2, q, nearest);
1151  if (solution) { // Position is reachable
1152  for (unsigned int i = 0; i < njoint; i++) {
1153  _destination[i] = q[i];
1154  }
1155  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1156  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1157  } else {
1158  // Cartesian position is out of range
1159  error = -1;
1160  }
1161 
1162  break;
1163  }
1164  case vpRobot::ARTICULAR_FRAME: {
1165  for (unsigned int i = 0; i < njoint; i++) {
1166  _destination[i] = position[i];
1167  }
1168  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1169  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1170  break;
1171  }
1172  case vpRobot::REFERENCE_FRAME: {
1173  // Set fMc from the input position
1174  vpTranslationVector txyz;
1175  vpRxyzVector rxyz;
1176  for (unsigned int i = 0; i < 3; i++) {
1177  txyz[i] = position[i];
1178  rxyz[i] = position[i + 3];
1179  }
1180  // Compute fMc from the input position
1181  vpRotationMatrix fRc(rxyz);
1182  vpHomogeneousMatrix fMc(txyz, fRc);
1183 
1184  // Compute the corresponding joint position from the inverse kinematics
1185  vpColVector q(6);
1186  bool nearest = true;
1187  int solution = this->getInverseKinematics(fMc, q, nearest);
1188  if (solution) { // Position is reachable
1189  for (unsigned int i = 0; i < njoint; i++) {
1190  _destination[i] = q[i];
1191  }
1192  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1193  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1194  } else {
1195  // Cartesian position is out of range
1196  error = -1;
1197  }
1198 
1199  break;
1200  }
1201  case vpRobot::MIXT_FRAME: {
1202  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1203  "Mixt frame not implemented.");
1204  }
1206  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1207  "end-effector frame not implemented.");
1208  }
1209  }
1210 
1211  CatchPrint();
1212  if (TryStt == InvalidPosition || TryStt == -1023)
1213  std::cout << " : Position out of range.\n";
1214  else if (TryStt < 0)
1215  std::cout << " : Unknown error (see Fabien).\n";
1216  else if (error == -1)
1217  std::cout << "Position out of range.\n";
1218 
1219  if (TryStt < 0 || error < 0) {
1220  vpERROR_TRACE("Positionning error.");
1221  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1222  }
1223 
1224  return;
1225 }
1226 
1294 void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, const double pos1, const double pos2,
1295  const double pos3, const double pos4, const double pos5, const double pos6)
1296 {
1297  try {
1298  vpColVector position(6);
1299  position[0] = pos1;
1300  position[1] = pos2;
1301  position[2] = pos3;
1302  position[3] = pos4;
1303  position[4] = pos5;
1304  position[5] = pos6;
1305 
1306  setPosition(frame, position);
1307  } catch (...) {
1308  vpERROR_TRACE("Error caught");
1309  throw;
1310  }
1311 }
1312 
1354 void vpRobotAfma6::setPosition(const std::string &filename)
1355 {
1356  vpColVector q;
1357  bool ret;
1358 
1359  ret = this->readPosFile(filename, q);
1360 
1361  if (ret == false) {
1362  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1363  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1364  }
1367 }
1368 
1423 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1424 {
1425 
1426  InitTry;
1427 
1428  position.resize(6);
1429 
1430  switch (frame) {
1431  case vpRobot::CAMERA_FRAME: {
1432  position = 0;
1433  return;
1434  }
1435  case vpRobot::ARTICULAR_FRAME: {
1436  double _q[njoint];
1437  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1438  for (unsigned int i = 0; i < njoint; i++) {
1439  position[i] = _q[i];
1440  }
1441 
1442  return;
1443  }
1444  case vpRobot::REFERENCE_FRAME: {
1445  double _q[njoint];
1446  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1447 
1448  vpColVector q(njoint);
1449  for (unsigned int i = 0; i < njoint; i++)
1450  q[i] = _q[i];
1451 
1452  // Compute fMc
1453  vpHomogeneousMatrix fMc;
1454  vpAfma6::get_fMc(q, fMc);
1455 
1456  // From fMc extract the pose
1457  vpRotationMatrix fRc;
1458  fMc.extract(fRc);
1459  vpRxyzVector rxyz;
1460  rxyz.buildFrom(fRc);
1461 
1462  for (unsigned int i = 0; i < 3; i++) {
1463  position[i] = fMc[i][3]; // translation x,y,z
1464  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1465  }
1466  break;
1467  }
1468  case vpRobot::MIXT_FRAME: {
1469  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1470  "not implemented");
1471  }
1473  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1474  "not implemented");
1475  }
1476  }
1477 
1478  CatchPrint();
1479  if (TryStt < 0) {
1480  vpERROR_TRACE("Cannot get position.");
1481  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1482  }
1483 
1484  return;
1485 }
1491 {
1492  double timestamp;
1493  PrimitiveACQ_TIME_Afma6(&timestamp);
1494  return timestamp;
1495 }
1496 
1508 {
1509  double timestamp;
1510  getPosition(frame, position, timestamp);
1511 }
1512 
1522 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1523 {
1524  vpColVector posRxyz;
1525  // recupere position en Rxyz
1526  this->getPosition(frame, posRxyz, timestamp);
1527  vpRxyzVector RxyzVect;
1528  for (unsigned int j = 0; j < 3; j++)
1529  RxyzVect[j] = posRxyz[j + 3];
1530  // recupere le vecteur thetaU correspondant
1531  vpThetaUVector RtuVect(RxyzVect);
1532 
1533  // remplit le vpPoseVector avec translation et rotation ThetaU
1534  for (unsigned int j = 0; j < 3; j++) {
1535  position[j] = posRxyz[j];
1536  position[j + 3] = RtuVect[j];
1537  }
1538 }
1539 
1551 {
1552  double timestamp;
1553  getPosition(frame, position, timestamp);
1554 }
1555 
1620 {
1622  vpERROR_TRACE("Cannot send a velocity to the robot "
1623  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1625  "Cannot send a velocity to the robot "
1626  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1627  }
1628 
1629  vpColVector vel_max(6);
1630 
1631  for (unsigned int i = 0; i < 3; i++)
1632  vel_max[i] = getMaxTranslationVelocity();
1633  for (unsigned int i = 3; i < 6; i++)
1634  vel_max[i] = getMaxRotationVelocity();
1635 
1636  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1637 
1638  InitTry;
1639 
1640  switch (frame) {
1641  case vpRobot::CAMERA_FRAME: {
1642  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
1643  break;
1644  }
1646  // Tranform in camera frame
1647  vpHomogeneousMatrix cMe;
1648  this->get_cMe(cMe);
1649  vpVelocityTwistMatrix cVe(cMe);
1650  vpColVector v_c = cVe * vel_sat;
1651  // Send velocities in m/s and rad/s
1652  Try(PrimitiveMOVESPEED_CART_Afma6(v_c.data, REPCAM_AFMA6));
1653  break;
1654  }
1655  case vpRobot::ARTICULAR_FRAME: {
1656  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1657  Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
1658  break;
1659  }
1660  case vpRobot::REFERENCE_FRAME: {
1661  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
1662  break;
1663  }
1664  case vpRobot::MIXT_FRAME: {
1665  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
1666  break;
1667  }
1668  default: {
1669  vpERROR_TRACE("Error in spec of vpRobot. "
1670  "Case not taken in account.");
1671  return;
1672  }
1673  }
1674 
1675  Catch();
1676  if (TryStt < 0) {
1677  if (TryStt == VelStopOnJoint) {
1678  Int32 axisInJoint[njoint];
1679  PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1680  for (unsigned int i = 0; i < njoint; i++) {
1681  if (axisInJoint[i])
1682  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1683  }
1684  } else {
1685  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1686  if (TryString != NULL) {
1687  // The statement is in TryString, but we need to check the validity
1688  printf(" Error sentence %s\n", TryString); // Print the TryString
1689  } else {
1690  printf("\n");
1691  }
1692  }
1693  }
1694 
1695  return;
1696 }
1697 
1698 /* ------------------------------------------------------------------------ */
1699 /* --- GET ---------------------------------------------------------------- */
1700 /* ------------------------------------------------------------------------ */
1701 
1751 void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1752 {
1753  velocity.resize(6);
1754  velocity = 0;
1755 
1756  double q[6];
1757  vpColVector q_cur(6);
1758  vpHomogeneousMatrix fMc_cur;
1759  vpHomogeneousMatrix cMc; // camera displacement
1760  double time_cur;
1761 
1762  InitTry;
1763 
1764  // Get the current joint position
1765  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
1766  time_cur = timestamp;
1767  for (unsigned int i = 0; i < njoint; i++) {
1768  q_cur[i] = q[i];
1769  }
1770 
1771  // Get the camera pose from the direct kinematics
1772  vpAfma6::get_fMc(q_cur, fMc_cur);
1773 
1774  if (!first_time_getvel) {
1775 
1776  switch (frame) {
1777  case vpRobot::CAMERA_FRAME: {
1778  // Compute the displacement of the camera since the previous call
1779  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1780 
1781  // Compute the velocity of the camera from this displacement
1782  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1783 
1784  break;
1785  }
1786 
1787  case vpRobot::ARTICULAR_FRAME: {
1788  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1789  break;
1790  }
1791 
1792  case vpRobot::REFERENCE_FRAME: {
1793  // Compute the displacement of the camera since the previous call
1794  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1795 
1796  // Compute the velocity of the camera from this displacement
1797  vpColVector v;
1798  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1799 
1800  // Express this velocity in the reference frame
1801  vpVelocityTwistMatrix fVc(fMc_cur);
1802  velocity = fVc * v;
1803 
1804  break;
1805  }
1806 
1807  case vpRobot::MIXT_FRAME: {
1808  // Compute the displacement of the camera since the previous call
1809  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1810 
1811  // Compute the ThetaU representation for the rotation
1812  vpRotationMatrix cRc;
1813  cMc.extract(cRc);
1814  vpThetaUVector thetaU;
1815  thetaU.buildFrom(cRc);
1816 
1817  for (unsigned int i = 0; i < 3; i++) {
1818  // Compute the translation displacement in the reference frame
1819  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1820  // Update the rotation displacement in the camera frame
1821  velocity[i + 3] = thetaU[i];
1822  }
1823 
1824  // Compute the velocity
1825  velocity /= (time_cur - time_prev_getvel);
1826  break;
1827  }
1828  default: {
1830  "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1831  }
1832  }
1833  } else {
1834  first_time_getvel = false;
1835  }
1836 
1837  // Memorize the camera pose for the next call
1838  fMc_prev_getvel = fMc_cur;
1839 
1840  // Memorize the joint position for the next call
1841  q_prev_getvel = q_cur;
1842 
1843  // Memorize the time associated to the joint position for the next call
1844  time_prev_getvel = time_cur;
1845 
1846  CatchPrint();
1847  if (TryStt < 0) {
1848  vpERROR_TRACE("Cannot get velocity.");
1849  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1850  }
1851 }
1852 
1862 {
1863  double timestamp;
1864  getVelocity(frame, velocity, timestamp);
1865 }
1866 
1909 {
1910  vpColVector velocity;
1911  getVelocity(frame, velocity, timestamp);
1912 
1913  return velocity;
1914 }
1915 
1925 {
1926  vpColVector velocity;
1927  double timestamp;
1928  getVelocity(frame, velocity, timestamp);
1929 
1930  return velocity;
1931 }
1932 
1982 bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
1983 {
1984  std::ifstream fd(filename.c_str(), std::ios::in);
1985 
1986  if (!fd.is_open()) {
1987  return false;
1988  }
1989 
1990  std::string line;
1991  std::string key("R:");
1992  std::string id("#AFMA6 - Position");
1993  bool pos_found = false;
1994  int lineNum = 0;
1995 
1996  q.resize(njoint);
1997 
1998  while (std::getline(fd, line)) {
1999  lineNum++;
2000  if (lineNum == 1) {
2001  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
2002  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
2003  return false;
2004  }
2005  }
2006  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2007  continue;
2008  }
2009  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2010  // check if there are at least njoint values in the line
2011  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2012  if (chain.size() < njoint + 1) // try to split with tab separator
2013  chain = vpIoTools::splitChain(line, std::string("\t"));
2014  if (chain.size() < njoint + 1)
2015  continue;
2016 
2017  std::istringstream ss(line);
2018  std::string key_;
2019  ss >> key_;
2020  for (unsigned int i = 0; i < njoint; i++)
2021  ss >> q[i];
2022  pos_found = true;
2023  break;
2024  }
2025  }
2026 
2027  // converts rotations from degrees into radians
2028  q[3] = vpMath::rad(q[3]);
2029  q[4] = vpMath::rad(q[4]);
2030  q[5] = vpMath::rad(q[5]);
2031 
2032  fd.close();
2033 
2034  if (!pos_found) {
2035  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2036  return false;
2037  }
2038 
2039  return true;
2040 }
2041 
2066 bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2067 {
2068 
2069  FILE *fd;
2070  fd = fopen(filename.c_str(), "w");
2071  if (fd == NULL)
2072  return false;
2073 
2074  fprintf(fd, "\
2075 #AFMA6 - Position - Version 2.01\n\
2076 #\n\
2077 # R: X Y Z A B C\n\
2078 # Joint position: X, Y, Z: translations in meters\n\
2079 # A, B, C: rotations in degrees\n\
2080 #\n\
2081 #\n\n");
2082 
2083  // Save positions in mm and deg
2084  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2085  vpMath::deg(q[5]));
2086 
2087  fclose(fd);
2088  return (true);
2089 }
2090 
2101 void vpRobotAfma6::move(const std::string &filename)
2102 {
2103  vpColVector q;
2104 
2105  this->readPosFile(filename, q);
2107  this->setPositioningVelocity(10);
2109 }
2110 
2123 void vpRobotAfma6::move(const std::string &filename, const double velocity)
2124 {
2125  vpColVector q;
2126 
2127  this->readPosFile(filename, q);
2129  this->setPositioningVelocity(velocity);
2131 }
2132 
2140 {
2141  InitTry;
2142  Try(PrimitiveGripper_Afma6(1));
2143  std::cout << "Open the gripper..." << std::endl;
2144  CatchPrint();
2145  if (TryStt < 0) {
2146  vpERROR_TRACE("Cannot open the gripper");
2147  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2148  }
2149 }
2150 
2159 {
2160  InitTry;
2161  Try(PrimitiveGripper_Afma6(0));
2162  std::cout << "Close the gripper..." << std::endl;
2163  CatchPrint();
2164  if (TryStt < 0) {
2165  vpERROR_TRACE("Cannot close the gripper");
2166  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2167  }
2168 }
2169 
2189 {
2190  displacement.resize(6);
2191  displacement = 0;
2192 
2193  double q[6];
2194  vpColVector q_cur(6);
2195  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2196  double timestamp;
2197 
2198  InitTry;
2199 
2200  // Get the current joint position
2201  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
2202  for (unsigned int i = 0; i < njoint; i++) {
2203  q_cur[i] = q[i];
2204  }
2205 
2206  // Compute the camera pose in the reference frame
2207  fMc_cur = get_fMc(q_cur);
2208 
2209  if (!first_time_getdis) {
2210  switch (frame) {
2211  case vpRobot::CAMERA_FRAME: {
2212  // Compute the camera dispacement from the previous pose
2213  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2214 
2216  vpRotationMatrix R;
2217  c_prevMc_cur.extract(t);
2218  c_prevMc_cur.extract(R);
2219 
2220  vpRxyzVector rxyz;
2221  rxyz.buildFrom(R);
2222 
2223  for (unsigned int i = 0; i < 3; i++) {
2224  displacement[i] = t[i];
2225  displacement[i + 3] = rxyz[i];
2226  }
2227  break;
2228  }
2229 
2230  case vpRobot::ARTICULAR_FRAME: {
2231  displacement = q_cur - q_prev_getdis;
2232  break;
2233  }
2234 
2235  case vpRobot::REFERENCE_FRAME: {
2236  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2237  return;
2238  }
2239 
2240  case vpRobot::MIXT_FRAME: {
2241  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2242  return;
2243  }
2245  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2246  return;
2247  }
2248  }
2249  } else {
2250  first_time_getdis = false;
2251  }
2252 
2253  // Memorize the joint position for the next call
2254  q_prev_getdis = q_cur;
2255 
2256  // Memorize the pose for the next call
2257  fMc_prev_getdis = fMc_cur;
2258 
2259  CatchPrint();
2260  if (TryStt < 0) {
2261  vpERROR_TRACE("Cannot get velocity.");
2262  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2263  }
2264 }
2265 
2277 {
2278  Int32 axisInJoint[njoint];
2279  bool status = true;
2280  jointsStatus.resize(6);
2281  InitTry;
2282 
2283  Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
2284  for (unsigned int i = 0; i < njoint; i++) {
2285  if (axisInJoint[i]) {
2286  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
2287  jointsStatus[i] = axisInJoint[i];
2288  status = false;
2289  } else {
2290  jointsStatus[i] = 0;
2291  }
2292  }
2293 
2294  Catch();
2295  if (TryStt < 0) {
2296  vpERROR_TRACE("Cannot check joint limits.");
2297  throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
2298  }
2299 
2300  return status;
2301 }
2302 
2303 #elif !defined(VISP_BUILD_SHARED_LIBS)
2304 // Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no
2305 // symbols
2306 void dummy_vpRobotAfma6(){};
2307 #endif
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Modelisation of Irisa&#39;s gantry robot named Afma6.
Definition: vpAfma6.h:78
static vpColVector inverse(const vpHomogeneousMatrix &M)
bool getPowerState()
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpRxyzVector _erc
Definition: vpAfma6.h:198
bool checkJointLimits(vpColVector &jointsStatus)
vpTranslationVector _etc
Definition: vpAfma6.h:197
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:185
void get_eJe(vpMatrix &_eJe)
Error that can be emited by the vpRobot class and its derivates.
void set_eMc(const vpHomogeneousMatrix &eMc)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:159
#define vpERROR_TRACE
Definition: vpDebug.h:393
double getTime() const
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double _coupl_56
Definition: vpAfma6.h:192
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:532
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
Initialize the position controller.
Definition: vpRobot.h:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:118
vpHomogeneousMatrix inverse() const
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpControlFrameType
Definition: vpRobot.h:75
void get_cMe(vpHomogeneousMatrix &_cMe) const
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:206
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
double getPositioningVelocity(void)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:707
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:201
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:185
Implementation of a rotation matrix and operations on such kind of matrices.
void init(void)
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:127
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static bool readPosFile(const std::string &filename, vpColVector &q)
static bool savePosFile(const std::string &filename, const vpColVector &q)
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
void move(const std::string &filename)
bool verbose_
Definition: vpRobot.h:116
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
void get_fJe(vpMatrix &_fJe)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:864
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1759
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:934
static double rad(double deg)
Definition: vpMath.h:108
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1119
double _long_56
Definition: vpAfma6.h:193
static double deg(double rad)
Definition: vpMath.h:101
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:820
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void init(void)
Definition: vpAfma6.cpp:153
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double _joint_max[6]
Definition: vpAfma6.h:194
Function not implemented.
Definition: vpException.h:90
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double _joint_min[6]
Definition: vpAfma6.h:195
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:253
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310