Visual Servoing Platform  version 3.4.0
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(double velocity) { positioningVelocity = velocity; }
920 
926 double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; }
927 
1003 
1004 {
1005  vpColVector position(6);
1006  vpRxyzVector rxyz;
1007  vpRotationMatrix R;
1008 
1009  R.buildFrom(pose[3], pose[4], pose[5]); // thetau
1010  rxyz.buildFrom(R);
1011 
1012  for (unsigned int i = 0; i < 3; i++) {
1013  position[i] = pose[i];
1014  position[i + 3] = rxyz[i];
1015  }
1016  if (frame == vpRobot::ARTICULAR_FRAME) {
1017  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1018  "Joint frame not implemented for pose positionning.");
1019  }
1020  setPosition(frame, position);
1021 }
1105 {
1106 
1108  vpERROR_TRACE("Robot was not in position-based control\n"
1109  "Modification of the robot state");
1111  }
1112 
1113  double _destination[6];
1114  int error = 0;
1115  double timestamp;
1116 
1117  InitTry;
1118  switch (frame) {
1119  case vpRobot::CAMERA_FRAME: {
1120  double _q[njoint];
1121  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1122 
1123  vpColVector q(njoint);
1124  for (unsigned int i = 0; i < njoint; i++)
1125  q[i] = _q[i];
1126 
1127  // Get fMc from the inverse kinematics
1128  vpHomogeneousMatrix fMc;
1129  vpAfma6::get_fMc(q, fMc);
1130 
1131  // Set cMc from the input position
1132  vpTranslationVector txyz;
1133  vpRxyzVector rxyz;
1134  for (unsigned int i = 0; i < 3; i++) {
1135  txyz[i] = position[i];
1136  rxyz[i] = position[i + 3];
1137  }
1138 
1139  // Compute cMc2
1140  vpRotationMatrix cRc2(rxyz);
1141  vpHomogeneousMatrix cMc2(txyz, cRc2);
1142 
1143  // Compute the new position to reach: fMc*cMc2
1144  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1145 
1146  // Compute the corresponding joint position from the inverse kinematics
1147  bool nearest = true;
1148  int solution = this->getInverseKinematics(fMc2, q, nearest);
1149  if (solution) { // Position is reachable
1150  for (unsigned int i = 0; i < njoint; i++) {
1151  _destination[i] = q[i];
1152  }
1153  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1154  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1155  } else {
1156  // Cartesian position is out of range
1157  error = -1;
1158  }
1159 
1160  break;
1161  }
1162  case vpRobot::ARTICULAR_FRAME: {
1163  for (unsigned int i = 0; i < njoint; i++) {
1164  _destination[i] = position[i];
1165  }
1166  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1167  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1168  break;
1169  }
1170  case vpRobot::REFERENCE_FRAME: {
1171  // Set fMc from the input position
1172  vpTranslationVector txyz;
1173  vpRxyzVector rxyz;
1174  for (unsigned int i = 0; i < 3; i++) {
1175  txyz[i] = position[i];
1176  rxyz[i] = position[i + 3];
1177  }
1178  // Compute fMc from the input position
1179  vpRotationMatrix fRc(rxyz);
1180  vpHomogeneousMatrix fMc(txyz, fRc);
1181 
1182  // Compute the corresponding joint position from the inverse kinematics
1183  vpColVector q(6);
1184  bool nearest = true;
1185  int solution = this->getInverseKinematics(fMc, q, nearest);
1186  if (solution) { // Position is reachable
1187  for (unsigned int i = 0; i < njoint; i++) {
1188  _destination[i] = q[i];
1189  }
1190  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1191  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1192  } else {
1193  // Cartesian position is out of range
1194  error = -1;
1195  }
1196 
1197  break;
1198  }
1199  case vpRobot::MIXT_FRAME: {
1200  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1201  "Mixt frame not implemented.");
1202  }
1204  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1205  "end-effector frame not implemented.");
1206  }
1207  }
1208 
1209  CatchPrint();
1210  if (TryStt == InvalidPosition || TryStt == -1023)
1211  std::cout << " : Position out of range.\n";
1212  else if (TryStt < 0)
1213  std::cout << " : Unknown error (see Fabien).\n";
1214  else if (error == -1)
1215  std::cout << "Position out of range.\n";
1216 
1217  if (TryStt < 0 || error < 0) {
1218  vpERROR_TRACE("Positionning error.");
1219  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1220  }
1221 
1222  return;
1223 }
1224 
1291 void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1292  double pos3, double pos4, double pos5, double pos6)
1293 {
1294  try {
1295  vpColVector position(6);
1296  position[0] = pos1;
1297  position[1] = pos2;
1298  position[2] = pos3;
1299  position[3] = pos4;
1300  position[4] = pos5;
1301  position[5] = pos6;
1302 
1303  setPosition(frame, position);
1304  } catch (...) {
1305  vpERROR_TRACE("Error caught");
1306  throw;
1307  }
1308 }
1309 
1350 void vpRobotAfma6::setPosition(const std::string &filename)
1351 {
1352  vpColVector q;
1353  bool ret;
1354 
1355  ret = this->readPosFile(filename, q);
1356 
1357  if (ret == false) {
1358  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1359  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1360  }
1363 }
1364 
1419 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1420 {
1421 
1422  InitTry;
1423 
1424  position.resize(6);
1425 
1426  switch (frame) {
1427  case vpRobot::CAMERA_FRAME: {
1428  position = 0;
1429  return;
1430  }
1431  case vpRobot::ARTICULAR_FRAME: {
1432  double _q[njoint];
1433  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1434  for (unsigned int i = 0; i < njoint; i++) {
1435  position[i] = _q[i];
1436  }
1437 
1438  return;
1439  }
1440  case vpRobot::REFERENCE_FRAME: {
1441  double _q[njoint];
1442  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1443 
1444  vpColVector q(njoint);
1445  for (unsigned int i = 0; i < njoint; i++)
1446  q[i] = _q[i];
1447 
1448  // Compute fMc
1449  vpHomogeneousMatrix fMc;
1450  vpAfma6::get_fMc(q, fMc);
1451 
1452  // From fMc extract the pose
1453  vpRotationMatrix fRc;
1454  fMc.extract(fRc);
1455  vpRxyzVector rxyz;
1456  rxyz.buildFrom(fRc);
1457 
1458  for (unsigned int i = 0; i < 3; i++) {
1459  position[i] = fMc[i][3]; // translation x,y,z
1460  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1461  }
1462  break;
1463  }
1464  case vpRobot::MIXT_FRAME: {
1465  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1466  "not implemented");
1467  }
1469  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1470  "not implemented");
1471  }
1472  }
1473 
1474  CatchPrint();
1475  if (TryStt < 0) {
1476  vpERROR_TRACE("Cannot get position.");
1477  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1478  }
1479 
1480  return;
1481 }
1487 {
1488  double timestamp;
1489  PrimitiveACQ_TIME_Afma6(&timestamp);
1490  return timestamp;
1491 }
1492 
1504 {
1505  double timestamp;
1506  getPosition(frame, position, timestamp);
1507 }
1508 
1518 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1519 {
1520  vpColVector posRxyz;
1521  // recupere position en Rxyz
1522  this->getPosition(frame, posRxyz, timestamp);
1523  vpRxyzVector RxyzVect;
1524  for (unsigned int j = 0; j < 3; j++)
1525  RxyzVect[j] = posRxyz[j + 3];
1526  // recupere le vecteur thetaU correspondant
1527  vpThetaUVector RtuVect(RxyzVect);
1528 
1529  // remplit le vpPoseVector avec translation et rotation ThetaU
1530  for (unsigned int j = 0; j < 3; j++) {
1531  position[j] = posRxyz[j];
1532  position[j + 3] = RtuVect[j];
1533  }
1534 }
1535 
1547 {
1548  double timestamp;
1549  getPosition(frame, position, timestamp);
1550 }
1551 
1616 {
1618  vpERROR_TRACE("Cannot send a velocity to the robot "
1619  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1621  "Cannot send a velocity to the robot "
1622  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1623  }
1624 
1625  vpColVector vel_max(6);
1626 
1627  for (unsigned int i = 0; i < 3; i++)
1628  vel_max[i] = getMaxTranslationVelocity();
1629  for (unsigned int i = 3; i < 6; i++)
1630  vel_max[i] = getMaxRotationVelocity();
1631 
1632  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1633 
1634  InitTry;
1635 
1636  switch (frame) {
1637  case vpRobot::CAMERA_FRAME: {
1638  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
1639  break;
1640  }
1642  // Tranform in camera frame
1643  vpHomogeneousMatrix cMe;
1644  this->get_cMe(cMe);
1645  vpVelocityTwistMatrix cVe(cMe);
1646  vpColVector v_c = cVe * vel_sat;
1647  // Send velocities in m/s and rad/s
1648  Try(PrimitiveMOVESPEED_CART_Afma6(v_c.data, REPCAM_AFMA6));
1649  break;
1650  }
1651  case vpRobot::ARTICULAR_FRAME: {
1652  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1653  Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
1654  break;
1655  }
1656  case vpRobot::REFERENCE_FRAME: {
1657  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
1658  break;
1659  }
1660  case vpRobot::MIXT_FRAME: {
1661  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
1662  break;
1663  }
1664  default: {
1665  vpERROR_TRACE("Error in spec of vpRobot. "
1666  "Case not taken in account.");
1667  return;
1668  }
1669  }
1670 
1671  Catch();
1672  if (TryStt < 0) {
1673  if (TryStt == VelStopOnJoint) {
1674  Int32 axisInJoint[njoint];
1675  PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1676  for (unsigned int i = 0; i < njoint; i++) {
1677  if (axisInJoint[i])
1678  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1679  }
1680  } else {
1681  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1682  if (TryString != NULL) {
1683  // The statement is in TryString, but we need to check the validity
1684  printf(" Error sentence %s\n", TryString); // Print the TryString
1685  } else {
1686  printf("\n");
1687  }
1688  }
1689  }
1690 
1691  return;
1692 }
1693 
1694 /* ------------------------------------------------------------------------ */
1695 /* --- GET ---------------------------------------------------------------- */
1696 /* ------------------------------------------------------------------------ */
1697 
1747 void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1748 {
1749  velocity.resize(6);
1750  velocity = 0;
1751 
1752  double q[6];
1753  vpColVector q_cur(6);
1754  vpHomogeneousMatrix fMc_cur;
1755  vpHomogeneousMatrix cMc; // camera displacement
1756  double time_cur;
1757 
1758  InitTry;
1759 
1760  // Get the current joint position
1761  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
1762  time_cur = timestamp;
1763  for (unsigned int i = 0; i < njoint; i++) {
1764  q_cur[i] = q[i];
1765  }
1766 
1767  // Get the camera pose from the direct kinematics
1768  vpAfma6::get_fMc(q_cur, fMc_cur);
1769 
1770  if (!first_time_getvel) {
1771 
1772  switch (frame) {
1773  case vpRobot::CAMERA_FRAME: {
1774  // Compute the displacement of the camera since the previous call
1775  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1776 
1777  // Compute the velocity of the camera from this displacement
1778  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1779 
1780  break;
1781  }
1782 
1783  case vpRobot::ARTICULAR_FRAME: {
1784  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1785  break;
1786  }
1787 
1788  case vpRobot::REFERENCE_FRAME: {
1789  // Compute the displacement of the camera since the previous call
1790  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1791 
1792  // Compute the velocity of the camera from this displacement
1793  vpColVector v;
1794  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1795 
1796  // Express this velocity in the reference frame
1797  vpVelocityTwistMatrix fVc(fMc_cur);
1798  velocity = fVc * v;
1799 
1800  break;
1801  }
1802 
1803  case vpRobot::MIXT_FRAME: {
1804  // Compute the displacement of the camera since the previous call
1805  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1806 
1807  // Compute the ThetaU representation for the rotation
1808  vpRotationMatrix cRc;
1809  cMc.extract(cRc);
1810  vpThetaUVector thetaU;
1811  thetaU.buildFrom(cRc);
1812 
1813  for (unsigned int i = 0; i < 3; i++) {
1814  // Compute the translation displacement in the reference frame
1815  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1816  // Update the rotation displacement in the camera frame
1817  velocity[i + 3] = thetaU[i];
1818  }
1819 
1820  // Compute the velocity
1821  velocity /= (time_cur - time_prev_getvel);
1822  break;
1823  }
1824  default: {
1826  "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1827  }
1828  }
1829  } else {
1830  first_time_getvel = false;
1831  }
1832 
1833  // Memorize the camera pose for the next call
1834  fMc_prev_getvel = fMc_cur;
1835 
1836  // Memorize the joint position for the next call
1837  q_prev_getvel = q_cur;
1838 
1839  // Memorize the time associated to the joint position for the next call
1840  time_prev_getvel = time_cur;
1841 
1842  CatchPrint();
1843  if (TryStt < 0) {
1844  vpERROR_TRACE("Cannot get velocity.");
1845  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1846  }
1847 }
1848 
1858 {
1859  double timestamp;
1860  getVelocity(frame, velocity, timestamp);
1861 }
1862 
1905 {
1906  vpColVector velocity;
1907  getVelocity(frame, velocity, timestamp);
1908 
1909  return velocity;
1910 }
1911 
1921 {
1922  vpColVector velocity;
1923  double timestamp;
1924  getVelocity(frame, velocity, timestamp);
1925 
1926  return velocity;
1927 }
1928 
1978 bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
1979 {
1980  std::ifstream fd(filename.c_str(), std::ios::in);
1981 
1982  if (!fd.is_open()) {
1983  return false;
1984  }
1985 
1986  std::string line;
1987  std::string key("R:");
1988  std::string id("#AFMA6 - Position");
1989  bool pos_found = false;
1990  int lineNum = 0;
1991 
1992  q.resize(njoint);
1993 
1994  while (std::getline(fd, line)) {
1995  lineNum++;
1996  if (lineNum == 1) {
1997  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1998  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1999  return false;
2000  }
2001  }
2002  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2003  continue;
2004  }
2005  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2006  // check if there are at least njoint values in the line
2007  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2008  if (chain.size() < njoint + 1) // try to split with tab separator
2009  chain = vpIoTools::splitChain(line, std::string("\t"));
2010  if (chain.size() < njoint + 1)
2011  continue;
2012 
2013  std::istringstream ss(line);
2014  std::string key_;
2015  ss >> key_;
2016  for (unsigned int i = 0; i < njoint; i++)
2017  ss >> q[i];
2018  pos_found = true;
2019  break;
2020  }
2021  }
2022 
2023  // converts rotations from degrees into radians
2024  q[3] = vpMath::rad(q[3]);
2025  q[4] = vpMath::rad(q[4]);
2026  q[5] = vpMath::rad(q[5]);
2027 
2028  fd.close();
2029 
2030  if (!pos_found) {
2031  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2032  return false;
2033  }
2034 
2035  return true;
2036 }
2037 
2062 bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2063 {
2064 
2065  FILE *fd;
2066  fd = fopen(filename.c_str(), "w");
2067  if (fd == NULL)
2068  return false;
2069 
2070  fprintf(fd, "\
2071 #AFMA6 - Position - Version 2.01\n\
2072 #\n\
2073 # R: X Y Z A B C\n\
2074 # Joint position: X, Y, Z: translations in meters\n\
2075 # A, B, C: rotations in degrees\n\
2076 #\n\
2077 #\n\n");
2078 
2079  // Save positions in mm and deg
2080  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2081  vpMath::deg(q[5]));
2082 
2083  fclose(fd);
2084  return (true);
2085 }
2086 
2097 void vpRobotAfma6::move(const std::string &filename)
2098 {
2099  vpColVector q;
2100 
2101  this->readPosFile(filename, q);
2103  this->setPositioningVelocity(10);
2105 }
2106 
2119 void vpRobotAfma6::move(const std::string &filename, double velocity)
2120 {
2121  vpColVector q;
2122 
2123  this->readPosFile(filename, q);
2125  this->setPositioningVelocity(velocity);
2127 }
2128 
2136 {
2137  InitTry;
2138  Try(PrimitiveGripper_Afma6(1));
2139  std::cout << "Open the gripper..." << std::endl;
2140  CatchPrint();
2141  if (TryStt < 0) {
2142  vpERROR_TRACE("Cannot open the gripper");
2143  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2144  }
2145 }
2146 
2155 {
2156  InitTry;
2157  Try(PrimitiveGripper_Afma6(0));
2158  std::cout << "Close the gripper..." << std::endl;
2159  CatchPrint();
2160  if (TryStt < 0) {
2161  vpERROR_TRACE("Cannot close the gripper");
2162  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2163  }
2164 }
2165 
2185 {
2186  displacement.resize(6);
2187  displacement = 0;
2188 
2189  double q[6];
2190  vpColVector q_cur(6);
2191  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2192  double timestamp;
2193 
2194  InitTry;
2195 
2196  // Get the current joint position
2197  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
2198  for (unsigned int i = 0; i < njoint; i++) {
2199  q_cur[i] = q[i];
2200  }
2201 
2202  // Compute the camera pose in the reference frame
2203  fMc_cur = get_fMc(q_cur);
2204 
2205  if (!first_time_getdis) {
2206  switch (frame) {
2207  case vpRobot::CAMERA_FRAME: {
2208  // Compute the camera dispacement from the previous pose
2209  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2210 
2212  vpRotationMatrix R;
2213  c_prevMc_cur.extract(t);
2214  c_prevMc_cur.extract(R);
2215 
2216  vpRxyzVector rxyz;
2217  rxyz.buildFrom(R);
2218 
2219  for (unsigned int i = 0; i < 3; i++) {
2220  displacement[i] = t[i];
2221  displacement[i + 3] = rxyz[i];
2222  }
2223  break;
2224  }
2225 
2226  case vpRobot::ARTICULAR_FRAME: {
2227  displacement = q_cur - q_prev_getdis;
2228  break;
2229  }
2230 
2231  case vpRobot::REFERENCE_FRAME: {
2232  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2233  return;
2234  }
2235 
2236  case vpRobot::MIXT_FRAME: {
2237  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2238  return;
2239  }
2241  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2242  return;
2243  }
2244  }
2245  } else {
2246  first_time_getdis = false;
2247  }
2248 
2249  // Memorize the joint position for the next call
2250  q_prev_getdis = q_cur;
2251 
2252  // Memorize the pose for the next call
2253  fMc_prev_getdis = fMc_cur;
2254 
2255  CatchPrint();
2256  if (TryStt < 0) {
2257  vpERROR_TRACE("Cannot get velocity.");
2258  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2259  }
2260 }
2261 
2273 {
2274  Int32 axisInJoint[njoint];
2275  bool status = true;
2276  jointsStatus.resize(6);
2277  InitTry;
2278 
2279  Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
2280  for (unsigned int i = 0; i < njoint; i++) {
2281  if (axisInJoint[i]) {
2282  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
2283  jointsStatus[i] = axisInJoint[i];
2284  status = false;
2285  } else {
2286  jointsStatus[i] = 0;
2287  }
2288  }
2289 
2290  Catch();
2291  if (TryStt < 0) {
2292  vpERROR_TRACE("Cannot check joint limits.");
2293  throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
2294  }
2295 
2296  return status;
2297 }
2298 
2299 #elif !defined(VISP_BUILD_SHARED_LIBS)
2300 // Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no
2301 // symbols
2302 void dummy_vpRobotAfma6(){};
2303 #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:153
vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpRxyzVector _erc
Definition: vpAfma6.h:207
bool checkJointLimits(vpColVector &jointsStatus)
vpTranslationVector _etc
Definition: vpAfma6.h:206
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
void get_eJe(vpMatrix &_eJe)
Error that can be emited by the vpRobot class and its derivates.
void set_eMc(const vpHomogeneousMatrix &eMc)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:159
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
#define vpERROR_TRACE
Definition: vpDebug.h:393
double _coupl_56
Definition: vpAfma6.h:201
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
Initialize the position controller.
Definition: vpRobot.h:67
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
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:126
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
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
double getPositioningVelocity(void)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual ~vpRobotAfma6(void)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
Implementation of a rotation matrix and operations on such kind of matrices.
void init(void)
void get_cMe(vpHomogeneousMatrix &_cMe) const
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
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
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_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
void get_fJe(vpMatrix &_fJe)
void extract(vpRotationMatrix &R) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1676
static double rad(double deg)
Definition: vpMath.h:110
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1187
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
double _long_56
Definition: vpAfma6.h:202
static double deg(double rad)
Definition: vpMath.h:103
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
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void init(void)
Definition: vpAfma6.cpp:160
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double _joint_max[6]
Definition: vpAfma6.h:203
Function not implemented.
Definition: vpException.h:90
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
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:204
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:252
void setPositioningVelocity(double velocity)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getTime() const
void get_cVe(vpVelocityTwistMatrix &_cVe) const