Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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
427  vpViper850::init(tool, projModel);
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 {
968  vpViper850::get_eJe(q, eJe);
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 {
1001  vpViper850::get_fJe(q, fJe);
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 
1131 {
1132 
1134  vpERROR_TRACE("Robot was not in position-based control\n"
1135  "Modification of the robot state");
1137  }
1138 
1139  vpColVector destination(njoint);
1140  int error = 0;
1141  double timestamp;
1142 
1143  InitTry;
1144  switch (frame) {
1145  case vpRobot::CAMERA_FRAME: {
1146  vpColVector q(njoint);
1147  Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1148 
1149  // Convert degrees into rad
1150  q.deg2rad();
1151 
1152  // Get fMc from the inverse kinematics
1153  vpHomogeneousMatrix fMc;
1154  vpViper850::get_fMc(q, fMc);
1155 
1156  // Set cMc from the input position
1157  vpTranslationVector txyz;
1158  vpRxyzVector rxyz;
1159  for (unsigned int i = 0; i < 3; i++) {
1160  txyz[i] = position[i];
1161  rxyz[i] = position[i + 3];
1162  }
1163 
1164  // Compute cMc2
1165  vpRotationMatrix cRc2(rxyz);
1166  vpHomogeneousMatrix cMc2(txyz, cRc2);
1167 
1168  // Compute the new position to reach: fMc*cMc2
1169  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1170 
1171  // Compute the corresponding joint position from the inverse kinematics
1172  unsigned int solution = this->getInverseKinematics(fMc2, q);
1173  if (solution) { // Position is reachable
1174  destination = q;
1175  // convert rad to deg requested for the low level controller
1176  destination.rad2deg();
1177  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1178  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1179  } else {
1180  // Cartesian position is out of range
1181  error = -1;
1182  }
1183 
1184  break;
1185  }
1186  case vpRobot::ARTICULAR_FRAME: {
1187  destination = position;
1188  // convert rad to deg requested for the low level controller
1189  destination.rad2deg();
1190 
1191  // std::cout << "Joint destination (deg): " << destination.t() <<
1192  // std::endl;
1193  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1194  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1195  break;
1196  }
1197  case vpRobot::REFERENCE_FRAME: {
1198  // Convert angles from Rxyz representation to Rzyz representation
1199  vpRxyzVector rxyz(position[3], position[4], position[5]);
1200  vpRotationMatrix R(rxyz);
1201  vpRzyzVector rzyz(R);
1202 
1203  for (unsigned int i = 0; i < 3; i++) {
1204  destination[i] = position[i];
1205  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1206  }
1207  int configuration = 0; // keep the actual configuration
1208 
1209  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1210  // << std::endl;
1211  Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, m_positioningVelocity));
1212  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1213 
1214  break;
1215  }
1216  case vpRobot::MIXT_FRAME: {
1217  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1218  "Mixt frame not implemented.");
1219  }
1221  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1222  "End-effector frame not implemented.");
1223  }
1224  }
1225 
1226  CatchPrint();
1227  if (TryStt == InvalidPosition || TryStt == -1023)
1228  std::cout << " : Position out of range.\n";
1229  else if (TryStt == -3019) {
1230  if (frame == vpRobot::ARTICULAR_FRAME)
1231  std::cout << " : Joint position out of range.\n";
1232  else
1233  std::cout << " : Cartesian position leads to a joint position out of "
1234  "range.\n";
1235  } else if (TryStt < 0)
1236  std::cout << " : Unknown error (see Fabien).\n";
1237  else if (error == -1)
1238  std::cout << "Position out of range.\n";
1239 
1240  if (TryStt < 0 || error < 0) {
1241  vpERROR_TRACE("Positionning error.");
1242  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1243  }
1244 
1245  return;
1246 }
1247 
1312 void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1313  double pos3, double pos4, double pos5, double pos6)
1314 {
1315  try {
1316  vpColVector position(6);
1317  position[0] = pos1;
1318  position[1] = pos2;
1319  position[2] = pos3;
1320  position[3] = pos4;
1321  position[4] = pos5;
1322  position[5] = pos6;
1323 
1324  setPosition(frame, position);
1325  } catch (...) {
1326  vpERROR_TRACE("Error caught");
1327  throw;
1328  }
1329 }
1330 
1369 void vpRobotViper850::setPosition(const std::string &filename)
1370 {
1371  vpColVector q;
1372  bool ret;
1373 
1374  ret = this->readPosFile(filename, q);
1375 
1376  if (ret == false) {
1377  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1378  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1379  }
1382 }
1383 
1451 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1452 {
1453 
1454  InitTry;
1455 
1456  position.resize(6);
1457 
1458  switch (frame) {
1459  case vpRobot::CAMERA_FRAME: {
1460  position = 0;
1461  return;
1462  }
1463  case vpRobot::ARTICULAR_FRAME: {
1464  Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1465  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1466  position.deg2rad();
1467 
1468  return;
1469  }
1470  case vpRobot::REFERENCE_FRAME: {
1471  vpColVector q(njoint);
1472  Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1473 
1474  // Compute fMc
1476 
1477  // From fMc extract the pose
1478  vpRotationMatrix fRc;
1479  fMc.extract(fRc);
1480  vpRxyzVector rxyz;
1481  rxyz.buildFrom(fRc);
1482 
1483  for (unsigned int i = 0; i < 3; i++) {
1484  position[i] = fMc[i][3]; // translation x,y,z
1485  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1486  }
1487 
1488  break;
1489  }
1490  case vpRobot::MIXT_FRAME: {
1491  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1492  "not implemented");
1493  }
1495  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1496  "not implemented");
1497  }
1498  }
1499 
1500  CatchPrint();
1501  if (TryStt < 0) {
1502  vpERROR_TRACE("Cannot get position.");
1503  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1504  }
1505 
1506  return;
1507 }
1508 
1514 {
1515  double timestamp;
1516  PrimitiveACQ_TIME_Viper850(&timestamp);
1517  return timestamp;
1518 }
1519 
1531 {
1532  double timestamp;
1533  getPosition(frame, position, timestamp);
1534 }
1535 
1547 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1548 {
1549  vpColVector posRxyz;
1550  // recupere position en Rxyz
1551  this->getPosition(frame, posRxyz, timestamp);
1552  vpRxyzVector RxyzVect;
1553  for (unsigned int j = 0; j < 3; j++)
1554  RxyzVect[j] = posRxyz[j + 3];
1555  // recupere le vecteur thetaU correspondant
1556  vpThetaUVector RtuVect(RxyzVect);
1557 
1558  // remplit le vpPoseVector avec translation et rotation ThetaU
1559  for (unsigned int j = 0; j < 3; j++) {
1560  position[j] = posRxyz[j];
1561  position[j + 3] = RtuVect[j];
1562  }
1563 }
1564 
1576 {
1577  vpColVector posRxyz;
1578  double timestamp;
1579  // recupere position en Rxyz
1580  this->getPosition(frame, posRxyz, timestamp);
1581  vpRxyzVector RxyzVect;
1582  for (unsigned int j = 0; j < 3; j++)
1583  RxyzVect[j] = posRxyz[j + 3];
1584  // recupere le vecteur thetaU correspondant
1585  vpThetaUVector RtuVect(RxyzVect);
1586 
1587  // remplit le vpPoseVector avec translation et rotation ThetaU
1588  for (unsigned int j = 0; j < 3; j++) {
1589  position[j] = posRxyz[j];
1590  position[j + 3] = RtuVect[j];
1591  }
1592 }
1593 
1672 {
1674  vpERROR_TRACE("Cannot send a velocity to the robot "
1675  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1677  "Cannot send a velocity to the robot "
1678  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1679  }
1680 
1681  vpColVector vel_sat(6);
1682 
1683  // Velocity saturation
1684  switch (frame) {
1685  // saturation in cartesian space
1686  case vpRobot::CAMERA_FRAME:
1689  case vpRobot::MIXT_FRAME: {
1690  vpColVector vel_max(6);
1691 
1692  for (unsigned int i = 0; i < 3; i++)
1693  vel_max[i] = getMaxTranslationVelocity();
1694  for (unsigned int i = 3; i < 6; i++)
1695  vel_max[i] = getMaxRotationVelocity();
1696 
1697  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1698 
1699  break;
1700  }
1701  // saturation in joint space
1702  case vpRobot::ARTICULAR_FRAME: {
1703  vpColVector vel_max(6);
1704 
1705  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1706  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1707 
1708  for (unsigned int i = 0; i < 6; i++)
1709  vel_max[i] = getMaxRotationVelocity();
1710  } else {
1711  for (unsigned int i = 0; i < 5; i++)
1712  vel_max[i] = getMaxRotationVelocity();
1713  vel_max[5] = getMaxRotationVelocityJoint6();
1714  }
1715 
1716  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1717  }
1718  }
1719 
1720  InitTry;
1721 
1722  switch (frame) {
1723  case vpRobot::CAMERA_FRAME: {
1724  // Send velocities in m/s and rad/s
1725  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1726  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1727  break;
1728  }
1730  // Transform in camera frame
1731  vpHomogeneousMatrix cMe;
1732  this->get_cMe(cMe);
1733  vpVelocityTwistMatrix cVe(cMe);
1734  vpColVector v_c = cVe * vel_sat;
1735  // Send velocities in m/s and rad/s
1736  Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1737  break;
1738  }
1739  case vpRobot::ARTICULAR_FRAME: {
1740  // Convert all the velocities from rad/s into deg/s
1741  vel_sat.rad2deg();
1742  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1743  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1744  Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1745  break;
1746  }
1747  case vpRobot::REFERENCE_FRAME: {
1748  // Send velocities in m/s and rad/s
1749  //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1750  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1751  break;
1752  }
1753  case vpRobot::MIXT_FRAME: {
1754  // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1755  break;
1756  }
1757  default: {
1758  vpERROR_TRACE("Error in spec of vpRobot. "
1759  "Case not taken in account.");
1760  return;
1761  }
1762  }
1763 
1764  Catch();
1765  if (TryStt < 0) {
1766  if (TryStt == VelStopOnJoint) {
1767  UInt32 axisInJoint[njoint];
1768  PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1769  for (unsigned int i = 0; i < njoint; i++) {
1770  if (axisInJoint[i])
1771  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1772  }
1773  } else {
1774  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1775  if (TryString != NULL) {
1776  // The statement is in TryString, but we need to check the validity
1777  printf(" Error sentence %s\n", TryString); // Print the TryString
1778  } else {
1779  printf("\n");
1780  }
1781  }
1782  }
1783 
1784  return;
1785 }
1786 
1787 /* ------------------------------------------------------------------------ */
1788 /* --- GET ---------------------------------------------------------------- */
1789 /* ------------------------------------------------------------------------ */
1790 
1847 void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1848 {
1849  velocity.resize(6);
1850  velocity = 0;
1851 
1852  vpColVector q_cur(6);
1853  vpHomogeneousMatrix fMe_cur, fMc_cur;
1854  vpHomogeneousMatrix eMe, cMc; // camera displacement
1855  double time_cur;
1856 
1857  InitTry;
1858 
1859  // Get the current joint position
1860  Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1861  time_cur = timestamp;
1862  q_cur.deg2rad();
1863 
1864  // Get the end-effector pose from the direct kinematics
1865  vpViper850::get_fMe(q_cur, fMe_cur);
1866  // Get the camera pose from the direct kinematics
1867  vpViper850::get_fMc(q_cur, fMc_cur);
1868 
1869  if (!m_first_time_getvel) {
1870 
1871  switch (frame) {
1873  // Compute the displacement of the end-effector since the previous call
1874  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1875 
1876  // Compute the velocity of the end-effector from this displacement
1877  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1878 
1879  break;
1880  }
1881 
1882  case vpRobot::CAMERA_FRAME: {
1883  // Compute the displacement of the camera since the previous call
1884  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1885 
1886  // Compute the velocity of the camera from this displacement
1887  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1888 
1889  break;
1890  }
1891 
1892  case vpRobot::ARTICULAR_FRAME: {
1893  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1894  break;
1895  }
1896 
1897  case vpRobot::REFERENCE_FRAME: {
1898  // Compute the displacement of the camera since the previous call
1899  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1900 
1901  // Compute the velocity of the camera from this displacement
1902  vpColVector v;
1903  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1904 
1905  // Express this velocity in the reference frame
1906  vpVelocityTwistMatrix fVc(fMc_cur);
1907  velocity = fVc * v;
1908 
1909  break;
1910  }
1911 
1912  case vpRobot::MIXT_FRAME: {
1913  // Compute the displacement of the camera since the previous call
1914  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1915 
1916  // Compute the ThetaU representation for the rotation
1917  vpRotationMatrix cRc;
1918  cMc.extract(cRc);
1919  vpThetaUVector thetaU;
1920  thetaU.buildFrom(cRc);
1921 
1922  for (unsigned int i = 0; i < 3; i++) {
1923  // Compute the translation displacement in the reference frame
1924  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1925  // Update the rotation displacement in the camera frame
1926  velocity[i + 3] = thetaU[i];
1927  }
1928 
1929  // Compute the velocity
1930  velocity /= (time_cur - m_time_prev_getvel);
1931  break;
1932  }
1933  default: {
1935  "vpRobotViper850::getVelocity() not implemented in end-effector"));
1936  }
1937  }
1938  } else {
1939  m_first_time_getvel = false;
1940  }
1941 
1942  // Memorize the end-effector pose for the next call
1943  m_fMe_prev_getvel = fMe_cur;
1944  // Memorize the camera pose for the next call
1945  m_fMc_prev_getvel = fMc_cur;
1946 
1947  // Memorize the joint position for the next call
1948  m_q_prev_getvel = q_cur;
1949 
1950  // Memorize the time associated to the joint position for the next call
1951  m_time_prev_getvel = time_cur;
1952 
1953  CatchPrint();
1954  if (TryStt < 0) {
1955  vpERROR_TRACE("Cannot get velocity.");
1956  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1957  }
1958 }
1959 
1969 {
1970  double timestamp;
1971  getVelocity(frame, velocity, timestamp);
1972 }
1973 
2024 {
2025  vpColVector velocity;
2026  getVelocity(frame, velocity, timestamp);
2027 
2028  return velocity;
2029 }
2030 
2040 {
2041  vpColVector velocity;
2042  double timestamp;
2043  getVelocity(frame, velocity, timestamp);
2044 
2045  return velocity;
2046 }
2047 
2113 bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2114 {
2115  std::ifstream fd(filename.c_str(), std::ios::in);
2116 
2117  if (!fd.is_open()) {
2118  return false;
2119  }
2120 
2121  std::string line;
2122  std::string key("R:");
2123  std::string id("#Viper850 - Position");
2124  bool pos_found = false;
2125  int lineNum = 0;
2126 
2127  q.resize(njoint);
2128 
2129  while (std::getline(fd, line)) {
2130  lineNum++;
2131  if (lineNum == 1) {
2132  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2133  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2134  return false;
2135  }
2136  }
2137  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2138  continue;
2139  }
2140  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2141  // check if there are at least njoint values in the line
2142  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2143  if (chain.size() < njoint + 1) // try to split with tab separator
2144  chain = vpIoTools::splitChain(line, std::string("\t"));
2145  if (chain.size() < njoint + 1)
2146  continue;
2147 
2148  std::istringstream ss(line);
2149  std::string key_;
2150  ss >> key_;
2151  for (unsigned int i = 0; i < njoint; i++)
2152  ss >> q[i];
2153  pos_found = true;
2154  break;
2155  }
2156  }
2157 
2158  // converts rotations from degrees into radians
2159  q.deg2rad();
2160 
2161  fd.close();
2162 
2163  if (!pos_found) {
2164  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2165  return false;
2166  }
2167 
2168  return true;
2169 }
2170 
2194 bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2195 {
2196 
2197  FILE *fd;
2198  fd = fopen(filename.c_str(), "w");
2199  if (fd == NULL)
2200  return false;
2201 
2202  fprintf(fd, "\
2203 #Viper850 - Position - Version 1.00\n\
2204 #\n\
2205 # R: A B C D E F\n\
2206 # Joint position in degrees\n\
2207 #\n\
2208 #\n\n");
2209 
2210  // Save positions in mm and deg
2211  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2212  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2213 
2214  fclose(fd);
2215  return (true);
2216 }
2217 
2228 void vpRobotViper850::move(const std::string &filename)
2229 {
2230  vpColVector q;
2231 
2232  try {
2233  this->readPosFile(filename, q);
2235  this->setPositioningVelocity(10);
2237  } catch (...) {
2238  throw;
2239  }
2240 }
2241 
2261 {
2262  displacement.resize(6);
2263  displacement = 0;
2264 
2265  double q[6];
2266  vpColVector q_cur(6);
2267  double timestamp;
2268 
2269  InitTry;
2270 
2271  // Get the current joint position
2272  Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2273  for (unsigned int i = 0; i < njoint; i++) {
2274  q_cur[i] = q[i];
2275  }
2276 
2277  if (!m_first_time_getdis) {
2278  switch (frame) {
2279  case vpRobot::CAMERA_FRAME: {
2280  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2281  return;
2282  }
2283 
2284  case vpRobot::ARTICULAR_FRAME: {
2285  displacement = q_cur - m_q_prev_getdis;
2286  break;
2287  }
2288 
2289  case vpRobot::REFERENCE_FRAME: {
2290  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2291  return;
2292  }
2293 
2294  case vpRobot::MIXT_FRAME: {
2295  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2296  return;
2297  }
2299  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2300  return;
2301  }
2302  }
2303  } else {
2304  m_first_time_getdis = false;
2305  }
2306 
2307  // Memorize the joint position for the next call
2308  m_q_prev_getdis = q_cur;
2309 
2310  CatchPrint();
2311  if (TryStt < 0) {
2312  vpERROR_TRACE("Cannot get velocity.");
2313  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2314  }
2315 }
2316 
2325 {
2326 #if defined(USE_ATI_DAQ)
2327 #if defined(VISP_HAVE_COMEDI)
2328  m_ati.bias();
2329 #else
2330  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2331  "apt-get install libcomedi-dev"));
2332 #endif
2333 #else // Use serial link
2334  InitTry;
2335 
2336  Try(PrimitiveTFS_BIAS_Viper850());
2337 
2338  // Wait 500 ms to be sure the next measures take into account the bias
2339  vpTime::wait(500);
2340 
2341  CatchPrint();
2342  if (TryStt < 0) {
2343  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2344  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2345  }
2346 #endif
2347 }
2348 
2357 {
2358 #if defined(USE_ATI_DAQ)
2359 #if defined(VISP_HAVE_COMEDI)
2360  m_ati.unbias();
2361 #else
2362  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2363  "apt-get install libcomedi-dev"));
2364 #endif
2365 #else // Use serial link
2366 // Not implemented
2367 #endif
2368 }
2369 
2410 {
2411 #if defined(USE_ATI_DAQ)
2412 #if defined(VISP_HAVE_COMEDI)
2413  H = m_ati.getForceTorque();
2414 #else
2415  (void)H;
2416  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2417  "apt-get install libcomedi-dev"));
2418 #endif
2419 #else // Use serial link
2420  InitTry;
2421 
2422  H.resize(6);
2423 
2424  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2425 
2426  CatchPrint();
2427  if (TryStt < 0) {
2428  vpERROR_TRACE("Cannot get the force/torque measures.");
2429  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2430  }
2431 #endif
2432 }
2433 
2472 {
2473 #if defined(USE_ATI_DAQ)
2474 #if defined(VISP_HAVE_COMEDI)
2475  vpColVector H = m_ati.getForceTorque();
2476  return H;
2477 #else
2478  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2479  "apt-get install libcomedi-dev"));
2480 #endif
2481 #else // Use serial link
2482  InitTry;
2483 
2484  vpColVector H(6);
2485 
2486  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2487  return H;
2488 
2489  CatchPrint();
2490  if (TryStt < 0) {
2491  vpERROR_TRACE("Cannot get the force/torque measures.");
2492  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2493  }
2494  return H; // Here to avoid a warning, but should never be called
2495 #endif
2496 }
2497 
2505 {
2506  InitTry;
2507  Try(PrimitivePneumaticGripper_Viper850(1));
2508  std::cout << "Open the pneumatic gripper..." << std::endl;
2509  CatchPrint();
2510  if (TryStt < 0) {
2511  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2512  }
2513 }
2514 
2523 {
2524  InitTry;
2525  Try(PrimitivePneumaticGripper_Viper850(0));
2526  std::cout << "Close the pneumatic gripper..." << std::endl;
2527  CatchPrint();
2528  if (TryStt < 0) {
2529  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2530  }
2531 }
2532 
2539 {
2540  InitTry;
2541  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2542  std::cout << "Enable joint limits on axis 6..." << std::endl;
2543  CatchPrint();
2544  if (TryStt < 0) {
2545  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2546  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2547  }
2548 }
2549 
2561 {
2562  InitTry;
2563  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2564  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2565  CatchPrint();
2566  if (TryStt < 0) {
2567  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2568  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2569  }
2570 }
2571 
2581 {
2584 
2585  return;
2586 }
2587 
2609 {
2610  maxRotationVelocity_joint6 = w6_max;
2611  return;
2612 }
2613 
2621 double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2622 
2623 #elif !defined(VISP_BUILD_SHARED_LIBS)
2624 // Work arround to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2625 // no symbols
2626 void dummy_vpRobotViper850(){};
2627 #endif
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocity(double w_max)
void set_eMc(const vpHomogeneousMatrix &eMc_)
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
double getMaxRotationVelocityJoint6() const
Error that can be emited by the vpRobot class and its derivates.
void disableJoint6Limits() const
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
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
vpColVector getForceTorque() const
double maxRotationVelocity
Definition: vpRobot.h:98
Initialize the position controller.
Definition: vpRobot.h:67
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
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
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 move(const std::string &filename)
void get_eJe(vpMatrix &eJe)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
double getTime() const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void enableJoint6Limits() const
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.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
vpColVector getForceTorque() const
double getPositioningVelocity(void) const
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:116
void setPositioningVelocity(double velocity)
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
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:103
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
bool getPowerState() const
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 get_fJe(vpMatrix &fJe)
static double rad(double deg)
Definition: vpMath.h:110
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
static bool readPosFile(const std::string &filename, vpColVector &q)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_cMe(vpHomogeneousMatrix &cMe) const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
void get_cVe(vpVelocityTwistMatrix &cVe) const
void setMaxRotationVelocityJoint6(double w6_max)
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
static double deg(double rad)
Definition: vpMath.h:103
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
Emergency stop activated.
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
virtual ~vpRobotViper850(void)
void init(void)
Definition: vpViper850.cpp:136
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
static const double m_defaultPositioningVelocity
Automatic control mode (default).
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 closeGripper() const
vpColVector joint_min
Definition: vpViper.h:173