Visual Servoing Platform  version 3.6.1 under development (2024-10-14)
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 BEGIN_VISP_NAMESPACE
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 
448 {
449  InitTry;
450  // Read the robot constants from files
451  // - joint [min,max], coupl_56, long_56
452  // ans set camera extrinsic parameters equal to eMc
453  vpAfma6::init(tool, eMc);
454 
455  // Set the robot constant (coupl_56, long_56) in the MotionBlox
456  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
457 
458  // Set the camera constant (eMc pose) in the MotionBlox
459  double eMc_pose[6];
460  for (unsigned int i = 0; i < 3; i++) {
461  eMc_pose[i] = _etc[i]; // translation in meters
462  eMc_pose[i + 3] = _erc[i]; // rotation in rad
463  }
464  // Update the eMc pose in the low level controller
465  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
466 
467  // get real joint min/max from the MotionBlox
468  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
469 
470  setToolType(tool);
471 
472  CatchPrint();
473 }
474 
529 void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
530 {
531  InitTry;
532  // Read the robot constants from files
533  // - joint [min,max], coupl_56, long_56
534  // ans set camera extrinsic parameters from file name
535  vpAfma6::init(tool, filename);
536 
537  // Set the robot constant (coupl_56, long_56) in the MotionBlox
538  Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
539 
540  // Set the camera constant (eMc pose) in the MotionBlox
541  double eMc_pose[6];
542  for (unsigned int i = 0; i < 3; i++) {
543  eMc_pose[i] = _etc[i]; // translation in meters
544  eMc_pose[i + 3] = _erc[i]; // rotation in rad
545  }
546  // Update the eMc pose in the low level controller
547  Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
548 
549  // get real joint min/max from the MotionBlox
550  Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
551 
552  setToolType(tool);
553 
554  CatchPrint();
555 }
556 
557 /* ------------------------------------------------------------------------ */
558 /* --- DESTRUCTOR --------------------------------------------------------- */
559 /* ------------------------------------------------------------------------ */
560 
568 {
569  InitTry;
570 
572 
573  // Look if the power is on or off
574  UInt32 HIPowerStatus;
575  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
576  CAL_Wait(0.1);
577 
578  // if (HIPowerStatus == 1) {
579  // fprintf(stdout, "Power OFF the robot\n");
580  // fflush(stdout);
581 
582  // Try( PrimitivePOWEROFF_Afma6() );
583  // }
584 
585  // Free allocated resources
586  ShutDownConnection();
587 
588  vpRobotAfma6::robotAlreadyCreated = false;
589 
590  CatchPrint();
591  return;
592 }
593 
601 {
602  InitTry;
603 
604  switch (newState) {
605  case vpRobot::STATE_STOP: {
607  Try(PrimitiveSTOP_Afma6());
608  }
609  break;
610  }
613  std::cout << "Change the control mode from velocity to position control.\n";
614  Try(PrimitiveSTOP_Afma6());
615  }
616  else {
617  // std::cout << "Change the control mode from stop to position
618  // control.\n";
619  }
620  this->powerOn();
621  break;
622  }
625  std::cout << "Change the control mode from stop to velocity control.\n";
626  }
627  this->powerOn();
628  break;
629  }
630  default:
631  break;
632  }
633 
634  CatchPrint();
635 
636  return vpRobot::setRobotState(newState);
637 }
638 
639 /* ------------------------------------------------------------------------ */
640 /* --- STOP --------------------------------------------------------------- */
641 /* ------------------------------------------------------------------------ */
642 
651 {
652  InitTry;
653  Try(PrimitiveSTOP_Afma6());
655 
656  CatchPrint();
657  if (TryStt < 0) {
658  vpERROR_TRACE("Cannot stop robot motion");
659  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
660  }
661 }
662 
673 {
674  InitTry;
675 
676  // Look if the power is on or off
677  UInt32 HIPowerStatus;
678  UInt32 EStopStatus;
679  bool firsttime = true;
680  unsigned int nitermax = 10;
681 
682  for (unsigned int i = 0; i < nitermax; i++) {
683  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
684  if (EStopStatus == ESTOP_AUTO) {
685  break; // exit for loop
686  }
687  else if (EStopStatus == ESTOP_MANUAL) {
688  break; // exit for loop
689  }
690  else if (EStopStatus == ESTOP_ACTIVATED) {
691  if (firsttime) {
692  std::cout << "Emergency stop is activated! \n"
693  << "Check the emergency stop button and push the yellow "
694  "button before continuing."
695  << std::endl;
696  firsttime = false;
697  }
698  fprintf(stdout, "Remaining time %us \r", nitermax - i);
699  fflush(stdout);
700  CAL_Wait(1);
701  }
702  else {
703  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
704  std::cout << "You have to call Adept for maintenance..." << std::endl;
705  // Free allocated resources
706  ShutDownConnection();
707  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
708  }
709  }
710 
711  if (EStopStatus == ESTOP_ACTIVATED)
712  std::cout << std::endl;
713 
714  if (EStopStatus == ESTOP_ACTIVATED) {
715  std::cout << "Sorry, cannot power on the robot." << std::endl;
716  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
717  }
718 
719  if (HIPowerStatus == 0) {
720  fprintf(stdout, "Power ON the Afma6 robot\n");
721  fflush(stdout);
722 
723  Try(PrimitivePOWERON_Afma6());
724  }
725 
726  CatchPrint();
727  if (TryStt < 0) {
728  vpERROR_TRACE("Cannot power on the robot");
729  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
730  }
731 }
732 
743 {
744  InitTry;
745 
746  // Look if the power is on or off
747  UInt32 HIPowerStatus;
748  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
749  CAL_Wait(0.1);
750 
751  if (HIPowerStatus == 1) {
752  fprintf(stdout, "Power OFF the Afma6 robot\n");
753  fflush(stdout);
754 
755  Try(PrimitivePOWEROFF_Afma6());
756  }
757 
758  CatchPrint();
759  if (TryStt < 0) {
760  vpERROR_TRACE("Cannot power off the robot");
761  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
762  }
763 }
764 
777 {
778  InitTry;
779  bool status = false;
780  // Look if the power is on or off
781  UInt32 HIPowerStatus;
782  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
783  CAL_Wait(0.1);
784 
785  if (HIPowerStatus == 1) {
786  status = true;
787  }
788 
789  CatchPrint();
790  if (TryStt < 0) {
791  vpERROR_TRACE("Cannot get the power status");
792  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
793  }
794  return status;
795 }
796 
807 {
809  vpAfma6::get_cMe(cMe);
810 
811  cVe.buildFrom(cMe);
812 }
813 
825 
837 {
838 
839  double position[6];
840  double timestamp;
841 
842  InitTry;
843  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
844  CatchPrint();
845 
846  vpColVector q(6);
847  for (unsigned int i = 0; i < njoint; i++)
848  q[i] = position[i];
849 
850  try {
851  vpAfma6::get_eJe(q, eJe);
852  }
853  catch (...) {
854  vpERROR_TRACE("catch exception ");
855  throw;
856  }
857 }
882 {
883 
884  double position[6];
885  double timestamp;
886 
887  InitTry;
888  Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
889  CatchPrint();
890 
891  vpColVector q(6);
892  for (unsigned int i = 0; i < njoint; i++)
893  q[i] = position[i];
894 
895  try {
896  vpAfma6::get_fJe(q, fJe);
897  }
898  catch (...) {
899  vpERROR_TRACE("Error caught");
900  throw;
901  }
902 }
903 
932 void vpRobotAfma6::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
933 
939 double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; }
940 
1019 
1020 {
1021  vpColVector position(6);
1022  vpRxyzVector rxyz;
1023  vpRotationMatrix R;
1024 
1025  R.buildFrom(pose[3], pose[4], pose[5]); // thetau
1026  rxyz.buildFrom(R);
1027 
1028  for (unsigned int i = 0; i < 3; i++) {
1029  position[i] = pose[i];
1030  position[i + 3] = rxyz[i];
1031  }
1032  if (frame == vpRobot::ARTICULAR_FRAME) {
1033  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1034  "Joint frame not implemented for pose positioning.");
1035  }
1036  setPosition(frame, position);
1037 }
1125 {
1126 
1128  vpERROR_TRACE("Robot was not in position-based control\n"
1129  "Modification of the robot state");
1131  }
1132 
1133  double _destination[6];
1134  int error = 0;
1135  double timestamp;
1136 
1137  InitTry;
1138  switch (frame) {
1139  case vpRobot::CAMERA_FRAME: {
1140  double _q[njoint];
1141  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1142 
1143  vpColVector q(njoint);
1144  for (unsigned int i = 0; i < njoint; i++)
1145  q[i] = _q[i];
1146 
1147  // Get fMc from the inverse kinematics
1148  vpHomogeneousMatrix fMc;
1149  vpAfma6::get_fMc(q, fMc);
1150 
1151  // Set cMc from the input position
1152  vpTranslationVector txyz;
1153  vpRxyzVector rxyz;
1154  for (unsigned int i = 0; i < 3; i++) {
1155  txyz[i] = position[i];
1156  rxyz[i] = position[i + 3];
1157  }
1158 
1159  // Compute cMc2
1160  vpRotationMatrix cRc2(rxyz);
1161  vpHomogeneousMatrix cMc2(txyz, cRc2);
1162 
1163  // Compute the new position to reach: fMc*cMc2
1164  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1165 
1166  // Compute the corresponding joint position from the inverse kinematics
1167  bool nearest = true;
1168  int solution = this->getInverseKinematics(fMc2, q, nearest);
1169  if (solution) { // Position is reachable
1170  for (unsigned int i = 0; i < njoint; i++) {
1171  _destination[i] = q[i];
1172  }
1173  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1174  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1175  }
1176  else {
1177  // Cartesian position is out of range
1178  error = -1;
1179  }
1180 
1181  break;
1182  }
1183  case vpRobot::ARTICULAR_FRAME: {
1184  for (unsigned int i = 0; i < njoint; i++) {
1185  _destination[i] = position[i];
1186  }
1187  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1188  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1189  break;
1190  }
1191  case vpRobot::REFERENCE_FRAME: {
1192  // Set fMc from the input position
1193  vpTranslationVector txyz;
1194  vpRxyzVector rxyz;
1195  for (unsigned int i = 0; i < 3; i++) {
1196  txyz[i] = position[i];
1197  rxyz[i] = position[i + 3];
1198  }
1199  // Compute fMc from the input position
1200  vpRotationMatrix fRc(rxyz);
1201  vpHomogeneousMatrix fMc(txyz, fRc);
1202 
1203  // Compute the corresponding joint position from the inverse kinematics
1204  vpColVector q(6);
1205  bool nearest = true;
1206  int solution = this->getInverseKinematics(fMc, q, nearest);
1207  if (solution) { // Position is reachable
1208  for (unsigned int i = 0; i < njoint; i++) {
1209  _destination[i] = q[i];
1210  }
1211  Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1212  Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1213  }
1214  else {
1215  // Cartesian position is out of range
1216  error = -1;
1217  }
1218 
1219  break;
1220  }
1221  case vpRobot::MIXT_FRAME: {
1222  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1223  "Mixt frame not implemented.");
1224  }
1226  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1227  "end-effector frame not implemented.");
1228  }
1229  }
1230 
1231  CatchPrint();
1232  if (TryStt == InvalidPosition || TryStt == -1023)
1233  std::cout << " : Position out of range.\n";
1234  else if (TryStt < 0)
1235  std::cout << " : Unknown error (see Fabien).\n";
1236  else if (error == -1)
1237  std::cout << "Position out of range.\n";
1238 
1239  if (TryStt < 0 || error < 0) {
1240  vpERROR_TRACE("Positioning error.");
1241  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1242  }
1243 
1244  return;
1245 }
1246 
1317 void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1318  double pos4, double pos5, double pos6)
1319 {
1320  try {
1321  vpColVector position(6);
1322  position[0] = pos1;
1323  position[1] = pos2;
1324  position[2] = pos3;
1325  position[3] = pos4;
1326  position[4] = pos5;
1327  position[5] = pos6;
1328 
1329  setPosition(frame, position);
1330  }
1331  catch (...) {
1332  vpERROR_TRACE("Error caught");
1333  throw;
1334  }
1335 }
1336 
1381 void vpRobotAfma6::setPosition(const std::string &filename)
1382 {
1383  vpColVector q;
1384  bool ret;
1385 
1386  ret = this->readPosFile(filename, q);
1387 
1388  if (ret == false) {
1389  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1390  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1391  }
1394 }
1395 
1450 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1451 {
1452 
1453  InitTry;
1454 
1455  position.resize(6);
1456 
1457  switch (frame) {
1458  case vpRobot::CAMERA_FRAME: {
1459  position = 0;
1460  return;
1461  }
1462  case vpRobot::ARTICULAR_FRAME: {
1463  double _q[njoint];
1464  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1465  for (unsigned int i = 0; i < njoint; i++) {
1466  position[i] = _q[i];
1467  }
1468 
1469  return;
1470  }
1471  case vpRobot::REFERENCE_FRAME: {
1472  double _q[njoint];
1473  Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1474 
1475  vpColVector q(njoint);
1476  for (unsigned int i = 0; i < njoint; i++)
1477  q[i] = _q[i];
1478 
1479  // Compute fMc
1480  vpHomogeneousMatrix fMc;
1481  vpAfma6::get_fMc(q, fMc);
1482 
1483  // From fMc extract the pose
1484  vpRotationMatrix fRc;
1485  fMc.extract(fRc);
1486  vpRxyzVector rxyz;
1487  rxyz.buildFrom(fRc);
1488 
1489  for (unsigned int i = 0; i < 3; i++) {
1490  position[i] = fMc[i][3]; // translation x,y,z
1491  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1492  }
1493  break;
1494  }
1495  case vpRobot::MIXT_FRAME: {
1496  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1497  "not implemented");
1498  }
1500  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1501  "not implemented");
1502  }
1503  }
1504 
1505  CatchPrint();
1506  if (TryStt < 0) {
1507  vpERROR_TRACE("Cannot get position.");
1508  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1509  }
1510 
1511  return;
1512 }
1518 {
1519  double timestamp;
1520  PrimitiveACQ_TIME_Afma6(&timestamp);
1521  return timestamp;
1522 }
1523 
1535 {
1536  double timestamp;
1537  getPosition(frame, position, timestamp);
1538 }
1539 
1549 void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1550 {
1551  vpColVector posRxyz;
1552  // recupere position en Rxyz
1553  this->getPosition(frame, posRxyz, timestamp);
1554  vpRxyzVector RxyzVect;
1555  for (unsigned int j = 0; j < 3; j++)
1556  RxyzVect[j] = posRxyz[j + 3];
1557  // recupere le vecteur thetaU correspondant
1558  vpThetaUVector RtuVect(RxyzVect);
1559 
1560  // remplit le vpPoseVector avec translation et rotation ThetaU
1561  for (unsigned int j = 0; j < 3; j++) {
1562  position[j] = posRxyz[j];
1563  position[j + 3] = RtuVect[j];
1564  }
1565 }
1566 
1578 {
1579  double timestamp;
1580  getPosition(frame, position, timestamp);
1581 }
1582 
1647 {
1649  vpERROR_TRACE("Cannot send a velocity to the robot "
1650  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1652  "Cannot send a velocity to the robot "
1653  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1654  }
1655 
1656  vpColVector vel_max(6);
1657 
1658  for (unsigned int i = 0; i < 3; i++)
1659  vel_max[i] = getMaxTranslationVelocity();
1660  for (unsigned int i = 3; i < 6; i++)
1661  vel_max[i] = getMaxRotationVelocity();
1662 
1663  vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1664 
1665  InitTry;
1666 
1667  switch (frame) {
1668  case vpRobot::CAMERA_FRAME: {
1669  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
1670  break;
1671  }
1673  // Tranform in camera frame
1674  vpHomogeneousMatrix cMe;
1675  this->get_cMe(cMe);
1676  vpVelocityTwistMatrix cVe(cMe);
1677  vpColVector v_c = cVe * vel_sat;
1678  // Send velocities in m/s and rad/s
1679  Try(PrimitiveMOVESPEED_CART_Afma6(v_c.data, REPCAM_AFMA6));
1680  break;
1681  }
1682  case vpRobot::ARTICULAR_FRAME: {
1683  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1684  Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
1685  break;
1686  }
1687  case vpRobot::REFERENCE_FRAME: {
1688  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
1689  break;
1690  }
1691  case vpRobot::MIXT_FRAME: {
1692  Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
1693  break;
1694  }
1695  default: {
1696  vpERROR_TRACE("Error in spec of vpRobot. "
1697  "Case not taken in account.");
1698  return;
1699  }
1700  }
1701 
1702  Catch();
1703  if (TryStt < 0) {
1704  if (TryStt == VelStopOnJoint) {
1705  Int32 axisInJoint[njoint];
1706  PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
1707  for (unsigned int i = 0; i < njoint; i++) {
1708  if (axisInJoint[i])
1709  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1710  }
1711  }
1712  else {
1713  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1714  if (TryString != nullptr) {
1715  // The statement is in TryString, but we need to check the validity
1716  printf(" Error sentence %s\n", TryString); // Print the TryString
1717  }
1718  else {
1719  printf("\n");
1720  }
1721  }
1722  }
1723 
1724  return;
1725 }
1726 
1727 /* ------------------------------------------------------------------------ */
1728 /* --- GET ---------------------------------------------------------------- */
1729 /* ------------------------------------------------------------------------ */
1730 
1780 void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1781 {
1782  velocity.resize(6);
1783  velocity = 0;
1784 
1785  double q[6];
1786  vpColVector q_cur(6);
1787  vpHomogeneousMatrix fMc_cur;
1788  vpHomogeneousMatrix cMc; // camera displacement
1789  double time_cur;
1790 
1791  InitTry;
1792 
1793  // Get the current joint position
1794  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
1795  time_cur = timestamp;
1796  for (unsigned int i = 0; i < njoint; i++) {
1797  q_cur[i] = q[i];
1798  }
1799 
1800  // Get the camera pose from the direct kinematics
1801  vpAfma6::get_fMc(q_cur, fMc_cur);
1802 
1803  if (!first_time_getvel) {
1804 
1805  switch (frame) {
1806  case vpRobot::CAMERA_FRAME: {
1807  // Compute the displacement of the camera since the previous call
1808  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1809 
1810  // Compute the velocity of the camera from this displacement
1811  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1812 
1813  break;
1814  }
1815 
1816  case vpRobot::ARTICULAR_FRAME: {
1817  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1818  break;
1819  }
1820 
1821  case vpRobot::REFERENCE_FRAME: {
1822  // Compute the displacement of the camera since the previous call
1823  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1824 
1825  // Compute the velocity of the camera from this displacement
1826  vpColVector v;
1827  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1828 
1829  // Express this velocity in the reference frame
1830  vpVelocityTwistMatrix fVc(fMc_cur);
1831  velocity = fVc * v;
1832 
1833  break;
1834  }
1835 
1836  case vpRobot::MIXT_FRAME: {
1837  // Compute the displacement of the camera since the previous call
1838  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1839 
1840  // Compute the ThetaU representation for the rotation
1841  vpRotationMatrix cRc;
1842  cMc.extract(cRc);
1843  vpThetaUVector thetaU;
1844  thetaU.buildFrom(cRc);
1845 
1846  for (unsigned int i = 0; i < 3; i++) {
1847  // Compute the translation displacement in the reference frame
1848  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1849  // Update the rotation displacement in the camera frame
1850  velocity[i + 3] = thetaU[i];
1851  }
1852 
1853  // Compute the velocity
1854  velocity /= (time_cur - time_prev_getvel);
1855  break;
1856  }
1857  default: {
1859  "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1860  }
1861  }
1862  }
1863  else {
1864  first_time_getvel = false;
1865  }
1866 
1867  // Memorize the camera pose for the next call
1868  fMc_prev_getvel = fMc_cur;
1869 
1870  // Memorize the joint position for the next call
1871  q_prev_getvel = q_cur;
1872 
1873  // Memorize the time associated to the joint position for the next call
1874  time_prev_getvel = time_cur;
1875 
1876  CatchPrint();
1877  if (TryStt < 0) {
1878  vpERROR_TRACE("Cannot get velocity.");
1879  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1880  }
1881 }
1882 
1892 {
1893  double timestamp;
1894  getVelocity(frame, velocity, timestamp);
1895 }
1896 
1939 {
1940  vpColVector velocity;
1941  getVelocity(frame, velocity, timestamp);
1942 
1943  return velocity;
1944 }
1945 
1955 {
1956  vpColVector velocity;
1957  double timestamp;
1958  getVelocity(frame, velocity, timestamp);
1959 
1960  return velocity;
1961 }
1962 
2012 bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
2013 {
2014  std::ifstream fd(filename.c_str(), std::ios::in);
2015 
2016  if (!fd.is_open()) {
2017  return false;
2018  }
2019 
2020  std::string line;
2021  std::string key("R:");
2022  std::string id("#AFMA6 - Position");
2023  bool pos_found = false;
2024  int lineNum = 0;
2025 
2026  q.resize(njoint);
2027 
2028  while (std::getline(fd, line)) {
2029  lineNum++;
2030  if (lineNum == 1) {
2031  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
2032  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
2033  return false;
2034  }
2035  }
2036  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2037  continue;
2038  }
2039  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2040  // check if there are at least njoint values in the line
2041  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2042  if (chain.size() < njoint + 1) // try to split with tab separator
2043  chain = vpIoTools::splitChain(line, std::string("\t"));
2044  if (chain.size() < njoint + 1)
2045  continue;
2046 
2047  std::istringstream ss(line);
2048  std::string key_;
2049  ss >> key_;
2050  for (unsigned int i = 0; i < njoint; i++)
2051  ss >> q[i];
2052  pos_found = true;
2053  break;
2054  }
2055  }
2056 
2057  // converts rotations from degrees into radians
2058  q[3] = vpMath::rad(q[3]);
2059  q[4] = vpMath::rad(q[4]);
2060  q[5] = vpMath::rad(q[5]);
2061 
2062  fd.close();
2063 
2064  if (!pos_found) {
2065  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2066  return false;
2067  }
2068 
2069  return true;
2070 }
2071 
2096 bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2097 {
2098 
2099  FILE *fd;
2100  fd = fopen(filename.c_str(), "w");
2101  if (fd == nullptr)
2102  return false;
2103 
2104  fprintf(fd, "\
2105 #AFMA6 - Position - Version 2.01\n\
2106 #\n\
2107 # R: X Y Z A B C\n\
2108 # Joint position: X, Y, Z: translations in meters\n\
2109 # A, B, C: rotations in degrees\n\
2110 #\n\
2111 #\n\n");
2112 
2113  // Save positions in mm and deg
2114  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2115  vpMath::deg(q[5]));
2116 
2117  fclose(fd);
2118  return (true);
2119 }
2120 
2131 void vpRobotAfma6::move(const std::string &filename)
2132 {
2133  vpColVector q;
2134 
2135  this->readPosFile(filename, q);
2137  this->setPositioningVelocity(10);
2139 }
2140 
2153 void vpRobotAfma6::move(const std::string &filename, double velocity)
2154 {
2155  vpColVector q;
2156 
2157  this->readPosFile(filename, q);
2159  this->setPositioningVelocity(velocity);
2161 }
2162 
2170 {
2171  InitTry;
2172  Try(PrimitiveGripper_Afma6(1));
2173  std::cout << "Open the gripper..." << std::endl;
2174  CatchPrint();
2175  if (TryStt < 0) {
2176  vpERROR_TRACE("Cannot open the gripper");
2177  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2178  }
2179 }
2180 
2189 {
2190  InitTry;
2191  Try(PrimitiveGripper_Afma6(0));
2192  std::cout << "Close the gripper..." << std::endl;
2193  CatchPrint();
2194  if (TryStt < 0) {
2195  vpERROR_TRACE("Cannot close the gripper");
2196  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2197  }
2198 }
2199 
2219 {
2220  displacement.resize(6);
2221  displacement = 0;
2222 
2223  double q[6];
2224  vpColVector q_cur(6);
2225  vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2226  double timestamp;
2227 
2228  InitTry;
2229 
2230  // Get the current joint position
2231  Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
2232  for (unsigned int i = 0; i < njoint; i++) {
2233  q_cur[i] = q[i];
2234  }
2235 
2236  // Compute the camera pose in the reference frame
2237  fMc_cur = get_fMc(q_cur);
2238 
2239  if (!first_time_getdis) {
2240  switch (frame) {
2241  case vpRobot::CAMERA_FRAME: {
2242  // Compute the camera dispacement from the previous pose
2243  c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2244 
2246  vpRotationMatrix R;
2247  c_prevMc_cur.extract(t);
2248  c_prevMc_cur.extract(R);
2249 
2250  vpRxyzVector rxyz;
2251  rxyz.buildFrom(R);
2252 
2253  for (unsigned int i = 0; i < 3; i++) {
2254  displacement[i] = t[i];
2255  displacement[i + 3] = rxyz[i];
2256  }
2257  break;
2258  }
2259 
2260  case vpRobot::ARTICULAR_FRAME: {
2261  displacement = q_cur - q_prev_getdis;
2262  break;
2263  }
2264 
2265  case vpRobot::REFERENCE_FRAME: {
2266  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2267  return;
2268  }
2269 
2270  case vpRobot::MIXT_FRAME: {
2271  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2272  return;
2273  }
2275  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2276  return;
2277  }
2278  }
2279  }
2280  else {
2281  first_time_getdis = false;
2282  }
2283 
2284  // Memorize the joint position for the next call
2285  q_prev_getdis = q_cur;
2286 
2287  // Memorize the pose for the next call
2288  fMc_prev_getdis = fMc_cur;
2289 
2290  CatchPrint();
2291  if (TryStt < 0) {
2292  vpERROR_TRACE("Cannot get velocity.");
2293  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2294  }
2295 }
2296 
2308 {
2309  Int32 axisInJoint[njoint];
2310  bool status = true;
2311  jointsStatus.resize(6);
2312  InitTry;
2313 
2314  Try(PrimitiveSTATUS_Afma6(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr));
2315  for (unsigned int i = 0; i < njoint; i++) {
2316  if (axisInJoint[i]) {
2317  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
2318  jointsStatus[i] = axisInJoint[i];
2319  status = false;
2320  }
2321  else {
2322  jointsStatus[i] = 0;
2323  }
2324  }
2325 
2326  Catch();
2327  if (TryStt < 0) {
2328  vpERROR_TRACE("Cannot check joint limits.");
2329  throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
2330  }
2331 
2332  return status;
2333 }
2334 END_VISP_NAMESPACE
2335 #elif !defined(VISP_BUILD_SHARED_LIBS)
2336 // Work around to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no symbols
2337 void dummy_vpRobotAfma6() { };
2338 #endif
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:78
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1196
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:157
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:603
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:897
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:784
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:941
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1011
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:148
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
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:1661
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:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
void get_cVe(vpVelocityTwistMatrix &_cVe) const
bool checkJointLimits(vpColVector &jointsStatus)
void get_eJe(vpMatrix &_eJe) VP_OVERRIDE
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
void get_fJe(vpMatrix &_fJe) VP_OVERRIDE
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getTime() const
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:252
void init(void)
void get_cMe(vpHomogeneousMatrix &_cMe) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) VP_OVERRIDE
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getPositioningVelocity(void)
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)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) VP_OVERRIDE
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:164
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:274
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:202
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252
bool verbose_
Definition: vpRobot.h:118
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:183
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)