Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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::m_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 
177 vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
178 {
179 
180  /*
181  #define SIGHUP 1 // hangup
182  #define SIGINT 2 // interrupt (rubout)
183  #define SIGQUIT 3 // quit (ASCII FS)
184  #define SIGILL 4 // illegal instruction (not reset when caught)
185  #define SIGTRAP 5 // trace trap (not reset when caught)
186  #define SIGIOT 6 // IOT instruction
187  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
188  #define SIGEMT 7 // EMT instruction
189  #define SIGFPE 8 // floating point exception
190  #define SIGKILL 9 // kill (cannot be caught or ignored)
191  #define SIGBUS 10 // bus error
192  #define SIGSEGV 11 // segmentation violation
193  #define SIGSYS 12 // bad argument to system call
194  #define SIGPIPE 13 // write on a pipe with no one to read it
195  #define SIGALRM 14 // alarm clock
196  #define SIGTERM 15 // software termination signal from kill
197  */
198 
199  signal(SIGINT, emergencyStopViper650);
200  signal(SIGBUS, emergencyStopViper650);
201  signal(SIGSEGV, emergencyStopViper650);
202  signal(SIGKILL, emergencyStopViper650);
203  signal(SIGQUIT, emergencyStopViper650);
204 
205  setVerbose(verbose);
206  if (verbose_)
207  std::cout << "Open communication with MotionBlox.\n";
208  try {
209  this->init();
211  } catch (...) {
212  // vpERROR_TRACE("Error caught") ;
213  throw;
214  }
215  m_positioningVelocity = m_defaultPositioningVelocity;
216 
217  m_maxRotationVelocity_joint6 = maxRotationVelocity;
218 
219  vpRobotViper650::m_robotAlreadyCreated = true;
220 
221  return;
222 }
223 
224 /* ------------------------------------------------------------------------ */
225 /* --- INITIALISATION ----------------------------------------------------- */
226 /* ------------------------------------------------------------------------ */
227 
251 {
252  InitTry;
253 
254  // Initialise private variables used to compute the measured velocities
255  m_q_prev_getvel.resize(6);
256  m_q_prev_getvel = 0;
257  m_time_prev_getvel = 0;
258  m_first_time_getvel = true;
259 
260  // Initialise private variables used to compute the measured displacement
261  m_q_prev_getdis.resize(6);
262  m_q_prev_getdis = 0;
263  m_first_time_getdis = true;
264 
265  // Initialize the firewire connection
266  Try(InitializeConnection(verbose_));
267 
268  // Connect to the servoboard using the servo board GUID
269  Try(InitializeNode_Viper650());
270 
271  Try(PrimitiveRESET_Viper650());
272 
273  // Enable the joint limits on axis 6
274  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
275 
276  // Update the eMc matrix in the low level controller
278 
279  // Look if the power is on or off
280  UInt32 HIPowerStatus;
281  UInt32 EStopStatus;
282  Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
283  CAL_Wait(0.1);
284 
285  // Print the robot status
286  if (verbose_) {
287  std::cout << "Robot status: ";
288  switch (EStopStatus) {
289  case ESTOP_AUTO:
290  m_controlMode = AUTO;
291  if (HIPowerStatus == 0)
292  std::cout << "Power is OFF" << std::endl;
293  else
294  std::cout << "Power is ON" << std::endl;
295  break;
296 
297  case ESTOP_MANUAL:
298  m_controlMode = MANUAL;
299  if (HIPowerStatus == 0)
300  std::cout << "Power is OFF" << std::endl;
301  else
302  std::cout << "Power is ON" << std::endl;
303  break;
304  case ESTOP_ACTIVATED:
305  m_controlMode = ESTOP;
306  std::cout << "Emergency stop is activated" << std::endl;
307  break;
308  default:
309  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
310  std::cout << "You have to call Adept for maintenance..." << std::endl;
311  // Free allocated resources
312  }
313  std::cout << std::endl;
314  }
315  // get real joint min/max from the MotionBlox
316  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
317  // Convert units from degrees to radians
318  joint_min.deg2rad();
319  joint_max.deg2rad();
320 
321  // for (unsigned int i=0; i < njoint; i++) {
322  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
323  // joint_max[i]);
324  // }
325 
326  // If an error occur in the low level controller, goto here
327  // CatchPrint();
328  Catch();
329 
330  // Test if an error occurs
331  if (TryStt == -20001)
332  printf("No connection detected. Check if the robot is powered on \n"
333  "and if the firewire link exist between the MotionBlox and this "
334  "computer.\n");
335  else if (TryStt == -675)
336  printf(" Timeout enabling power...\n");
337 
338  if (TryStt < 0) {
339  // Power off the robot
340  PrimitivePOWEROFF_Viper650();
341  // Free allocated resources
342  ShutDownConnection();
343 
344  std::cout << "Cannot open connection with the motionblox..." << std::endl;
345  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
346  }
347  return;
348 }
349 
407 {
408  // Read the robot constants from files
409  // - joint [min,max], coupl_56, long_56
410  // - camera extrinsic parameters relative to eMc
411  vpViper650::init(tool, projModel);
412 
413  InitTry;
414 
415  // Get real joint min/max from the MotionBlox
416  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
417  // Convert units from degrees to radians
418  joint_min.deg2rad();
419  joint_max.deg2rad();
420 
421  // for (unsigned int i=0; i < njoint; i++) {
422  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
423  // joint_max[i]);
424  // }
425 
426  // Set the camera constant (eMc pose) in the MotionBlox
427  double eMc_pose[6];
428  for (unsigned int i = 0; i < 3; i++) {
429  eMc_pose[i] = etc[i]; // translation in meters
430  eMc_pose[i + 3] = erc[i]; // rotation in rad
431  }
432  // Update the eMc pose in the low level controller
433  Try(PrimitiveCONST_Viper650(eMc_pose));
434 
435  CatchPrint();
436  return;
437 }
438 
489 void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename)
490 {
491  vpViper650::init(tool, filename);
492 
493  InitTry;
494 
495  // Get real joint min/max from the MotionBlox
496  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
497  // Convert units from degrees to radians
498  joint_min.deg2rad();
499  joint_max.deg2rad();
500 
501  // for (unsigned int i=0; i < njoint; i++) {
502  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
503  // joint_max[i]);
504  // }
505 
506  // Set the camera constant (eMc pose) in the MotionBlox
507  double eMc_pose[6];
508  for (unsigned int i = 0; i < 3; i++) {
509  eMc_pose[i] = etc[i]; // translation in meters
510  eMc_pose[i + 3] = erc[i]; // rotation in rad
511  }
512  // Update the eMc pose in the low level controller
513  Try(PrimitiveCONST_Viper650(eMc_pose));
514 
515  CatchPrint();
516  return;
517 }
518 
556 {
557  vpViper650::init(tool, eMc_);
558 
559  InitTry;
560 
561  // Get real joint min/max from the MotionBlox
562  Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
563  // Convert units from degrees to radians
564  joint_min.deg2rad();
565  joint_max.deg2rad();
566 
567  // for (unsigned int i=0; i < njoint; i++) {
568  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
569  // joint_max[i]);
570  // }
571 
572  // Set the camera constant (eMc pose) in the MotionBlox
573  double eMc_pose[6];
574  for (unsigned int i = 0; i < 3; i++) {
575  eMc_pose[i] = etc[i]; // translation in meters
576  eMc_pose[i + 3] = erc[i]; // rotation in rad
577  }
578  // Update the eMc pose in the low level controller
579  Try(PrimitiveCONST_Viper650(eMc_pose));
580 
581  CatchPrint();
582  return;
583 }
584 
597 {
598  this->vpViper650::set_eMc(eMc_);
599 
600  InitTry;
601 
602  // Set the camera constant (eMc pose) in the MotionBlox
603  double eMc_pose[6];
604  for (unsigned int i = 0; i < 3; i++) {
605  eMc_pose[i] = etc[i]; // translation in meters
606  eMc_pose[i + 3] = erc[i]; // rotation in rad
607  }
608  // Update the eMc pose in the low level controller
609  Try(PrimitiveCONST_Viper650(eMc_pose));
610 
611  CatchPrint();
612 
613  return;
614 }
615 
629 {
630  this->vpViper650::set_eMc(etc_, erc_);
631 
632  InitTry;
633 
634  // Set the camera constant (eMc pose) in the MotionBlox
635  double eMc_pose[6];
636  for (unsigned int i = 0; i < 3; i++) {
637  eMc_pose[i] = etc[i]; // translation in meters
638  eMc_pose[i + 3] = erc[i]; // rotation in rad
639  }
640  // Update the eMc pose in the low level controller
641  Try(PrimitiveCONST_Viper650(eMc_pose));
642 
643  CatchPrint();
644 
645  return;
646 }
647 
648 /* ------------------------------------------------------------------------ */
649 /* --- DESTRUCTOR --------------------------------------------------------- */
650 /* ------------------------------------------------------------------------ */
651 
659 {
660  InitTry;
661 
663 
664  // Look if the power is on or off
665  UInt32 HIPowerStatus;
666  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
667  CAL_Wait(0.1);
668 
669  // if (HIPowerStatus == 1) {
670  // fprintf(stdout, "Power OFF the robot\n");
671  // fflush(stdout);
672 
673  // Try( PrimitivePOWEROFF_Viper650() );
674  // }
675 
676  // Free allocated resources
677  ShutDownConnection();
678 
679  vpRobotViper650::m_robotAlreadyCreated = false;
680 
681  CatchPrint();
682  return;
683 }
684 
692 {
693  InitTry;
694 
695  switch (newState) {
696  case vpRobot::STATE_STOP: {
697  // Start primitive STOP only if the current state is Velocity
699  Try(PrimitiveSTOP_Viper650());
700  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
701  }
702  break;
703  }
706  std::cout << "Change the control mode from velocity to position control.\n";
707  Try(PrimitiveSTOP_Viper650());
708  } else {
709  // std::cout << "Change the control mode from stop to position
710  // control.\n";
711  }
712  this->powerOn();
713  break;
714  }
717  std::cout << "Change the control mode from stop to velocity control.\n";
718  }
719  this->powerOn();
720  break;
721  }
722  default:
723  break;
724  }
725 
726  CatchPrint();
727 
728  return vpRobot::setRobotState(newState);
729 }
730 
731 /* ------------------------------------------------------------------------ */
732 /* --- STOP --------------------------------------------------------------- */
733 /* ------------------------------------------------------------------------ */
734 
743 {
745  return;
746 
747  InitTry;
748  Try(PrimitiveSTOP_Viper650());
750 
751  CatchPrint();
752  if (TryStt < 0) {
753  vpERROR_TRACE("Cannot stop robot motion");
754  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
755  }
756 }
757 
768 {
769  InitTry;
770 
771  // Look if the power is on or off
772  UInt32 HIPowerStatus;
773  UInt32 EStopStatus;
774  bool firsttime = true;
775  unsigned int nitermax = 10;
776 
777  for (unsigned int i = 0; i < nitermax; i++) {
778  Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
779  if (EStopStatus == ESTOP_AUTO) {
780  m_controlMode = AUTO;
781  break; // exit for loop
782  } else if (EStopStatus == ESTOP_MANUAL) {
783  m_controlMode = MANUAL;
784  break; // exit for loop
785  } else if (EStopStatus == ESTOP_ACTIVATED) {
786  m_controlMode = ESTOP;
787  if (firsttime) {
788  std::cout << "Emergency stop is activated! \n"
789  << "Check the emergency stop button and push the yellow "
790  "button before continuing."
791  << std::endl;
792  firsttime = false;
793  }
794  fprintf(stdout, "Remaining time %us \r", nitermax - i);
795  fflush(stdout);
796  CAL_Wait(1);
797  } else {
798  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
799  std::cout << "You have to call Adept for maintenance..." << std::endl;
800  // Free allocated resources
801  ShutDownConnection();
802  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
803  }
804  }
805 
806  if (EStopStatus == ESTOP_ACTIVATED)
807  std::cout << std::endl;
808 
809  if (EStopStatus == ESTOP_ACTIVATED) {
810  std::cout << "Sorry, cannot power on the robot." << std::endl;
811  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
812  }
813 
814  if (HIPowerStatus == 0) {
815  fprintf(stdout, "Power ON the Viper650 robot\n");
816  fflush(stdout);
817 
818  Try(PrimitivePOWERON_Viper650());
819  }
820 
821  CatchPrint();
822  if (TryStt < 0) {
823  vpERROR_TRACE("Cannot power on the robot");
824  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
825  }
826 }
827 
838 {
839  InitTry;
840 
841  // Look if the power is on or off
842  UInt32 HIPowerStatus;
843  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
844  CAL_Wait(0.1);
845 
846  if (HIPowerStatus == 1) {
847  fprintf(stdout, "Power OFF the Viper650 robot\n");
848  fflush(stdout);
849 
850  Try(PrimitivePOWEROFF_Viper650());
851  }
852 
853  CatchPrint();
854  if (TryStt < 0) {
855  vpERROR_TRACE("Cannot power off the robot");
856  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
857  }
858 }
859 
872 {
873  InitTry;
874  bool status = false;
875  // Look if the power is on or off
876  UInt32 HIPowerStatus;
877  Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
878  CAL_Wait(0.1);
879 
880  if (HIPowerStatus == 1) {
881  status = true;
882  }
883 
884  CatchPrint();
885  if (TryStt < 0) {
886  vpERROR_TRACE("Cannot get the power status");
887  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
888  }
889  return status;
890 }
891 
902 {
904  vpViper650::get_cMe(cMe);
905 
906  cVe.buildFrom(cMe);
907 }
908 
921 
934 {
935 
936  double position[6];
937  double timestamp;
938 
939  InitTry;
940  Try(PrimitiveACQ_POS_J_Viper650(position, &timestamp));
941  CatchPrint();
942 
943  vpColVector q(6);
944  for (unsigned int i = 0; i < njoint; i++)
945  q[i] = vpMath::rad(position[i]);
946 
947  try {
948  vpViper650::get_eJe(q, eJe);
949  } catch (...) {
950  vpERROR_TRACE("catch exception ");
951  throw;
952  }
953 }
967 {
968 
969  double position[6];
970  double timestamp;
971 
972  InitTry;
973  Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
974  CatchPrint();
975 
976  vpColVector q(6);
977  for (unsigned int i = 0; i < njoint; i++)
978  q[i] = position[i];
979 
980  try {
981  vpViper650::get_fJe(q, fJe);
982  } catch (...) {
983  vpERROR_TRACE("Error caught");
984  throw;
985  }
986 }
987 
1024 void vpRobotViper650::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1025 
1031 double vpRobotViper650::getPositioningVelocity(void) const { return m_positioningVelocity; }
1032 
1111 {
1112 
1114  vpERROR_TRACE("Robot was not in position-based control\n"
1115  "Modification of the robot state");
1117  }
1118 
1119  vpColVector destination(njoint);
1120  int error = 0;
1121  double timestamp;
1122 
1123  InitTry;
1124  switch (frame) {
1125  case vpRobot::CAMERA_FRAME: {
1126  vpColVector q(njoint);
1127  Try(PrimitiveACQ_POS_Viper650(q.data, &timestamp));
1128 
1129  // Convert degrees into rad
1130  q.deg2rad();
1131 
1132  // Get fMc from the inverse kinematics
1133  vpHomogeneousMatrix fMc;
1134  vpViper650::get_fMc(q, fMc);
1135 
1136  // Set cMc from the input position
1137  vpTranslationVector txyz;
1138  vpRxyzVector rxyz;
1139  for (unsigned int i = 0; i < 3; i++) {
1140  txyz[i] = position[i];
1141  rxyz[i] = position[i + 3];
1142  }
1143 
1144  // Compute cMc2
1145  vpRotationMatrix cRc2(rxyz);
1146  vpHomogeneousMatrix cMc2(txyz, cRc2);
1147 
1148  // Compute the new position to reach: fMc*cMc2
1149  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1150 
1151  // Compute the corresponding joint position from the inverse kinematics
1152  unsigned int solution = this->getInverseKinematics(fMc2, q);
1153  if (solution) { // Position is reachable
1154  destination = q;
1155  // convert rad to deg requested for the low level controller
1156  destination.rad2deg();
1157  Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1158  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1159  } else {
1160  // Cartesian position is out of range
1161  error = -1;
1162  }
1163 
1164  break;
1165  }
1166  case vpRobot::ARTICULAR_FRAME: {
1167  destination = position;
1168  // convert rad to deg requested for the low level controller
1169  destination.rad2deg();
1170 
1171  // std::cout << "Joint destination (deg): " << destination.t() <<
1172  // std::endl;
1173  Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1174  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1175  break;
1176  }
1177  case vpRobot::REFERENCE_FRAME: {
1178  // Convert angles from Rxyz representation to Rzyz representation
1179  vpRxyzVector rxyz(position[3], position[4], position[5]);
1180  vpRotationMatrix R(rxyz);
1181  vpRzyzVector rzyz(R);
1182 
1183  for (unsigned int i = 0; i < 3; i++) {
1184  destination[i] = position[i];
1185  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1186  }
1187  int configuration = 0; // keep the actual configuration
1188 
1189  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1190  // << std::endl;
1191  Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, m_positioningVelocity));
1192  Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1193 
1194  break;
1195  }
1196  case vpRobot::MIXT_FRAME: {
1197  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1198  "Mixt frame not implemented.");
1199  }
1201  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1202  "Mixt frame not implemented.");
1203  }
1204  }
1205 
1206  CatchPrint();
1207  if (TryStt == InvalidPosition || TryStt == -1023)
1208  std::cout << " : Position out of range.\n";
1209 
1210  else if (TryStt == -3019) {
1211  if (frame == vpRobot::ARTICULAR_FRAME)
1212  std::cout << " : Joint position out of range.\n";
1213  else
1214  std::cout << " : Cartesian position leads to a joint position out of "
1215  "range.\n";
1216  } else if (TryStt < 0)
1217  std::cout << " : Unknown error (see Fabien).\n";
1218  else if (error == -1)
1219  std::cout << "Position out of range.\n";
1220 
1221  if (TryStt < 0 || error < 0) {
1222  vpERROR_TRACE("Positionning error.");
1223  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1224  }
1225 
1226  return;
1227 }
1228 
1293 void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1294  double pos3, double pos4, double pos5, double pos6)
1295 {
1296  try {
1297  vpColVector position(6);
1298  position[0] = pos1;
1299  position[1] = pos2;
1300  position[2] = pos3;
1301  position[3] = pos4;
1302  position[4] = pos5;
1303  position[5] = pos6;
1304 
1305  setPosition(frame, position);
1306  } catch (...) {
1307  vpERROR_TRACE("Error caught");
1308  throw;
1309  }
1310 }
1311 
1350 void vpRobotViper650::setPosition(const std::string &filename)
1351 {
1352  vpColVector q;
1353  bool ret;
1354 
1355  ret = this->readPosFile(filename, q);
1356 
1357  if (ret == false) {
1358  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1359  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1360  }
1363 }
1364 
1432 void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1433 {
1434 
1435  InitTry;
1436 
1437  position.resize(6);
1438 
1439  switch (frame) {
1440  case vpRobot::CAMERA_FRAME: {
1441  position = 0;
1442  return;
1443  }
1444  case vpRobot::ARTICULAR_FRAME: {
1445  Try(PrimitiveACQ_POS_J_Viper650(position.data, &timestamp));
1446  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1447  position.deg2rad();
1448 
1449  return;
1450  }
1451  case vpRobot::REFERENCE_FRAME: {
1452  vpColVector q(njoint);
1453  Try(PrimitiveACQ_POS_J_Viper650(q.data, &timestamp));
1454 
1455  // Compute fMc
1457 
1458  // From fMc extract the pose
1459  vpRotationMatrix fRc;
1460  fMc.extract(fRc);
1461  vpRxyzVector rxyz;
1462  rxyz.buildFrom(fRc);
1463 
1464  for (unsigned int i = 0; i < 3; i++) {
1465  position[i] = fMc[i][3]; // translation x,y,z
1466  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1467  }
1468 
1469  break;
1470  }
1471  case vpRobot::MIXT_FRAME: {
1472  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1473  "not implemented");
1474  }
1476  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1477  "not implemented");
1478  }
1479  }
1480 
1481  CatchPrint();
1482  if (TryStt < 0) {
1483  vpERROR_TRACE("Cannot get position.");
1484  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1485  }
1486 
1487  return;
1488 }
1489 
1501 {
1502  double timestamp;
1503  getPosition(frame, position, timestamp);
1504 }
1505 
1519 void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1520 {
1521  vpColVector posRxyz;
1522  // recupere position en Rxyz
1523  this->getPosition(frame, posRxyz, timestamp);
1524  vpRxyzVector RxyzVect;
1525  for (unsigned int j = 0; j < 3; j++)
1526  RxyzVect[j] = posRxyz[j + 3];
1527  // recupere le vecteur thetaU correspondant
1528  vpThetaUVector RtuVect(RxyzVect);
1529 
1530  // remplit le vpPoseVector avec translation et rotation ThetaU
1531  for (unsigned int j = 0; j < 3; j++) {
1532  position[j] = posRxyz[j];
1533  position[j + 3] = RtuVect[j];
1534  }
1535 }
1536 
1548 {
1549  double timestamp;
1550  getPosition(frame, position, timestamp);
1551 }
1552 
1558 {
1559  double timestamp;
1560  PrimitiveACQ_TIME_Viper650(&timestamp);
1561  return timestamp;
1562 }
1563 
1642 {
1644  vpERROR_TRACE("Cannot send a velocity to the robot "
1645  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1647  "Cannot send a velocity to the robot "
1648  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1649  }
1650 
1651  vpColVector vel_sat(6);
1652 
1653  // Velocity saturation
1654  switch (frame) {
1655  // saturation in cartesian space
1656  case vpRobot::CAMERA_FRAME:
1659  case vpRobot::MIXT_FRAME: {
1660  vpColVector vel_max(6);
1661 
1662  for (unsigned int i = 0; i < 3; i++)
1663  vel_max[i] = getMaxTranslationVelocity();
1664  for (unsigned int i = 3; i < 6; i++)
1665  vel_max[i] = getMaxRotationVelocity();
1666 
1667  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1668 
1669  break;
1670  }
1671  // saturation in joint space
1672  case vpRobot::ARTICULAR_FRAME: {
1673  vpColVector vel_max(6);
1674 
1675  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1676  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1677  for (unsigned int i = 0; i < 6; i++)
1678  vel_max[i] = getMaxRotationVelocity();
1679  } else {
1680  for (unsigned int i = 0; i < 5; i++)
1681  vel_max[i] = getMaxRotationVelocity();
1682  vel_max[5] = getMaxRotationVelocityJoint6();
1683  }
1684 
1685  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1686  }
1687  }
1688 
1689  InitTry;
1690 
1691  switch (frame) {
1692  case vpRobot::CAMERA_FRAME: {
1693  // Send velocities in m/s and rad/s
1694  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1695  Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
1696  break;
1697  }
1699  // Transform in camera frame
1700  vpHomogeneousMatrix cMe;
1701  this->get_cMe(cMe);
1702  vpVelocityTwistMatrix cVe(cMe);
1703  vpColVector v_c = cVe * vel_sat;
1704  // Send velocities in m/s and rad/s
1705  Try(PrimitiveMOVESPEED_CART_Viper650(v_c.data, REPCAM_VIPER650));
1706  break;
1707  }
1708  case vpRobot::ARTICULAR_FRAME: {
1709  // Convert all the velocities from rad/s into deg/s
1710  vel_sat.rad2deg();
1711  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1712  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
1713  Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
1714  break;
1715  }
1716  case vpRobot::REFERENCE_FRAME: {
1717  // Send velocities in m/s and rad/s
1718  //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1719  Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
1720  break;
1721  }
1722  case vpRobot::MIXT_FRAME: {
1723  // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
1724  break;
1725  }
1726  default: {
1727  vpERROR_TRACE("Error in spec of vpRobot. "
1728  "Case not taken in account.");
1729  return;
1730  }
1731  }
1732 
1733  Catch();
1734  if (TryStt < 0) {
1735  if (TryStt == VelStopOnJoint) {
1736  UInt32 axisInJoint[njoint];
1737  PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1738  for (unsigned int i = 0; i < njoint; i++) {
1739  if (axisInJoint[i])
1740  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1741  }
1742  } else {
1743  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1744  if (TryString != NULL) {
1745  // The statement is in TryString, but we need to check the validity
1746  printf(" Error sentence %s\n", TryString); // Print the TryString
1747  } else {
1748  printf("\n");
1749  }
1750  }
1751  }
1752 
1753  return;
1754 }
1755 
1756 /* ------------------------------------------------------------------------ */
1757 /* --- GET ---------------------------------------------------------------- */
1758 /* ------------------------------------------------------------------------ */
1759 
1816 void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1817 {
1818  velocity.resize(6);
1819  velocity = 0;
1820 
1821  vpColVector q_cur(6);
1822  vpHomogeneousMatrix fMe_cur, fMc_cur;
1823  vpHomogeneousMatrix eMe, cMc; // camera displacement
1824  double time_cur;
1825 
1826  InitTry;
1827 
1828  // Get the current joint position
1829  Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
1830  time_cur = timestamp;
1831  q_cur.deg2rad();
1832 
1833  // Get the end-effector pose from the direct kinematics
1834  vpViper650::get_fMe(q_cur, fMe_cur);
1835  // Get the camera pose from the direct kinematics
1836  vpViper650::get_fMc(q_cur, fMc_cur);
1837 
1838  if (!m_first_time_getvel) {
1839 
1840  switch (frame) {
1842  // Compute the displacement of the end-effector since the previous call
1843  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1844 
1845  // Compute the velocity of the end-effector from this displacement
1846  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1847 
1848  break;
1849  }
1850 
1851  case vpRobot::CAMERA_FRAME: {
1852  // Compute the displacement of the camera since the previous call
1853  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1854 
1855  // Compute the velocity of the camera from this displacement
1856  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1857 
1858  break;
1859  }
1860 
1861  case vpRobot::ARTICULAR_FRAME: {
1862  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1863  break;
1864  }
1865 
1866  case vpRobot::REFERENCE_FRAME: {
1867  // Compute the displacement of the camera since the previous call
1868  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1869 
1870  // Compute the velocity of the camera from this displacement
1871  vpColVector v;
1872  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1873 
1874  // Express this velocity in the reference frame
1875  vpVelocityTwistMatrix fVc(fMc_cur);
1876  velocity = fVc * v;
1877 
1878  break;
1879  }
1880 
1881  case vpRobot::MIXT_FRAME: {
1882  // Compute the displacement of the camera since the previous call
1883  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1884 
1885  // Compute the ThetaU representation for the rotation
1886  vpRotationMatrix cRc;
1887  cMc.extract(cRc);
1888  vpThetaUVector thetaU;
1889  thetaU.buildFrom(cRc);
1890 
1891  for (unsigned int i = 0; i < 3; i++) {
1892  // Compute the translation displacement in the reference frame
1893  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1894  // Update the rotation displacement in the camera frame
1895  velocity[i + 3] = thetaU[i];
1896  }
1897 
1898  // Compute the velocity
1899  velocity /= (time_cur - m_time_prev_getvel);
1900  break;
1901  }
1902  default: {
1904  "vpRobotViper650::getVelocity() not implemented in end-effector"));
1905  }
1906  }
1907  } else {
1908  m_first_time_getvel = false;
1909  }
1910 
1911  // Memorize the end-effector pose for the next call
1912  m_fMe_prev_getvel = fMe_cur;
1913  // Memorize the camera pose for the next call
1914  m_fMc_prev_getvel = fMc_cur;
1915 
1916  // Memorize the joint position for the next call
1917  m_q_prev_getvel = q_cur;
1918 
1919  // Memorize the time associated to the joint position for the next call
1920  m_time_prev_getvel = time_cur;
1921 
1922  CatchPrint();
1923  if (TryStt < 0) {
1924  vpERROR_TRACE("Cannot get velocity.");
1925  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1926  }
1927 }
1928 
1938 {
1939  double timestamp;
1940  getVelocity(frame, velocity, timestamp);
1941 }
1942 
1993 {
1994  vpColVector velocity;
1995  getVelocity(frame, velocity, timestamp);
1996 
1997  return velocity;
1998 }
1999 
2009 {
2010  vpColVector velocity;
2011  double timestamp;
2012  getVelocity(frame, velocity, timestamp);
2013 
2014  return velocity;
2015 }
2016 
2082 bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
2083 {
2084  std::ifstream fd(filename.c_str(), std::ios::in);
2085 
2086  if (!fd.is_open()) {
2087  return false;
2088  }
2089 
2090  std::string line;
2091  std::string key("R:");
2092  std::string id("#Viper650 - Position");
2093  bool pos_found = false;
2094  int lineNum = 0;
2095 
2096  q.resize(njoint);
2097 
2098  while (std::getline(fd, line)) {
2099  lineNum++;
2100  if (lineNum == 1) {
2101  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
2102  std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl;
2103  return false;
2104  }
2105  }
2106  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2107  continue;
2108  }
2109  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2110  // check if there are at least njoint values in the line
2111  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2112  if (chain.size() < njoint + 1) // try to split with tab separator
2113  chain = vpIoTools::splitChain(line, std::string("\t"));
2114  if (chain.size() < njoint + 1)
2115  continue;
2116 
2117  std::istringstream ss(line);
2118  std::string key_;
2119  ss >> key_;
2120  for (unsigned int i = 0; i < njoint; i++)
2121  ss >> q[i];
2122  pos_found = true;
2123  break;
2124  }
2125  }
2126 
2127  // converts rotations from degrees into radians
2128  q.deg2rad();
2129 
2130  fd.close();
2131 
2132  if (!pos_found) {
2133  std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2134  return false;
2135  }
2136 
2137  return true;
2138 }
2139 
2163 bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
2164 {
2165 
2166  FILE *fd;
2167  fd = fopen(filename.c_str(), "w");
2168  if (fd == NULL)
2169  return false;
2170 
2171  fprintf(fd, "\
2172 #Viper650 - Position - Version 1.00\n\
2173 #\n\
2174 # R: A B C D E F\n\
2175 # Joint position in degrees\n\
2176 #\n\
2177 #\n\n");
2178 
2179  // Save positions in mm and deg
2180  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2181  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2182 
2183  fclose(fd);
2184  return (true);
2185 }
2186 
2197 void vpRobotViper650::move(const std::string &filename)
2198 {
2199  vpColVector q;
2200 
2201  try {
2202  this->readPosFile(filename, q);
2204  this->setPositioningVelocity(10);
2206  } catch (...) {
2207  throw;
2208  }
2209 }
2210 
2230 {
2231  displacement.resize(6);
2232  displacement = 0;
2233 
2234  double q[6];
2235  vpColVector q_cur(6);
2236  double timestamp;
2237 
2238  InitTry;
2239 
2240  // Get the current joint position
2241  Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
2242  for (unsigned int i = 0; i < njoint; i++) {
2243  q_cur[i] = q[i];
2244  }
2245 
2246  if (!m_first_time_getdis) {
2247  switch (frame) {
2248  case vpRobot::CAMERA_FRAME: {
2249  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2250  return;
2251  }
2252 
2253  case vpRobot::ARTICULAR_FRAME: {
2254  displacement = q_cur - m_q_prev_getdis;
2255  break;
2256  }
2257 
2258  case vpRobot::REFERENCE_FRAME: {
2259  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2260  return;
2261  }
2262 
2263  case vpRobot::MIXT_FRAME: {
2264  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2265  return;
2266  }
2268  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2269  return;
2270  }
2271  }
2272  } else {
2273  m_first_time_getdis = false;
2274  }
2275 
2276  // Memorize the joint position for the next call
2277  m_q_prev_getdis = q_cur;
2278 
2279  CatchPrint();
2280  if (TryStt < 0) {
2281  vpERROR_TRACE("Cannot get velocity.");
2282  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2283  }
2284 }
2285 
2300 {
2301  InitTry;
2302 
2303  Try(PrimitiveTFS_BIAS_Viper650());
2304 
2305  // Wait 500 ms to be sure the next measures take into account the bias
2306  vpTime::wait(500);
2307 
2308  CatchPrint();
2309  if (TryStt < 0) {
2310  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2311  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2312  }
2313 }
2314 
2355 {
2356  InitTry;
2357 
2358  H.resize(6);
2359 
2360  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2361 
2362  CatchPrint();
2363  if (TryStt < 0) {
2364  vpERROR_TRACE("Cannot get the force/torque measures.");
2365  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2366  }
2367 }
2368 
2407 {
2408  InitTry;
2409 
2410  vpColVector H(6);
2411 
2412  Try(PrimitiveTFS_ACQ_Viper650(H.data));
2413 
2414  return H;
2415 
2416  CatchPrint();
2417  if (TryStt < 0) {
2418  vpERROR_TRACE("Cannot get the force/torque measures.");
2419  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2420  }
2421  return H; // Here to avoid a warning, but should never be called
2422 }
2423 
2430 {
2431  InitTry;
2432  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2433  std::cout << "Enable joint limits on axis 6..." << std::endl;
2434  CatchPrint();
2435  if (TryStt < 0) {
2436  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2437  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2438  }
2439 }
2440 
2452 {
2453  InitTry;
2454  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2455  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2456  CatchPrint();
2457  if (TryStt < 0) {
2458  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2459  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2460  }
2461 }
2462 
2472 {
2475 
2476  return;
2477 }
2478 
2499 {
2500  m_maxRotationVelocity_joint6 = w6_max;
2501  return;
2502 }
2503 
2511 double vpRobotViper650::getMaxRotationVelocityJoint6() const { return m_maxRotationVelocity_joint6; }
2512 
2520 {
2521  InitTry;
2522  Try(PrimitivePneumaticGripper_Viper650(1));
2523  std::cout << "Open the pneumatic gripper..." << std::endl;
2524  CatchPrint();
2525  if (TryStt < 0) {
2526  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2527  }
2528 }
2529 
2538 {
2539  InitTry;
2540  Try(PrimitivePneumaticGripper_Viper650(0));
2541  std::cout << "Close the pneumatic gripper..." << std::endl;
2542  CatchPrint();
2543  if (TryStt < 0) {
2544  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2545  }
2546 }
2547 
2548 #elif !defined(VISP_BUILD_SHARED_LIBS)
2549 // Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
2550 // no symbols
2551 void dummy_vpRobotViper650(){};
2552 #endif
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpRxyzVector buildFrom(const vpRotationMatrix &R)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
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
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:176
double maxRotationVelocity
Definition: vpRobot.h:98
void setMaxRotationVelocityJoint6(double w6_max)
Initialize the position controller.
Definition: vpRobot.h:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
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
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpControlFrameType
Definition: vpRobot.h:75
vpRxyzVector erc
Definition: vpViper.h:160
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
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 setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
void deg2rad()
Definition: vpColVector.h:196
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
void biasForceTorqueSensor() const
static const double m_defaultPositioningVelocity
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:136
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
Definition: vpRobot.h:66
Emergency stop activated.
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
bool getPowerState() const
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:271
Modelisation of the ADEPT Viper 650 robot.
Definition: vpViper650.h:102
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
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
static double rad(double deg)
Definition: vpMath.h:110
void enableJoint6Limits() const
void get_fJe(vpMatrix &fJe)
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
double getPositioningVelocity(void) const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
static double deg(double rad)
Definition: vpMath.h:103
double getTime() const
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
vpColVector getForceTorque() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void move(const std::string &filename)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void disableJoint6Limits() const
void setMaxRotationVelocity(double w_max)
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cMe(vpHomogeneousMatrix &cMe) const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
void get_cVe(vpVelocityTwistMatrix &cVe) const
double getMaxRotationVelocityJoint6() const
void closeGripper() const
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Function not implemented.
Definition: vpException.h:90
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:294
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
void get_eJe(vpMatrix &eJe)
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
vpColVector joint_min
Definition: vpViper.h:173