Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
vpRobotViper850.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Viper S850 robot.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_VIPER850
39 
40 #include <signal.h>
41 #include <stdlib.h>
42 #include <sys/types.h>
43 #include <unistd.h>
44 
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpExponentialMap.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpThetaUVector.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/robot/vpRobot.h>
51 #include <visp3/robot/vpRobotException.h>
52 #include <visp3/robot/vpRobotViper850.h>
53 
54 /* ---------------------------------------------------------------------- */
55 /* --- STATIC ----------------------------------------------------------- */
56 /* ---------------------------------------------------------------------- */
57 
58 bool vpRobotViper850::m_robotAlreadyCreated = false;
59 
68 
69 /* ---------------------------------------------------------------------- */
70 /* --- EMERGENCY STOP --------------------------------------------------- */
71 /* ---------------------------------------------------------------------- */
72 
80 void emergencyStopViper850(int signo)
81 {
82  std::cout << "Stop the Viper850 application by signal (" << signo << "): " << (char)7;
83  switch (signo) {
84  case SIGINT:
85  std::cout << "SIGINT (stop by ^C) " << std::endl;
86  break;
87  case SIGBUS:
88  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
89  break;
90  case SIGSEGV:
91  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
92  break;
93  case SIGKILL:
94  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
95  break;
96  case SIGQUIT:
97  std::cout << "SIGQUIT " << std::endl;
98  break;
99  default:
100  std::cout << signo << std::endl;
101  }
102  // std::cout << "Emergency stop called\n";
103  // PrimitiveESTOP_Viper850();
104  PrimitiveSTOP_Viper850();
105  std::cout << "Robot was stopped\n";
106 
107  // Free allocated resources
108  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109 
110  fprintf(stdout, "Application ");
111  fflush(stdout);
112  kill(getpid(), SIGKILL);
113  exit(1);
114 }
115 
116 /* ---------------------------------------------------------------------- */
117 /* --- CONSTRUCTOR ------------------------------------------------------ */
118 /* ---------------------------------------------------------------------- */
119 
173 vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
174 {
175 
176  /*
177  #define SIGHUP 1 // hangup
178  #define SIGINT 2 // interrupt (rubout)
179  #define SIGQUIT 3 // quit (ASCII FS)
180  #define SIGILL 4 // illegal instruction (not reset when caught)
181  #define SIGTRAP 5 // trace trap (not reset when caught)
182  #define SIGIOT 6 // IOT instruction
183  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
184  #define SIGEMT 7 // EMT instruction
185  #define SIGFPE 8 // floating point exception
186  #define SIGKILL 9 // kill (cannot be caught or ignored)
187  #define SIGBUS 10 // bus error
188  #define SIGSEGV 11 // segmentation violation
189  #define SIGSYS 12 // bad argument to system call
190  #define SIGPIPE 13 // write on a pipe with no one to read it
191  #define SIGALRM 14 // alarm clock
192  #define SIGTERM 15 // software termination signal from kill
193  */
194 
195  signal(SIGINT, emergencyStopViper850);
196  signal(SIGBUS, emergencyStopViper850);
197  signal(SIGSEGV, emergencyStopViper850);
198  signal(SIGKILL, emergencyStopViper850);
199  signal(SIGQUIT, emergencyStopViper850);
200 
201  setVerbose(verbose);
202  if (verbose_)
203  std::cout << "Open communication with MotionBlox.\n";
204  try {
205  this->init();
207  } catch (...) {
208  // vpERROR_TRACE("Error caught") ;
209  throw;
210  }
211  m_positioningVelocity = m_defaultPositioningVelocity;
212 
213  maxRotationVelocity_joint6 = maxRotationVelocity;
214 
215  vpRobotViper850::m_robotAlreadyCreated = true;
216 
217  return;
218 }
219 
220 /* ------------------------------------------------------------------------ */
221 /* --- INITIALISATION ----------------------------------------------------- */
222 /* ------------------------------------------------------------------------ */
223 
249 {
250  InitTry;
251 
252  // Initialise private variables used to compute the measured velocities
253  m_q_prev_getvel.resize(6);
254  m_q_prev_getvel = 0;
255  m_time_prev_getvel = 0;
256  m_first_time_getvel = true;
257 
258  // Initialise private variables used to compute the measured displacement
259  m_q_prev_getdis.resize(6);
260  m_q_prev_getdis = 0;
261  m_first_time_getdis = true;
262 
263 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
264  std::string calibfile;
265 #ifdef VISP_HAVE_VIPER850_DATA
266  calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
267  if (!vpIoTools::checkFilename(calibfile))
268  throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str()));
269 #else
270  throw(vpException(vpException::ioError, "You don't have access to Viper850 "
271  "data to retrieve ATI F/T calib "
272  "file"));
273 #endif
274  m_ati.setCalibrationFile(calibfile);
275  m_ati.open();
276 #endif
277 
278  // Initialize the firewire connection
279  Try(InitializeConnection(verbose_));
280 
281  // Connect to the servoboard using the servo board GUID
282  Try(InitializeNode_Viper850());
283 
284  Try(PrimitiveRESET_Viper850());
285 
286  // Enable the joint limits on axis 6
287  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
288 
289  // Update the eMc matrix in the low level controller
291 
292  // Look if the power is on or off
293  UInt32 HIPowerStatus;
294  UInt32 EStopStatus;
295  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
296  CAL_Wait(0.1);
297 
298  // Print the robot status
299  if (verbose_) {
300  std::cout << "Robot status: ";
301  switch (EStopStatus) {
302  case ESTOP_AUTO:
303  m_controlMode = AUTO;
304  if (HIPowerStatus == 0)
305  std::cout << "Power is OFF" << std::endl;
306  else
307  std::cout << "Power is ON" << std::endl;
308  break;
309 
310  case ESTOP_MANUAL:
311  m_controlMode = MANUAL;
312  if (HIPowerStatus == 0)
313  std::cout << "Power is OFF" << std::endl;
314  else
315  std::cout << "Power is ON" << std::endl;
316  break;
317  case ESTOP_ACTIVATED:
318  m_controlMode = ESTOP;
319  std::cout << "Emergency stop is activated" << std::endl;
320  break;
321  default:
322  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
323  std::cout << "You have to call Adept for maintenance..." << std::endl;
324  // Free allocated resources
325  }
326  std::cout << std::endl;
327  }
328  // get real joint min/max from the MotionBlox
329  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
330  // Convert units from degrees to radians
331  joint_min.deg2rad();
332  joint_max.deg2rad();
333 
334  // for (unsigned int i=0; i < njoint; i++) {
335  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
336  // joint_max[i]);
337  // }
338 
339  // If an error occur in the low level controller, goto here
340  // CatchPrint();
341  Catch();
342 
343  // Test if an error occurs
344  if (TryStt == -20001)
345  printf("No connection detected. Check if the robot is powered on \n"
346  "and if the firewire link exist between the MotionBlox and this "
347  "computer.\n");
348  else if (TryStt == -675)
349  printf(" Timeout enabling power...\n");
350 
351  if (TryStt < 0) {
352  // Power off the robot
353  PrimitivePOWEROFF_Viper850();
354  // Free allocated resources
355  ShutDownConnection();
356 
357  std::cout << "Cannot open connection with the motionblox..." << std::endl;
358  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
359  }
360  return;
361 }
362 
420 {
421  // Read the robot constants from files
422  // - joint [min,max], coupl_56, long_56
423  // - camera extrinsic parameters relative to eMc
425 
426  InitTry;
427 
428  // get real joint min/max from the MotionBlox
429  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
430  // Convert units from degrees to radians
431  joint_min.deg2rad();
432  joint_max.deg2rad();
433 
434  // for (unsigned int i=0; i < njoint; i++) {
435  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
436  // joint_max[i]);
437  // }
438 
439  // Set the camera constant (eMc pose) in the MotionBlox
440  double eMc_pose[6];
441  for (unsigned int i = 0; i < 3; i++) {
442  eMc_pose[i] = etc[i]; // translation in meters
443  eMc_pose[i + 3] = erc[i]; // rotation in rad
444  }
445  // Update the eMc pose in the low level controller
446  Try(PrimitiveCONST_Viper850(eMc_pose));
447 
448  CatchPrint();
449  return;
450 }
451 
502 void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename)
503 {
504  vpViper850::init(tool, filename);
505 
506  InitTry;
507 
508  // Get real joint min/max from the MotionBlox
509  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
510  // Convert units from degrees to radians
511  joint_min.deg2rad();
512  joint_max.deg2rad();
513 
514  // for (unsigned int i=0; i < njoint; i++) {
515  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
516  // joint_max[i]);
517  // }
518 
519  // Set the camera constant (eMc pose) in the MotionBlox
520  double eMc_pose[6];
521  for (unsigned int i = 0; i < 3; i++) {
522  eMc_pose[i] = etc[i]; // translation in meters
523  eMc_pose[i + 3] = erc[i]; // rotation in rad
524  }
525  // Update the eMc pose in the low level controller
526  Try(PrimitiveCONST_Viper850(eMc_pose));
527 
528  CatchPrint();
529  return;
530 }
531 
569 {
570  vpViper850::init(tool, eMc_);
571 
572  InitTry;
573 
574  // Get real joint min/max from the MotionBlox
575  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
576  // Convert units from degrees to radians
577  joint_min.deg2rad();
578  joint_max.deg2rad();
579 
580  // for (unsigned int i=0; i < njoint; i++) {
581  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
582  // joint_max[i]);
583  // }
584 
585  // Set the camera constant (eMc pose) in the MotionBlox
586  double eMc_pose[6];
587  for (unsigned int i = 0; i < 3; i++) {
588  eMc_pose[i] = etc[i]; // translation in meters
589  eMc_pose[i + 3] = erc[i]; // rotation in rad
590  }
591  // Update the eMc pose in the low level controller
592  Try(PrimitiveCONST_Viper850(eMc_pose));
593 
594  CatchPrint();
595  return;
596 }
597 
610 {
611  this->vpViper850::set_eMc(eMc_);
612 
613  InitTry;
614 
615  // Set the camera constant (eMc pose) in the MotionBlox
616  double eMc_pose[6];
617  for (unsigned int i = 0; i < 3; i++) {
618  eMc_pose[i] = etc[i]; // translation in meters
619  eMc_pose[i + 3] = erc[i]; // rotation in rad
620  }
621  // Update the eMc pose in the low level controller
622  Try(PrimitiveCONST_Viper850(eMc_pose));
623 
624  CatchPrint();
625 
626  return;
627 }
628 
642 {
643  this->vpViper850::set_eMc(etc_, erc_);
644 
645  InitTry;
646 
647  // Set the camera constant (eMc pose) in the MotionBlox
648  double eMc_pose[6];
649  for (unsigned int i = 0; i < 3; i++) {
650  eMc_pose[i] = etc[i]; // translation in meters
651  eMc_pose[i + 3] = erc[i]; // rotation in rad
652  }
653  // Update the eMc pose in the low level controller
654  Try(PrimitiveCONST_Viper850(eMc_pose));
655 
656  CatchPrint();
657 
658  return;
659 }
660 
661 /* ------------------------------------------------------------------------ */
662 /* --- DESTRUCTOR --------------------------------------------------------- */
663 /* ------------------------------------------------------------------------ */
664 
672 {
673 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
674  m_ati.close();
675 #endif
676 
677  InitTry;
678 
680 
681  // Look if the power is on or off
682  UInt32 HIPowerStatus;
683  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
684  CAL_Wait(0.1);
685 
686  // if (HIPowerStatus == 1) {
687  // fprintf(stdout, "Power OFF the robot\n");
688  // fflush(stdout);
689 
690  // Try( PrimitivePOWEROFF_Viper850() );
691  // }
692 
693  // Free allocated resources
694  ShutDownConnection();
695 
696  vpRobotViper850::m_robotAlreadyCreated = false;
697 
698  CatchPrint();
699  return;
700 }
701 
709 {
710  InitTry;
711 
712  switch (newState) {
713  case vpRobot::STATE_STOP: {
714  // Start primitive STOP only if the current state is Velocity
716  Try(PrimitiveSTOP_Viper850());
717  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
718  }
719  break;
720  }
723  std::cout << "Change the control mode from velocity to position control.\n";
724  Try(PrimitiveSTOP_Viper850());
725  } else {
726  // std::cout << "Change the control mode from stop to position
727  // control.\n";
728  }
729  this->powerOn();
730  break;
731  }
734  std::cout << "Change the control mode from stop to velocity control.\n";
735  }
736  this->powerOn();
737  break;
738  }
739  default:
740  break;
741  }
742 
743  CatchPrint();
744 
745  return vpRobot::setRobotState(newState);
746 }
747 
748 /* ------------------------------------------------------------------------ */
749 /* --- STOP --------------------------------------------------------------- */
750 /* ------------------------------------------------------------------------ */
751 
760 {
762  return;
763 
764  InitTry;
765  Try(PrimitiveSTOP_Viper850());
767 
768  CatchPrint();
769  if (TryStt < 0) {
770  vpERROR_TRACE("Cannot stop robot motion");
771  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
772  }
773 }
774 
785 {
786  InitTry;
787 
788  // Look if the power is on or off
789  UInt32 HIPowerStatus;
790  UInt32 EStopStatus;
791  bool firsttime = true;
792  unsigned int nitermax = 10;
793 
794  for (unsigned int i = 0; i < nitermax; i++) {
795  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
796  if (EStopStatus == ESTOP_AUTO) {
797  m_controlMode = AUTO;
798  break; // exit for loop
799  } else if (EStopStatus == ESTOP_MANUAL) {
800  m_controlMode = MANUAL;
801  break; // exit for loop
802  } else if (EStopStatus == ESTOP_ACTIVATED) {
803  m_controlMode = ESTOP;
804  if (firsttime) {
805  std::cout << "Emergency stop is activated! \n"
806  << "Check the emergency stop button and push the yellow "
807  "button before continuing."
808  << std::endl;
809  firsttime = false;
810  }
811  fprintf(stdout, "Remaining time %us \r", nitermax - i);
812  fflush(stdout);
813  CAL_Wait(1);
814  } else {
815  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
816  std::cout << "You have to call Adept for maintenance..." << std::endl;
817  // Free allocated resources
818  ShutDownConnection();
819  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
820  }
821  }
822 
823  if (EStopStatus == ESTOP_ACTIVATED)
824  std::cout << std::endl;
825 
826  if (EStopStatus == ESTOP_ACTIVATED) {
827  std::cout << "Sorry, cannot power on the robot." << std::endl;
828  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
829  }
830 
831  if (HIPowerStatus == 0) {
832  fprintf(stdout, "Power ON the Viper850 robot\n");
833  fflush(stdout);
834 
835  Try(PrimitivePOWERON_Viper850());
836  }
837 
838  CatchPrint();
839  if (TryStt < 0) {
840  vpERROR_TRACE("Cannot power on the robot");
841  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
842  }
843 }
844 
855 {
856  InitTry;
857 
858  // Look if the power is on or off
859  UInt32 HIPowerStatus;
860  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
861  CAL_Wait(0.1);
862 
863  if (HIPowerStatus == 1) {
864  fprintf(stdout, "Power OFF the Viper850 robot\n");
865  fflush(stdout);
866 
867  Try(PrimitivePOWEROFF_Viper850());
868  }
869 
870  CatchPrint();
871  if (TryStt < 0) {
872  vpERROR_TRACE("Cannot power off the robot");
873  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
874  }
875 }
876 
889 {
890  InitTry;
891  bool status = false;
892  // Look if the power is on or off
893  UInt32 HIPowerStatus;
894  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
895  CAL_Wait(0.1);
896 
897  if (HIPowerStatus == 1) {
898  status = true;
899  }
900 
901  CatchPrint();
902  if (TryStt < 0) {
903  vpERROR_TRACE("Cannot get the power status");
904  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
905  }
906  return status;
907 }
908 
919 {
921  vpViper850::get_cMe(cMe);
922 
923  cVe.buildFrom(cMe);
924 }
925 
938 
951 {
952 
953  double position[6];
954  double timestamp;
955 
956  InitTry;
957  Try(PrimitiveACQ_POS_J_Viper850(position, &timestamp));
958  CatchPrint();
959 
960  vpColVector q(6);
961  for (unsigned int i = 0; i < njoint; i++)
962  q[i] = vpMath::rad(position[i]);
963 
964  try {
966  } catch (...) {
967  vpERROR_TRACE("catch exception ");
968  throw;
969  }
970 }
984 {
985 
986  double position[6];
987  double timestamp;
988 
989  InitTry;
990  Try(PrimitiveACQ_POS_Viper850(position, &timestamp));
991  CatchPrint();
992 
993  vpColVector q(6);
994  for (unsigned int i = 0; i < njoint; i++)
995  q[i] = position[i];
996 
997  try {
999  } catch (...) {
1000  vpERROR_TRACE("Error caught");
1001  throw;
1002  }
1003 }
1004 
1041 void vpRobotViper850::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1042 
1048 double vpRobotViper850::getPositioningVelocity(void) const { return m_positioningVelocity; }
1049 
1127 {
1128 
1130  vpERROR_TRACE("Robot was not in position-based control\n"
1131  "Modification of the robot state");
1133  }
1134 
1135  vpColVector destination(njoint);
1136  int error = 0;
1137  double timestamp;
1138 
1139  InitTry;
1140  switch (frame) {
1141  case vpRobot::CAMERA_FRAME: {
1142  vpColVector q(njoint);
1143  Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1144 
1145  // Convert degrees into rad
1146  q.deg2rad();
1147 
1148  // Get fMc from the inverse kinematics
1149  vpHomogeneousMatrix fMc;
1150  vpViper850::get_fMc(q, fMc);
1151 
1152  // Set cMc from the input position
1153  vpTranslationVector txyz;
1154  vpRxyzVector rxyz;
1155  for (unsigned int i = 0; i < 3; i++) {
1156  txyz[i] = position[i];
1157  rxyz[i] = position[i + 3];
1158  }
1159 
1160  // Compute cMc2
1161  vpRotationMatrix cRc2(rxyz);
1162  vpHomogeneousMatrix cMc2(txyz, cRc2);
1163 
1164  // Compute the new position to reach: fMc*cMc2
1165  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1166 
1167  // Compute the corresponding joint position from the inverse kinematics
1168  unsigned int solution = this->getInverseKinematics(fMc2, q);
1169  if (solution) { // Position is reachable
1170  destination = q;
1171  // convert rad to deg requested for the low level controller
1172  destination.rad2deg();
1173  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1174  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1175  } else {
1176  // Cartesian position is out of range
1177  error = -1;
1178  }
1179 
1180  break;
1181  }
1182  case vpRobot::ARTICULAR_FRAME: {
1183  destination = position;
1184  // convert rad to deg requested for the low level controller
1185  destination.rad2deg();
1186 
1187  // std::cout << "Joint destination (deg): " << destination.t() <<
1188  // std::endl;
1189  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1190  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1191  break;
1192  }
1193  case vpRobot::REFERENCE_FRAME: {
1194  // Convert angles from Rxyz representation to Rzyz representation
1195  vpRxyzVector rxyz(position[3], position[4], position[5]);
1196  vpRotationMatrix R(rxyz);
1197  vpRzyzVector rzyz(R);
1198 
1199  for (unsigned int i = 0; i < 3; i++) {
1200  destination[i] = position[i];
1201  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1202  }
1203  int configuration = 0; // keep the actual configuration
1204 
1205  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1206  // << std::endl;
1207  Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, m_positioningVelocity));
1208  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1209 
1210  break;
1211  }
1212  case vpRobot::MIXT_FRAME: {
1213  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1214  "Mixt frame not implemented.");
1215  }
1217  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1218  "End-effector frame not implemented.");
1219  }
1220  }
1221 
1222  CatchPrint();
1223  if (TryStt == InvalidPosition || TryStt == -1023)
1224  std::cout << " : Position out of range.\n";
1225  else if (TryStt == -3019) {
1226  if (frame == vpRobot::ARTICULAR_FRAME)
1227  std::cout << " : Joint position out of range.\n";
1228  else
1229  std::cout << " : Cartesian position leads to a joint position out of "
1230  "range.\n";
1231  } else if (TryStt < 0)
1232  std::cout << " : Unknown error (see Fabien).\n";
1233  else if (error == -1)
1234  std::cout << "Position out of range.\n";
1235 
1236  if (TryStt < 0 || error < 0) {
1237  vpERROR_TRACE("Positioning error.");
1238  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1239  }
1240 
1241  return;
1242 }
1243 
1308 void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1309  double pos4, double pos5, double pos6)
1310 {
1311  try {
1312  vpColVector position(6);
1313  position[0] = pos1;
1314  position[1] = pos2;
1315  position[2] = pos3;
1316  position[3] = pos4;
1317  position[4] = pos5;
1318  position[5] = pos6;
1319 
1320  setPosition(frame, position);
1321  } catch (...) {
1322  vpERROR_TRACE("Error caught");
1323  throw;
1324  }
1325 }
1326 
1365 void vpRobotViper850::setPosition(const std::string &filename)
1366 {
1367  vpColVector q;
1368  bool ret;
1369 
1370  ret = this->readPosFile(filename, q);
1371 
1372  if (ret == false) {
1373  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1374  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1375  }
1378 }
1379 
1447 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1448 {
1449 
1450  InitTry;
1451 
1452  position.resize(6);
1453 
1454  switch (frame) {
1455  case vpRobot::CAMERA_FRAME: {
1456  position = 0;
1457  return;
1458  }
1459  case vpRobot::ARTICULAR_FRAME: {
1460  Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1461  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1462  position.deg2rad();
1463 
1464  return;
1465  }
1466  case vpRobot::REFERENCE_FRAME: {
1467  vpColVector q(njoint);
1468  Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1469 
1470  // Compute fMc
1472 
1473  // From fMc extract the pose
1474  vpRotationMatrix fRc;
1475  fMc.extract(fRc);
1476  vpRxyzVector rxyz;
1477  rxyz.buildFrom(fRc);
1478 
1479  for (unsigned int i = 0; i < 3; i++) {
1480  position[i] = fMc[i][3]; // translation x,y,z
1481  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1482  }
1483 
1484  break;
1485  }
1486  case vpRobot::MIXT_FRAME: {
1487  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1488  "not implemented");
1489  }
1491  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1492  "not implemented");
1493  }
1494  }
1495 
1496  CatchPrint();
1497  if (TryStt < 0) {
1498  vpERROR_TRACE("Cannot get position.");
1499  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1500  }
1501 
1502  return;
1503 }
1504 
1510 {
1511  double timestamp;
1512  PrimitiveACQ_TIME_Viper850(&timestamp);
1513  return timestamp;
1514 }
1515 
1527 {
1528  double timestamp;
1529  getPosition(frame, position, timestamp);
1530 }
1531 
1543 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1544 {
1545  vpColVector posRxyz;
1546  // recupere position en Rxyz
1547  this->getPosition(frame, posRxyz, timestamp);
1548  vpRxyzVector RxyzVect;
1549  for (unsigned int j = 0; j < 3; j++)
1550  RxyzVect[j] = posRxyz[j + 3];
1551  // recupere le vecteur thetaU correspondant
1552  vpThetaUVector RtuVect(RxyzVect);
1553 
1554  // remplit le vpPoseVector avec translation et rotation ThetaU
1555  for (unsigned int j = 0; j < 3; j++) {
1556  position[j] = posRxyz[j];
1557  position[j + 3] = RtuVect[j];
1558  }
1559 }
1560 
1572 {
1573  vpColVector posRxyz;
1574  double timestamp;
1575  // recupere position en Rxyz
1576  this->getPosition(frame, posRxyz, timestamp);
1577  vpRxyzVector RxyzVect;
1578  for (unsigned int j = 0; j < 3; j++)
1579  RxyzVect[j] = posRxyz[j + 3];
1580  // recupere le vecteur thetaU correspondant
1581  vpThetaUVector RtuVect(RxyzVect);
1582 
1583  // remplit le vpPoseVector avec translation et rotation ThetaU
1584  for (unsigned int j = 0; j < 3; j++) {
1585  position[j] = posRxyz[j];
1586  position[j + 3] = RtuVect[j];
1587  }
1588 }
1589 
1668 {
1670  vpERROR_TRACE("Cannot send a velocity to the robot "
1671  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1673  "Cannot send a velocity to the robot "
1674  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1675  }
1676 
1677  vpColVector vel_sat(6);
1678 
1679  // Velocity saturation
1680  switch (frame) {
1681  // saturation in cartesian space
1682  case vpRobot::CAMERA_FRAME:
1685  case vpRobot::MIXT_FRAME: {
1686  vpColVector vel_max(6);
1687 
1688  for (unsigned int i = 0; i < 3; i++)
1689  vel_max[i] = getMaxTranslationVelocity();
1690  for (unsigned int i = 3; i < 6; i++)
1691  vel_max[i] = getMaxRotationVelocity();
1692 
1693  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1694 
1695  break;
1696  }
1697  // saturation in joint space
1698  case vpRobot::ARTICULAR_FRAME: {
1699  vpColVector vel_max(6);
1700 
1701  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1702  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1703 
1704  for (unsigned int i = 0; i < 6; i++)
1705  vel_max[i] = getMaxRotationVelocity();
1706  } else {
1707  for (unsigned int i = 0; i < 5; i++)
1708  vel_max[i] = getMaxRotationVelocity();
1709  vel_max[5] = getMaxRotationVelocityJoint6();
1710  }
1711 
1712  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1713  }
1714  }
1715 
1716  InitTry;
1717 
1718  switch (frame) {
1719  case vpRobot::CAMERA_FRAME: {
1720  // Send velocities in m/s and rad/s
1721  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1722  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1723  break;
1724  }
1726  // Transform in camera frame
1727  vpHomogeneousMatrix cMe;
1728  this->get_cMe(cMe);
1729  vpVelocityTwistMatrix cVe(cMe);
1730  vpColVector v_c = cVe * vel_sat;
1731  // Send velocities in m/s and rad/s
1732  Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1733  break;
1734  }
1735  case vpRobot::ARTICULAR_FRAME: {
1736  // Convert all the velocities from rad/s into deg/s
1737  vel_sat.rad2deg();
1738  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1739  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1740  Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1741  break;
1742  }
1743  case vpRobot::REFERENCE_FRAME: {
1744  // Send velocities in m/s and rad/s
1745  // std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1746  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1747  break;
1748  }
1749  case vpRobot::MIXT_FRAME: {
1750  // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1751  break;
1752  }
1753  default: {
1754  vpERROR_TRACE("Error in spec of vpRobot. "
1755  "Case not taken in account.");
1756  return;
1757  }
1758  }
1759 
1760  Catch();
1761  if (TryStt < 0) {
1762  if (TryStt == VelStopOnJoint) {
1763  UInt32 axisInJoint[njoint];
1764  PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
1765  for (unsigned int i = 0; i < njoint; i++) {
1766  if (axisInJoint[i])
1767  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1768  }
1769  } else {
1770  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1771  if (TryString != nullptr) {
1772  // The statement is in TryString, but we need to check the validity
1773  printf(" Error sentence %s\n", TryString); // Print the TryString
1774  } else {
1775  printf("\n");
1776  }
1777  }
1778  }
1779 
1780  return;
1781 }
1782 
1783 /* ------------------------------------------------------------------------ */
1784 /* --- GET ---------------------------------------------------------------- */
1785 /* ------------------------------------------------------------------------ */
1786 
1843 void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1844 {
1845  velocity.resize(6);
1846  velocity = 0;
1847 
1848  vpColVector q_cur(6);
1849  vpHomogeneousMatrix fMe_cur, fMc_cur;
1850  vpHomogeneousMatrix eMe, cMc; // camera displacement
1851  double time_cur;
1852 
1853  InitTry;
1854 
1855  // Get the current joint position
1856  Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1857  time_cur = timestamp;
1858  q_cur.deg2rad();
1859 
1860  // Get the end-effector pose from the direct kinematics
1861  vpViper850::get_fMe(q_cur, fMe_cur);
1862  // Get the camera pose from the direct kinematics
1863  vpViper850::get_fMc(q_cur, fMc_cur);
1864 
1865  if (!m_first_time_getvel) {
1866 
1867  switch (frame) {
1869  // Compute the displacement of the end-effector since the previous call
1870  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1871 
1872  // Compute the velocity of the end-effector from this displacement
1873  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1874 
1875  break;
1876  }
1877 
1878  case vpRobot::CAMERA_FRAME: {
1879  // Compute the displacement of the camera since the previous call
1880  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1881 
1882  // Compute the velocity of the camera from this displacement
1883  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1884 
1885  break;
1886  }
1887 
1888  case vpRobot::ARTICULAR_FRAME: {
1889  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1890  break;
1891  }
1892 
1893  case vpRobot::REFERENCE_FRAME: {
1894  // Compute the displacement of the camera since the previous call
1895  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1896 
1897  // Compute the velocity of the camera from this displacement
1898  vpColVector v;
1899  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1900 
1901  // Express this velocity in the reference frame
1902  vpVelocityTwistMatrix fVc(fMc_cur);
1903  velocity = fVc * v;
1904 
1905  break;
1906  }
1907 
1908  case vpRobot::MIXT_FRAME: {
1909  // Compute the displacement of the camera since the previous call
1910  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1911 
1912  // Compute the ThetaU representation for the rotation
1913  vpRotationMatrix cRc;
1914  cMc.extract(cRc);
1915  vpThetaUVector thetaU;
1916  thetaU.buildFrom(cRc);
1917 
1918  for (unsigned int i = 0; i < 3; i++) {
1919  // Compute the translation displacement in the reference frame
1920  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1921  // Update the rotation displacement in the camera frame
1922  velocity[i + 3] = thetaU[i];
1923  }
1924 
1925  // Compute the velocity
1926  velocity /= (time_cur - m_time_prev_getvel);
1927  break;
1928  }
1929  default: {
1931  "vpRobotViper850::getVelocity() not implemented in end-effector"));
1932  }
1933  }
1934  } else {
1935  m_first_time_getvel = false;
1936  }
1937 
1938  // Memorize the end-effector pose for the next call
1939  m_fMe_prev_getvel = fMe_cur;
1940  // Memorize the camera pose for the next call
1941  m_fMc_prev_getvel = fMc_cur;
1942 
1943  // Memorize the joint position for the next call
1944  m_q_prev_getvel = q_cur;
1945 
1946  // Memorize the time associated to the joint position for the next call
1947  m_time_prev_getvel = time_cur;
1948 
1949  CatchPrint();
1950  if (TryStt < 0) {
1951  vpERROR_TRACE("Cannot get velocity.");
1952  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1953  }
1954 }
1955 
1965 {
1966  double timestamp;
1967  getVelocity(frame, velocity, timestamp);
1968 }
1969 
2020 {
2021  vpColVector velocity;
2022  getVelocity(frame, velocity, timestamp);
2023 
2024  return velocity;
2025 }
2026 
2036 {
2037  vpColVector velocity;
2038  double timestamp;
2039  getVelocity(frame, velocity, timestamp);
2040 
2041  return velocity;
2042 }
2043 
2109 bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2110 {
2111  std::ifstream fd(filename.c_str(), std::ios::in);
2112 
2113  if (!fd.is_open()) {
2114  return false;
2115  }
2116 
2117  std::string line;
2118  std::string key("R:");
2119  std::string id("#Viper850 - Position");
2120  bool pos_found = false;
2121  int lineNum = 0;
2122 
2123  q.resize(njoint);
2124 
2125  while (std::getline(fd, line)) {
2126  lineNum++;
2127  if (lineNum == 1) {
2128  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2129  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2130  return false;
2131  }
2132  }
2133  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2134  continue;
2135  }
2136  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2137  // check if there are at least njoint values in the line
2138  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2139  if (chain.size() < njoint + 1) // try to split with tab separator
2140  chain = vpIoTools::splitChain(line, std::string("\t"));
2141  if (chain.size() < njoint + 1)
2142  continue;
2143 
2144  std::istringstream ss(line);
2145  std::string key_;
2146  ss >> key_;
2147  for (unsigned int i = 0; i < njoint; i++)
2148  ss >> q[i];
2149  pos_found = true;
2150  break;
2151  }
2152  }
2153 
2154  // converts rotations from degrees into radians
2155  q.deg2rad();
2156 
2157  fd.close();
2158 
2159  if (!pos_found) {
2160  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2161  return false;
2162  }
2163 
2164  return true;
2165 }
2166 
2190 bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2191 {
2192 
2193  FILE *fd;
2194  fd = fopen(filename.c_str(), "w");
2195  if (fd == nullptr)
2196  return false;
2197 
2198  fprintf(fd, "\
2199 #Viper850 - Position - Version 1.00\n\
2200 #\n\
2201 # R: A B C D E F\n\
2202 # Joint position in degrees\n\
2203 #\n\
2204 #\n\n");
2205 
2206  // Save positions in mm and deg
2207  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2208  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2209 
2210  fclose(fd);
2211  return (true);
2212 }
2213 
2224 void vpRobotViper850::move(const std::string &filename)
2225 {
2226  vpColVector q;
2227 
2228  try {
2229  this->readPosFile(filename, q);
2231  this->setPositioningVelocity(10);
2233  } catch (...) {
2234  throw;
2235  }
2236 }
2237 
2257 {
2258  displacement.resize(6);
2259  displacement = 0;
2260 
2261  double q[6];
2262  vpColVector q_cur(6);
2263  double timestamp;
2264 
2265  InitTry;
2266 
2267  // Get the current joint position
2268  Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2269  for (unsigned int i = 0; i < njoint; i++) {
2270  q_cur[i] = q[i];
2271  }
2272 
2273  if (!m_first_time_getdis) {
2274  switch (frame) {
2275  case vpRobot::CAMERA_FRAME: {
2276  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2277  return;
2278  }
2279 
2280  case vpRobot::ARTICULAR_FRAME: {
2281  displacement = q_cur - m_q_prev_getdis;
2282  break;
2283  }
2284 
2285  case vpRobot::REFERENCE_FRAME: {
2286  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2287  return;
2288  }
2289 
2290  case vpRobot::MIXT_FRAME: {
2291  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2292  return;
2293  }
2295  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2296  return;
2297  }
2298  }
2299  } else {
2300  m_first_time_getdis = false;
2301  }
2302 
2303  // Memorize the joint position for the next call
2304  m_q_prev_getdis = q_cur;
2305 
2306  CatchPrint();
2307  if (TryStt < 0) {
2308  vpERROR_TRACE("Cannot get velocity.");
2309  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2310  }
2311 }
2312 
2321 {
2322 #if defined(USE_ATI_DAQ)
2323 #if defined(VISP_HAVE_COMEDI)
2324  m_ati.bias();
2325 #else
2326  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2327  "apt-get install libcomedi-dev"));
2328 #endif
2329 #else // Use serial link
2330  InitTry;
2331 
2332  Try(PrimitiveTFS_BIAS_Viper850());
2333 
2334  // Wait 500 ms to be sure the next measures take into account the bias
2335  vpTime::wait(500);
2336 
2337  CatchPrint();
2338  if (TryStt < 0) {
2339  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2340  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2341  }
2342 #endif
2343 }
2344 
2353 {
2354 #if defined(USE_ATI_DAQ)
2355 #if defined(VISP_HAVE_COMEDI)
2356  m_ati.unbias();
2357 #else
2358  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2359  "apt-get install libcomedi-dev"));
2360 #endif
2361 #else // Use serial link
2362 // Not implemented
2363 #endif
2364 }
2365 
2406 {
2407 #if defined(USE_ATI_DAQ)
2408 #if defined(VISP_HAVE_COMEDI)
2409  H = m_ati.getForceTorque();
2410 #else
2411  (void)H;
2412  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2413  "apt-get install libcomedi-dev"));
2414 #endif
2415 #else // Use serial link
2416  InitTry;
2417 
2418  H.resize(6);
2419 
2420  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2421 
2422  CatchPrint();
2423  if (TryStt < 0) {
2424  vpERROR_TRACE("Cannot get the force/torque measures.");
2425  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2426  }
2427 #endif
2428 }
2429 
2468 {
2469 #if defined(USE_ATI_DAQ)
2470 #if defined(VISP_HAVE_COMEDI)
2471  vpColVector H = m_ati.getForceTorque();
2472  return H;
2473 #else
2474  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2475  "apt-get install libcomedi-dev"));
2476 #endif
2477 #else // Use serial link
2478  InitTry;
2479 
2480  vpColVector H(6);
2481 
2482  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2483  return H;
2484 
2485  CatchPrint();
2486  if (TryStt < 0) {
2487  vpERROR_TRACE("Cannot get the force/torque measures.");
2488  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2489  }
2490  return H; // Here to avoid a warning, but should never be called
2491 #endif
2492 }
2493 
2501 {
2502  InitTry;
2503  Try(PrimitivePneumaticGripper_Viper850(1));
2504  std::cout << "Open the pneumatic gripper..." << std::endl;
2505  CatchPrint();
2506  if (TryStt < 0) {
2507  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2508  }
2509 }
2510 
2519 {
2520  InitTry;
2521  Try(PrimitivePneumaticGripper_Viper850(0));
2522  std::cout << "Close the pneumatic gripper..." << std::endl;
2523  CatchPrint();
2524  if (TryStt < 0) {
2525  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2526  }
2527 }
2528 
2535 {
2536  InitTry;
2537  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2538  std::cout << "Enable joint limits on axis 6..." << std::endl;
2539  CatchPrint();
2540  if (TryStt < 0) {
2541  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2542  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2543  }
2544 }
2545 
2557 {
2558  InitTry;
2559  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2560  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2561  CatchPrint();
2562  if (TryStt < 0) {
2563  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2564  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2565  }
2566 }
2567 
2577 {
2580 
2581  return;
2582 }
2583 
2605 {
2606  maxRotationVelocity_joint6 = w6_max;
2607  return;
2608 }
2609 
2617 double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2618 
2619 #elif !defined(VISP_BUILD_SHARED_LIBS)
2620 // Work around to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2621 // no symbols
2622 void dummy_vpRobotViper850(){};
2623 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & rad2deg()
Definition: vpColVector.h:970
vpColVector & deg2rad()
Definition: vpColVector.h:342
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ fatalError
Fatal error.
Definition: vpException.h:84
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:2427
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:1215
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
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 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 getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
void get_eJe(vpMatrix &eJe) vp_override
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
double getPositioningVelocity(void) const
static bool readPosFile(const std::string &filename, vpColVector &q)
void setPositioningVelocity(double velocity)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position) vp_override
void get_cMe(vpHomogeneousMatrix &cMe) const
void move(const std::string &filename)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getMaxRotationVelocityJoint6() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_fJe(vpMatrix &fJe) vp_override
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void setVerbose(bool verbose)
Definition: vpRobot.h:168
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
double maxRotationVelocity
Definition: vpRobot.h:98
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:257
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:175
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)
Modelization of the ADEPT Viper 850 robot.
Definition: vpViper850.h:95
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:129
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:168
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:120
void init(void)
Definition: vpViper850.cpp:133
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1212
vpTranslationVector etc
Definition: vpViper.h:156
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:904
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1141
vpColVector joint_max
Definition: vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:555
vpRxyzVector erc
Definition: vpViper.h:157
vpColVector joint_min
Definition: vpViper.h:170
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:151
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:952
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:707
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:592
#define vpERROR_TRACE
Definition: vpDebug.h:384
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)