Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpRobotAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_AFMA6
39 
40 #include <signal.h>
41 #include <stdlib.h>
42 #include <sys/types.h>
43 #include <unistd.h>
44 
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpExponentialMap.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpRotationMatrix.h>
49 #include <visp3/core/vpThetaUVector.h>
50 #include <visp3/core/vpVelocityTwistMatrix.h>
51 #include <visp3/robot/vpRobotAfma6.h>
52 #include <visp3/robot/vpRobotException.h>
53 
54 /* ---------------------------------------------------------------------- */
55 /* --- STATIC ----------------------------------------------------------- */
56 /* ---------------------------------------------------------------------- */
57 
58 bool vpRobotAfma6::robotAlreadyCreated = false;
59 
68 
69 /* ---------------------------------------------------------------------- */
70 /* --- EMERGENCY STOP --------------------------------------------------- */
71 /* ---------------------------------------------------------------------- */
72 
80 void emergencyStopAfma6(int signo)
81 {
82  std::cout << "Stop the Afma6 application by signal (" << signo << "): " << (char)7;
83  switch (signo) {
84  case SIGINT:
85  std::cout << "SIGINT (stop by ^C) " << std::endl;
86  break;
87  case SIGBUS:
88  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
89  break;
90  case SIGSEGV:
91  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
92  break;
93  case SIGKILL:
94  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
95  break;
96  case SIGQUIT:
97  std::cout << "SIGQUIT " << std::endl;
98  break;
99  default:
100  std::cout << signo << std::endl;
101  }
102  // std::cout << "Emergency stop called\n";
103  // PrimitiveESTOP_Afma6();
104  PrimitiveSTOP_Afma6();
105  std::cout << "Robot was stopped\n";
106 
107  // Free allocated resources
108  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109 
110  fprintf(stdout, "Application ");
111  fflush(stdout);
112  kill(getpid(), SIGKILL);
113  exit(1);
114 }
115 
116 /* ---------------------------------------------------------------------- */
117 /* --- CONSTRUCTOR ------------------------------------------------------ */
118 /* ---------------------------------------------------------------------- */
119 
156 vpRobotAfma6::vpRobotAfma6(bool verbose) : vpAfma6(), vpRobot()
157 {
158 
159  /*
160  #define SIGHUP 1 // hangup
161  #define SIGINT 2 // interrupt (rubout)
162  #define SIGQUIT 3 // quit (ASCII FS)
163  #define SIGILL 4 // illegal instruction (not reset when caught)
164  #define SIGTRAP 5 // trace trap (not reset when caught)
165  #define SIGIOT 6 // IOT instruction
166  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
167  #define SIGEMT 7 // EMT instruction
168  #define SIGFPE 8 // floating point exception
169  #define SIGKILL 9 // kill (cannot be caught or ignored)
170  #define SIGBUS 10 // bus error
171  #define SIGSEGV 11 // segmentation violation
172  #define SIGSYS 12 // bad argument to system call
173  #define SIGPIPE 13 // write on a pipe with no one to read it
174  #define SIGALRM 14 // alarm clock
175  #define SIGTERM 15 // software termination signal from kill
176  */
177 
178  signal(SIGINT, emergencyStopAfma6);
179  signal(SIGBUS, emergencyStopAfma6);
180  signal(SIGSEGV, emergencyStopAfma6);
181  signal(SIGKILL, emergencyStopAfma6);
182  signal(SIGQUIT, emergencyStopAfma6);
183 
184  setVerbose(verbose);
185  if (verbose_)
186  std::cout << "Open communication with MotionBlox.\n";
187  try {
188  this->init();
190  }
191  catch (...) {
192  // vpERROR_TRACE("Error caught") ;
193  throw;
194  }
195  positioningVelocity = defaultPositioningVelocity;
196 
197  vpRobotAfma6::robotAlreadyCreated = true;
198 
199  return;
200 }
201 
202 /* ------------------------------------------------------------------------ */
203 /* --- INITIALISATION ----------------------------------------------------- */
204 /* ------------------------------------------------------------------------ */
205 
226 {
227  InitTry;
228 
229  // Initialise private variables used to compute the measured velocities
230  q_prev_getvel.resize(6);
231  q_prev_getvel = 0;
232  time_prev_getvel = 0;
233  first_time_getvel = true;
234 
235  // Initialise private variables used to compute the measured displacement
236  q_prev_getdis.resize(6);
237  q_prev_getdis = 0;
238  first_time_getdis = true;
239 
240  // Initialize the firewire connection
241  Try(InitializeConnection(verbose_));
242 
243  // Connect to the servoboard using the servo board GUID
244  Try(InitializeNode_Afma6());
245 
246  Try(PrimitiveRESET_Afma6());
247 
248  // Update the eMc matrix in the low level controller
250 
251  // Look if the power is on or off
252  UInt32 HIPowerStatus;
253  UInt32 EStopStatus;
254  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
255  CAL_Wait(0.1);
256 
257  // Print the robot status
258  if (verbose_) {
259  std::cout << "Robot status: ";
260  switch (EStopStatus) {
261  case ESTOP_AUTO:
262  case ESTOP_MANUAL:
263  if (HIPowerStatus == 0)
264  std::cout << "Power is OFF" << std::endl;
265  else
266  std::cout << "Power is ON" << std::endl;
267  break;
268  case ESTOP_ACTIVATED:
269  std::cout << "Emergency stop is activated" << std::endl;
270  break;
271  default:
272  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
273  std::cout << "You have to call Adept for maintenance..." << std::endl;
274  // Free allocated resources
275  }
276  std::cout << std::endl;
277  }
278  // get real joint min/max from the MotionBlox
279  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
280  // for (unsigned int i=0; i < njoint; i++) {
281  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
282  // _joint_max[i]);
283  // }
284 
285  // If an error occur in the low level controller, goto here
286  // CatchPrint();
287  Catch();
288 
289  // Test if an error occurs
290  if (TryStt == -20001)
291  printf("No connection detected. Check if the robot is powered on \n"
292  "and if the firewire link exist between the MotionBlox and this "
293  "computer.\n");
294  else if (TryStt == -675)
295  printf(" Timeout enabling power...\n");
296 
297  if (TryStt < 0) {
298  // Power off the robot
299  PrimitivePOWEROFF_Afma6();
300  // Free allocated resources
301  ShutDownConnection();
302 
303  std::cout << "Cannot open connection with the motionblox..." << std::endl;
304  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
305  }
306  return;
307 }
308 
346 {
347  InitTry;
348  // Read the robot constants from files
349  // - joint [min,max], coupl_56, long_56
350  // - camera extrinsic parameters relative to eMc
351  vpAfma6::init(tool, projModel);
352 
353  // Set the robot constant (coupl_56, long_56) in the MotionBlox
354  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
355 
356  // Set the camera constant (eMc pose) in the MotionBlox
357  double eMc_pose[6];
358  for (unsigned int i = 0; i < 3; i++) {
359  eMc_pose[i] = _etc[i]; // translation in meters
360  eMc_pose[i + 3] = _erc[i]; // rotation in rad
361  }
362  // Update the eMc pose in the low level controller
363  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
364 
365  // get real joint min/max from the MotionBlox
366  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
367  // for (unsigned int i=0; i < njoint; i++) {
368  // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
369  // _joint_max[i]);
370  // }
371 
372  setToolType(tool);
373 
374  CatchPrint();
375  return;
376 }
377 
390 {
391  InitTry;
392  // Set camera extrinsic parameters equal to eMc
393  this->vpAfma6::set_eMc(eMc);
394 
395  // Set the camera constant (eMc pose) in the MotionBlox
396  double eMc_pose[6];
397  for (unsigned int i = 0; i < 3; i++) {
398  eMc_pose[i] = _etc[i]; // translation in meters
399  eMc_pose[i + 3] = _erc[i]; // rotation in rad
400  }
401  // Update the eMc pose in the low level controller
402  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
403 
404  CatchPrint();
405 }
406 
443 {
444  InitTry;
445  // Read the robot constants from files
446  // - joint [min,max], coupl_56, long_56
447  // ans set camera extrinsic parameters equal to eMc
448  vpAfma6::init(tool, eMc);
449 
450  // Set the robot constant (coupl_56, long_56) in the MotionBlox
451  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
452 
453  // Set the camera constant (eMc pose) in the MotionBlox
454  double eMc_pose[6];
455  for (unsigned int i = 0; i < 3; i++) {
456  eMc_pose[i] = _etc[i]; // translation in meters
457  eMc_pose[i + 3] = _erc[i]; // rotation in rad
458  }
459  // Update the eMc pose in the low level controller
460  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
461 
462  // get real joint min/max from the MotionBlox
463  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
464 
465  setToolType(tool);
466 
467  CatchPrint();
468 }
469 
520 void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
521 {
522  InitTry;
523  // Read the robot constants from files
524  // - joint [min,max], coupl_56, long_56
525  // ans set camera extrinsic parameters from file name
526  vpAfma6::init(tool, filename);
527 
528  // Set the robot constant (coupl_56, long_56) in the MotionBlox
529  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
530 
531  // Set the camera constant (eMc pose) in the MotionBlox
532  double eMc_pose[6];
533  for (unsigned int i = 0; i < 3; i++) {
534  eMc_pose[i] = _etc[i]; // translation in meters
535  eMc_pose[i + 3] = _erc[i]; // rotation in rad
536  }
537  // Update the eMc pose in the low level controller
538  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
539 
540  // get real joint min/max from the MotionBlox
541  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
542 
543  setToolType(tool);
544 
545  CatchPrint();
546 }
547 
548 /* ------------------------------------------------------------------------ */
549 /* --- DESTRUCTOR --------------------------------------------------------- */
550 /* ------------------------------------------------------------------------ */
551 
559 {
560  InitTry;
561 
563 
564  // Look if the power is on or off
565  UInt32 HIPowerStatus;
566  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
567  CAL_Wait(0.1);
568 
569  // if (HIPowerStatus == 1) {
570  // fprintf(stdout, "Power OFF the robot\n");
571  // fflush(stdout);
572 
573  // Try( PrimitivePOWEROFF_Afma6() );
574  // }
575 
576  // Free allocated resources
577  ShutDownConnection();
578 
579  vpRobotAfma6::robotAlreadyCreated = false;
580 
581  CatchPrint();
582  return;
583 }
584 
592 {
593  InitTry;
594 
595  switch (newState) {
596  case vpRobot::STATE_STOP: {
598  Try(PrimitiveSTOP_Afma6());
599  }
600  break;
601  }
604  std::cout << "Change the control mode from velocity to position control.\n";
605  Try(PrimitiveSTOP_Afma6());
606  }
607  else {
608  // std::cout << "Change the control mode from stop to position
609  // control.\n";
610  }
611  this->powerOn();
612  break;
613  }
616  std::cout << "Change the control mode from stop to velocity control.\n";
617  }
618  this->powerOn();
619  break;
620  }
621  default:
622  break;
623  }
624 
625  CatchPrint();
626 
627  return vpRobot::setRobotState(newState);
628 }
629 
630 /* ------------------------------------------------------------------------ */
631 /* --- STOP --------------------------------------------------------------- */
632 /* ------------------------------------------------------------------------ */
633 
642 {
643  InitTry;
644  Try(PrimitiveSTOP_Afma6());
646 
647  CatchPrint();
648  if (TryStt < 0) {
649  vpERROR_TRACE("Cannot stop robot motion");
650  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
651  }
652 }
653 
664 {
665  InitTry;
666 
667  // Look if the power is on or off
668  UInt32 HIPowerStatus;
669  UInt32 EStopStatus;
670  bool firsttime = true;
671  unsigned int nitermax = 10;
672 
673  for (unsigned int i = 0; i < nitermax; i++) {
674  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
675  if (EStopStatus == ESTOP_AUTO) {
676  break; // exit for loop
677  }
678  else if (EStopStatus == ESTOP_MANUAL) {
679  break; // exit for loop
680  }
681  else if (EStopStatus == ESTOP_ACTIVATED) {
682  if (firsttime) {
683  std::cout << "Emergency stop is activated! \n"
684  << "Check the emergency stop button and push the yellow "
685  "button before continuing."
686  << std::endl;
687  firsttime = false;
688  }
689  fprintf(stdout, "Remaining time %us \r", nitermax - i);
690  fflush(stdout);
691  CAL_Wait(1);
692  }
693  else {
694  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
695  std::cout << "You have to call Adept for maintenance..." << std::endl;
696  // Free allocated resources
697  ShutDownConnection();
698  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
699  }
700  }
701 
702  if (EStopStatus == ESTOP_ACTIVATED)
703  std::cout << std::endl;
704 
705  if (EStopStatus == ESTOP_ACTIVATED) {
706  std::cout << "Sorry, cannot power on the robot." << std::endl;
707  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
708  }
709 
710  if (HIPowerStatus == 0) {
711  fprintf(stdout, "Power ON the Afma6 robot\n");
712  fflush(stdout);
713 
714  Try(PrimitivePOWERON_Afma6());
715  }
716 
717  CatchPrint();
718  if (TryStt < 0) {
719  vpERROR_TRACE("Cannot power on the robot");
720  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
721  }
722 }
723 
734 {
735  InitTry;
736 
737  // Look if the power is on or off
738  UInt32 HIPowerStatus;
739  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
740  CAL_Wait(0.1);
741 
742  if (HIPowerStatus == 1) {
743  fprintf(stdout, "Power OFF the Afma6 robot\n");
744  fflush(stdout);
745 
746  Try(PrimitivePOWEROFF_Afma6());
747  }
748 
749  CatchPrint();
750  if (TryStt < 0) {
751  vpERROR_TRACE("Cannot power off the robot");
752  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
753  }
754 }
755 
768 {
769  InitTry;
770  bool status = false;
771  // Look if the power is on or off
772  UInt32 HIPowerStatus;
773  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
774  CAL_Wait(0.1);
775 
776  if (HIPowerStatus == 1) {
777  status = true;
778  }
779 
780  CatchPrint();
781  if (TryStt < 0) {
782  vpERROR_TRACE("Cannot get the power status");
783  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
784  }
785  return status;
786 }
787 
798 {
800  vpAfma6::get_cMe(cMe);
801 
802  cVe.buildFrom(cMe);
803 }
804 
816 
828 {
829 
830  double position[6];
831  double timestamp;
832 
833  InitTry;
834  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
835  CatchPrint();
836 
837  vpColVector q(6);
838  for (unsigned int i = 0; i < njoint; i++)
839  q[i] = position[i];
840 
841  try {
842  vpAfma6::get_eJe(q, eJe);
843  }
844  catch (...) {
845  vpERROR_TRACE("catch exception ");
846  throw;
847  }
848 }
873 {
874 
875  double position[6];
876  double timestamp;
877 
878  InitTry;
879  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
880  CatchPrint();
881 
882  vpColVector q(6);
883  for (unsigned int i = 0; i < njoint; i++)
884  q[i] = position[i];
885 
886  try {
887  vpAfma6::get_fJe(q, fJe);
888  }
889  catch (...) {
890  vpERROR_TRACE("Error caught");
891  throw;
892  }
893 }
894 
923 void vpRobotAfma6::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
924 
930 double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; }
931 
1007 
1008 {
1009  vpColVector position(6);
1010  vpRxyzVector rxyz;
1011  vpRotationMatrix R;
1012 
1013  R.buildFrom(pose[3], pose[4], pose[5]); // thetau
1014  rxyz.buildFrom(R);
1015 
1016  for (unsigned int i = 0; i < 3; i++) {
1017  position[i] = pose[i];
1018  position[i + 3] = rxyz[i];
1019  }
1020  if (frame == vpRobot::ARTICULAR_FRAME) {
1021  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1022  "Joint frame not implemented for pose positioning.");
1023  }
1024  setPosition(frame, position);
1025 }
1109 {
1110 
1112  vpERROR_TRACE("Robot was not in position-based control\n"
1113  "Modification of the robot state");
1115  }
1116 
1117  double _destination[6];
1118  int error = 0;
1119  double timestamp;
1120 
1121  InitTry;
1122  switch (frame) {
1123  case vpRobot::CAMERA_FRAME: {
1124  double _q[njoint];
1125  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1126 
1127  vpColVector q(njoint);
1128  for (unsigned int i = 0; i < njoint; i++)
1129  q[i] = _q[i];
1130 
1131  // Get fMc from the inverse kinematics
1132  vpHomogeneousMatrix fMc;
1133  vpAfma6::get_fMc(q, fMc);
1134 
1135  // Set cMc from the input position
1136  vpTranslationVector txyz;
1137  vpRxyzVector rxyz;
1138  for (unsigned int i = 0; i < 3; i++) {
1139  txyz[i] = position[i];
1140  rxyz[i] = position[i + 3];
1141  }
1142 
1143  // Compute cMc2
1144  vpRotationMatrix cRc2(rxyz);
1145  vpHomogeneousMatrix cMc2(txyz, cRc2);
1146 
1147  // Compute the new position to reach: fMc*cMc2
1148  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1149 
1150  // Compute the corresponding joint position from the inverse kinematics
1151  bool nearest = true;
1152  int solution = this->getInverseKinematics(fMc2, q, nearest);
1153  if (solution) { // Position is reachable
1154  for (unsigned int i = 0; i < njoint; i++) {
1155  _destination[i] = q[i];
1156  }
1157  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1158  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1159  }
1160  else {
1161  // Cartesian position is out of range
1162  error = -1;
1163  }
1164 
1165  break;
1166  }
1167  case vpRobot::ARTICULAR_FRAME: {
1168  for (unsigned int i = 0; i < njoint; i++) {
1169  _destination[i] = position[i];
1170  }
1171  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1172  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1173  break;
1174  }
1175  case vpRobot::REFERENCE_FRAME: {
1176  // Set fMc from the input position
1177  vpTranslationVector txyz;
1178  vpRxyzVector rxyz;
1179  for (unsigned int i = 0; i < 3; i++) {
1180  txyz[i] = position[i];
1181  rxyz[i] = position[i + 3];
1182  }
1183  // Compute fMc from the input position
1184  vpRotationMatrix fRc(rxyz);
1185  vpHomogeneousMatrix fMc(txyz, fRc);
1186 
1187  // Compute the corresponding joint position from the inverse kinematics
1188  vpColVector q(6);
1189  bool nearest = true;
1190  int solution = this->getInverseKinematics(fMc, q, nearest);
1191  if (solution) { // Position is reachable
1192  for (unsigned int i = 0; i < njoint; i++) {
1193  _destination[i] = q[i];
1194  }
1195  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1196  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1197  }
1198  else {
1199  // Cartesian position is out of range
1200  error = -1;
1201  }
1202 
1203  break;
1204  }
1205  case vpRobot::MIXT_FRAME: {
1206  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1207  "Mixt frame not implemented.");
1208  }
1210  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1211  "end-effector frame not implemented.");
1212  }
1213  }
1214 
1215  CatchPrint();
1216  if (TryStt == InvalidPosition || TryStt == -1023)
1217  std::cout << " : Position out of range.\n";
1218  else if (TryStt < 0)
1219  std::cout << " : Unknown error (see Fabien).\n";
1220  else if (error == -1)
1221  std::cout << "Position out of range.\n";
1222 
1223  if (TryStt < 0 || error < 0) {
1224  vpERROR_TRACE("Positioning error.");
1225  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1226  }
1227 
1228  return;
1229 }
1230 
1297 void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1298  double pos4, double pos5, double pos6)
1299 {
1300  try {
1301  vpColVector position(6);
1302  position[0] = pos1;
1303  position[1] = pos2;
1304  position[2] = pos3;
1305  position[3] = pos4;
1306  position[4] = pos5;
1307  position[5] = pos6;
1308 
1309  setPosition(frame, position);
1310  }
1311  catch (...) {
1312  vpERROR_TRACE("Error caught");
1313  throw;
1314  }
1315 }
1316 
1357 void vpRobotAfma6::setPosition(const std::string &filename)
1358 {
1359  vpColVector q;
1360  bool ret;
1361 
1362  ret = this->readPosFile(filename, q);
1363 
1364  if (ret == false) {
1365  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1366  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1367  }
1370 }
1371 
1426 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1427 {
1428 
1429  InitTry;
1430 
1431  position.resize(6);
1432 
1433  switch (frame) {
1434  case vpRobot::CAMERA_FRAME: {
1435  position = 0;
1436  return;
1437  }
1438  case vpRobot::ARTICULAR_FRAME: {
1439  double _q[njoint];
1440  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1441  for (unsigned int i = 0; i < njoint; i++) {
1442  position[i] = _q[i];
1443  }
1444 
1445  return;
1446  }
1447  case vpRobot::REFERENCE_FRAME: {
1448  double _q[njoint];
1449  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1450 
1451  vpColVector q(njoint);
1452  for (unsigned int i = 0; i < njoint; i++)
1453  q[i] = _q[i];
1454 
1455  // Compute fMc
1456  vpHomogeneousMatrix fMc;
1457  vpAfma6::get_fMc(q, fMc);
1458 
1459  // From fMc extract the pose
1460  vpRotationMatrix fRc;
1461  fMc.extract(fRc);
1462  vpRxyzVector rxyz;
1463  rxyz.buildFrom(fRc);
1464 
1465  for (unsigned int i = 0; i < 3; i++) {
1466  position[i] = fMc[i][3]; // translation x,y,z
1467  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1468  }
1469  break;
1470  }
1471  case vpRobot::MIXT_FRAME: {
1472  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1473  "not implemented");
1474  }
1476  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1477  "not implemented");
1478  }
1479  }
1480 
1481  CatchPrint();
1482  if (TryStt < 0) {
1483  vpERROR_TRACE("Cannot get position.");
1484  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1485  }
1486 
1487  return;
1488 }
1494 {
1495  double timestamp;
1496  PrimitiveACQ_TIME_Afma6(&timestamp);
1497  return timestamp;
1498 }
1499 
1511 {
1512  double timestamp;
1513  getPosition(frame, position, timestamp);
1514 }
1515 
1525 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1526 {
1527  vpColVector posRxyz;
1528  // recupere position en Rxyz
1529  this->getPosition(frame, posRxyz, timestamp);
1530  vpRxyzVector RxyzVect;
1531  for (unsigned int j = 0; j < 3; j++)
1532  RxyzVect[j] = posRxyz[j + 3];
1533  // recupere le vecteur thetaU correspondant
1534  vpThetaUVector RtuVect(RxyzVect);
1535 
1536  // remplit le vpPoseVector avec translation et rotation ThetaU
1537  for (unsigned int j = 0; j < 3; j++) {
1538  position[j] = posRxyz[j];
1539  position[j + 3] = RtuVect[j];
1540  }
1541 }
1542 
1554 {
1555  double timestamp;
1556  getPosition(frame, position, timestamp);
1557 }
1558 
1623 {
1625  vpERROR_TRACE("Cannot send a velocity to the robot "
1626  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1628  "Cannot send a velocity to the robot "
1629  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1630  }
1631 
1632  vpColVector vel_max(6);
1633 
1634  for (unsigned int i = 0; i < 3; i++)
1635  vel_max[i] = getMaxTranslationVelocity();
1636  for (unsigned int i = 3; i < 6; i++)
1637  vel_max[i] = getMaxRotationVelocity();
1638 
1639  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1640 
1641  InitTry;
1642 
1643  switch (frame) {
1644  case vpRobot::CAMERA_FRAME: {
1645  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
1646  break;
1647  }
1649  // Tranform in camera frame
1650  vpHomogeneousMatrix cMe;
1651  this->get_cMe(cMe);
1652  vpVelocityTwistMatrix cVe(cMe);
1653  vpColVector v_c = cVe * vel_sat;
1654  // Send velocities in m/s and rad/s
1655  Try(PrimitiveMOVESPEED_CART_Afma6(v_c.data, REPCAM_AFMA6));
1656  break;
1657  }
1658  case vpRobot::ARTICULAR_FRAME: {
1659  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1660  Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
1661  break;
1662  }
1663  case vpRobot::REFERENCE_FRAME: {
1664  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
1665  break;
1666  }
1667  case vpRobot::MIXT_FRAME: {
1668  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
1669  break;
1670  }
1671  default: {
1672  vpERROR_TRACE("Error in spec of vpRobot. "
1673  "Case not taken in account.");
1674  return;
1675  }
1676  }
1677 
1678  Catch();
1679  if (TryStt < 0) {
1680  if (TryStt == VelStopOnJoint) {
1681  Int32 axisInJoint[njoint];
1682  PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
1683  for (unsigned int i = 0; i < njoint; i++) {
1684  if (axisInJoint[i])
1685  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1686  }
1687  }
1688  else {
1689  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1690  if (TryString != nullptr) {
1691  // The statement is in TryString, but we need to check the validity
1692  printf(" Error sentence %s\n", TryString); // Print the TryString
1693  }
1694  else {
1695  printf("\n");
1696  }
1697  }
1698  }
1699 
1700  return;
1701 }
1702 
1703 /* ------------------------------------------------------------------------ */
1704 /* --- GET ---------------------------------------------------------------- */
1705 /* ------------------------------------------------------------------------ */
1706 
1756 void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1757 {
1758  velocity.resize(6);
1759  velocity = 0;
1760 
1761  double q[6];
1762  vpColVector q_cur(6);
1763  vpHomogeneousMatrix fMc_cur;
1764  vpHomogeneousMatrix cMc; // camera displacement
1765  double time_cur;
1766 
1767  InitTry;
1768 
1769  // Get the current joint position
1770  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
1771  time_cur = timestamp;
1772  for (unsigned int i = 0; i < njoint; i++) {
1773  q_cur[i] = q[i];
1774  }
1775 
1776  // Get the camera pose from the direct kinematics
1777  vpAfma6::get_fMc(q_cur, fMc_cur);
1778 
1779  if (!first_time_getvel) {
1780 
1781  switch (frame) {
1782  case vpRobot::CAMERA_FRAME: {
1783  // Compute the displacement of the camera since the previous call
1784  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1785 
1786  // Compute the velocity of the camera from this displacement
1787  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1788 
1789  break;
1790  }
1791 
1792  case vpRobot::ARTICULAR_FRAME: {
1793  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1794  break;
1795  }
1796 
1797  case vpRobot::REFERENCE_FRAME: {
1798  // Compute the displacement of the camera since the previous call
1799  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1800 
1801  // Compute the velocity of the camera from this displacement
1802  vpColVector v;
1803  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1804 
1805  // Express this velocity in the reference frame
1806  vpVelocityTwistMatrix fVc(fMc_cur);
1807  velocity = fVc * v;
1808 
1809  break;
1810  }
1811 
1812  case vpRobot::MIXT_FRAME: {
1813  // Compute the displacement of the camera since the previous call
1814  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1815 
1816  // Compute the ThetaU representation for the rotation
1817  vpRotationMatrix cRc;
1818  cMc.extract(cRc);
1819  vpThetaUVector thetaU;
1820  thetaU.buildFrom(cRc);
1821 
1822  for (unsigned int i = 0; i < 3; i++) {
1823  // Compute the translation displacement in the reference frame
1824  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1825  // Update the rotation displacement in the camera frame
1826  velocity[i + 3] = thetaU[i];
1827  }
1828 
1829  // Compute the velocity
1830  velocity /= (time_cur - time_prev_getvel);
1831  break;
1832  }
1833  default: {
1835  "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1836  }
1837  }
1838  }
1839  else {
1840  first_time_getvel = false;
1841  }
1842 
1843  // Memorize the camera pose for the next call
1844  fMc_prev_getvel = fMc_cur;
1845 
1846  // Memorize the joint position for the next call
1847  q_prev_getvel = q_cur;
1848 
1849  // Memorize the time associated to the joint position for the next call
1850  time_prev_getvel = time_cur;
1851 
1852  CatchPrint();
1853  if (TryStt < 0) {
1854  vpERROR_TRACE("Cannot get velocity.");
1855  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1856  }
1857 }
1858 
1868 {
1869  double timestamp;
1870  getVelocity(frame, velocity, timestamp);
1871 }
1872 
1915 {
1916  vpColVector velocity;
1917  getVelocity(frame, velocity, timestamp);
1918 
1919  return velocity;
1920 }
1921 
1931 {
1932  vpColVector velocity;
1933  double timestamp;
1934  getVelocity(frame, velocity, timestamp);
1935 
1936  return velocity;
1937 }
1938 
1988 bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
1989 {
1990  std::ifstream fd(filename.c_str(), std::ios::in);
1991 
1992  if (!fd.is_open()) {
1993  return false;
1994  }
1995 
1996  std::string line;
1997  std::string key("R:");
1998  std::string id("#AFMA6 - Position");
1999  bool pos_found = false;
2000  int lineNum = 0;
2001 
2002  q.resize(njoint);
2003 
2004  while (std::getline(fd, line)) {
2005  lineNum++;
2006  if (lineNum == 1) {
2007  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
2008  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
2009  return false;
2010  }
2011  }
2012  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2013  continue;
2014  }
2015  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2016  // check if there are at least njoint values in the line
2017  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2018  if (chain.size() < njoint + 1) // try to split with tab separator
2019  chain = vpIoTools::splitChain(line, std::string("\t"));
2020  if (chain.size() < njoint + 1)
2021  continue;
2022 
2023  std::istringstream ss(line);
2024  std::string key_;
2025  ss >> key_;
2026  for (unsigned int i = 0; i < njoint; i++)
2027  ss >> q[i];
2028  pos_found = true;
2029  break;
2030  }
2031  }
2032 
2033  // converts rotations from degrees into radians
2034  q[3] = vpMath::rad(q[3]);
2035  q[4] = vpMath::rad(q[4]);
2036  q[5] = vpMath::rad(q[5]);
2037 
2038  fd.close();
2039 
2040  if (!pos_found) {
2041  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2042  return false;
2043  }
2044 
2045  return true;
2046 }
2047 
2072 bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2073 {
2074 
2075  FILE *fd;
2076  fd = fopen(filename.c_str(), "w");
2077  if (fd == nullptr)
2078  return false;
2079 
2080  fprintf(fd, "\
2081 #AFMA6 - Position - Version 2.01\n\
2082 #\n\
2083 # R: X Y Z A B C\n\
2084 # Joint position: X, Y, Z: translations in meters\n\
2085 # A, B, C: rotations in degrees\n\
2086 #\n\
2087 #\n\n");
2088 
2089  // Save positions in mm and deg
2090  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2091  vpMath::deg(q[5]));
2092 
2093  fclose(fd);
2094  return (true);
2095 }
2096 
2107 void vpRobotAfma6::move(const std::string &filename)
2108 {
2109  vpColVector q;
2110 
2111  this->readPosFile(filename, q);
2113  this->setPositioningVelocity(10);
2115 }
2116 
2129 void vpRobotAfma6::move(const std::string &filename, double velocity)
2130 {
2131  vpColVector q;
2132 
2133  this->readPosFile(filename, q);
2135  this->setPositioningVelocity(velocity);
2137 }
2138 
2146 {
2147  InitTry;
2148  Try(PrimitiveGripper_Afma6(1));
2149  std::cout << "Open the gripper..." << std::endl;
2150  CatchPrint();
2151  if (TryStt < 0) {
2152  vpERROR_TRACE("Cannot open the gripper");
2153  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2154  }
2155 }
2156 
2165 {
2166  InitTry;
2167  Try(PrimitiveGripper_Afma6(0));
2168  std::cout << "Close the gripper..." << std::endl;
2169  CatchPrint();
2170  if (TryStt < 0) {
2171  vpERROR_TRACE("Cannot close the gripper");
2172  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2173  }
2174 }
2175 
2195 {
2196  displacement.resize(6);
2197  displacement = 0;
2198 
2199  double q[6];
2200  vpColVector q_cur(6);
2201  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2202  double timestamp;
2203 
2204  InitTry;
2205 
2206  // Get the current joint position
2207  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
2208  for (unsigned int i = 0; i < njoint; i++) {
2209  q_cur[i] = q[i];
2210  }
2211 
2212  // Compute the camera pose in the reference frame
2213  fMc_cur = get_fMc(q_cur);
2214 
2215  if (!first_time_getdis) {
2216  switch (frame) {
2217  case vpRobot::CAMERA_FRAME: {
2218  // Compute the camera dispacement from the previous pose
2219  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2220 
2222  vpRotationMatrix R;
2223  c_prevMc_cur.extract(t);
2224  c_prevMc_cur.extract(R);
2225 
2226  vpRxyzVector rxyz;
2227  rxyz.buildFrom(R);
2228 
2229  for (unsigned int i = 0; i < 3; i++) {
2230  displacement[i] = t[i];
2231  displacement[i + 3] = rxyz[i];
2232  }
2233  break;
2234  }
2235 
2236  case vpRobot::ARTICULAR_FRAME: {
2237  displacement = q_cur - q_prev_getdis;
2238  break;
2239  }
2240 
2241  case vpRobot::REFERENCE_FRAME: {
2242  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2243  return;
2244  }
2245 
2246  case vpRobot::MIXT_FRAME: {
2247  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2248  return;
2249  }
2251  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2252  return;
2253  }
2254  }
2255  }
2256  else {
2257  first_time_getdis = false;
2258  }
2259 
2260  // Memorize the joint position for the next call
2261  q_prev_getdis = q_cur;
2262 
2263  // Memorize the pose for the next call
2264  fMc_prev_getdis = fMc_cur;
2265 
2266  CatchPrint();
2267  if (TryStt < 0) {
2268  vpERROR_TRACE("Cannot get velocity.");
2269  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2270  }
2271 }
2272 
2284 {
2285  Int32 axisInJoint[njoint];
2286  bool status = true;
2287  jointsStatus.resize(6);
2288  InitTry;
2289 
2290  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr));
2291  for (unsigned int i = 0; i < njoint; i++) {
2292  if (axisInJoint[i]) {
2293  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
2294  jointsStatus[i] = axisInJoint[i];
2295  status = false;
2296  }
2297  else {
2298  jointsStatus[i] = 0;
2299  }
2300  }
2301 
2302  Catch();
2303  if (TryStt < 0) {
2304  vpERROR_TRACE("Cannot check joint limits.");
2305  throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
2306  }
2307 
2308  return status;
2309 }
2310 
2311 #elif !defined(VISP_BUILD_SHARED_LIBS)
2312 // Work around to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no symbols
2313 void dummy_vpRobotAfma6() { };
2314 #endif
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:76
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1192
double _joint_min[6]
Definition: vpAfma6.h:201
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:191
void init(void)
Definition: vpAfma6.cpp:157
vpRxyzVector _erc
Definition: vpAfma6.h:204
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:599
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:893
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:191
double _coupl_56
Definition: vpAfma6.h:198
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:133
double _long_56
Definition: vpAfma6.h:199
vpTranslationVector _etc
Definition: vpAfma6.h:203
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:780
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:212
double _joint_max[6]
Definition: vpAfma6.h:200
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:937
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1007
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:123
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2372
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void get_fJe(vpMatrix &_fJe) vp_override
bool checkJointLimits(vpColVector &jointsStatus)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getTime() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:249
void init(void)
void get_cMe(vpHomogeneousMatrix &_cMe) const
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getPositioningVelocity(void)
void get_eJe(vpMatrix &_eJe) vp_override
void set_eMc(const vpHomogeneousMatrix &eMc)
virtual ~vpRobotAfma6(void)
void move(const std::string &filename)
bool getPowerState()
void setPositioningVelocity(double velocity)
static bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void setVerbose(bool verbose)
Definition: vpRobot.h:168
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:382