Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpRobotViper650.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Viper S650 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_VIPER650
42 
43 #include <signal.h>
44 #include <stdlib.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/robot/vpRobot.h>
54 #include <visp3/robot/vpRobotException.h>
55 #include <visp3/robot/vpRobotViper650.h>
56 
57 /* ---------------------------------------------------------------------- */
58 /* --- STATIC ----------------------------------------------------------- */
59 /* ---------------------------------------------------------------------- */
60 
61 bool vpRobotViper650::robotAlreadyCreated = false;
62 
71 
72 /* ---------------------------------------------------------------------- */
73 /* --- EMERGENCY STOP --------------------------------------------------- */
74 /* ---------------------------------------------------------------------- */
75 
83 void emergencyStopViper650(int signo)
84 {
85  std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7;
86  switch (signo) {
87  case SIGINT:
88  std::cout << "SIGINT (stop by ^C) " << std::endl;
89  break;
90  case SIGBUS:
91  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
92  break;
93  case SIGSEGV:
94  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
95  break;
96  case SIGKILL:
97  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
98  break;
99  case SIGQUIT:
100  std::cout << "SIGQUIT " << std::endl;
101  break;
102  default:
103  std::cout << signo << std::endl;
104  }
105  // std::cout << "Emergency stop called\n";
106  // PrimitiveESTOP_Viper650();
107  PrimitiveSTOP_Viper650();
108  std::cout << "Robot was stopped\n";
109 
110  // Free allocated resources
111  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
112 
113  fprintf(stdout, "Application ");
114  fflush(stdout);
115  kill(getpid(), SIGKILL);
116  exit(1);
117 }
118 
119 /* ---------------------------------------------------------------------- */
120 /* --- CONSTRUCTOR ------------------------------------------------------ */
121 /* ---------------------------------------------------------------------- */
122 
178 vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
179 {
180 
181  /*
182  #define SIGHUP 1 // hangup
183  #define SIGINT 2 // interrupt (rubout)
184  #define SIGQUIT 3 // quit (ASCII FS)
185  #define SIGILL 4 // illegal instruction (not reset when caught)
186  #define SIGTRAP 5 // trace trap (not reset when caught)
187  #define SIGIOT 6 // IOT instruction
188  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
189  #define SIGEMT 7 // EMT instruction
190  #define SIGFPE 8 // floating point exception
191  #define SIGKILL 9 // kill (cannot be caught or ignored)
192  #define SIGBUS 10 // bus error
193  #define SIGSEGV 11 // segmentation violation
194  #define SIGSYS 12 // bad argument to system call
195  #define SIGPIPE 13 // write on a pipe with no one to read it
196  #define SIGALRM 14 // alarm clock
197  #define SIGTERM 15 // software termination signal from kill
198  */
199 
200  signal(SIGINT, emergencyStopViper650);
201  signal(SIGBUS, emergencyStopViper650);
202  signal(SIGSEGV, emergencyStopViper650);
203  signal(SIGKILL, emergencyStopViper650);
204  signal(SIGQUIT, emergencyStopViper650);
205 
206  setVerbose(verbose);
207  if (verbose_)
208  std::cout << "Open communication with MotionBlox.\n";
209  try {
210  this->init();
212  } catch (...) {
213  // vpERROR_TRACE("Error caught") ;
214  throw;
215  }
216  positioningVelocity = defaultPositioningVelocity;
217 
218  maxRotationVelocity_joint6 = maxRotationVelocity;
219 
220  vpRobotViper650::robotAlreadyCreated = true;
221 
222  return;
223 }
224 
225 /* ------------------------------------------------------------------------ */
226 /* --- INITIALISATION ----------------------------------------------------- */
227 /* ------------------------------------------------------------------------ */
228 
252 {
253  InitTry;
254 
255  // Initialise private variables used to compute the measured velocities
256  q_prev_getvel.resize(6);
257  q_prev_getvel = 0;
258  time_prev_getvel = 0;
259  first_time_getvel = true;
260 
261  // Initialise private variables used to compute the measured displacement
262  q_prev_getdis.resize(6);
263  q_prev_getdis = 0;
264  first_time_getdis = true;
265 
266  // Initialize the firewire connection
267  Try(InitializeConnection(verbose_));
268 
269  // Connect to the servoboard using the servo board GUID
270  Try(InitializeNode_Viper650());
271 
272  Try(PrimitiveRESET_Viper650());
273 
274  // Enable the joint limits on axis 6
275  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
276 
277  // Update the eMc matrix in the low level controller
279 
280  // Look if the power is on or off
281  UInt32 HIPowerStatus;
282  UInt32 EStopStatus;
283  Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
284  CAL_Wait(0.1);
285 
286  // Print the robot status
287  if (verbose_) {
288  std::cout << "Robot status: ";
289  switch (EStopStatus) {
290  case ESTOP_AUTO:
291  controlMode = AUTO;
292  if (HIPowerStatus == 0)
293  std::cout << "Power is OFF" << std::endl;
294  else
295  std::cout << "Power is ON" << std::endl;
296  break;
297 
298  case ESTOP_MANUAL:
299  controlMode = MANUAL;
300  if (HIPowerStatus == 0)
301  std::cout << "Power is OFF" << std::endl;
302  else
303  std::cout << "Power is ON" << std::endl;
304  break;
305  case ESTOP_ACTIVATED:
306  controlMode = ESTOP;
307  std::cout << "Emergency stop is activated" << std::endl;
308  break;
309  default:
310  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
311  std::cout << "You have to call Adept for maintenance..." << std::endl;
312  // Free allocated resources
313  }
314  std::cout << std::endl;
315  }
316  // get real joint min/max from the MotionBlox
317  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
318  // Convert units from degrees to radians
319  joint_min.deg2rad();
320  joint_max.deg2rad();
321 
322  // for (unsigned int i=0; i < njoint; i++) {
323  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
324  // joint_max[i]);
325  // }
326 
327  // If an error occur in the low level controller, goto here
328  // CatchPrint();
329  Catch();
330 
331  // Test if an error occurs
332  if (TryStt == -20001)
333  printf("No connection detected. Check if the robot is powered on \n"
334  "and if the firewire link exist between the MotionBlox and this "
335  "computer.\n");
336  else if (TryStt == -675)
337  printf(" Timeout enabling power...\n");
338 
339  if (TryStt < 0) {
340  // Power off the robot
341  PrimitivePOWEROFF_Viper650();
342  // Free allocated resources
343  ShutDownConnection();
344 
345  std::cout << "Cannot open connection with the motionblox..." << std::endl;
346  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
347  }
348  return;
349 }
350 
409 {
410  // Read the robot constants from files
411  // - joint [min,max], coupl_56, long_56
412  // - camera extrinsic parameters relative to eMc
413  vpViper650::init(tool, projModel);
414 
415  InitTry;
416 
417  // Get real joint min/max from the MotionBlox
418  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
419  // Convert units from degrees to radians
420  joint_min.deg2rad();
421  joint_max.deg2rad();
422 
423  // for (unsigned int i=0; i < njoint; i++) {
424  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
425  // joint_max[i]);
426  // }
427 
428  // Set the camera constant (eMc pose) in the MotionBlox
429  double eMc_pose[6];
430  for (unsigned int i = 0; i < 3; i++) {
431  eMc_pose[i] = etc[i]; // translation in meters
432  eMc_pose[i + 3] = erc[i]; // rotation in rad
433  }
434  // Update the eMc pose in the low level controller
435  Try(PrimitiveCONST_Viper650(eMc_pose));
436 
437  CatchPrint();
438  return;
439 }
440 
491 void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename)
492 {
493  vpViper650::init(tool, filename);
494 
495  InitTry;
496 
497  // Get real joint min/max from the MotionBlox
498  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
499  // Convert units from degrees to radians
500  joint_min.deg2rad();
501  joint_max.deg2rad();
502 
503  // for (unsigned int i=0; i < njoint; i++) {
504  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
505  // joint_max[i]);
506  // }
507 
508  // Set the camera constant (eMc pose) in the MotionBlox
509  double eMc_pose[6];
510  for (unsigned int i = 0; i < 3; i++) {
511  eMc_pose[i] = etc[i]; // translation in meters
512  eMc_pose[i + 3] = erc[i]; // rotation in rad
513  }
514  // Update the eMc pose in the low level controller
515  Try(PrimitiveCONST_Viper650(eMc_pose));
516 
517  CatchPrint();
518  return;
519 }
520 
558 {
559  vpViper650::init(tool, eMc_);
560 
561  InitTry;
562 
563  // Get real joint min/max from the MotionBlox
564  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
565  // Convert units from degrees to radians
566  joint_min.deg2rad();
567  joint_max.deg2rad();
568 
569  // for (unsigned int i=0; i < njoint; i++) {
570  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
571  // joint_max[i]);
572  // }
573 
574  // Set the camera constant (eMc pose) in the MotionBlox
575  double eMc_pose[6];
576  for (unsigned int i = 0; i < 3; i++) {
577  eMc_pose[i] = etc[i]; // translation in meters
578  eMc_pose[i + 3] = erc[i]; // rotation in rad
579  }
580  // Update the eMc pose in the low level controller
581  Try(PrimitiveCONST_Viper650(eMc_pose));
582 
583  CatchPrint();
584  return;
585 }
586 
599 {
600  this->vpViper650::set_eMc(eMc_);
601 
602  InitTry;
603 
604  // Set the camera constant (eMc pose) in the MotionBlox
605  double eMc_pose[6];
606  for (unsigned int i = 0; i < 3; i++) {
607  eMc_pose[i] = etc[i]; // translation in meters
608  eMc_pose[i + 3] = erc[i]; // rotation in rad
609  }
610  // Update the eMc pose in the low level controller
611  Try(PrimitiveCONST_Viper650(eMc_pose));
612 
613  CatchPrint();
614 
615  return;
616 }
617 
631 {
632  this->vpViper650::set_eMc(etc_, erc_);
633 
634  InitTry;
635 
636  // Set the camera constant (eMc pose) in the MotionBlox
637  double eMc_pose[6];
638  for (unsigned int i = 0; i < 3; i++) {
639  eMc_pose[i] = etc[i]; // translation in meters
640  eMc_pose[i + 3] = erc[i]; // rotation in rad
641  }
642  // Update the eMc pose in the low level controller
643  Try(PrimitiveCONST_Viper650(eMc_pose));
644 
645  CatchPrint();
646 
647  return;
648 }
649 
650 /* ------------------------------------------------------------------------ */
651 /* --- DESTRUCTOR --------------------------------------------------------- */
652 /* ------------------------------------------------------------------------ */
653 
661 {
662  InitTry;
663 
665 
666  // Look if the power is on or off
667  UInt32 HIPowerStatus;
668  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
669  CAL_Wait(0.1);
670 
671  // if (HIPowerStatus == 1) {
672  // fprintf(stdout, "Power OFF the robot\n");
673  // fflush(stdout);
674 
675  // Try( PrimitivePOWEROFF_Viper650() );
676  // }
677 
678  // Free allocated resources
679  ShutDownConnection();
680 
681  vpRobotViper650::robotAlreadyCreated = false;
682 
683  CatchPrint();
684  return;
685 }
686 
694 {
695  InitTry;
696 
697  switch (newState) {
698  case vpRobot::STATE_STOP: {
699  // Start primitive STOP only if the current state is Velocity
701  Try(PrimitiveSTOP_Viper650());
702  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
703  }
704  break;
705  }
708  std::cout << "Change the control mode from velocity to position control.\n";
709  Try(PrimitiveSTOP_Viper650());
710  } else {
711  // std::cout << "Change the control mode from stop to position
712  // control.\n";
713  }
714  this->powerOn();
715  break;
716  }
719  std::cout << "Change the control mode from stop to velocity control.\n";
720  }
721  this->powerOn();
722  break;
723  }
724  default:
725  break;
726  }
727 
728  CatchPrint();
729 
730  return vpRobot::setRobotState(newState);
731 }
732 
733 /* ------------------------------------------------------------------------ */
734 /* --- STOP --------------------------------------------------------------- */
735 /* ------------------------------------------------------------------------ */
736 
745 {
747  return;
748 
749  InitTry;
750  Try(PrimitiveSTOP_Viper650());
752 
753  CatchPrint();
754  if (TryStt < 0) {
755  vpERROR_TRACE("Cannot stop robot motion");
756  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
757  }
758 }
759 
770 {
771  InitTry;
772 
773  // Look if the power is on or off
774  UInt32 HIPowerStatus;
775  UInt32 EStopStatus;
776  bool firsttime = true;
777  unsigned int nitermax = 10;
778 
779  for (unsigned int i = 0; i < nitermax; i++) {
780  Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
781  if (EStopStatus == ESTOP_AUTO) {
782  controlMode = AUTO;
783  break; // exit for loop
784  } else if (EStopStatus == ESTOP_MANUAL) {
785  controlMode = MANUAL;
786  break; // exit for loop
787  } else if (EStopStatus == ESTOP_ACTIVATED) {
788  controlMode = ESTOP;
789  if (firsttime) {
790  std::cout << "Emergency stop is activated! \n"
791  << "Check the emergency stop button and push the yellow "
792  "button before continuing."
793  << std::endl;
794  firsttime = false;
795  }
796  fprintf(stdout, "Remaining time %us \r", nitermax - i);
797  fflush(stdout);
798  CAL_Wait(1);
799  } else {
800  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
801  std::cout << "You have to call Adept for maintenance..." << std::endl;
802  // Free allocated resources
803  ShutDownConnection();
804  exit(0);
805  }
806  }
807 
808  if (EStopStatus == ESTOP_ACTIVATED)
809  std::cout << std::endl;
810 
811  if (EStopStatus == ESTOP_ACTIVATED) {
812  std::cout << "Sorry, cannot power on the robot." << std::endl;
813  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot.");
814  }
815 
816  if (HIPowerStatus == 0) {
817  fprintf(stdout, "Power ON the Viper650 robot\n");
818  fflush(stdout);
819 
820  Try(PrimitivePOWERON_Viper650());
821  }
822 
823  CatchPrint();
824  if (TryStt < 0) {
825  vpERROR_TRACE("Cannot power on the robot");
826  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
827  }
828 }
829 
840 {
841  InitTry;
842 
843  // Look if the power is on or off
844  UInt32 HIPowerStatus;
845  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
846  CAL_Wait(0.1);
847 
848  if (HIPowerStatus == 1) {
849  fprintf(stdout, "Power OFF the Viper650 robot\n");
850  fflush(stdout);
851 
852  Try(PrimitivePOWEROFF_Viper650());
853  }
854 
855  CatchPrint();
856  if (TryStt < 0) {
857  vpERROR_TRACE("Cannot power off the robot");
858  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
859  }
860 }
861 
874 {
875  InitTry;
876  bool status = false;
877  // Look if the power is on or off
878  UInt32 HIPowerStatus;
879  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
880  CAL_Wait(0.1);
881 
882  if (HIPowerStatus == 1) {
883  status = true;
884  }
885 
886  CatchPrint();
887  if (TryStt < 0) {
888  vpERROR_TRACE("Cannot get the power status");
889  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
890  }
891  return status;
892 }
893 
904 {
906  vpViper650::get_cMe(cMe);
907 
908  cVe.buildFrom(cMe);
909 }
910 
923 
936 {
937 
938  double position[6];
939  double timestamp;
940 
941  InitTry;
942  Try(PrimitiveACQ_POS_J_Viper650(position, &timestamp));
943  CatchPrint();
944 
945  vpColVector q(6);
946  for (unsigned int i = 0; i < njoint; i++)
947  q[i] = vpMath::rad(position[i]);
948 
949  try {
950  vpViper650::get_eJe(q, eJe);
951  } catch (...) {
952  vpERROR_TRACE("catch exception ");
953  throw;
954  }
955 }
969 {
970 
971  double position[6];
972  double timestamp;
973 
974  InitTry;
975  Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
976  CatchPrint();
977 
978  vpColVector q(6);
979  for (unsigned int i = 0; i < njoint; i++)
980  q[i] = position[i];
981 
982  try {
983  vpViper650::get_fJe(q, fJe);
984  } catch (...) {
985  vpERROR_TRACE("Error caught");
986  throw;
987  }
988 }
989 
1027 void vpRobotViper650::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
1028 
1034 double vpRobotViper650::getPositioningVelocity(void) const { return positioningVelocity; }
1035 
1115 {
1116 
1118  vpERROR_TRACE("Robot was not in position-based control\n"
1119  "Modification of the robot state");
1121  }
1122 
1123  vpColVector destination(njoint);
1124  int error = 0;
1125  double timestamp;
1126 
1127  InitTry;
1128  switch (frame) {
1129  case vpRobot::CAMERA_FRAME: {
1130  vpColVector q(njoint);
1131  Try(PrimitiveACQ_POS_Viper650(q.data, &timestamp));
1132 
1133  // Convert degrees into rad
1134  q.deg2rad();
1135 
1136  // Get fMc from the inverse kinematics
1137  vpHomogeneousMatrix fMc;
1138  vpViper650::get_fMc(q, fMc);
1139 
1140  // Set cMc from the input position
1141  vpTranslationVector txyz;
1142  vpRxyzVector rxyz;
1143  for (unsigned int i = 0; i < 3; i++) {
1144  txyz[i] = position[i];
1145  rxyz[i] = position[i + 3];
1146  }
1147 
1148  // Compute cMc2
1149  vpRotationMatrix cRc2(rxyz);
1150  vpHomogeneousMatrix cMc2(txyz, cRc2);
1151 
1152  // Compute the new position to reach: fMc*cMc2
1153  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1154 
1155  // Compute the corresponding joint position from the inverse kinematics
1156  unsigned int solution = this->getInverseKinematics(fMc2, q);
1157  if (solution) { // Position is reachable
1158  destination = q;
1159  // convert rad to deg requested for the low level controller
1160  destination.rad2deg();
1161  Try(PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity));
1162  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1163  } else {
1164  // Cartesian position is out of range
1165  error = -1;
1166  }
1167 
1168  break;
1169  }
1170  case vpRobot::ARTICULAR_FRAME: {
1171  destination = position;
1172  // convert rad to deg requested for the low level controller
1173  destination.rad2deg();
1174 
1175  // std::cout << "Joint destination (deg): " << destination.t() <<
1176  // std::endl;
1177  Try(PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity));
1178  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1179  break;
1180  }
1181  case vpRobot::REFERENCE_FRAME: {
1182  // Convert angles from Rxyz representation to Rzyz representation
1183  vpRxyzVector rxyz(position[3], position[4], position[5]);
1184  vpRotationMatrix R(rxyz);
1185  vpRzyzVector rzyz(R);
1186 
1187  for (unsigned int i = 0; i < 3; i++) {
1188  destination[i] = position[i];
1189  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1190  }
1191  int configuration = 0; // keep the actual configuration
1192 
1193  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1194  // << std::endl;
1195  Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, positioningVelocity));
1196  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1197 
1198  break;
1199  }
1200  case vpRobot::MIXT_FRAME: {
1201  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1202  "Mixt frame not implemented.");
1203  }
1205  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1206  "Mixt frame not implemented.");
1207  }
1208  }
1209 
1210  CatchPrint();
1211  if (TryStt == InvalidPosition || TryStt == -1023)
1212  std::cout << " : Position out of range.\n";
1213 
1214  else if (TryStt == -3019) {
1215  if (frame == vpRobot::ARTICULAR_FRAME)
1216  std::cout << " : Joint position out of range.\n";
1217  else
1218  std::cout << " : Cartesian position leads to a joint position out of "
1219  "range.\n";
1220  } else if (TryStt < 0)
1221  std::cout << " : Unknown error (see Fabien).\n";
1222  else if (error == -1)
1223  std::cout << "Position out of range.\n";
1224 
1225  if (TryStt < 0 || error < 0) {
1226  vpERROR_TRACE("Positionning error.");
1227  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1228  }
1229 
1230  return;
1231 }
1232 
1298 void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, const double pos1, const double pos2,
1299  const double pos3, const double pos4, const double pos5, const 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  } catch (...) {
1312  vpERROR_TRACE("Error caught");
1313  throw;
1314  }
1315 }
1316 
1356 void vpRobotViper650::setPosition(const std::string &filename)
1357 {
1358  vpColVector q;
1359  bool ret;
1360 
1361  ret = this->readPosFile(filename, q);
1362 
1363  if (ret == false) {
1364  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1365  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1366  }
1369 }
1370 
1439 void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1440 {
1441 
1442  InitTry;
1443 
1444  position.resize(6);
1445 
1446  switch (frame) {
1447  case vpRobot::CAMERA_FRAME: {
1448  position = 0;
1449  return;
1450  }
1451  case vpRobot::ARTICULAR_FRAME: {
1452  Try(PrimitiveACQ_POS_J_Viper650(position.data, &timestamp));
1453  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1454  position.deg2rad();
1455 
1456  return;
1457  }
1458  case vpRobot::REFERENCE_FRAME: {
1459  vpColVector q(njoint);
1460  Try(PrimitiveACQ_POS_J_Viper650(q.data, &timestamp));
1461 
1462  // Compute fMc
1464 
1465  // From fMc extract the pose
1466  vpRotationMatrix fRc;
1467  fMc.extract(fRc);
1468  vpRxyzVector rxyz;
1469  rxyz.buildFrom(fRc);
1470 
1471  for (unsigned int i = 0; i < 3; i++) {
1472  position[i] = fMc[i][3]; // translation x,y,z
1473  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1474  }
1475 
1476  break;
1477  }
1478  case vpRobot::MIXT_FRAME: {
1479  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1480  "not implemented");
1481  }
1483  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1484  "not implemented");
1485  }
1486  }
1487 
1488  CatchPrint();
1489  if (TryStt < 0) {
1490  vpERROR_TRACE("Cannot get position.");
1491  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1492  }
1493 
1494  return;
1495 }
1496 
1508 {
1509  double timestamp;
1510  getPosition(frame, position, timestamp);
1511 }
1512 
1526 void vpRobotViper650::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 
1565 {
1566  double timestamp;
1567  PrimitiveACQ_TIME_Viper650(&timestamp);
1568  return timestamp;
1569 }
1570 
1650 {
1652  vpERROR_TRACE("Cannot send a velocity to the robot "
1653  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1655  "Cannot send a velocity to the robot "
1656  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1657  }
1658 
1659  vpColVector vel_sat(6);
1660 
1661  // Velocity saturation
1662  switch (frame) {
1663  // saturation in cartesian space
1664  case vpRobot::CAMERA_FRAME:
1667  case vpRobot::MIXT_FRAME: {
1668  vpColVector vel_max(6);
1669 
1670  for (unsigned int i = 0; i < 3; i++)
1671  vel_max[i] = getMaxTranslationVelocity();
1672  for (unsigned int i = 3; i < 6; i++)
1673  vel_max[i] = getMaxRotationVelocity();
1674 
1675  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1676 
1677  break;
1678  }
1679  // saturation in joint space
1680  case vpRobot::ARTICULAR_FRAME: {
1681  vpColVector vel_max(6);
1682 
1683  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1684  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1685  for (unsigned int i = 0; i < 6; i++)
1686  vel_max[i] = getMaxRotationVelocity();
1687  } else {
1688  for (unsigned int i = 0; i < 5; i++)
1689  vel_max[i] = getMaxRotationVelocity();
1690  vel_max[5] = getMaxRotationVelocityJoint6();
1691  }
1692 
1693  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1694  }
1695  }
1696 
1697  InitTry;
1698 
1699  switch (frame) {
1700  case vpRobot::CAMERA_FRAME: {
1701  // Send velocities in m/s and rad/s
1702  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1703  Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
1704  break;
1705  }
1707  // Transform in camera frame
1708  vpHomogeneousMatrix cMe;
1709  this->get_cMe(cMe);
1710  vpVelocityTwistMatrix cVe(cMe);
1711  vpColVector v_c = cVe * vel_sat;
1712  // Send velocities in m/s and rad/s
1713  Try(PrimitiveMOVESPEED_CART_Viper650(v_c.data, REPCAM_VIPER650));
1714  break;
1715  }
1716  case vpRobot::ARTICULAR_FRAME: {
1717  // Convert all the velocities from rad/s into deg/s
1718  vel_sat.rad2deg();
1719  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1720  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
1721  Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
1722  break;
1723  }
1724  case vpRobot::REFERENCE_FRAME: {
1725  // Send velocities in m/s and rad/s
1726  //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1727  Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
1728  break;
1729  }
1730  case vpRobot::MIXT_FRAME: {
1731  // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
1732  break;
1733  }
1734  default: {
1735  vpERROR_TRACE("Error in spec of vpRobot. "
1736  "Case not taken in account.");
1737  return;
1738  }
1739  }
1740 
1741  Catch();
1742  if (TryStt < 0) {
1743  if (TryStt == VelStopOnJoint) {
1744  UInt32 axisInJoint[njoint];
1745  PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1746  for (unsigned int i = 0; i < njoint; i++) {
1747  if (axisInJoint[i])
1748  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1749  }
1750  } else {
1751  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1752  if (TryString != NULL) {
1753  // The statement is in TryString, but we need to check the validity
1754  printf(" Error sentence %s\n", TryString); // Print the TryString
1755  } else {
1756  printf("\n");
1757  }
1758  }
1759  }
1760 
1761  return;
1762 }
1763 
1764 /* ------------------------------------------------------------------------ */
1765 /* --- GET ---------------------------------------------------------------- */
1766 /* ------------------------------------------------------------------------ */
1767 
1825 void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1826 {
1827  velocity.resize(6);
1828  velocity = 0;
1829 
1830  vpColVector q_cur(6);
1831  vpHomogeneousMatrix fMc_cur;
1832  vpHomogeneousMatrix cMc; // camera displacement
1833  double time_cur;
1834 
1835  InitTry;
1836 
1837  // Get the current joint position
1838  Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
1839  time_cur = timestamp;
1840  q_cur.deg2rad();
1841 
1842  // Get the camera pose from the direct kinematics
1843  vpViper650::get_fMc(q_cur, fMc_cur);
1844 
1845  if (!first_time_getvel) {
1846 
1847  switch (frame) {
1848  case vpRobot::CAMERA_FRAME: {
1849  // Compute the displacement of the camera since the previous call
1850  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1851 
1852  // Compute the velocity of the camera from this displacement
1853  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1854 
1855  break;
1856  }
1857 
1858  case vpRobot::ARTICULAR_FRAME: {
1859  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1860  break;
1861  }
1862 
1863  case vpRobot::REFERENCE_FRAME: {
1864  // Compute the displacement of the camera since the previous call
1865  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1866 
1867  // Compute the velocity of the camera from this displacement
1868  vpColVector v;
1869  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1870 
1871  // Express this velocity in the reference frame
1872  vpVelocityTwistMatrix fVc(fMc_cur);
1873  velocity = fVc * v;
1874 
1875  break;
1876  }
1877 
1878  case vpRobot::MIXT_FRAME: {
1879  // Compute the displacement of the camera since the previous call
1880  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1881 
1882  // Compute the ThetaU representation for the rotation
1883  vpRotationMatrix cRc;
1884  cMc.extract(cRc);
1885  vpThetaUVector thetaU;
1886  thetaU.buildFrom(cRc);
1887 
1888  for (unsigned int i = 0; i < 3; i++) {
1889  // Compute the translation displacement in the reference frame
1890  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1891  // Update the rotation displacement in the camera frame
1892  velocity[i + 3] = thetaU[i];
1893  }
1894 
1895  // Compute the velocity
1896  velocity /= (time_cur - time_prev_getvel);
1897  break;
1898  }
1899  default: {
1901  "vpRobotViper650::getVelocity() not implemented in end-effector"));
1902  }
1903  }
1904  } else {
1905  first_time_getvel = false;
1906  }
1907 
1908  // Memorize the camera pose for the next call
1909  fMc_prev_getvel = fMc_cur;
1910 
1911  // Memorize the joint position for the next call
1912  q_prev_getvel = q_cur;
1913 
1914  // Memorize the time associated to the joint position for the next call
1915  time_prev_getvel = time_cur;
1916 
1917  CatchPrint();
1918  if (TryStt < 0) {
1919  vpERROR_TRACE("Cannot get velocity.");
1920  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1921  }
1922 }
1923 
1933 {
1934  double timestamp;
1935  getVelocity(frame, velocity, timestamp);
1936 }
1937 
1989 {
1990  vpColVector velocity;
1991  getVelocity(frame, velocity, timestamp);
1992 
1993  return velocity;
1994 }
1995 
2005 {
2006  vpColVector velocity;
2007  double timestamp;
2008  getVelocity(frame, velocity, timestamp);
2009 
2010  return velocity;
2011 }
2012 
2079 bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
2080 {
2081  std::ifstream fd(filename.c_str(), std::ios::in);
2082 
2083  if (!fd.is_open()) {
2084  return false;
2085  }
2086 
2087  std::string line;
2088  std::string key("R:");
2089  std::string id("#Viper650 - Position");
2090  bool pos_found = false;
2091  int lineNum = 0;
2092 
2093  q.resize(njoint);
2094 
2095  while (std::getline(fd, line)) {
2096  lineNum++;
2097  if (lineNum == 1) {
2098  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
2099  std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl;
2100  return false;
2101  }
2102  }
2103  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2104  continue;
2105  }
2106  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2107  // check if there are at least njoint values in the line
2108  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2109  if (chain.size() < njoint + 1) // try to split with tab separator
2110  chain = vpIoTools::splitChain(line, std::string("\t"));
2111  if (chain.size() < njoint + 1)
2112  continue;
2113 
2114  std::istringstream ss(line);
2115  std::string key_;
2116  ss >> key_;
2117  for (unsigned int i = 0; i < njoint; i++)
2118  ss >> q[i];
2119  pos_found = true;
2120  break;
2121  }
2122  }
2123 
2124  // converts rotations from degrees into radians
2125  q.deg2rad();
2126 
2127  fd.close();
2128 
2129  if (!pos_found) {
2130  std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2131  return false;
2132  }
2133 
2134  return true;
2135 }
2136 
2160 bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
2161 {
2162 
2163  FILE *fd;
2164  fd = fopen(filename.c_str(), "w");
2165  if (fd == NULL)
2166  return false;
2167 
2168  fprintf(fd, "\
2169 #Viper650 - Position - Version 1.00\n\
2170 #\n\
2171 # R: A B C D E F\n\
2172 # Joint position in degrees\n\
2173 #\n\
2174 #\n\n");
2175 
2176  // Save positions in mm and deg
2177  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2178  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2179 
2180  fclose(fd);
2181  return (true);
2182 }
2183 
2194 void vpRobotViper650::move(const std::string &filename)
2195 {
2196  vpColVector q;
2197 
2198  try {
2199  this->readPosFile(filename, q);
2201  this->setPositioningVelocity(10);
2203  } catch (...) {
2204  throw;
2205  }
2206 }
2207 
2227 {
2228  displacement.resize(6);
2229  displacement = 0;
2230 
2231  double q[6];
2232  vpColVector q_cur(6);
2233  double timestamp;
2234 
2235  InitTry;
2236 
2237  // Get the current joint position
2238  Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
2239  for (unsigned int i = 0; i < njoint; i++) {
2240  q_cur[i] = q[i];
2241  }
2242 
2243  if (!first_time_getdis) {
2244  switch (frame) {
2245  case vpRobot::CAMERA_FRAME: {
2246  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2247  return;
2248  }
2249 
2250  case vpRobot::ARTICULAR_FRAME: {
2251  displacement = q_cur - q_prev_getdis;
2252  break;
2253  }
2254 
2255  case vpRobot::REFERENCE_FRAME: {
2256  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2257  return;
2258  }
2259 
2260  case vpRobot::MIXT_FRAME: {
2261  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2262  return;
2263  }
2265  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2266  return;
2267  }
2268  }
2269  } else {
2270  first_time_getdis = false;
2271  }
2272 
2273  // Memorize the joint position for the next call
2274  q_prev_getdis = q_cur;
2275 
2276  CatchPrint();
2277  if (TryStt < 0) {
2278  vpERROR_TRACE("Cannot get velocity.");
2279  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2280  }
2281 }
2282 
2297 {
2298  InitTry;
2299 
2300  Try(PrimitiveTFS_BIAS_Viper650());
2301 
2302  // Wait 500 ms to be sure the next measures take into account the bias
2303  vpTime::wait(500);
2304 
2305  CatchPrint();
2306  if (TryStt < 0) {
2307  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2308  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2309  }
2310 }
2311 
2353 {
2354  InitTry;
2355 
2356  H.resize(6);
2357 
2358  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2359 
2360  CatchPrint();
2361  if (TryStt < 0) {
2362  vpERROR_TRACE("Cannot get the force/torque measures.");
2363  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2364  }
2365 }
2366 
2406 {
2407  InitTry;
2408 
2409  vpColVector H(6);
2410 
2411  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2412 
2413  return H;
2414 
2415  CatchPrint();
2416  if (TryStt < 0) {
2417  vpERROR_TRACE("Cannot get the force/torque measures.");
2418  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2419  }
2420  return H; // Here to avoid a warning, but should never be called
2421 }
2422 
2429 {
2430  InitTry;
2431  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2432  std::cout << "Enable joint limits on axis 6..." << std::endl;
2433  CatchPrint();
2434  if (TryStt < 0) {
2435  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2436  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2437  }
2438 }
2439 
2451 {
2452  InitTry;
2453  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2454  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2455  CatchPrint();
2456  if (TryStt < 0) {
2457  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2458  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2459  }
2460 }
2461 
2471 {
2474 
2475  return;
2476 }
2477 
2498 {
2499  maxRotationVelocity_joint6 = w6_max;
2500  return;
2501 }
2502 
2510 double vpRobotViper650::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2511 
2519 {
2520  InitTry;
2521  Try(PrimitivePneumaticGripper_Viper650(1));
2522  std::cout << "Open the pneumatic gripper..." << std::endl;
2523  CatchPrint();
2524  if (TryStt < 0) {
2525  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2526  }
2527 }
2528 
2537 {
2538  InitTry;
2539  Try(PrimitivePneumaticGripper_Viper650(0));
2540  std::cout << "Close the pneumatic gripper..." << std::endl;
2541  CatchPrint();
2542  if (TryStt < 0) {
2543  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2544  }
2545 }
2546 
2547 #elif !defined(VISP_BUILD_SHARED_LIBS)
2548 // Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
2549 // no symbols
2550 void dummy_vpRobotViper650(){};
2551 #endif
static const double defaultPositioningVelocity
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotViper650(void)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:159
#define vpERROR_TRACE
Definition: vpDebug.h:393
void setPositioningVelocity(const double velocity)
vpColVector getForceTorque() const
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:176
double maxRotationVelocity
Definition: vpRobot.h:98
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void setMaxRotationVelocityJoint6(double w6_max)
Initialize the position controller.
Definition: vpRobot.h:68
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
Automatic control mode (default).
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
void get_cVe(vpVelocityTwistMatrix &cVe) const
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
vpControlFrameType
Definition: vpRobot.h:75
vpRxyzVector erc
Definition: vpViper.h:160
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void closeGripper() const
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
static bool savePosFile(const std::string &filename, const vpColVector &q)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void deg2rad()
Definition: vpColVector.h:134
double getTime() const
Implementation of a rotation matrix and operations on such kind of matrices.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:136
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
Definition: vpRobot.h:67
Emergency stop activated.
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:116
vpTranslationVector etc
Definition: vpViper.h:159
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpColVector joint_max
Definition: vpViper.h:172
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:258
Modelisation of the ADEPT Viper 650 robot.
Definition: vpViper650.h:102
void enableJoint6Limits() const
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1771
double getPositioningVelocity(void) const
void init(void)
Definition: vpViper650.cpp:135
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:127
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
static double rad(double deg)
Definition: vpMath.h:102
void get_fJe(vpMatrix &fJe)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:260
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
static double deg(double rad)
Definition: vpMath.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void biasForceTorqueSensor() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
void move(const std::string &filename)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
void disableJoint6Limits() const
double getMaxRotationVelocityJoint6() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
void setMaxRotationVelocity(double w_max)
static bool readPosFile(const std::string &filename, vpColVector &q)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:154
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Function not implemented.
Definition: vpException.h:90
void get_cMe(vpHomogeneousMatrix &cMe) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void rad2deg()
Definition: vpColVector.h:228
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_eJe(vpMatrix &eJe)
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
vpColVector joint_min
Definition: vpViper.h:173