Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpRobotViper850.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 S850 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_VIPER850
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/vpRobotViper850.h>
56 
57 /* ---------------------------------------------------------------------- */
58 /* --- STATIC ----------------------------------------------------------- */
59 /* ---------------------------------------------------------------------- */
60 
61 bool vpRobotViper850::m_robotAlreadyCreated = false;
62 
71 
72 /* ---------------------------------------------------------------------- */
73 /* --- EMERGENCY STOP --------------------------------------------------- */
74 /* ---------------------------------------------------------------------- */
75 
83 void emergencyStopViper850(int signo)
84 {
85  std::cout << "Stop the Viper850 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_Viper850();
107  PrimitiveSTOP_Viper850();
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 
176 vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
177 {
178 
179  /*
180  #define SIGHUP 1 // hangup
181  #define SIGINT 2 // interrupt (rubout)
182  #define SIGQUIT 3 // quit (ASCII FS)
183  #define SIGILL 4 // illegal instruction (not reset when caught)
184  #define SIGTRAP 5 // trace trap (not reset when caught)
185  #define SIGIOT 6 // IOT instruction
186  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
187  #define SIGEMT 7 // EMT instruction
188  #define SIGFPE 8 // floating point exception
189  #define SIGKILL 9 // kill (cannot be caught or ignored)
190  #define SIGBUS 10 // bus error
191  #define SIGSEGV 11 // segmentation violation
192  #define SIGSYS 12 // bad argument to system call
193  #define SIGPIPE 13 // write on a pipe with no one to read it
194  #define SIGALRM 14 // alarm clock
195  #define SIGTERM 15 // software termination signal from kill
196  */
197 
198  signal(SIGINT, emergencyStopViper850);
199  signal(SIGBUS, emergencyStopViper850);
200  signal(SIGSEGV, emergencyStopViper850);
201  signal(SIGKILL, emergencyStopViper850);
202  signal(SIGQUIT, emergencyStopViper850);
203 
204  setVerbose(verbose);
205  if (verbose_)
206  std::cout << "Open communication with MotionBlox.\n";
207  try {
208  this->init();
210  } catch (...) {
211  // vpERROR_TRACE("Error caught") ;
212  throw;
213  }
214  m_positioningVelocity = m_defaultPositioningVelocity;
215 
216  maxRotationVelocity_joint6 = maxRotationVelocity;
217 
218  vpRobotViper850::m_robotAlreadyCreated = true;
219 
220  return;
221 }
222 
223 /* ------------------------------------------------------------------------ */
224 /* --- INITIALISATION ----------------------------------------------------- */
225 /* ------------------------------------------------------------------------ */
226 
252 {
253  InitTry;
254 
255  // Initialise private variables used to compute the measured velocities
256  m_q_prev_getvel.resize(6);
257  m_q_prev_getvel = 0;
258  m_time_prev_getvel = 0;
259  m_first_time_getvel = true;
260 
261  // Initialise private variables used to compute the measured displacement
262  m_q_prev_getdis.resize(6);
263  m_q_prev_getdis = 0;
264  m_first_time_getdis = true;
265 
266 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
267  std::string calibfile;
268 #ifdef VISP_HAVE_VIPER850_DATA
269  calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
270  if (!vpIoTools::checkFilename(calibfile))
271  throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str()));
272 #else
273  throw(vpException(vpException::ioError, "You don't have access to Viper850 "
274  "data to retrive ATI F/T calib "
275  "file"));
276 #endif
277  m_ati.setCalibrationFile(calibfile);
278  m_ati.open();
279 #endif
280 
281  // Initialize the firewire connection
282  Try(InitializeConnection(verbose_));
283 
284  // Connect to the servoboard using the servo board GUID
285  Try(InitializeNode_Viper850());
286 
287  Try(PrimitiveRESET_Viper850());
288 
289  // Enable the joint limits on axis 6
290  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
291 
292  // Update the eMc matrix in the low level controller
294 
295  // Look if the power is on or off
296  UInt32 HIPowerStatus;
297  UInt32 EStopStatus;
298  Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
299  CAL_Wait(0.1);
300 
301  // Print the robot status
302  if (verbose_) {
303  std::cout << "Robot status: ";
304  switch (EStopStatus) {
305  case ESTOP_AUTO:
306  m_controlMode = AUTO;
307  if (HIPowerStatus == 0)
308  std::cout << "Power is OFF" << std::endl;
309  else
310  std::cout << "Power is ON" << std::endl;
311  break;
312 
313  case ESTOP_MANUAL:
314  m_controlMode = MANUAL;
315  if (HIPowerStatus == 0)
316  std::cout << "Power is OFF" << std::endl;
317  else
318  std::cout << "Power is ON" << std::endl;
319  break;
320  case ESTOP_ACTIVATED:
321  m_controlMode = ESTOP;
322  std::cout << "Emergency stop is activated" << std::endl;
323  break;
324  default:
325  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
326  std::cout << "You have to call Adept for maintenance..." << std::endl;
327  // Free allocated resources
328  }
329  std::cout << std::endl;
330  }
331  // get real joint min/max from the MotionBlox
332  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
333  // Convert units from degrees to radians
334  joint_min.deg2rad();
335  joint_max.deg2rad();
336 
337  // for (unsigned int i=0; i < njoint; i++) {
338  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
339  // joint_max[i]);
340  // }
341 
342  // If an error occur in the low level controller, goto here
343  // CatchPrint();
344  Catch();
345 
346  // Test if an error occurs
347  if (TryStt == -20001)
348  printf("No connection detected. Check if the robot is powered on \n"
349  "and if the firewire link exist between the MotionBlox and this "
350  "computer.\n");
351  else if (TryStt == -675)
352  printf(" Timeout enabling power...\n");
353 
354  if (TryStt < 0) {
355  // Power off the robot
356  PrimitivePOWEROFF_Viper850();
357  // Free allocated resources
358  ShutDownConnection();
359 
360  std::cout << "Cannot open connection with the motionblox..." << std::endl;
361  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
362  }
363  return;
364 }
365 
423 {
424  // Read the robot constants from files
425  // - joint [min,max], coupl_56, long_56
426  // - camera extrinsic parameters relative to eMc
428 
429  InitTry;
430 
431  // get real joint min/max from the MotionBlox
432  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
433  // Convert units from degrees to radians
434  joint_min.deg2rad();
435  joint_max.deg2rad();
436 
437  // for (unsigned int i=0; i < njoint; i++) {
438  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
439  // joint_max[i]);
440  // }
441 
442  // Set the camera constant (eMc pose) in the MotionBlox
443  double eMc_pose[6];
444  for (unsigned int i = 0; i < 3; i++) {
445  eMc_pose[i] = etc[i]; // translation in meters
446  eMc_pose[i + 3] = erc[i]; // rotation in rad
447  }
448  // Update the eMc pose in the low level controller
449  Try(PrimitiveCONST_Viper850(eMc_pose));
450 
451  CatchPrint();
452  return;
453 }
454 
505 void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename)
506 {
507  vpViper850::init(tool, filename);
508 
509  InitTry;
510 
511  // Get real joint min/max from the MotionBlox
512  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
513  // Convert units from degrees to radians
514  joint_min.deg2rad();
515  joint_max.deg2rad();
516 
517  // for (unsigned int i=0; i < njoint; i++) {
518  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
519  // joint_max[i]);
520  // }
521 
522  // Set the camera constant (eMc pose) in the MotionBlox
523  double eMc_pose[6];
524  for (unsigned int i = 0; i < 3; i++) {
525  eMc_pose[i] = etc[i]; // translation in meters
526  eMc_pose[i + 3] = erc[i]; // rotation in rad
527  }
528  // Update the eMc pose in the low level controller
529  Try(PrimitiveCONST_Viper850(eMc_pose));
530 
531  CatchPrint();
532  return;
533 }
534 
572 {
573  vpViper850::init(tool, eMc_);
574 
575  InitTry;
576 
577  // Get real joint min/max from the MotionBlox
578  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
579  // Convert units from degrees to radians
580  joint_min.deg2rad();
581  joint_max.deg2rad();
582 
583  // for (unsigned int i=0; i < njoint; i++) {
584  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
585  // joint_max[i]);
586  // }
587 
588  // Set the camera constant (eMc pose) in the MotionBlox
589  double eMc_pose[6];
590  for (unsigned int i = 0; i < 3; i++) {
591  eMc_pose[i] = etc[i]; // translation in meters
592  eMc_pose[i + 3] = erc[i]; // rotation in rad
593  }
594  // Update the eMc pose in the low level controller
595  Try(PrimitiveCONST_Viper850(eMc_pose));
596 
597  CatchPrint();
598  return;
599 }
600 
613 {
614  this->vpViper850::set_eMc(eMc_);
615 
616  InitTry;
617 
618  // Set the camera constant (eMc pose) in the MotionBlox
619  double eMc_pose[6];
620  for (unsigned int i = 0; i < 3; i++) {
621  eMc_pose[i] = etc[i]; // translation in meters
622  eMc_pose[i + 3] = erc[i]; // rotation in rad
623  }
624  // Update the eMc pose in the low level controller
625  Try(PrimitiveCONST_Viper850(eMc_pose));
626 
627  CatchPrint();
628 
629  return;
630 }
631 
645 {
646  this->vpViper850::set_eMc(etc_, erc_);
647 
648  InitTry;
649 
650  // Set the camera constant (eMc pose) in the MotionBlox
651  double eMc_pose[6];
652  for (unsigned int i = 0; i < 3; i++) {
653  eMc_pose[i] = etc[i]; // translation in meters
654  eMc_pose[i + 3] = erc[i]; // rotation in rad
655  }
656  // Update the eMc pose in the low level controller
657  Try(PrimitiveCONST_Viper850(eMc_pose));
658 
659  CatchPrint();
660 
661  return;
662 }
663 
664 /* ------------------------------------------------------------------------ */
665 /* --- DESTRUCTOR --------------------------------------------------------- */
666 /* ------------------------------------------------------------------------ */
667 
675 {
676 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
677  m_ati.close();
678 #endif
679 
680  InitTry;
681 
683 
684  // Look if the power is on or off
685  UInt32 HIPowerStatus;
686  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
687  CAL_Wait(0.1);
688 
689  // if (HIPowerStatus == 1) {
690  // fprintf(stdout, "Power OFF the robot\n");
691  // fflush(stdout);
692 
693  // Try( PrimitivePOWEROFF_Viper850() );
694  // }
695 
696  // Free allocated resources
697  ShutDownConnection();
698 
699  vpRobotViper850::m_robotAlreadyCreated = false;
700 
701  CatchPrint();
702  return;
703 }
704 
712 {
713  InitTry;
714 
715  switch (newState) {
716  case vpRobot::STATE_STOP: {
717  // Start primitive STOP only if the current state is Velocity
719  Try(PrimitiveSTOP_Viper850());
720  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
721  }
722  break;
723  }
726  std::cout << "Change the control mode from velocity to position control.\n";
727  Try(PrimitiveSTOP_Viper850());
728  } else {
729  // std::cout << "Change the control mode from stop to position
730  // control.\n";
731  }
732  this->powerOn();
733  break;
734  }
737  std::cout << "Change the control mode from stop to velocity control.\n";
738  }
739  this->powerOn();
740  break;
741  }
742  default:
743  break;
744  }
745 
746  CatchPrint();
747 
748  return vpRobot::setRobotState(newState);
749 }
750 
751 /* ------------------------------------------------------------------------ */
752 /* --- STOP --------------------------------------------------------------- */
753 /* ------------------------------------------------------------------------ */
754 
763 {
765  return;
766 
767  InitTry;
768  Try(PrimitiveSTOP_Viper850());
770 
771  CatchPrint();
772  if (TryStt < 0) {
773  vpERROR_TRACE("Cannot stop robot motion");
774  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
775  }
776 }
777 
788 {
789  InitTry;
790 
791  // Look if the power is on or off
792  UInt32 HIPowerStatus;
793  UInt32 EStopStatus;
794  bool firsttime = true;
795  unsigned int nitermax = 10;
796 
797  for (unsigned int i = 0; i < nitermax; i++) {
798  Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
799  if (EStopStatus == ESTOP_AUTO) {
800  m_controlMode = AUTO;
801  break; // exit for loop
802  } else if (EStopStatus == ESTOP_MANUAL) {
803  m_controlMode = MANUAL;
804  break; // exit for loop
805  } else if (EStopStatus == ESTOP_ACTIVATED) {
806  m_controlMode = ESTOP;
807  if (firsttime) {
808  std::cout << "Emergency stop is activated! \n"
809  << "Check the emergency stop button and push the yellow "
810  "button before continuing."
811  << std::endl;
812  firsttime = false;
813  }
814  fprintf(stdout, "Remaining time %us \r", nitermax - i);
815  fflush(stdout);
816  CAL_Wait(1);
817  } else {
818  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
819  std::cout << "You have to call Adept for maintenance..." << std::endl;
820  // Free allocated resources
821  ShutDownConnection();
822  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
823  }
824  }
825 
826  if (EStopStatus == ESTOP_ACTIVATED)
827  std::cout << std::endl;
828 
829  if (EStopStatus == ESTOP_ACTIVATED) {
830  std::cout << "Sorry, cannot power on the robot." << std::endl;
831  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
832  }
833 
834  if (HIPowerStatus == 0) {
835  fprintf(stdout, "Power ON the Viper850 robot\n");
836  fflush(stdout);
837 
838  Try(PrimitivePOWERON_Viper850());
839  }
840 
841  CatchPrint();
842  if (TryStt < 0) {
843  vpERROR_TRACE("Cannot power on the robot");
844  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
845  }
846 }
847 
858 {
859  InitTry;
860 
861  // Look if the power is on or off
862  UInt32 HIPowerStatus;
863  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
864  CAL_Wait(0.1);
865 
866  if (HIPowerStatus == 1) {
867  fprintf(stdout, "Power OFF the Viper850 robot\n");
868  fflush(stdout);
869 
870  Try(PrimitivePOWEROFF_Viper850());
871  }
872 
873  CatchPrint();
874  if (TryStt < 0) {
875  vpERROR_TRACE("Cannot power off the robot");
876  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
877  }
878 }
879 
892 {
893  InitTry;
894  bool status = false;
895  // Look if the power is on or off
896  UInt32 HIPowerStatus;
897  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
898  CAL_Wait(0.1);
899 
900  if (HIPowerStatus == 1) {
901  status = true;
902  }
903 
904  CatchPrint();
905  if (TryStt < 0) {
906  vpERROR_TRACE("Cannot get the power status");
907  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
908  }
909  return status;
910 }
911 
922 {
924  vpViper850::get_cMe(cMe);
925 
926  cVe.buildFrom(cMe);
927 }
928 
941 
954 {
955 
956  double position[6];
957  double timestamp;
958 
959  InitTry;
960  Try(PrimitiveACQ_POS_J_Viper850(position, &timestamp));
961  CatchPrint();
962 
963  vpColVector q(6);
964  for (unsigned int i = 0; i < njoint; i++)
965  q[i] = vpMath::rad(position[i]);
966 
967  try {
969  } catch (...) {
970  vpERROR_TRACE("catch exception ");
971  throw;
972  }
973 }
987 {
988 
989  double position[6];
990  double timestamp;
991 
992  InitTry;
993  Try(PrimitiveACQ_POS_Viper850(position, &timestamp));
994  CatchPrint();
995 
996  vpColVector q(6);
997  for (unsigned int i = 0; i < njoint; i++)
998  q[i] = position[i];
999 
1000  try {
1002  } catch (...) {
1003  vpERROR_TRACE("Error caught");
1004  throw;
1005  }
1006 }
1007 
1044 void vpRobotViper850::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1045 
1051 double vpRobotViper850::getPositioningVelocity(void) const { return m_positioningVelocity; }
1052 
1130 {
1131 
1133  vpERROR_TRACE("Robot was not in position-based control\n"
1134  "Modification of the robot state");
1136  }
1137 
1138  vpColVector destination(njoint);
1139  int error = 0;
1140  double timestamp;
1141 
1142  InitTry;
1143  switch (frame) {
1144  case vpRobot::CAMERA_FRAME: {
1145  vpColVector q(njoint);
1146  Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1147 
1148  // Convert degrees into rad
1149  q.deg2rad();
1150 
1151  // Get fMc from the inverse kinematics
1152  vpHomogeneousMatrix fMc;
1153  vpViper850::get_fMc(q, fMc);
1154 
1155  // Set cMc from the input position
1156  vpTranslationVector txyz;
1157  vpRxyzVector rxyz;
1158  for (unsigned int i = 0; i < 3; i++) {
1159  txyz[i] = position[i];
1160  rxyz[i] = position[i + 3];
1161  }
1162 
1163  // Compute cMc2
1164  vpRotationMatrix cRc2(rxyz);
1165  vpHomogeneousMatrix cMc2(txyz, cRc2);
1166 
1167  // Compute the new position to reach: fMc*cMc2
1168  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1169 
1170  // Compute the corresponding joint position from the inverse kinematics
1171  unsigned int solution = this->getInverseKinematics(fMc2, q);
1172  if (solution) { // Position is reachable
1173  destination = q;
1174  // convert rad to deg requested for the low level controller
1175  destination.rad2deg();
1176  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1177  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1178  } else {
1179  // Cartesian position is out of range
1180  error = -1;
1181  }
1182 
1183  break;
1184  }
1185  case vpRobot::ARTICULAR_FRAME: {
1186  destination = position;
1187  // convert rad to deg requested for the low level controller
1188  destination.rad2deg();
1189 
1190  // std::cout << "Joint destination (deg): " << destination.t() <<
1191  // std::endl;
1192  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1193  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1194  break;
1195  }
1196  case vpRobot::REFERENCE_FRAME: {
1197  // Convert angles from Rxyz representation to Rzyz representation
1198  vpRxyzVector rxyz(position[3], position[4], position[5]);
1199  vpRotationMatrix R(rxyz);
1200  vpRzyzVector rzyz(R);
1201 
1202  for (unsigned int i = 0; i < 3; i++) {
1203  destination[i] = position[i];
1204  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1205  }
1206  int configuration = 0; // keep the actual configuration
1207 
1208  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1209  // << std::endl;
1210  Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, m_positioningVelocity));
1211  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1212 
1213  break;
1214  }
1215  case vpRobot::MIXT_FRAME: {
1216  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1217  "Mixt frame not implemented.");
1218  }
1220  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1221  "End-effector frame not implemented.");
1222  }
1223  }
1224 
1225  CatchPrint();
1226  if (TryStt == InvalidPosition || TryStt == -1023)
1227  std::cout << " : Position out of range.\n";
1228  else if (TryStt == -3019) {
1229  if (frame == vpRobot::ARTICULAR_FRAME)
1230  std::cout << " : Joint position out of range.\n";
1231  else
1232  std::cout << " : Cartesian position leads to a joint position out of "
1233  "range.\n";
1234  } else if (TryStt < 0)
1235  std::cout << " : Unknown error (see Fabien).\n";
1236  else if (error == -1)
1237  std::cout << "Position out of range.\n";
1238 
1239  if (TryStt < 0 || error < 0) {
1240  vpERROR_TRACE("Positionning error.");
1241  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1242  }
1243 
1244  return;
1245 }
1246 
1311 void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1312  double pos4, double pos5, double pos6)
1313 {
1314  try {
1315  vpColVector position(6);
1316  position[0] = pos1;
1317  position[1] = pos2;
1318  position[2] = pos3;
1319  position[3] = pos4;
1320  position[4] = pos5;
1321  position[5] = pos6;
1322 
1323  setPosition(frame, position);
1324  } catch (...) {
1325  vpERROR_TRACE("Error caught");
1326  throw;
1327  }
1328 }
1329 
1368 void vpRobotViper850::setPosition(const std::string &filename)
1369 {
1370  vpColVector q;
1371  bool ret;
1372 
1373  ret = this->readPosFile(filename, q);
1374 
1375  if (ret == false) {
1376  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1377  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1378  }
1381 }
1382 
1450 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1451 {
1452 
1453  InitTry;
1454 
1455  position.resize(6);
1456 
1457  switch (frame) {
1458  case vpRobot::CAMERA_FRAME: {
1459  position = 0;
1460  return;
1461  }
1462  case vpRobot::ARTICULAR_FRAME: {
1463  Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1464  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1465  position.deg2rad();
1466 
1467  return;
1468  }
1469  case vpRobot::REFERENCE_FRAME: {
1470  vpColVector q(njoint);
1471  Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1472 
1473  // Compute fMc
1475 
1476  // From fMc extract the pose
1477  vpRotationMatrix fRc;
1478  fMc.extract(fRc);
1479  vpRxyzVector rxyz;
1480  rxyz.buildFrom(fRc);
1481 
1482  for (unsigned int i = 0; i < 3; i++) {
1483  position[i] = fMc[i][3]; // translation x,y,z
1484  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1485  }
1486 
1487  break;
1488  }
1489  case vpRobot::MIXT_FRAME: {
1490  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1491  "not implemented");
1492  }
1494  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1495  "not implemented");
1496  }
1497  }
1498 
1499  CatchPrint();
1500  if (TryStt < 0) {
1501  vpERROR_TRACE("Cannot get position.");
1502  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1503  }
1504 
1505  return;
1506 }
1507 
1513 {
1514  double timestamp;
1515  PrimitiveACQ_TIME_Viper850(&timestamp);
1516  return timestamp;
1517 }
1518 
1530 {
1531  double timestamp;
1532  getPosition(frame, position, timestamp);
1533 }
1534 
1546 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1547 {
1548  vpColVector posRxyz;
1549  // recupere position en Rxyz
1550  this->getPosition(frame, posRxyz, timestamp);
1551  vpRxyzVector RxyzVect;
1552  for (unsigned int j = 0; j < 3; j++)
1553  RxyzVect[j] = posRxyz[j + 3];
1554  // recupere le vecteur thetaU correspondant
1555  vpThetaUVector RtuVect(RxyzVect);
1556 
1557  // remplit le vpPoseVector avec translation et rotation ThetaU
1558  for (unsigned int j = 0; j < 3; j++) {
1559  position[j] = posRxyz[j];
1560  position[j + 3] = RtuVect[j];
1561  }
1562 }
1563 
1575 {
1576  vpColVector posRxyz;
1577  double timestamp;
1578  // recupere position en Rxyz
1579  this->getPosition(frame, posRxyz, timestamp);
1580  vpRxyzVector RxyzVect;
1581  for (unsigned int j = 0; j < 3; j++)
1582  RxyzVect[j] = posRxyz[j + 3];
1583  // recupere le vecteur thetaU correspondant
1584  vpThetaUVector RtuVect(RxyzVect);
1585 
1586  // remplit le vpPoseVector avec translation et rotation ThetaU
1587  for (unsigned int j = 0; j < 3; j++) {
1588  position[j] = posRxyz[j];
1589  position[j + 3] = RtuVect[j];
1590  }
1591 }
1592 
1671 {
1673  vpERROR_TRACE("Cannot send a velocity to the robot "
1674  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1676  "Cannot send a velocity to the robot "
1677  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1678  }
1679 
1680  vpColVector vel_sat(6);
1681 
1682  // Velocity saturation
1683  switch (frame) {
1684  // saturation in cartesian space
1685  case vpRobot::CAMERA_FRAME:
1688  case vpRobot::MIXT_FRAME: {
1689  vpColVector vel_max(6);
1690 
1691  for (unsigned int i = 0; i < 3; i++)
1692  vel_max[i] = getMaxTranslationVelocity();
1693  for (unsigned int i = 3; i < 6; i++)
1694  vel_max[i] = getMaxRotationVelocity();
1695 
1696  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1697 
1698  break;
1699  }
1700  // saturation in joint space
1701  case vpRobot::ARTICULAR_FRAME: {
1702  vpColVector vel_max(6);
1703 
1704  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1705  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1706 
1707  for (unsigned int i = 0; i < 6; i++)
1708  vel_max[i] = getMaxRotationVelocity();
1709  } else {
1710  for (unsigned int i = 0; i < 5; i++)
1711  vel_max[i] = getMaxRotationVelocity();
1712  vel_max[5] = getMaxRotationVelocityJoint6();
1713  }
1714 
1715  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1716  }
1717  }
1718 
1719  InitTry;
1720 
1721  switch (frame) {
1722  case vpRobot::CAMERA_FRAME: {
1723  // Send velocities in m/s and rad/s
1724  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1725  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1726  break;
1727  }
1729  // Transform in camera frame
1730  vpHomogeneousMatrix cMe;
1731  this->get_cMe(cMe);
1732  vpVelocityTwistMatrix cVe(cMe);
1733  vpColVector v_c = cVe * vel_sat;
1734  // Send velocities in m/s and rad/s
1735  Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1736  break;
1737  }
1738  case vpRobot::ARTICULAR_FRAME: {
1739  // Convert all the velocities from rad/s into deg/s
1740  vel_sat.rad2deg();
1741  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1742  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1743  Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1744  break;
1745  }
1746  case vpRobot::REFERENCE_FRAME: {
1747  // Send velocities in m/s and rad/s
1748  // std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1749  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1750  break;
1751  }
1752  case vpRobot::MIXT_FRAME: {
1753  // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1754  break;
1755  }
1756  default: {
1757  vpERROR_TRACE("Error in spec of vpRobot. "
1758  "Case not taken in account.");
1759  return;
1760  }
1761  }
1762 
1763  Catch();
1764  if (TryStt < 0) {
1765  if (TryStt == VelStopOnJoint) {
1766  UInt32 axisInJoint[njoint];
1767  PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1768  for (unsigned int i = 0; i < njoint; i++) {
1769  if (axisInJoint[i])
1770  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1771  }
1772  } else {
1773  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1774  if (TryString != NULL) {
1775  // The statement is in TryString, but we need to check the validity
1776  printf(" Error sentence %s\n", TryString); // Print the TryString
1777  } else {
1778  printf("\n");
1779  }
1780  }
1781  }
1782 
1783  return;
1784 }
1785 
1786 /* ------------------------------------------------------------------------ */
1787 /* --- GET ---------------------------------------------------------------- */
1788 /* ------------------------------------------------------------------------ */
1789 
1846 void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1847 {
1848  velocity.resize(6);
1849  velocity = 0;
1850 
1851  vpColVector q_cur(6);
1852  vpHomogeneousMatrix fMe_cur, fMc_cur;
1853  vpHomogeneousMatrix eMe, cMc; // camera displacement
1854  double time_cur;
1855 
1856  InitTry;
1857 
1858  // Get the current joint position
1859  Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1860  time_cur = timestamp;
1861  q_cur.deg2rad();
1862 
1863  // Get the end-effector pose from the direct kinematics
1864  vpViper850::get_fMe(q_cur, fMe_cur);
1865  // Get the camera pose from the direct kinematics
1866  vpViper850::get_fMc(q_cur, fMc_cur);
1867 
1868  if (!m_first_time_getvel) {
1869 
1870  switch (frame) {
1872  // Compute the displacement of the end-effector since the previous call
1873  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1874 
1875  // Compute the velocity of the end-effector from this displacement
1876  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1877 
1878  break;
1879  }
1880 
1881  case vpRobot::CAMERA_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 velocity of the camera from this displacement
1886  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1887 
1888  break;
1889  }
1890 
1891  case vpRobot::ARTICULAR_FRAME: {
1892  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1893  break;
1894  }
1895 
1896  case vpRobot::REFERENCE_FRAME: {
1897  // Compute the displacement of the camera since the previous call
1898  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1899 
1900  // Compute the velocity of the camera from this displacement
1901  vpColVector v;
1902  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1903 
1904  // Express this velocity in the reference frame
1905  vpVelocityTwistMatrix fVc(fMc_cur);
1906  velocity = fVc * v;
1907 
1908  break;
1909  }
1910 
1911  case vpRobot::MIXT_FRAME: {
1912  // Compute the displacement of the camera since the previous call
1913  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1914 
1915  // Compute the ThetaU representation for the rotation
1916  vpRotationMatrix cRc;
1917  cMc.extract(cRc);
1918  vpThetaUVector thetaU;
1919  thetaU.buildFrom(cRc);
1920 
1921  for (unsigned int i = 0; i < 3; i++) {
1922  // Compute the translation displacement in the reference frame
1923  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1924  // Update the rotation displacement in the camera frame
1925  velocity[i + 3] = thetaU[i];
1926  }
1927 
1928  // Compute the velocity
1929  velocity /= (time_cur - m_time_prev_getvel);
1930  break;
1931  }
1932  default: {
1934  "vpRobotViper850::getVelocity() not implemented in end-effector"));
1935  }
1936  }
1937  } else {
1938  m_first_time_getvel = false;
1939  }
1940 
1941  // Memorize the end-effector pose for the next call
1942  m_fMe_prev_getvel = fMe_cur;
1943  // Memorize the camera pose for the next call
1944  m_fMc_prev_getvel = fMc_cur;
1945 
1946  // Memorize the joint position for the next call
1947  m_q_prev_getvel = q_cur;
1948 
1949  // Memorize the time associated to the joint position for the next call
1950  m_time_prev_getvel = time_cur;
1951 
1952  CatchPrint();
1953  if (TryStt < 0) {
1954  vpERROR_TRACE("Cannot get velocity.");
1955  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1956  }
1957 }
1958 
1968 {
1969  double timestamp;
1970  getVelocity(frame, velocity, timestamp);
1971 }
1972 
2023 {
2024  vpColVector velocity;
2025  getVelocity(frame, velocity, timestamp);
2026 
2027  return velocity;
2028 }
2029 
2039 {
2040  vpColVector velocity;
2041  double timestamp;
2042  getVelocity(frame, velocity, timestamp);
2043 
2044  return velocity;
2045 }
2046 
2112 bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2113 {
2114  std::ifstream fd(filename.c_str(), std::ios::in);
2115 
2116  if (!fd.is_open()) {
2117  return false;
2118  }
2119 
2120  std::string line;
2121  std::string key("R:");
2122  std::string id("#Viper850 - Position");
2123  bool pos_found = false;
2124  int lineNum = 0;
2125 
2126  q.resize(njoint);
2127 
2128  while (std::getline(fd, line)) {
2129  lineNum++;
2130  if (lineNum == 1) {
2131  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2132  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2133  return false;
2134  }
2135  }
2136  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2137  continue;
2138  }
2139  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2140  // check if there are at least njoint values in the line
2141  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2142  if (chain.size() < njoint + 1) // try to split with tab separator
2143  chain = vpIoTools::splitChain(line, std::string("\t"));
2144  if (chain.size() < njoint + 1)
2145  continue;
2146 
2147  std::istringstream ss(line);
2148  std::string key_;
2149  ss >> key_;
2150  for (unsigned int i = 0; i < njoint; i++)
2151  ss >> q[i];
2152  pos_found = true;
2153  break;
2154  }
2155  }
2156 
2157  // converts rotations from degrees into radians
2158  q.deg2rad();
2159 
2160  fd.close();
2161 
2162  if (!pos_found) {
2163  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2164  return false;
2165  }
2166 
2167  return true;
2168 }
2169 
2193 bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2194 {
2195 
2196  FILE *fd;
2197  fd = fopen(filename.c_str(), "w");
2198  if (fd == NULL)
2199  return false;
2200 
2201  fprintf(fd, "\
2202 #Viper850 - Position - Version 1.00\n\
2203 #\n\
2204 # R: A B C D E F\n\
2205 # Joint position in degrees\n\
2206 #\n\
2207 #\n\n");
2208 
2209  // Save positions in mm and deg
2210  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2211  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2212 
2213  fclose(fd);
2214  return (true);
2215 }
2216 
2227 void vpRobotViper850::move(const std::string &filename)
2228 {
2229  vpColVector q;
2230 
2231  try {
2232  this->readPosFile(filename, q);
2234  this->setPositioningVelocity(10);
2236  } catch (...) {
2237  throw;
2238  }
2239 }
2240 
2260 {
2261  displacement.resize(6);
2262  displacement = 0;
2263 
2264  double q[6];
2265  vpColVector q_cur(6);
2266  double timestamp;
2267 
2268  InitTry;
2269 
2270  // Get the current joint position
2271  Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2272  for (unsigned int i = 0; i < njoint; i++) {
2273  q_cur[i] = q[i];
2274  }
2275 
2276  if (!m_first_time_getdis) {
2277  switch (frame) {
2278  case vpRobot::CAMERA_FRAME: {
2279  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2280  return;
2281  }
2282 
2283  case vpRobot::ARTICULAR_FRAME: {
2284  displacement = q_cur - m_q_prev_getdis;
2285  break;
2286  }
2287 
2288  case vpRobot::REFERENCE_FRAME: {
2289  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2290  return;
2291  }
2292 
2293  case vpRobot::MIXT_FRAME: {
2294  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2295  return;
2296  }
2298  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2299  return;
2300  }
2301  }
2302  } else {
2303  m_first_time_getdis = false;
2304  }
2305 
2306  // Memorize the joint position for the next call
2307  m_q_prev_getdis = q_cur;
2308 
2309  CatchPrint();
2310  if (TryStt < 0) {
2311  vpERROR_TRACE("Cannot get velocity.");
2312  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2313  }
2314 }
2315 
2324 {
2325 #if defined(USE_ATI_DAQ)
2326 #if defined(VISP_HAVE_COMEDI)
2327  m_ati.bias();
2328 #else
2329  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2330  "apt-get install libcomedi-dev"));
2331 #endif
2332 #else // Use serial link
2333  InitTry;
2334 
2335  Try(PrimitiveTFS_BIAS_Viper850());
2336 
2337  // Wait 500 ms to be sure the next measures take into account the bias
2338  vpTime::wait(500);
2339 
2340  CatchPrint();
2341  if (TryStt < 0) {
2342  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2343  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2344  }
2345 #endif
2346 }
2347 
2356 {
2357 #if defined(USE_ATI_DAQ)
2358 #if defined(VISP_HAVE_COMEDI)
2359  m_ati.unbias();
2360 #else
2361  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2362  "apt-get install libcomedi-dev"));
2363 #endif
2364 #else // Use serial link
2365 // Not implemented
2366 #endif
2367 }
2368 
2409 {
2410 #if defined(USE_ATI_DAQ)
2411 #if defined(VISP_HAVE_COMEDI)
2412  H = m_ati.getForceTorque();
2413 #else
2414  (void)H;
2415  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2416  "apt-get install libcomedi-dev"));
2417 #endif
2418 #else // Use serial link
2419  InitTry;
2420 
2421  H.resize(6);
2422 
2423  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2424 
2425  CatchPrint();
2426  if (TryStt < 0) {
2427  vpERROR_TRACE("Cannot get the force/torque measures.");
2428  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2429  }
2430 #endif
2431 }
2432 
2471 {
2472 #if defined(USE_ATI_DAQ)
2473 #if defined(VISP_HAVE_COMEDI)
2474  vpColVector H = m_ati.getForceTorque();
2475  return H;
2476 #else
2477  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2478  "apt-get install libcomedi-dev"));
2479 #endif
2480 #else // Use serial link
2481  InitTry;
2482 
2483  vpColVector H(6);
2484 
2485  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2486  return H;
2487 
2488  CatchPrint();
2489  if (TryStt < 0) {
2490  vpERROR_TRACE("Cannot get the force/torque measures.");
2491  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2492  }
2493  return H; // Here to avoid a warning, but should never be called
2494 #endif
2495 }
2496 
2504 {
2505  InitTry;
2506  Try(PrimitivePneumaticGripper_Viper850(1));
2507  std::cout << "Open the pneumatic gripper..." << std::endl;
2508  CatchPrint();
2509  if (TryStt < 0) {
2510  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2511  }
2512 }
2513 
2522 {
2523  InitTry;
2524  Try(PrimitivePneumaticGripper_Viper850(0));
2525  std::cout << "Close the pneumatic gripper..." << std::endl;
2526  CatchPrint();
2527  if (TryStt < 0) {
2528  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2529  }
2530 }
2531 
2538 {
2539  InitTry;
2540  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2541  std::cout << "Enable joint limits on axis 6..." << std::endl;
2542  CatchPrint();
2543  if (TryStt < 0) {
2544  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2545  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2546  }
2547 }
2548 
2560 {
2561  InitTry;
2562  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2563  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2564  CatchPrint();
2565  if (TryStt < 0) {
2566  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2567  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2568  }
2569 }
2570 
2580 {
2583 
2584  return;
2585 }
2586 
2608 {
2609  maxRotationVelocity_joint6 = w6_max;
2610  return;
2611 }
2612 
2620 double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2621 
2622 #elif !defined(VISP_BUILD_SHARED_LIBS)
2623 // Work around to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2624 // no symbols
2625 void dummy_vpRobotViper850(){};
2626 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector & rad2deg()
Definition: vpColVector.h:297
vpColVector & deg2rad()
Definition: vpColVector.h:197
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ fatalError
Fatal error.
Definition: vpException.h:96
static vpColVector inverse(const vpHomogeneousMatrix &M)
vpColVector getForceTorque() const
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
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:1929
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:807
static double rad(double deg)
Definition: vpMath.h:121
static double deg(double rad)
Definition: vpMath.h:111
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Error that can be emited by the vpRobot class and its derivates.
vpColVector getForceTorque() const
void get_cVe(vpVelocityTwistMatrix &cVe) const
virtual ~vpRobotViper850(void)
static const double m_defaultPositioningVelocity
double getTime() const
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocity(double w_max)
void closeGripper() const
void setMaxRotationVelocityJoint6(double w6_max)
void disableJoint6Limits() const
void enableJoint6Limits() const
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_eJe(vpMatrix &eJe)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
double getPositioningVelocity(void) const
static bool readPosFile(const std::string &filename, vpColVector &q)
void setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_cMe(vpHomogeneousMatrix &cMe) const
void move(const std::string &filename)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getMaxRotationVelocityJoint6() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
Class that defines a generic virtual robot.
Definition: vpRobot.h:60
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:105
void setVerbose(bool verbose)
Definition: vpRobot.h:160
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:109
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:99
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
bool verbose_
Definition: vpRobot.h:117
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:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:183
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:104
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
void init(void)
Definition: vpViper850.cpp:136
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpTranslationVector etc
Definition: vpViper.h:159
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector joint_max
Definition: vpViper.h:172
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector joint_min
Definition: vpViper.h:173
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_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)