Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
vpRobotViper650.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 Viper S650 robot.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_VIPER650
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/vpThetaUVector.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/robot/vpRobot.h>
51 #include <visp3/robot/vpRobotException.h>
52 #include <visp3/robot/vpRobotViper650.h>
53 
54 /* ---------------------------------------------------------------------- */
55 /* --- STATIC ----------------------------------------------------------- */
56 /* ---------------------------------------------------------------------- */
57 
58 bool vpRobotViper650::m_robotAlreadyCreated = false;
59 
68 
69 /* ---------------------------------------------------------------------- */
70 /* --- EMERGENCY STOP --------------------------------------------------- */
71 /* ---------------------------------------------------------------------- */
72 
80 void emergencyStopViper650(int signo)
81 {
82  std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7;
83  switch (signo) {
84  case SIGINT:
85  std::cout << "SIGINT (stop by ^C) " << std::endl;
86  break;
87  case SIGBUS:
88  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
89  break;
90  case SIGSEGV:
91  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
92  break;
93  case SIGKILL:
94  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
95  break;
96  case SIGQUIT:
97  std::cout << "SIGQUIT " << std::endl;
98  break;
99  default:
100  std::cout << signo << std::endl;
101  }
102  // std::cout << "Emergency stop called\n";
103  // PrimitiveESTOP_Viper650();
104  PrimitiveSTOP_Viper650();
105  std::cout << "Robot was stopped\n";
106 
107  // Free allocated resources
108  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109 
110  fprintf(stdout, "Application ");
111  fflush(stdout);
112  kill(getpid(), SIGKILL);
113  exit(1);
114 }
115 
116 /* ---------------------------------------------------------------------- */
117 /* --- CONSTRUCTOR ------------------------------------------------------ */
118 /* ---------------------------------------------------------------------- */
119 
174 vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
175 {
176 
177  /*
178  #define SIGHUP 1 // hangup
179  #define SIGINT 2 // interrupt (rubout)
180  #define SIGQUIT 3 // quit (ASCII FS)
181  #define SIGILL 4 // illegal instruction (not reset when caught)
182  #define SIGTRAP 5 // trace trap (not reset when caught)
183  #define SIGIOT 6 // IOT instruction
184  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
185  #define SIGEMT 7 // EMT instruction
186  #define SIGFPE 8 // floating point exception
187  #define SIGKILL 9 // kill (cannot be caught or ignored)
188  #define SIGBUS 10 // bus error
189  #define SIGSEGV 11 // segmentation violation
190  #define SIGSYS 12 // bad argument to system call
191  #define SIGPIPE 13 // write on a pipe with no one to read it
192  #define SIGALRM 14 // alarm clock
193  #define SIGTERM 15 // software termination signal from kill
194  */
195 
196  signal(SIGINT, emergencyStopViper650);
197  signal(SIGBUS, emergencyStopViper650);
198  signal(SIGSEGV, emergencyStopViper650);
199  signal(SIGKILL, emergencyStopViper650);
200  signal(SIGQUIT, emergencyStopViper650);
201 
202  setVerbose(verbose);
203  if (verbose_)
204  std::cout << "Open communication with MotionBlox.\n";
205  try {
206  this->init();
208  }
209  catch (...) {
210  // vpERROR_TRACE("Error caught") ;
211  throw;
212  }
213  m_positioningVelocity = m_defaultPositioningVelocity;
214 
215  m_maxRotationVelocity_joint6 = maxRotationVelocity;
216 
217  vpRobotViper650::m_robotAlreadyCreated = true;
218 
219  return;
220 }
221 
222 /* ------------------------------------------------------------------------ */
223 /* --- INITIALISATION ----------------------------------------------------- */
224 /* ------------------------------------------------------------------------ */
225 
249 {
250  InitTry;
251 
252  // Initialise private variables used to compute the measured velocities
253  m_q_prev_getvel.resize(6);
254  m_q_prev_getvel = 0;
255  m_time_prev_getvel = 0;
256  m_first_time_getvel = true;
257 
258  // Initialise private variables used to compute the measured displacement
259  m_q_prev_getdis.resize(6);
260  m_q_prev_getdis = 0;
261  m_first_time_getdis = true;
262 
263  // Initialize the firewire connection
264  Try(InitializeConnection(verbose_));
265 
266  // Connect to the servoboard using the servo board GUID
267  Try(InitializeNode_Viper650());
268 
269  Try(PrimitiveRESET_Viper650());
270 
271  // Enable the joint limits on axis 6
272  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
273 
274  // Update the eMc matrix in the low level controller
276 
277  // Look if the power is on or off
278  UInt32 HIPowerStatus;
279  UInt32 EStopStatus;
280  Try(PrimitiveSTATUS_Viper650(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
281  CAL_Wait(0.1);
282 
283  // Print the robot status
284  if (verbose_) {
285  std::cout << "Robot status: ";
286  switch (EStopStatus) {
287  case ESTOP_AUTO:
288  m_controlMode = AUTO;
289  if (HIPowerStatus == 0)
290  std::cout << "Power is OFF" << std::endl;
291  else
292  std::cout << "Power is ON" << std::endl;
293  break;
294 
295  case ESTOP_MANUAL:
296  m_controlMode = MANUAL;
297  if (HIPowerStatus == 0)
298  std::cout << "Power is OFF" << std::endl;
299  else
300  std::cout << "Power is ON" << std::endl;
301  break;
302  case ESTOP_ACTIVATED:
303  m_controlMode = ESTOP;
304  std::cout << "Emergency stop is activated" << std::endl;
305  break;
306  default:
307  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
308  std::cout << "You have to call Adept for maintenance..." << std::endl;
309  // Free allocated resources
310  }
311  std::cout << std::endl;
312  }
313  // get real joint min/max from the MotionBlox
314  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
315  // Convert units from degrees to radians
316  joint_min.deg2rad();
317  joint_max.deg2rad();
318 
319  // for (unsigned int i=0; i < njoint; i++) {
320  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
321  // joint_max[i]);
322  // }
323 
324  // If an error occur in the low level controller, goto here
325  // CatchPrint();
326  Catch();
327 
328  // Test if an error occurs
329  if (TryStt == -20001)
330  printf("No connection detected. Check if the robot is powered on \n"
331  "and if the firewire link exist between the MotionBlox and this "
332  "computer.\n");
333  else if (TryStt == -675)
334  printf(" Timeout enabling power...\n");
335 
336  if (TryStt < 0) {
337  // Power off the robot
338  PrimitivePOWEROFF_Viper650();
339  // Free allocated resources
340  ShutDownConnection();
341 
342  std::cout << "Cannot open connection with the motionblox..." << std::endl;
343  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
344  }
345  return;
346 }
347 
405 {
406  // Read the robot constants from files
407  // - joint [min,max], coupl_56, long_56
408  // - camera extrinsic parameters relative to eMc
410 
411  InitTry;
412 
413  // Get real joint min/max from the MotionBlox
414  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
415  // Convert units from degrees to radians
416  joint_min.deg2rad();
417  joint_max.deg2rad();
418 
419  // for (unsigned int i=0; i < njoint; i++) {
420  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
421  // joint_max[i]);
422  // }
423 
424  // Set the camera constant (eMc pose) in the MotionBlox
425  double eMc_pose[6];
426  for (unsigned int i = 0; i < 3; i++) {
427  eMc_pose[i] = etc[i]; // translation in meters
428  eMc_pose[i + 3] = erc[i]; // rotation in rad
429  }
430  // Update the eMc pose in the low level controller
431  Try(PrimitiveCONST_Viper650(eMc_pose));
432 
433  CatchPrint();
434  return;
435 }
436 
487 void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename)
488 {
489  vpViper650::init(tool, filename);
490 
491  InitTry;
492 
493  // Get real joint min/max from the MotionBlox
494  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
495  // Convert units from degrees to radians
496  joint_min.deg2rad();
497  joint_max.deg2rad();
498 
499  // for (unsigned int i=0; i < njoint; i++) {
500  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
501  // joint_max[i]);
502  // }
503 
504  // Set the camera constant (eMc pose) in the MotionBlox
505  double eMc_pose[6];
506  for (unsigned int i = 0; i < 3; i++) {
507  eMc_pose[i] = etc[i]; // translation in meters
508  eMc_pose[i + 3] = erc[i]; // rotation in rad
509  }
510  // Update the eMc pose in the low level controller
511  Try(PrimitiveCONST_Viper650(eMc_pose));
512 
513  CatchPrint();
514  return;
515 }
516 
554 {
555  vpViper650::init(tool, eMc_);
556 
557  InitTry;
558 
559  // Get real joint min/max from the MotionBlox
560  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
561  // Convert units from degrees to radians
562  joint_min.deg2rad();
563  joint_max.deg2rad();
564 
565  // for (unsigned int i=0; i < njoint; i++) {
566  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
567  // joint_max[i]);
568  // }
569 
570  // Set the camera constant (eMc pose) in the MotionBlox
571  double eMc_pose[6];
572  for (unsigned int i = 0; i < 3; i++) {
573  eMc_pose[i] = etc[i]; // translation in meters
574  eMc_pose[i + 3] = erc[i]; // rotation in rad
575  }
576  // Update the eMc pose in the low level controller
577  Try(PrimitiveCONST_Viper650(eMc_pose));
578 
579  CatchPrint();
580  return;
581 }
582 
595 {
596  this->vpViper650::set_eMc(eMc_);
597 
598  InitTry;
599 
600  // Set the camera constant (eMc pose) in the MotionBlox
601  double eMc_pose[6];
602  for (unsigned int i = 0; i < 3; i++) {
603  eMc_pose[i] = etc[i]; // translation in meters
604  eMc_pose[i + 3] = erc[i]; // rotation in rad
605  }
606  // Update the eMc pose in the low level controller
607  Try(PrimitiveCONST_Viper650(eMc_pose));
608 
609  CatchPrint();
610 
611  return;
612 }
613 
627 {
628  this->vpViper650::set_eMc(etc_, erc_);
629 
630  InitTry;
631 
632  // Set the camera constant (eMc pose) in the MotionBlox
633  double eMc_pose[6];
634  for (unsigned int i = 0; i < 3; i++) {
635  eMc_pose[i] = etc[i]; // translation in meters
636  eMc_pose[i + 3] = erc[i]; // rotation in rad
637  }
638  // Update the eMc pose in the low level controller
639  Try(PrimitiveCONST_Viper650(eMc_pose));
640 
641  CatchPrint();
642 
643  return;
644 }
645 
646 /* ------------------------------------------------------------------------ */
647 /* --- DESTRUCTOR --------------------------------------------------------- */
648 /* ------------------------------------------------------------------------ */
649 
657 {
658  InitTry;
659 
661 
662  // Look if the power is on or off
663  UInt32 HIPowerStatus;
664  Try(PrimitiveSTATUS_Viper650(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
665  CAL_Wait(0.1);
666 
667  // if (HIPowerStatus == 1) {
668  // fprintf(stdout, "Power OFF the robot\n");
669  // fflush(stdout);
670 
671  // Try( PrimitivePOWEROFF_Viper650() );
672  // }
673 
674  // Free allocated resources
675  ShutDownConnection();
676 
677  vpRobotViper650::m_robotAlreadyCreated = false;
678 
679  CatchPrint();
680  return;
681 }
682 
690 {
691  InitTry;
692 
693  switch (newState) {
694  case vpRobot::STATE_STOP: {
695  // Start primitive STOP only if the current state is Velocity
697  Try(PrimitiveSTOP_Viper650());
698  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
699  }
700  break;
701  }
704  std::cout << "Change the control mode from velocity to position control.\n";
705  Try(PrimitiveSTOP_Viper650());
706  }
707  else {
708  // std::cout << "Change the control mode from stop to position
709  // control.\n";
710  }
711  this->powerOn();
712  break;
713  }
716  std::cout << "Change the control mode from stop to velocity control.\n";
717  }
718  this->powerOn();
719  break;
720  }
721  default:
722  break;
723  }
724 
725  CatchPrint();
726 
727  return vpRobot::setRobotState(newState);
728 }
729 
730 /* ------------------------------------------------------------------------ */
731 /* --- STOP --------------------------------------------------------------- */
732 /* ------------------------------------------------------------------------ */
733 
742 {
744  return;
745 
746  InitTry;
747  Try(PrimitiveSTOP_Viper650());
749 
750  CatchPrint();
751  if (TryStt < 0) {
752  vpERROR_TRACE("Cannot stop robot motion");
753  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
754  }
755 }
756 
767 {
768  InitTry;
769 
770  // Look if the power is on or off
771  UInt32 HIPowerStatus;
772  UInt32 EStopStatus;
773  bool firsttime = true;
774  unsigned int nitermax = 10;
775 
776  for (unsigned int i = 0; i < nitermax; i++) {
777  Try(PrimitiveSTATUS_Viper650(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
778  if (EStopStatus == ESTOP_AUTO) {
779  m_controlMode = AUTO;
780  break; // exit for loop
781  }
782  else if (EStopStatus == ESTOP_MANUAL) {
783  m_controlMode = MANUAL;
784  break; // exit for loop
785  }
786  else if (EStopStatus == ESTOP_ACTIVATED) {
787  m_controlMode = ESTOP;
788  if (firsttime) {
789  std::cout << "Emergency stop is activated! \n"
790  << "Check the emergency stop button and push the yellow "
791  "button before continuing."
792  << std::endl;
793  firsttime = false;
794  }
795  fprintf(stdout, "Remaining time %us \r", nitermax - i);
796  fflush(stdout);
797  CAL_Wait(1);
798  }
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  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
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(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &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(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &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.build(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 {
951  }
952  catch (...) {
953  vpERROR_TRACE("catch exception ");
954  throw;
955  }
956 }
970 {
971 
972  double position[6];
973  double timestamp;
974 
975  InitTry;
976  Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
977  CatchPrint();
978 
979  vpColVector q(6);
980  for (unsigned int i = 0; i < njoint; i++)
981  q[i] = position[i];
982 
983  try {
985  }
986  catch (...) {
987  vpERROR_TRACE("Error caught");
988  throw;
989  }
990 }
991 
1028 void vpRobotViper650::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1029 
1035 double vpRobotViper650::getPositioningVelocity(void) const { return m_positioningVelocity; }
1036 
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, m_positioningVelocity));
1162  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1163  }
1164  else {
1165  // Cartesian position is out of range
1166  error = -1;
1167  }
1168 
1169  break;
1170  }
1171  case vpRobot::ARTICULAR_FRAME: {
1172  destination = position;
1173  // convert rad to deg requested for the low level controller
1174  destination.rad2deg();
1175 
1176  // std::cout << "Joint destination (deg): " << destination.t() <<
1177  // std::endl;
1178  Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1179  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1180  break;
1181  }
1182  case vpRobot::REFERENCE_FRAME: {
1183  // Convert angles from Rxyz representation to Rzyz representation
1184  vpRxyzVector rxyz(position[3], position[4], position[5]);
1185  vpRotationMatrix R(rxyz);
1186  vpRzyzVector rzyz(R);
1187 
1188  for (unsigned int i = 0; i < 3; i++) {
1189  destination[i] = position[i];
1190  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1191  }
1192  int configuration = 0; // keep the actual configuration
1193 
1194  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1195  // << std::endl;
1196  Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, m_positioningVelocity));
1197  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1198 
1199  break;
1200  }
1201  case vpRobot::MIXT_FRAME: {
1202  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1203  "Mixt frame not implemented.");
1204  }
1206  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1207  "Mixt frame not implemented.");
1208  }
1209  }
1210 
1211  CatchPrint();
1212  if (TryStt == InvalidPosition || TryStt == -1023)
1213  std::cout << " : Position out of range.\n";
1214 
1215  else if (TryStt == -3019) {
1216  if (frame == vpRobot::ARTICULAR_FRAME)
1217  std::cout << " : Joint position out of range.\n";
1218  else
1219  std::cout << " : Cartesian position leads to a joint position out of "
1220  "range.\n";
1221  }
1222  else if (TryStt < 0)
1223  std::cout << " : Unknown error (see Fabien).\n";
1224  else if (error == -1)
1225  std::cout << "Position out of range.\n";
1226 
1227  if (TryStt < 0 || error < 0) {
1228  vpERROR_TRACE("Positioning error.");
1229  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1230  }
1231 
1232  return;
1233 }
1234 
1299 void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1300  double pos4, double pos5, double pos6)
1301 {
1302  try {
1303  vpColVector position(6);
1304  position[0] = pos1;
1305  position[1] = pos2;
1306  position[2] = pos3;
1307  position[3] = pos4;
1308  position[4] = pos5;
1309  position[5] = pos6;
1310 
1311  setPosition(frame, position);
1312  }
1313  catch (...) {
1314  vpERROR_TRACE("Error caught");
1315  throw;
1316  }
1317 }
1318 
1357 void vpRobotViper650::setPosition(const std::string &filename)
1358 {
1359  vpColVector q;
1360  bool ret;
1361 
1362  ret = this->readPosFile(filename, q);
1363 
1364  if (ret == false) {
1365  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1366  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1367  }
1370 }
1371 
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.build(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 
1649 {
1651  vpERROR_TRACE("Cannot send a velocity to the robot "
1652  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1654  "Cannot send a velocity to the robot "
1655  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1656  }
1657 
1658  vpColVector vel_sat(6);
1659 
1660  // Velocity saturation
1661  switch (frame) {
1662  // saturation in cartesian space
1663  case vpRobot::CAMERA_FRAME:
1666  case vpRobot::MIXT_FRAME: {
1667  vpColVector vel_max(6);
1668 
1669  for (unsigned int i = 0; i < 3; i++)
1670  vel_max[i] = getMaxTranslationVelocity();
1671  for (unsigned int i = 3; i < 6; i++)
1672  vel_max[i] = getMaxRotationVelocity();
1673 
1674  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1675 
1676  break;
1677  }
1678  // saturation in joint space
1679  case vpRobot::ARTICULAR_FRAME: {
1680  vpColVector vel_max(6);
1681 
1682  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1683  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1684  for (unsigned int i = 0; i < 6; i++)
1685  vel_max[i] = getMaxRotationVelocity();
1686  }
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(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
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  }
1751  else {
1752  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1753  if (TryString != nullptr) {
1754  // The statement is in TryString, but we need to check the validity
1755  printf(" Error sentence %s\n", TryString); // Print the TryString
1756  }
1757  else {
1758  printf("\n");
1759  }
1760  }
1761  }
1762 
1763  return;
1764 }
1765 
1766 /* ------------------------------------------------------------------------ */
1767 /* --- GET ---------------------------------------------------------------- */
1768 /* ------------------------------------------------------------------------ */
1769 
1826 void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1827 {
1828  velocity.resize(6);
1829  velocity = 0;
1830 
1831  vpColVector q_cur(6);
1832  vpHomogeneousMatrix fMe_cur, fMc_cur;
1833  vpHomogeneousMatrix eMe, cMc; // camera displacement
1834  double time_cur;
1835 
1836  InitTry;
1837 
1838  // Get the current joint position
1839  Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
1840  time_cur = timestamp;
1841  q_cur.deg2rad();
1842 
1843  // Get the end-effector pose from the direct kinematics
1844  vpViper650::get_fMe(q_cur, fMe_cur);
1845  // Get the camera pose from the direct kinematics
1846  vpViper650::get_fMc(q_cur, fMc_cur);
1847 
1848  if (!m_first_time_getvel) {
1849 
1850  switch (frame) {
1852  // Compute the displacement of the end-effector since the previous call
1853  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1854 
1855  // Compute the velocity of the end-effector from this displacement
1856  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1857 
1858  break;
1859  }
1860 
1861  case vpRobot::CAMERA_FRAME: {
1862  // Compute the displacement of the camera since the previous call
1863  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1864 
1865  // Compute the velocity of the camera from this displacement
1866  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1867 
1868  break;
1869  }
1870 
1871  case vpRobot::ARTICULAR_FRAME: {
1872  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1873  break;
1874  }
1875 
1876  case vpRobot::REFERENCE_FRAME: {
1877  // Compute the displacement of the camera since the previous call
1878  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1879 
1880  // Compute the velocity of the camera from this displacement
1881  vpColVector v;
1882  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1883 
1884  // Express this velocity in the reference frame
1885  vpVelocityTwistMatrix fVc(fMc_cur);
1886  velocity = fVc * v;
1887 
1888  break;
1889  }
1890 
1891  case vpRobot::MIXT_FRAME: {
1892  // Compute the displacement of the camera since the previous call
1893  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1894 
1895  // Compute the ThetaU representation for the rotation
1896  vpRotationMatrix cRc;
1897  cMc.extract(cRc);
1898  vpThetaUVector thetaU;
1899  thetaU.build(cRc);
1900 
1901  for (unsigned int i = 0; i < 3; i++) {
1902  // Compute the translation displacement in the reference frame
1903  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1904  // Update the rotation displacement in the camera frame
1905  velocity[i + 3] = thetaU[i];
1906  }
1907 
1908  // Compute the velocity
1909  velocity /= (time_cur - m_time_prev_getvel);
1910  break;
1911  }
1912  default: {
1914  "vpRobotViper650::getVelocity() not implemented in end-effector"));
1915  }
1916  }
1917  }
1918  else {
1919  m_first_time_getvel = false;
1920  }
1921 
1922  // Memorize the end-effector pose for the next call
1923  m_fMe_prev_getvel = fMe_cur;
1924  // Memorize the camera pose for the next call
1925  m_fMc_prev_getvel = fMc_cur;
1926 
1927  // Memorize the joint position for the next call
1928  m_q_prev_getvel = q_cur;
1929 
1930  // Memorize the time associated to the joint position for the next call
1931  m_time_prev_getvel = time_cur;
1932 
1933  CatchPrint();
1934  if (TryStt < 0) {
1935  vpERROR_TRACE("Cannot get velocity.");
1936  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1937  }
1938 }
1939 
1949 {
1950  double timestamp;
1951  getVelocity(frame, velocity, timestamp);
1952 }
1953 
2004 {
2005  vpColVector velocity;
2006  getVelocity(frame, velocity, timestamp);
2007 
2008  return velocity;
2009 }
2010 
2020 {
2021  vpColVector velocity;
2022  double timestamp;
2023  getVelocity(frame, velocity, timestamp);
2024 
2025  return velocity;
2026 }
2027 
2093 bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
2094 {
2095  std::ifstream fd(filename.c_str(), std::ios::in);
2096 
2097  if (!fd.is_open()) {
2098  return false;
2099  }
2100 
2101  std::string line;
2102  std::string key("R:");
2103  std::string id("#Viper650 - Position");
2104  bool pos_found = false;
2105  int lineNum = 0;
2106 
2107  q.resize(njoint);
2108 
2109  while (std::getline(fd, line)) {
2110  lineNum++;
2111  if (lineNum == 1) {
2112  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
2113  std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl;
2114  return false;
2115  }
2116  }
2117  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2118  continue;
2119  }
2120  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2121  // check if there are at least njoint values in the line
2122  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2123  if (chain.size() < njoint + 1) // try to split with tab separator
2124  chain = vpIoTools::splitChain(line, std::string("\t"));
2125  if (chain.size() < njoint + 1)
2126  continue;
2127 
2128  std::istringstream ss(line);
2129  std::string key_;
2130  ss >> key_;
2131  for (unsigned int i = 0; i < njoint; i++)
2132  ss >> q[i];
2133  pos_found = true;
2134  break;
2135  }
2136  }
2137 
2138  // converts rotations from degrees into radians
2139  q.deg2rad();
2140 
2141  fd.close();
2142 
2143  if (!pos_found) {
2144  std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2145  return false;
2146  }
2147 
2148  return true;
2149 }
2150 
2174 bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
2175 {
2176 
2177  FILE *fd;
2178  fd = fopen(filename.c_str(), "w");
2179  if (fd == nullptr)
2180  return false;
2181 
2182  fprintf(fd, "\
2183 #Viper650 - Position - Version 1.00\n\
2184 #\n\
2185 # R: A B C D E F\n\
2186 # Joint position in degrees\n\
2187 #\n\
2188 #\n\n");
2189 
2190  // Save positions in mm and deg
2191  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2192  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2193 
2194  fclose(fd);
2195  return (true);
2196 }
2197 
2208 void vpRobotViper650::move(const std::string &filename)
2209 {
2210  vpColVector q;
2211 
2212  try {
2213  this->readPosFile(filename, q);
2215  this->setPositioningVelocity(10);
2217  }
2218  catch (...) {
2219  throw;
2220  }
2221 }
2222 
2242 {
2243  displacement.resize(6);
2244  displacement = 0;
2245 
2246  double q[6];
2247  vpColVector q_cur(6);
2248  double timestamp;
2249 
2250  InitTry;
2251 
2252  // Get the current joint position
2253  Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
2254  for (unsigned int i = 0; i < njoint; i++) {
2255  q_cur[i] = q[i];
2256  }
2257 
2258  if (!m_first_time_getdis) {
2259  switch (frame) {
2260  case vpRobot::CAMERA_FRAME: {
2261  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2262  return;
2263  }
2264 
2265  case vpRobot::ARTICULAR_FRAME: {
2266  displacement = q_cur - m_q_prev_getdis;
2267  break;
2268  }
2269 
2270  case vpRobot::REFERENCE_FRAME: {
2271  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2272  return;
2273  }
2274 
2275  case vpRobot::MIXT_FRAME: {
2276  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2277  return;
2278  }
2280  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2281  return;
2282  }
2283  }
2284  }
2285  else {
2286  m_first_time_getdis = false;
2287  }
2288 
2289  // Memorize the joint position for the next call
2290  m_q_prev_getdis = q_cur;
2291 
2292  CatchPrint();
2293  if (TryStt < 0) {
2294  vpERROR_TRACE("Cannot get velocity.");
2295  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2296  }
2297 }
2298 
2313 {
2314  InitTry;
2315 
2316  Try(PrimitiveTFS_BIAS_Viper650());
2317 
2318  // Wait 500 ms to be sure the next measures take into account the bias
2319  vpTime::wait(500);
2320 
2321  CatchPrint();
2322  if (TryStt < 0) {
2323  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2324  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2325  }
2326 }
2327 
2368 {
2369  InitTry;
2370 
2371  H.resize(6);
2372 
2373  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2374 
2375  CatchPrint();
2376  if (TryStt < 0) {
2377  vpERROR_TRACE("Cannot get the force/torque measures.");
2378  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2379  }
2380 }
2381 
2420 {
2421  InitTry;
2422 
2423  vpColVector H(6);
2424 
2425  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2426 
2427  return H;
2428 
2429  CatchPrint();
2430  if (TryStt < 0) {
2431  vpERROR_TRACE("Cannot get the force/torque measures.");
2432  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2433  }
2434  return H; // Here to avoid a warning, but should never be called
2435 }
2436 
2443 {
2444  InitTry;
2445  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2446  std::cout << "Enable joint limits on axis 6..." << std::endl;
2447  CatchPrint();
2448  if (TryStt < 0) {
2449  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2450  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2451  }
2452 }
2453 
2465 {
2466  InitTry;
2467  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2468  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2469  CatchPrint();
2470  if (TryStt < 0) {
2471  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2472  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2473  }
2474 }
2475 
2485 {
2488 
2489  return;
2490 }
2491 
2512 {
2513  m_maxRotationVelocity_joint6 = w6_max;
2514  return;
2515 }
2516 
2524 double vpRobotViper650::getMaxRotationVelocityJoint6() const { return m_maxRotationVelocity_joint6; }
2525 
2533 {
2534  InitTry;
2535  Try(PrimitivePneumaticGripper_Viper650(1));
2536  std::cout << "Open the pneumatic gripper..." << std::endl;
2537  CatchPrint();
2538  if (TryStt < 0) {
2539  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2540  }
2541 }
2542 
2551 {
2552  InitTry;
2553  Try(PrimitivePneumaticGripper_Viper650(0));
2554  std::cout << "Close the pneumatic gripper..." << std::endl;
2555  CatchPrint();
2556  if (TryStt < 0) {
2557  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2558  }
2559 }
2560 
2561 #elif !defined(VISP_BUILD_SHARED_LIBS)
2562 // Work around to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
2563 // no symbols
2564 void dummy_vpRobotViper650() { };
2565 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & rad2deg()
Definition: vpColVector.h:972
vpColVector & deg2rad()
Definition: vpColVector.h:344
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:65
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:2427
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:190
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.
void move(const std::string &filename)
void set_eMc(const vpHomogeneousMatrix &eMc_)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
static bool savePosFile(const std::string &filename, const vpColVector &q)
static const double m_defaultPositioningVelocity
virtual ~vpRobotViper650(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPositioningVelocity(double velocity)
void enableJoint6Limits() const
void get_cVe(vpVelocityTwistMatrix &cVe) const
vpColVector getForceTorque() const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
bool getPowerState() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocityJoint6(double w6_max)
void disableJoint6Limits() const
void biasForceTorqueSensor() const
void get_cMe(vpHomogeneousMatrix &cMe) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
double getMaxRotationVelocityJoint6() const
void get_fJe(vpMatrix &fJe) vp_override
void get_eJe(vpMatrix &eJe) vp_override
void setMaxRotationVelocity(double w_max)
double getTime() const
static bool readPosFile(const std::string &filename, vpColVector &q)
double getPositioningVelocity(void) const
void closeGripper() const
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void setVerbose(bool verbose)
Definition: vpRobot.h:168
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
double maxRotationVelocity
Definition: vpRobot.h:98
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:257
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:177
vpRxyzVector & build(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:176
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)
Modelization of the ADEPT Viper 650 robot.
Definition: vpViper650.h:93
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:166
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:127
void init(void)
Definition: vpViper650.cpp:132
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:118
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1224
vpTranslationVector etc
Definition: vpViper.h:156
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:916
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1153
vpColVector joint_max
Definition: vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:567
vpRxyzVector erc
Definition: vpViper.h:157
vpColVector joint_min
Definition: vpViper.h:170
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:151
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:964
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:719
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:604
#define vpERROR_TRACE
Definition: vpDebug.h:384
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)