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