Visual Servoing Platform  version 3.6.1 under development (2024-10-14)
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 BEGIN_VISP_NAMESPACE
55 /* ---------------------------------------------------------------------- */
56 /* --- STATIC ----------------------------------------------------------- */
57 /* ---------------------------------------------------------------------- */
58 
59 bool vpRobotViper850::m_robotAlreadyCreated = false;
60 
69 
70 /* ---------------------------------------------------------------------- */
71 /* --- EMERGENCY STOP --------------------------------------------------- */
72 /* ---------------------------------------------------------------------- */
73 
81 void emergencyStopViper850(int signo)
82 {
83  std::cout << "Stop the Viper850 application by signal (" << signo << "): " << (char)7;
84  switch (signo) {
85  case SIGINT:
86  std::cout << "SIGINT (stop by ^C) " << std::endl;
87  break;
88  case SIGBUS:
89  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
90  break;
91  case SIGSEGV:
92  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
93  break;
94  case SIGKILL:
95  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
96  break;
97  case SIGQUIT:
98  std::cout << "SIGQUIT " << std::endl;
99  break;
100  default:
101  std::cout << signo << std::endl;
102  }
103  // std::cout << "Emergency stop called\n";
104  // PrimitiveESTOP_Viper850();
105  PrimitiveSTOP_Viper850();
106  std::cout << "Robot was stopped\n";
107 
108  // Free allocated resources
109  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
110 
111  fprintf(stdout, "Application ");
112  fflush(stdout);
113  kill(getpid(), SIGKILL);
114  exit(1);
115 }
116 
117 /* ---------------------------------------------------------------------- */
118 /* --- CONSTRUCTOR ------------------------------------------------------ */
119 /* ---------------------------------------------------------------------- */
120 
178 vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
179 {
180 
181  /*
182  #define SIGHUP 1 // hangup
183  #define SIGINT 2 // interrupt (rubout)
184  #define SIGQUIT 3 // quit (ASCII FS)
185  #define SIGILL 4 // illegal instruction (not reset when caught)
186  #define SIGTRAP 5 // trace trap (not reset when caught)
187  #define SIGIOT 6 // IOT instruction
188  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
189  #define SIGEMT 7 // EMT instruction
190  #define SIGFPE 8 // floating point exception
191  #define SIGKILL 9 // kill (cannot be caught or ignored)
192  #define SIGBUS 10 // bus error
193  #define SIGSEGV 11 // segmentation violation
194  #define SIGSYS 12 // bad argument to system call
195  #define SIGPIPE 13 // write on a pipe with no one to read it
196  #define SIGALRM 14 // alarm clock
197  #define SIGTERM 15 // software termination signal from kill
198  */
199 
200  signal(SIGINT, emergencyStopViper850);
201  signal(SIGBUS, emergencyStopViper850);
202  signal(SIGSEGV, emergencyStopViper850);
203  signal(SIGKILL, emergencyStopViper850);
204  signal(SIGQUIT, emergencyStopViper850);
205 
206  setVerbose(verbose);
207  if (verbose_)
208  std::cout << "Open communication with MotionBlox.\n";
209  try {
210  this->init();
212  }
213  catch (...) {
214  // vpERROR_TRACE("Error caught") ;
215  throw;
216  }
217  m_positioningVelocity = m_defaultPositioningVelocity;
218 
219  maxRotationVelocity_joint6 = maxRotationVelocity;
220 
221  vpRobotViper850::m_robotAlreadyCreated = true;
222 
223  return;
224 }
225 
226 /* ------------------------------------------------------------------------ */
227 /* --- INITIALISATION ----------------------------------------------------- */
228 /* ------------------------------------------------------------------------ */
229 
255 {
256  InitTry;
257 
258  // Initialise private variables used to compute the measured velocities
259  m_q_prev_getvel.resize(6);
260  m_q_prev_getvel = 0;
261  m_time_prev_getvel = 0;
262  m_first_time_getvel = true;
263 
264  // Initialise private variables used to compute the measured displacement
265  m_q_prev_getdis.resize(6);
266  m_q_prev_getdis = 0;
267  m_first_time_getdis = true;
268 
269 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
270  std::string calibfile;
271 #ifdef VISP_HAVE_VIPER850_DATA
272  calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
273  if (!vpIoTools::checkFilename(calibfile))
274  throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str()));
275 #else
276  throw(vpException(vpException::ioError, "You don't have access to Viper850 "
277  "data to retrieve ATI F/T calib "
278  "file"));
279 #endif
280  m_ati.setCalibrationFile(calibfile);
281  m_ati.open();
282 #endif
283 
284  // Initialize the firewire connection
285  Try(InitializeConnection(verbose_));
286 
287  // Connect to the servoboard using the servo board GUID
288  Try(InitializeNode_Viper850());
289 
290  Try(PrimitiveRESET_Viper850());
291 
292  // Enable the joint limits on axis 6
293  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
294 
295  // Update the eMc matrix in the low level controller
297 
298  // Look if the power is on or off
299  UInt32 HIPowerStatus;
300  UInt32 EStopStatus;
301  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
302  CAL_Wait(0.1);
303 
304  // Print the robot status
305  if (verbose_) {
306  std::cout << "Robot status: ";
307  switch (EStopStatus) {
308  case ESTOP_AUTO:
309  m_controlMode = AUTO;
310  if (HIPowerStatus == 0)
311  std::cout << "Power is OFF" << std::endl;
312  else
313  std::cout << "Power is ON" << std::endl;
314  break;
315 
316  case ESTOP_MANUAL:
317  m_controlMode = MANUAL;
318  if (HIPowerStatus == 0)
319  std::cout << "Power is OFF" << std::endl;
320  else
321  std::cout << "Power is ON" << std::endl;
322  break;
323  case ESTOP_ACTIVATED:
324  m_controlMode = ESTOP;
325  std::cout << "Emergency stop is activated" << std::endl;
326  break;
327  default:
328  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
329  std::cout << "You have to call Adept for maintenance..." << std::endl;
330  // Free allocated resources
331  }
332  std::cout << std::endl;
333  }
334  // get real joint min/max from the MotionBlox
335  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
336  // Convert units from degrees to radians
337  joint_min.deg2rad();
338  joint_max.deg2rad();
339 
340  // for (unsigned int i=0; i < njoint; i++) {
341  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
342  // joint_max[i]);
343  // }
344 
345  // If an error occur in the low level controller, goto here
346  // CatchPrint();
347  Catch();
348 
349  // Test if an error occurs
350  if (TryStt == -20001)
351  printf("No connection detected. Check if the robot is powered on \n"
352  "and if the firewire link exist between the MotionBlox and this "
353  "computer.\n");
354  else if (TryStt == -675)
355  printf(" Timeout enabling power...\n");
356 
357  if (TryStt < 0) {
358  // Power off the robot
359  PrimitivePOWEROFF_Viper850();
360  // Free allocated resources
361  ShutDownConnection();
362 
363  std::cout << "Cannot open connection with the motionblox..." << std::endl;
364  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
365  }
366  return;
367 }
368 
430 {
431  // Read the robot constants from files
432  // - joint [min,max], coupl_56, long_56
433  // - camera extrinsic parameters relative to eMc
435 
436  InitTry;
437 
438  // get real joint min/max from the MotionBlox
439  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
440  // Convert units from degrees to radians
441  joint_min.deg2rad();
442  joint_max.deg2rad();
443 
444  // for (unsigned int i=0; i < njoint; i++) {
445  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
446  // joint_max[i]);
447  // }
448 
449  // Set the camera constant (eMc pose) in the MotionBlox
450  double eMc_pose[6];
451  for (unsigned int i = 0; i < 3; i++) {
452  eMc_pose[i] = etc[i]; // translation in meters
453  eMc_pose[i + 3] = erc[i]; // rotation in rad
454  }
455  // Update the eMc pose in the low level controller
456  Try(PrimitiveCONST_Viper850(eMc_pose));
457 
458  CatchPrint();
459  return;
460 }
461 
516 void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename)
517 {
518  vpViper850::init(tool, filename);
519 
520  InitTry;
521 
522  // Get real joint min/max from the MotionBlox
523  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
524  // Convert units from degrees to radians
525  joint_min.deg2rad();
526  joint_max.deg2rad();
527 
528  // for (unsigned int i=0; i < njoint; i++) {
529  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
530  // joint_max[i]);
531  // }
532 
533  // Set the camera constant (eMc pose) in the MotionBlox
534  double eMc_pose[6];
535  for (unsigned int i = 0; i < 3; i++) {
536  eMc_pose[i] = etc[i]; // translation in meters
537  eMc_pose[i + 3] = erc[i]; // rotation in rad
538  }
539  // Update the eMc pose in the low level controller
540  Try(PrimitiveCONST_Viper850(eMc_pose));
541 
542  CatchPrint();
543  return;
544 }
545 
587 {
588  vpViper850::init(tool, eMc_);
589 
590  InitTry;
591 
592  // Get real joint min/max from the MotionBlox
593  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
594  // Convert units from degrees to radians
595  joint_min.deg2rad();
596  joint_max.deg2rad();
597 
598  // for (unsigned int i=0; i < njoint; i++) {
599  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
600  // joint_max[i]);
601  // }
602 
603  // Set the camera constant (eMc pose) in the MotionBlox
604  double eMc_pose[6];
605  for (unsigned int i = 0; i < 3; i++) {
606  eMc_pose[i] = etc[i]; // translation in meters
607  eMc_pose[i + 3] = erc[i]; // rotation in rad
608  }
609  // Update the eMc pose in the low level controller
610  Try(PrimitiveCONST_Viper850(eMc_pose));
611 
612  CatchPrint();
613  return;
614 }
615 
628 {
629  this->vpViper850::set_eMc(eMc_);
630 
631  InitTry;
632 
633  // Set the camera constant (eMc pose) in the MotionBlox
634  double eMc_pose[6];
635  for (unsigned int i = 0; i < 3; i++) {
636  eMc_pose[i] = etc[i]; // translation in meters
637  eMc_pose[i + 3] = erc[i]; // rotation in rad
638  }
639  // Update the eMc pose in the low level controller
640  Try(PrimitiveCONST_Viper850(eMc_pose));
641 
642  CatchPrint();
643 
644  return;
645 }
646 
660 {
661  this->vpViper850::set_eMc(etc_, erc_);
662 
663  InitTry;
664 
665  // Set the camera constant (eMc pose) in the MotionBlox
666  double eMc_pose[6];
667  for (unsigned int i = 0; i < 3; i++) {
668  eMc_pose[i] = etc[i]; // translation in meters
669  eMc_pose[i + 3] = erc[i]; // rotation in rad
670  }
671  // Update the eMc pose in the low level controller
672  Try(PrimitiveCONST_Viper850(eMc_pose));
673 
674  CatchPrint();
675 
676  return;
677 }
678 
679 /* ------------------------------------------------------------------------ */
680 /* --- DESTRUCTOR --------------------------------------------------------- */
681 /* ------------------------------------------------------------------------ */
682 
690 {
691 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
692  m_ati.close();
693 #endif
694 
695  InitTry;
696 
698 
699  // Look if the power is on or off
700  UInt32 HIPowerStatus;
701  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
702  CAL_Wait(0.1);
703 
704  // if (HIPowerStatus == 1) {
705  // fprintf(stdout, "Power OFF the robot\n");
706  // fflush(stdout);
707 
708  // Try( PrimitivePOWEROFF_Viper850() );
709  // }
710 
711  // Free allocated resources
712  ShutDownConnection();
713 
714  vpRobotViper850::m_robotAlreadyCreated = false;
715 
716  CatchPrint();
717  return;
718 }
719 
727 {
728  InitTry;
729 
730  switch (newState) {
731  case vpRobot::STATE_STOP: {
732  // Start primitive STOP only if the current state is Velocity
734  Try(PrimitiveSTOP_Viper850());
735  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
736  }
737  break;
738  }
741  std::cout << "Change the control mode from velocity to position control.\n";
742  Try(PrimitiveSTOP_Viper850());
743  }
744  else {
745  // std::cout << "Change the control mode from stop to position
746  // control.\n";
747  }
748  this->powerOn();
749  break;
750  }
753  std::cout << "Change the control mode from stop to velocity control.\n";
754  }
755  this->powerOn();
756  break;
757  }
758  default:
759  break;
760  }
761 
762  CatchPrint();
763 
764  return vpRobot::setRobotState(newState);
765 }
766 
767 /* ------------------------------------------------------------------------ */
768 /* --- STOP --------------------------------------------------------------- */
769 /* ------------------------------------------------------------------------ */
770 
779 {
781  return;
782 
783  InitTry;
784  Try(PrimitiveSTOP_Viper850());
786 
787  CatchPrint();
788  if (TryStt < 0) {
789  vpERROR_TRACE("Cannot stop robot motion");
790  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
791  }
792 }
793 
804 {
805  InitTry;
806 
807  // Look if the power is on or off
808  UInt32 HIPowerStatus;
809  UInt32 EStopStatus;
810  bool firsttime = true;
811  unsigned int nitermax = 10;
812 
813  for (unsigned int i = 0; i < nitermax; i++) {
814  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, &EStopStatus, nullptr, nullptr, nullptr, &HIPowerStatus));
815  if (EStopStatus == ESTOP_AUTO) {
816  m_controlMode = AUTO;
817  break; // exit for loop
818  }
819  else if (EStopStatus == ESTOP_MANUAL) {
820  m_controlMode = MANUAL;
821  break; // exit for loop
822  }
823  else if (EStopStatus == ESTOP_ACTIVATED) {
824  m_controlMode = ESTOP;
825  if (firsttime) {
826  std::cout << "Emergency stop is activated! \n"
827  << "Check the emergency stop button and push the yellow "
828  "button before continuing."
829  << std::endl;
830  firsttime = false;
831  }
832  fprintf(stdout, "Remaining time %us \r", nitermax - i);
833  fflush(stdout);
834  CAL_Wait(1);
835  }
836  else {
837  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
838  std::cout << "You have to call Adept for maintenance..." << std::endl;
839  // Free allocated resources
840  ShutDownConnection();
841  throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
842  }
843  }
844 
845  if (EStopStatus == ESTOP_ACTIVATED)
846  std::cout << std::endl;
847 
848  if (EStopStatus == ESTOP_ACTIVATED) {
849  std::cout << "Sorry, cannot power on the robot." << std::endl;
850  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
851  }
852 
853  if (HIPowerStatus == 0) {
854  fprintf(stdout, "Power ON the Viper850 robot\n");
855  fflush(stdout);
856 
857  Try(PrimitivePOWERON_Viper850());
858  }
859 
860  CatchPrint();
861  if (TryStt < 0) {
862  vpERROR_TRACE("Cannot power on the robot");
863  throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
864  }
865 }
866 
877 {
878  InitTry;
879 
880  // Look if the power is on or off
881  UInt32 HIPowerStatus;
882  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
883  CAL_Wait(0.1);
884 
885  if (HIPowerStatus == 1) {
886  fprintf(stdout, "Power OFF the Viper850 robot\n");
887  fflush(stdout);
888 
889  Try(PrimitivePOWEROFF_Viper850());
890  }
891 
892  CatchPrint();
893  if (TryStt < 0) {
894  vpERROR_TRACE("Cannot power off the robot");
895  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
896  }
897 }
898 
911 {
912  InitTry;
913  bool status = false;
914  // Look if the power is on or off
915  UInt32 HIPowerStatus;
916  Try(PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, &HIPowerStatus));
917  CAL_Wait(0.1);
918 
919  if (HIPowerStatus == 1) {
920  status = true;
921  }
922 
923  CatchPrint();
924  if (TryStt < 0) {
925  vpERROR_TRACE("Cannot get the power status");
926  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
927  }
928  return status;
929 }
930 
941 {
943  vpViper850::get_cMe(cMe);
944 
945  cVe.buildFrom(cMe);
946 }
947 
960 
973 {
974 
975  double position[6];
976  double timestamp;
977 
978  InitTry;
979  Try(PrimitiveACQ_POS_J_Viper850(position, &timestamp));
980  CatchPrint();
981 
982  vpColVector q(6);
983  for (unsigned int i = 0; i < njoint; i++)
984  q[i] = vpMath::rad(position[i]);
985 
986  try {
988  }
989  catch (...) {
990  vpERROR_TRACE("catch exception ");
991  throw;
992  }
993 }
1007 {
1008 
1009  double position[6];
1010  double timestamp;
1011 
1012  InitTry;
1013  Try(PrimitiveACQ_POS_Viper850(position, &timestamp));
1014  CatchPrint();
1015 
1016  vpColVector q(6);
1017  for (unsigned int i = 0; i < njoint; i++)
1018  q[i] = position[i];
1019 
1020  try {
1022  }
1023  catch (...) {
1024  vpERROR_TRACE("Error caught");
1025  throw;
1026  }
1027 }
1028 
1069 void vpRobotViper850::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1070 
1076 double vpRobotViper850::getPositioningVelocity(void) const { return m_positioningVelocity; }
1077 
1159 {
1160 
1162  vpERROR_TRACE("Robot was not in position-based control\n"
1163  "Modification of the robot state");
1165  }
1166 
1167  vpColVector destination(njoint);
1168  int error = 0;
1169  double timestamp;
1170 
1171  InitTry;
1172  switch (frame) {
1173  case vpRobot::CAMERA_FRAME: {
1174  vpColVector q(njoint);
1175  Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1176 
1177  // Convert degrees into rad
1178  q.deg2rad();
1179 
1180  // Get fMc from the inverse kinematics
1181  vpHomogeneousMatrix fMc;
1182  vpViper850::get_fMc(q, fMc);
1183 
1184  // Set cMc from the input position
1185  vpTranslationVector txyz;
1186  vpRxyzVector rxyz;
1187  for (unsigned int i = 0; i < 3; i++) {
1188  txyz[i] = position[i];
1189  rxyz[i] = position[i + 3];
1190  }
1191 
1192  // Compute cMc2
1193  vpRotationMatrix cRc2(rxyz);
1194  vpHomogeneousMatrix cMc2(txyz, cRc2);
1195 
1196  // Compute the new position to reach: fMc*cMc2
1197  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1198 
1199  // Compute the corresponding joint position from the inverse kinematics
1200  unsigned int solution = this->getInverseKinematics(fMc2, q);
1201  if (solution) { // Position is reachable
1202  destination = q;
1203  // convert rad to deg requested for the low level controller
1204  destination.rad2deg();
1205  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1206  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1207  }
1208  else {
1209  // Cartesian position is out of range
1210  error = -1;
1211  }
1212 
1213  break;
1214  }
1215  case vpRobot::ARTICULAR_FRAME: {
1216  destination = position;
1217  // convert rad to deg requested for the low level controller
1218  destination.rad2deg();
1219 
1220  // std::cout << "Joint destination (deg): " << destination.t() <<
1221  // std::endl;
1222  Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1223  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1224  break;
1225  }
1226  case vpRobot::REFERENCE_FRAME: {
1227  // Convert angles from Rxyz representation to Rzyz representation
1228  vpRxyzVector rxyz(position[3], position[4], position[5]);
1229  vpRotationMatrix R(rxyz);
1230  vpRzyzVector rzyz(R);
1231 
1232  for (unsigned int i = 0; i < 3; i++) {
1233  destination[i] = position[i];
1234  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1235  }
1236  int configuration = 0; // keep the actual configuration
1237 
1238  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1239  // << std::endl;
1240  Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, m_positioningVelocity));
1241  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1242 
1243  break;
1244  }
1245  case vpRobot::MIXT_FRAME: {
1246  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1247  "Mixt frame not implemented.");
1248  }
1250  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1251  "End-effector frame not implemented.");
1252  }
1253  }
1254 
1255  CatchPrint();
1256  if (TryStt == InvalidPosition || TryStt == -1023)
1257  std::cout << " : Position out of range.\n";
1258  else if (TryStt == -3019) {
1259  if (frame == vpRobot::ARTICULAR_FRAME)
1260  std::cout << " : Joint position out of range.\n";
1261  else
1262  std::cout << " : Cartesian position leads to a joint position out of "
1263  "range.\n";
1264  }
1265  else if (TryStt < 0)
1266  std::cout << " : Unknown error (see Fabien).\n";
1267  else if (error == -1)
1268  std::cout << "Position out of range.\n";
1269 
1270  if (TryStt < 0 || error < 0) {
1271  vpERROR_TRACE("Positioning error.");
1272  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1273  }
1274 
1275  return;
1276 }
1277 
1346 void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1347  double pos4, double pos5, double pos6)
1348 {
1349  try {
1350  vpColVector position(6);
1351  position[0] = pos1;
1352  position[1] = pos2;
1353  position[2] = pos3;
1354  position[3] = pos4;
1355  position[4] = pos5;
1356  position[5] = pos6;
1357 
1358  setPosition(frame, position);
1359  }
1360  catch (...) {
1361  vpERROR_TRACE("Error caught");
1362  throw;
1363  }
1364 }
1365 
1408 void vpRobotViper850::setPosition(const std::string &filename)
1409 {
1410  vpColVector q;
1411  bool ret;
1412 
1413  ret = this->readPosFile(filename, q);
1414 
1415  if (ret == false) {
1416  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1417  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1418  }
1421 }
1422 
1494 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1495 {
1496 
1497  InitTry;
1498 
1499  position.resize(6);
1500 
1501  switch (frame) {
1502  case vpRobot::CAMERA_FRAME: {
1503  position = 0;
1504  return;
1505  }
1506  case vpRobot::ARTICULAR_FRAME: {
1507  Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1508  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1509  position.deg2rad();
1510 
1511  return;
1512  }
1513  case vpRobot::REFERENCE_FRAME: {
1514  vpColVector q(njoint);
1515  Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1516 
1517  // Compute fMc
1519 
1520  // From fMc extract the pose
1521  vpRotationMatrix fRc;
1522  fMc.extract(fRc);
1523  vpRxyzVector rxyz;
1524  rxyz.buildFrom(fRc);
1525 
1526  for (unsigned int i = 0; i < 3; i++) {
1527  position[i] = fMc[i][3]; // translation x,y,z
1528  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1529  }
1530 
1531  break;
1532  }
1533  case vpRobot::MIXT_FRAME: {
1534  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1535  "not implemented");
1536  }
1538  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1539  "not implemented");
1540  }
1541  }
1542 
1543  CatchPrint();
1544  if (TryStt < 0) {
1545  vpERROR_TRACE("Cannot get position.");
1546  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1547  }
1548 
1549  return;
1550 }
1551 
1557 {
1558  double timestamp;
1559  PrimitiveACQ_TIME_Viper850(&timestamp);
1560  return timestamp;
1561 }
1562 
1574 {
1575  double timestamp;
1576  getPosition(frame, position, timestamp);
1577 }
1578 
1590 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1591 {
1592  vpColVector posRxyz;
1593  // recupere position en Rxyz
1594  this->getPosition(frame, posRxyz, timestamp);
1595  vpRxyzVector RxyzVect;
1596  for (unsigned int j = 0; j < 3; j++)
1597  RxyzVect[j] = posRxyz[j + 3];
1598  // recupere le vecteur thetaU correspondant
1599  vpThetaUVector RtuVect(RxyzVect);
1600 
1601  // remplit le vpPoseVector avec translation et rotation ThetaU
1602  for (unsigned int j = 0; j < 3; j++) {
1603  position[j] = posRxyz[j];
1604  position[j + 3] = RtuVect[j];
1605  }
1606 }
1607 
1619 {
1620  vpColVector posRxyz;
1621  double timestamp;
1622  // recupere position en Rxyz
1623  this->getPosition(frame, posRxyz, timestamp);
1624  vpRxyzVector RxyzVect;
1625  for (unsigned int j = 0; j < 3; j++)
1626  RxyzVect[j] = posRxyz[j + 3];
1627  // recupere le vecteur thetaU correspondant
1628  vpThetaUVector RtuVect(RxyzVect);
1629 
1630  // remplit le vpPoseVector avec translation et rotation ThetaU
1631  for (unsigned int j = 0; j < 3; j++) {
1632  position[j] = posRxyz[j];
1633  position[j + 3] = RtuVect[j];
1634  }
1635 }
1636 
1719 {
1721  vpERROR_TRACE("Cannot send a velocity to the robot "
1722  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1724  "Cannot send a velocity to the robot "
1725  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1726  }
1727 
1728  vpColVector vel_sat(6);
1729 
1730  // Velocity saturation
1731  switch (frame) {
1732  // saturation in cartesian space
1733  case vpRobot::CAMERA_FRAME:
1736  case vpRobot::MIXT_FRAME: {
1737  vpColVector vel_max(6);
1738 
1739  for (unsigned int i = 0; i < 3; i++)
1740  vel_max[i] = getMaxTranslationVelocity();
1741  for (unsigned int i = 3; i < 6; i++)
1742  vel_max[i] = getMaxRotationVelocity();
1743 
1744  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1745 
1746  break;
1747  }
1748  // saturation in joint space
1749  case vpRobot::ARTICULAR_FRAME: {
1750  vpColVector vel_max(6);
1751 
1752  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1753  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1754 
1755  for (unsigned int i = 0; i < 6; i++)
1756  vel_max[i] = getMaxRotationVelocity();
1757  }
1758  else {
1759  for (unsigned int i = 0; i < 5; i++)
1760  vel_max[i] = getMaxRotationVelocity();
1761  vel_max[5] = getMaxRotationVelocityJoint6();
1762  }
1763 
1764  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1765  }
1766  }
1767 
1768  InitTry;
1769 
1770  switch (frame) {
1771  case vpRobot::CAMERA_FRAME: {
1772  // Send velocities in m/s and rad/s
1773  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1774  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1775  break;
1776  }
1778  // Transform in camera frame
1779  vpHomogeneousMatrix cMe;
1780  this->get_cMe(cMe);
1781  vpVelocityTwistMatrix cVe(cMe);
1782  vpColVector v_c = cVe * vel_sat;
1783  // Send velocities in m/s and rad/s
1784  Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1785  break;
1786  }
1787  case vpRobot::ARTICULAR_FRAME: {
1788  // Convert all the velocities from rad/s into deg/s
1789  vel_sat.rad2deg();
1790  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1791  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1792  Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1793  break;
1794  }
1795  case vpRobot::REFERENCE_FRAME: {
1796  // Send velocities in m/s and rad/s
1797  // std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1798  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1799  break;
1800  }
1801  case vpRobot::MIXT_FRAME: {
1802  // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1803  break;
1804  }
1805  default: {
1806  vpERROR_TRACE("Error in spec of vpRobot. "
1807  "Case not taken in account.");
1808  return;
1809  }
1810  }
1811 
1812  Catch();
1813  if (TryStt < 0) {
1814  if (TryStt == VelStopOnJoint) {
1815  UInt32 axisInJoint[njoint];
1816  PrimitiveSTATUS_Viper850(nullptr, nullptr, nullptr, nullptr, nullptr, axisInJoint, nullptr);
1817  for (unsigned int i = 0; i < njoint; i++) {
1818  if (axisInJoint[i])
1819  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1820  }
1821  }
1822  else {
1823  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1824  if (TryString != nullptr) {
1825  // The statement is in TryString, but we need to check the validity
1826  printf(" Error sentence %s\n", TryString); // Print the TryString
1827  }
1828  else {
1829  printf("\n");
1830  }
1831  }
1832  }
1833 
1834  return;
1835 }
1836 
1837 /* ------------------------------------------------------------------------ */
1838 /* --- GET ---------------------------------------------------------------- */
1839 /* ------------------------------------------------------------------------ */
1840 
1901 void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1902 {
1903  velocity.resize(6);
1904  velocity = 0;
1905 
1906  vpColVector q_cur(6);
1907  vpHomogeneousMatrix fMe_cur, fMc_cur;
1908  vpHomogeneousMatrix eMe, cMc; // camera displacement
1909  double time_cur;
1910 
1911  InitTry;
1912 
1913  // Get the current joint position
1914  Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1915  time_cur = timestamp;
1916  q_cur.deg2rad();
1917 
1918  // Get the end-effector pose from the direct kinematics
1919  vpViper850::get_fMe(q_cur, fMe_cur);
1920  // Get the camera pose from the direct kinematics
1921  vpViper850::get_fMc(q_cur, fMc_cur);
1922 
1923  if (!m_first_time_getvel) {
1924 
1925  switch (frame) {
1927  // Compute the displacement of the end-effector since the previous call
1928  eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1929 
1930  // Compute the velocity of the end-effector from this displacement
1931  velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1932 
1933  break;
1934  }
1935 
1936  case vpRobot::CAMERA_FRAME: {
1937  // Compute the displacement of the camera since the previous call
1938  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1939 
1940  // Compute the velocity of the camera from this displacement
1941  velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1942 
1943  break;
1944  }
1945 
1946  case vpRobot::ARTICULAR_FRAME: {
1947  velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1948  break;
1949  }
1950 
1951  case vpRobot::REFERENCE_FRAME: {
1952  // Compute the displacement of the camera since the previous call
1953  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1954 
1955  // Compute the velocity of the camera from this displacement
1956  vpColVector v;
1957  v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1958 
1959  // Express this velocity in the reference frame
1960  vpVelocityTwistMatrix fVc(fMc_cur);
1961  velocity = fVc * v;
1962 
1963  break;
1964  }
1965 
1966  case vpRobot::MIXT_FRAME: {
1967  // Compute the displacement of the camera since the previous call
1968  cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1969 
1970  // Compute the ThetaU representation for the rotation
1971  vpRotationMatrix cRc;
1972  cMc.extract(cRc);
1973  vpThetaUVector thetaU;
1974  thetaU.buildFrom(cRc);
1975 
1976  for (unsigned int i = 0; i < 3; i++) {
1977  // Compute the translation displacement in the reference frame
1978  velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1979  // Update the rotation displacement in the camera frame
1980  velocity[i + 3] = thetaU[i];
1981  }
1982 
1983  // Compute the velocity
1984  velocity /= (time_cur - m_time_prev_getvel);
1985  break;
1986  }
1987  default: {
1989  "vpRobotViper850::getVelocity() not implemented in end-effector"));
1990  }
1991  }
1992  }
1993  else {
1994  m_first_time_getvel = false;
1995  }
1996 
1997  // Memorize the end-effector pose for the next call
1998  m_fMe_prev_getvel = fMe_cur;
1999  // Memorize the camera pose for the next call
2000  m_fMc_prev_getvel = fMc_cur;
2001 
2002  // Memorize the joint position for the next call
2003  m_q_prev_getvel = q_cur;
2004 
2005  // Memorize the time associated to the joint position for the next call
2006  m_time_prev_getvel = time_cur;
2007 
2008  CatchPrint();
2009  if (TryStt < 0) {
2010  vpERROR_TRACE("Cannot get velocity.");
2011  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2012  }
2013 }
2014 
2024 {
2025  double timestamp;
2026  getVelocity(frame, velocity, timestamp);
2027 }
2028 
2083 {
2084  vpColVector velocity;
2085  getVelocity(frame, velocity, timestamp);
2086 
2087  return velocity;
2088 }
2089 
2099 {
2100  vpColVector velocity;
2101  double timestamp;
2102  getVelocity(frame, velocity, timestamp);
2103 
2104  return velocity;
2105 }
2106 
2176 bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2177 {
2178  std::ifstream fd(filename.c_str(), std::ios::in);
2179 
2180  if (!fd.is_open()) {
2181  return false;
2182  }
2183 
2184  std::string line;
2185  std::string key("R:");
2186  std::string id("#Viper850 - Position");
2187  bool pos_found = false;
2188  int lineNum = 0;
2189 
2190  q.resize(njoint);
2191 
2192  while (std::getline(fd, line)) {
2193  lineNum++;
2194  if (lineNum == 1) {
2195  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2196  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2197  return false;
2198  }
2199  }
2200  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2201  continue;
2202  }
2203  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2204  // check if there are at least njoint values in the line
2205  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2206  if (chain.size() < njoint + 1) // try to split with tab separator
2207  chain = vpIoTools::splitChain(line, std::string("\t"));
2208  if (chain.size() < njoint + 1)
2209  continue;
2210 
2211  std::istringstream ss(line);
2212  std::string key_;
2213  ss >> key_;
2214  for (unsigned int i = 0; i < njoint; i++)
2215  ss >> q[i];
2216  pos_found = true;
2217  break;
2218  }
2219  }
2220 
2221  // converts rotations from degrees into radians
2222  q.deg2rad();
2223 
2224  fd.close();
2225 
2226  if (!pos_found) {
2227  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2228  return false;
2229  }
2230 
2231  return true;
2232 }
2233 
2257 bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2258 {
2259 
2260  FILE *fd;
2261  fd = fopen(filename.c_str(), "w");
2262  if (fd == nullptr)
2263  return false;
2264 
2265  fprintf(fd, "\
2266 #Viper850 - Position - Version 1.00\n\
2267 #\n\
2268 # R: A B C D E F\n\
2269 # Joint position in degrees\n\
2270 #\n\
2271 #\n\n");
2272 
2273  // Save positions in mm and deg
2274  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2275  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2276 
2277  fclose(fd);
2278  return (true);
2279 }
2280 
2291 void vpRobotViper850::move(const std::string &filename)
2292 {
2293  vpColVector q;
2294 
2295  try {
2296  this->readPosFile(filename, q);
2298  this->setPositioningVelocity(10);
2300  }
2301  catch (...) {
2302  throw;
2303  }
2304 }
2305 
2325 {
2326  displacement.resize(6);
2327  displacement = 0;
2328 
2329  double q[6];
2330  vpColVector q_cur(6);
2331  double timestamp;
2332 
2333  InitTry;
2334 
2335  // Get the current joint position
2336  Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2337  for (unsigned int i = 0; i < njoint; i++) {
2338  q_cur[i] = q[i];
2339  }
2340 
2341  if (!m_first_time_getdis) {
2342  switch (frame) {
2343  case vpRobot::CAMERA_FRAME: {
2344  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2345  return;
2346  }
2347 
2348  case vpRobot::ARTICULAR_FRAME: {
2349  displacement = q_cur - m_q_prev_getdis;
2350  break;
2351  }
2352 
2353  case vpRobot::REFERENCE_FRAME: {
2354  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2355  return;
2356  }
2357 
2358  case vpRobot::MIXT_FRAME: {
2359  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2360  return;
2361  }
2363  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2364  return;
2365  }
2366  }
2367  }
2368  else {
2369  m_first_time_getdis = false;
2370  }
2371 
2372  // Memorize the joint position for the next call
2373  m_q_prev_getdis = q_cur;
2374 
2375  CatchPrint();
2376  if (TryStt < 0) {
2377  vpERROR_TRACE("Cannot get velocity.");
2378  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2379  }
2380 }
2381 
2390 {
2391 #if defined(USE_ATI_DAQ)
2392 #if defined(VISP_HAVE_COMEDI)
2393  m_ati.bias();
2394 #else
2395  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2396  "apt-get install libcomedi-dev"));
2397 #endif
2398 #else // Use serial link
2399  InitTry;
2400 
2401  Try(PrimitiveTFS_BIAS_Viper850());
2402 
2403  // Wait 500 ms to be sure the next measures take into account the bias
2404  vpTime::wait(500);
2405 
2406  CatchPrint();
2407  if (TryStt < 0) {
2408  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2409  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2410  }
2411 #endif
2412 }
2413 
2422 {
2423 #if defined(USE_ATI_DAQ)
2424 #if defined(VISP_HAVE_COMEDI)
2425  m_ati.unbias();
2426 #else
2427  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2428  "apt-get install libcomedi-dev"));
2429 #endif
2430 #else // Use serial link
2431 // Not implemented
2432 #endif
2433 }
2434 
2479 {
2480 #if defined(USE_ATI_DAQ)
2481 #if defined(VISP_HAVE_COMEDI)
2482  H = m_ati.getForceTorque();
2483 #else
2484  (void)H;
2485  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2486  "apt-get install libcomedi-dev"));
2487 #endif
2488 #else // Use serial link
2489  InitTry;
2490 
2491  H.resize(6);
2492 
2493  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2494 
2495  CatchPrint();
2496  if (TryStt < 0) {
2497  vpERROR_TRACE("Cannot get the force/torque measures.");
2498  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2499  }
2500 #endif
2501 }
2502 
2545 {
2546 #if defined(USE_ATI_DAQ)
2547 #if defined(VISP_HAVE_COMEDI)
2548  vpColVector H = m_ati.getForceTorque();
2549  return H;
2550 #else
2551  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2552  "apt-get install libcomedi-dev"));
2553 #endif
2554 #else // Use serial link
2555  InitTry;
2556 
2557  vpColVector H(6);
2558 
2559  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2560  return H;
2561 
2562  CatchPrint();
2563  if (TryStt < 0) {
2564  vpERROR_TRACE("Cannot get the force/torque measures.");
2565  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2566  }
2567  return H; // Here to avoid a warning, but should never be called
2568 #endif
2569 }
2570 
2578 {
2579  InitTry;
2580  Try(PrimitivePneumaticGripper_Viper850(1));
2581  std::cout << "Open the pneumatic gripper..." << std::endl;
2582  CatchPrint();
2583  if (TryStt < 0) {
2584  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2585  }
2586 }
2587 
2596 {
2597  InitTry;
2598  Try(PrimitivePneumaticGripper_Viper850(0));
2599  std::cout << "Close the pneumatic gripper..." << std::endl;
2600  CatchPrint();
2601  if (TryStt < 0) {
2602  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2603  }
2604 }
2605 
2612 {
2613  InitTry;
2614  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2615  std::cout << "Enable joint limits on axis 6..." << std::endl;
2616  CatchPrint();
2617  if (TryStt < 0) {
2618  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2619  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2620  }
2621 }
2622 
2634 {
2635  InitTry;
2636  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2637  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2638  CatchPrint();
2639  if (TryStt < 0) {
2640  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2641  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2642  }
2643 }
2644 
2654 {
2657 
2658  return;
2659 }
2660 
2682 {
2683  maxRotationVelocity_joint6 = w6_max;
2684  return;
2685 }
2686 
2694 double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2695 END_VISP_NAMESPACE
2696 #elif !defined(VISP_BUILD_SHARED_LIBS)
2697 // Work around to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2698 // no symbols
2699 void dummy_vpRobotViper850() { };
2700 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector & rad2deg()
Definition: vpColVector.h:1053
vpColVector & deg2rad()
Definition: vpColVector.h:380
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ ioError
I/O error.
Definition: vpException.h:67
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:66
@ fatalError
Fatal error.
Definition: vpException.h:72
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:1661
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:786
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
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.
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 getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) VP_OVERRIDE
void enableJoint6Limits() const
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_fJe(vpMatrix &fJe) 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 setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) VP_OVERRIDE
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getMaxRotationVelocityJoint6() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:106
void setVerbose(bool verbose)
Definition: vpRobot.h:170
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:155
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:164
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ ARTICULAR_FRAME
Definition: vpRobot.h:80
@ MIXT_FRAME
Definition: vpRobot.h:88
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:274
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:110
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252
double maxRotationVelocity
Definition: vpRobot.h:100
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:261
bool verbose_
Definition: vpRobot.h:118
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:183
vpRxyzVector & buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
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:134
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1229
vpTranslationVector etc
Definition: vpViper.h:158
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:153
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:921
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1158
vpColVector joint_max
Definition: vpViper.h:171
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:568
vpRxyzVector erc
Definition: vpViper.h:159
vpColVector joint_min
Definition: vpViper.h:172
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:969
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:724
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:605
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)