Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpRobotViper850.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Irisa's Viper S850 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_VIPER850
42 
43 #include <signal.h>
44 #include <stdlib.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47 
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/robot/vpRobot.h>
54 #include <visp3/robot/vpRobotException.h>
55 #include <visp3/robot/vpRobotViper850.h>
56 
57 /* ---------------------------------------------------------------------- */
58 /* --- STATIC ----------------------------------------------------------- */
59 /* ---------------------------------------------------------------------- */
60 
61 bool vpRobotViper850::robotAlreadyCreated = false;
62 
71 
72 /* ---------------------------------------------------------------------- */
73 /* --- EMERGENCY STOP --------------------------------------------------- */
74 /* ---------------------------------------------------------------------- */
75 
83 void emergencyStopViper850(int signo)
84 {
85  std::cout << "Stop the Viper850 application by signal (" << signo << "): " << (char)7;
86  switch (signo) {
87  case SIGINT:
88  std::cout << "SIGINT (stop by ^C) " << std::endl;
89  break;
90  case SIGBUS:
91  std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
92  break;
93  case SIGSEGV:
94  std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
95  break;
96  case SIGKILL:
97  std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
98  break;
99  case SIGQUIT:
100  std::cout << "SIGQUIT " << std::endl;
101  break;
102  default:
103  std::cout << signo << std::endl;
104  }
105  // std::cout << "Emergency stop called\n";
106  // PrimitiveESTOP_Viper850();
107  PrimitiveSTOP_Viper850();
108  std::cout << "Robot was stopped\n";
109 
110  // Free allocated resources
111  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
112 
113  fprintf(stdout, "Application ");
114  fflush(stdout);
115  kill(getpid(), SIGKILL);
116  exit(1);
117 }
118 
119 /* ---------------------------------------------------------------------- */
120 /* --- CONSTRUCTOR ------------------------------------------------------ */
121 /* ---------------------------------------------------------------------- */
122 
177 vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
178 {
179 
180  /*
181  #define SIGHUP 1 // hangup
182  #define SIGINT 2 // interrupt (rubout)
183  #define SIGQUIT 3 // quit (ASCII FS)
184  #define SIGILL 4 // illegal instruction (not reset when caught)
185  #define SIGTRAP 5 // trace trap (not reset when caught)
186  #define SIGIOT 6 // IOT instruction
187  #define SIGABRT 6 // used by abort, replace SIGIOT in the future
188  #define SIGEMT 7 // EMT instruction
189  #define SIGFPE 8 // floating point exception
190  #define SIGKILL 9 // kill (cannot be caught or ignored)
191  #define SIGBUS 10 // bus error
192  #define SIGSEGV 11 // segmentation violation
193  #define SIGSYS 12 // bad argument to system call
194  #define SIGPIPE 13 // write on a pipe with no one to read it
195  #define SIGALRM 14 // alarm clock
196  #define SIGTERM 15 // software termination signal from kill
197  */
198 
199  signal(SIGINT, emergencyStopViper850);
200  signal(SIGBUS, emergencyStopViper850);
201  signal(SIGSEGV, emergencyStopViper850);
202  signal(SIGKILL, emergencyStopViper850);
203  signal(SIGQUIT, emergencyStopViper850);
204 
205  setVerbose(verbose);
206  if (verbose_)
207  std::cout << "Open communication with MotionBlox.\n";
208  try {
209  this->init();
211  } catch (...) {
212  // vpERROR_TRACE("Error caught") ;
213  throw;
214  }
215  positioningVelocity = defaultPositioningVelocity;
216 
217  maxRotationVelocity_joint6 = maxRotationVelocity;
218 
219  vpRobotViper850::robotAlreadyCreated = true;
220 
221  return;
222 }
223 
224 /* ------------------------------------------------------------------------ */
225 /* --- INITIALISATION ----------------------------------------------------- */
226 /* ------------------------------------------------------------------------ */
227 
253 {
254  InitTry;
255 
256  // Initialise private variables used to compute the measured velocities
257  q_prev_getvel.resize(6);
258  q_prev_getvel = 0;
259  time_prev_getvel = 0;
260  first_time_getvel = true;
261 
262  // Initialise private variables used to compute the measured displacement
263  q_prev_getdis.resize(6);
264  q_prev_getdis = 0;
265  first_time_getdis = true;
266 
267 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
268  std::string calibfile;
269 #ifdef VISP_HAVE_VIPER850_DATA
270  calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
271  if (!vpIoTools::checkFilename(calibfile))
272  throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str()));
273 #else
274  throw(vpException(vpException::ioError, "You don't have access to Viper850 "
275  "data to retrive ATI F/T calib "
276  "file"));
277 #endif
278  ati.setCalibrationFile(calibfile);
279  ati.open();
280 #endif
281 
282  // Initialize the firewire connection
283  Try(InitializeConnection(verbose_));
284 
285  // Connect to the servoboard using the servo board GUID
286  Try(InitializeNode_Viper850());
287 
288  Try(PrimitiveRESET_Viper850());
289 
290  // Enable the joint limits on axis 6
291  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
292 
293  // Update the eMc matrix in the low level controller
295 
296  // Look if the power is on or off
297  UInt32 HIPowerStatus;
298  UInt32 EStopStatus;
299  Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
300  CAL_Wait(0.1);
301 
302  // Print the robot status
303  if (verbose_) {
304  std::cout << "Robot status: ";
305  switch (EStopStatus) {
306  case ESTOP_AUTO:
307  controlMode = AUTO;
308  if (HIPowerStatus == 0)
309  std::cout << "Power is OFF" << std::endl;
310  else
311  std::cout << "Power is ON" << std::endl;
312  break;
313 
314  case ESTOP_MANUAL:
315  controlMode = MANUAL;
316  if (HIPowerStatus == 0)
317  std::cout << "Power is OFF" << std::endl;
318  else
319  std::cout << "Power is ON" << std::endl;
320  break;
321  case ESTOP_ACTIVATED:
322  controlMode = ESTOP;
323  std::cout << "Emergency stop is activated" << std::endl;
324  break;
325  default:
326  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
327  std::cout << "You have to call Adept for maintenance..." << std::endl;
328  // Free allocated resources
329  }
330  std::cout << std::endl;
331  }
332  // get real joint min/max from the MotionBlox
333  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
334  // Convert units from degrees to radians
335  joint_min.deg2rad();
336  joint_max.deg2rad();
337 
338  // for (unsigned int i=0; i < njoint; i++) {
339  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
340  // joint_max[i]);
341  // }
342 
343  // If an error occur in the low level controller, goto here
344  // CatchPrint();
345  Catch();
346 
347  // Test if an error occurs
348  if (TryStt == -20001)
349  printf("No connection detected. Check if the robot is powered on \n"
350  "and if the firewire link exist between the MotionBlox and this "
351  "computer.\n");
352  else if (TryStt == -675)
353  printf(" Timeout enabling power...\n");
354 
355  if (TryStt < 0) {
356  // Power off the robot
357  PrimitivePOWEROFF_Viper850();
358  // Free allocated resources
359  ShutDownConnection();
360 
361  std::cout << "Cannot open connection with the motionblox..." << std::endl;
362  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
363  }
364  return;
365 }
366 
425 {
426  // Read the robot constants from files
427  // - joint [min,max], coupl_56, long_56
428  // - camera extrinsic parameters relative to eMc
429  vpViper850::init(tool, projModel);
430 
431  InitTry;
432 
433  // get real joint min/max from the MotionBlox
434  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
435  // Convert units from degrees to radians
436  joint_min.deg2rad();
437  joint_max.deg2rad();
438 
439  // for (unsigned int i=0; i < njoint; i++) {
440  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
441  // joint_max[i]);
442  // }
443 
444  // Set the camera constant (eMc pose) in the MotionBlox
445  double eMc_pose[6];
446  for (unsigned int i = 0; i < 3; i++) {
447  eMc_pose[i] = etc[i]; // translation in meters
448  eMc_pose[i + 3] = erc[i]; // rotation in rad
449  }
450  // Update the eMc pose in the low level controller
451  Try(PrimitiveCONST_Viper850(eMc_pose));
452 
453  CatchPrint();
454  return;
455 }
456 
507 void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename)
508 {
509  vpViper850::init(tool, filename);
510 
511  InitTry;
512 
513  // Get real joint min/max from the MotionBlox
514  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
515  // Convert units from degrees to radians
516  joint_min.deg2rad();
517  joint_max.deg2rad();
518 
519  // for (unsigned int i=0; i < njoint; i++) {
520  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
521  // joint_max[i]);
522  // }
523 
524  // Set the camera constant (eMc pose) in the MotionBlox
525  double eMc_pose[6];
526  for (unsigned int i = 0; i < 3; i++) {
527  eMc_pose[i] = etc[i]; // translation in meters
528  eMc_pose[i + 3] = erc[i]; // rotation in rad
529  }
530  // Update the eMc pose in the low level controller
531  Try(PrimitiveCONST_Viper850(eMc_pose));
532 
533  CatchPrint();
534  return;
535 }
536 
574 {
575  vpViper850::init(tool, eMc_);
576 
577  InitTry;
578 
579  // Get real joint min/max from the MotionBlox
580  Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
581  // Convert units from degrees to radians
582  joint_min.deg2rad();
583  joint_max.deg2rad();
584 
585  // for (unsigned int i=0; i < njoint; i++) {
586  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
587  // joint_max[i]);
588  // }
589 
590  // Set the camera constant (eMc pose) in the MotionBlox
591  double eMc_pose[6];
592  for (unsigned int i = 0; i < 3; i++) {
593  eMc_pose[i] = etc[i]; // translation in meters
594  eMc_pose[i + 3] = erc[i]; // rotation in rad
595  }
596  // Update the eMc pose in the low level controller
597  Try(PrimitiveCONST_Viper850(eMc_pose));
598 
599  CatchPrint();
600  return;
601 }
602 
615 {
616  this->vpViper850::set_eMc(eMc_);
617 
618  InitTry;
619 
620  // Set the camera constant (eMc pose) in the MotionBlox
621  double eMc_pose[6];
622  for (unsigned int i = 0; i < 3; i++) {
623  eMc_pose[i] = etc[i]; // translation in meters
624  eMc_pose[i + 3] = erc[i]; // rotation in rad
625  }
626  // Update the eMc pose in the low level controller
627  Try(PrimitiveCONST_Viper850(eMc_pose));
628 
629  CatchPrint();
630 
631  return;
632 }
633 
647 {
648  this->vpViper850::set_eMc(etc_, erc_);
649 
650  InitTry;
651 
652  // Set the camera constant (eMc pose) in the MotionBlox
653  double eMc_pose[6];
654  for (unsigned int i = 0; i < 3; i++) {
655  eMc_pose[i] = etc[i]; // translation in meters
656  eMc_pose[i + 3] = erc[i]; // rotation in rad
657  }
658  // Update the eMc pose in the low level controller
659  Try(PrimitiveCONST_Viper850(eMc_pose));
660 
661  CatchPrint();
662 
663  return;
664 }
665 
666 /* ------------------------------------------------------------------------ */
667 /* --- DESTRUCTOR --------------------------------------------------------- */
668 /* ------------------------------------------------------------------------ */
669 
677 {
678 #if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
679  ati.close();
680 #endif
681 
682  InitTry;
683 
685 
686  // Look if the power is on or off
687  UInt32 HIPowerStatus;
688  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
689  CAL_Wait(0.1);
690 
691  // if (HIPowerStatus == 1) {
692  // fprintf(stdout, "Power OFF the robot\n");
693  // fflush(stdout);
694 
695  // Try( PrimitivePOWEROFF_Viper850() );
696  // }
697 
698  // Free allocated resources
699  ShutDownConnection();
700 
701  vpRobotViper850::robotAlreadyCreated = false;
702 
703  CatchPrint();
704  return;
705 }
706 
714 {
715  InitTry;
716 
717  switch (newState) {
718  case vpRobot::STATE_STOP: {
719  // Start primitive STOP only if the current state is Velocity
721  Try(PrimitiveSTOP_Viper850());
722  vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
723  }
724  break;
725  }
728  std::cout << "Change the control mode from velocity to position control.\n";
729  Try(PrimitiveSTOP_Viper850());
730  } else {
731  // std::cout << "Change the control mode from stop to position
732  // control.\n";
733  }
734  this->powerOn();
735  break;
736  }
739  std::cout << "Change the control mode from stop to velocity control.\n";
740  }
741  this->powerOn();
742  break;
743  }
744  default:
745  break;
746  }
747 
748  CatchPrint();
749 
750  return vpRobot::setRobotState(newState);
751 }
752 
753 /* ------------------------------------------------------------------------ */
754 /* --- STOP --------------------------------------------------------------- */
755 /* ------------------------------------------------------------------------ */
756 
765 {
767  return;
768 
769  InitTry;
770  Try(PrimitiveSTOP_Viper850());
772 
773  CatchPrint();
774  if (TryStt < 0) {
775  vpERROR_TRACE("Cannot stop robot motion");
776  throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
777  }
778 }
779 
790 {
791  InitTry;
792 
793  // Look if the power is on or off
794  UInt32 HIPowerStatus;
795  UInt32 EStopStatus;
796  bool firsttime = true;
797  unsigned int nitermax = 10;
798 
799  for (unsigned int i = 0; i < nitermax; i++) {
800  Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
801  if (EStopStatus == ESTOP_AUTO) {
802  controlMode = AUTO;
803  break; // exit for loop
804  } else if (EStopStatus == ESTOP_MANUAL) {
805  controlMode = MANUAL;
806  break; // exit for loop
807  } else if (EStopStatus == ESTOP_ACTIVATED) {
808  controlMode = ESTOP;
809  if (firsttime) {
810  std::cout << "Emergency stop is activated! \n"
811  << "Check the emergency stop button and push the yellow "
812  "button before continuing."
813  << std::endl;
814  firsttime = false;
815  }
816  fprintf(stdout, "Remaining time %us \r", nitermax - i);
817  fflush(stdout);
818  CAL_Wait(1);
819  } else {
820  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
821  std::cout << "You have to call Adept for maintenance..." << std::endl;
822  // Free allocated resources
823  ShutDownConnection();
824  exit(0);
825  }
826  }
827 
828  if (EStopStatus == ESTOP_ACTIVATED)
829  std::cout << std::endl;
830 
831  if (EStopStatus == ESTOP_ACTIVATED) {
832  std::cout << "Sorry, cannot power on the robot." << std::endl;
833  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot.");
834  }
835 
836  if (HIPowerStatus == 0) {
837  fprintf(stdout, "Power ON the Viper850 robot\n");
838  fflush(stdout);
839 
840  Try(PrimitivePOWERON_Viper850());
841  }
842 
843  CatchPrint();
844  if (TryStt < 0) {
845  vpERROR_TRACE("Cannot power on the robot");
846  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
847  }
848 }
849 
860 {
861  InitTry;
862 
863  // Look if the power is on or off
864  UInt32 HIPowerStatus;
865  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
866  CAL_Wait(0.1);
867 
868  if (HIPowerStatus == 1) {
869  fprintf(stdout, "Power OFF the Viper850 robot\n");
870  fflush(stdout);
871 
872  Try(PrimitivePOWEROFF_Viper850());
873  }
874 
875  CatchPrint();
876  if (TryStt < 0) {
877  vpERROR_TRACE("Cannot power off the robot");
878  throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
879  }
880 }
881 
894 {
895  InitTry;
896  bool status = false;
897  // Look if the power is on or off
898  UInt32 HIPowerStatus;
899  Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
900  CAL_Wait(0.1);
901 
902  if (HIPowerStatus == 1) {
903  status = true;
904  }
905 
906  CatchPrint();
907  if (TryStt < 0) {
908  vpERROR_TRACE("Cannot get the power status");
909  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
910  }
911  return status;
912 }
913 
924 {
926  vpViper850::get_cMe(cMe);
927 
928  cVe.buildFrom(cMe);
929 }
930 
943 
956 {
957 
958  double position[6];
959  double timestamp;
960 
961  InitTry;
962  Try(PrimitiveACQ_POS_J_Viper850(position, &timestamp));
963  CatchPrint();
964 
965  vpColVector q(6);
966  for (unsigned int i = 0; i < njoint; i++)
967  q[i] = vpMath::rad(position[i]);
968 
969  try {
970  vpViper850::get_eJe(q, eJe);
971  } catch (...) {
972  vpERROR_TRACE("catch exception ");
973  throw;
974  }
975 }
989 {
990 
991  double position[6];
992  double timestamp;
993 
994  InitTry;
995  Try(PrimitiveACQ_POS_Viper850(position, &timestamp));
996  CatchPrint();
997 
998  vpColVector q(6);
999  for (unsigned int i = 0; i < njoint; i++)
1000  q[i] = position[i];
1001 
1002  try {
1003  vpViper850::get_fJe(q, fJe);
1004  } catch (...) {
1005  vpERROR_TRACE("Error caught");
1006  throw;
1007  }
1008 }
1009 
1047 void vpRobotViper850::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
1048 
1054 double vpRobotViper850::getPositioningVelocity(void) const { return positioningVelocity; }
1055 
1135 {
1136 
1138  vpERROR_TRACE("Robot was not in position-based control\n"
1139  "Modification of the robot state");
1141  }
1142 
1143  vpColVector destination(njoint);
1144  int error = 0;
1145  double timestamp;
1146 
1147  InitTry;
1148  switch (frame) {
1149  case vpRobot::CAMERA_FRAME: {
1150  vpColVector q(njoint);
1151  Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1152 
1153  // Convert degrees into rad
1154  q.deg2rad();
1155 
1156  // Get fMc from the inverse kinematics
1157  vpHomogeneousMatrix fMc;
1158  vpViper850::get_fMc(q, fMc);
1159 
1160  // Set cMc from the input position
1161  vpTranslationVector txyz;
1162  vpRxyzVector rxyz;
1163  for (unsigned int i = 0; i < 3; i++) {
1164  txyz[i] = position[i];
1165  rxyz[i] = position[i + 3];
1166  }
1167 
1168  // Compute cMc2
1169  vpRotationMatrix cRc2(rxyz);
1170  vpHomogeneousMatrix cMc2(txyz, cRc2);
1171 
1172  // Compute the new position to reach: fMc*cMc2
1173  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1174 
1175  // Compute the corresponding joint position from the inverse kinematics
1176  unsigned int solution = this->getInverseKinematics(fMc2, q);
1177  if (solution) { // Position is reachable
1178  destination = q;
1179  // convert rad to deg requested for the low level controller
1180  destination.rad2deg();
1181  Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1182  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1183  } else {
1184  // Cartesian position is out of range
1185  error = -1;
1186  }
1187 
1188  break;
1189  }
1190  case vpRobot::ARTICULAR_FRAME: {
1191  destination = position;
1192  // convert rad to deg requested for the low level controller
1193  destination.rad2deg();
1194 
1195  // std::cout << "Joint destination (deg): " << destination.t() <<
1196  // std::endl;
1197  Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
1198  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1199  break;
1200  }
1201  case vpRobot::REFERENCE_FRAME: {
1202  // Convert angles from Rxyz representation to Rzyz representation
1203  vpRxyzVector rxyz(position[3], position[4], position[5]);
1204  vpRotationMatrix R(rxyz);
1205  vpRzyzVector rzyz(R);
1206 
1207  for (unsigned int i = 0; i < 3; i++) {
1208  destination[i] = position[i];
1209  destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1210  }
1211  int configuration = 0; // keep the actual configuration
1212 
1213  // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1214  // << std::endl;
1215  Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, positioningVelocity));
1216  Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1217 
1218  break;
1219  }
1220  case vpRobot::MIXT_FRAME: {
1221  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1222  "Mixt frame not implemented.");
1223  }
1225  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1226  "End-effector frame not implemented.");
1227  }
1228  }
1229 
1230  CatchPrint();
1231  if (TryStt == InvalidPosition || TryStt == -1023)
1232  std::cout << " : Position out of range.\n";
1233  else if (TryStt == -3019) {
1234  if (frame == vpRobot::ARTICULAR_FRAME)
1235  std::cout << " : Joint position out of range.\n";
1236  else
1237  std::cout << " : Cartesian position leads to a joint position out of "
1238  "range.\n";
1239  } else if (TryStt < 0)
1240  std::cout << " : Unknown error (see Fabien).\n";
1241  else if (error == -1)
1242  std::cout << "Position out of range.\n";
1243 
1244  if (TryStt < 0 || error < 0) {
1245  vpERROR_TRACE("Positionning error.");
1246  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1247  }
1248 
1249  return;
1250 }
1251 
1317 void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, const double pos1, const double pos2,
1318  const double pos3, const double pos4, const double pos5, const double pos6)
1319 {
1320  try {
1321  vpColVector position(6);
1322  position[0] = pos1;
1323  position[1] = pos2;
1324  position[2] = pos3;
1325  position[3] = pos4;
1326  position[4] = pos5;
1327  position[5] = pos6;
1328 
1329  setPosition(frame, position);
1330  } catch (...) {
1331  vpERROR_TRACE("Error caught");
1332  throw;
1333  }
1334 }
1335 
1375 void vpRobotViper850::setPosition(const std::string &filename)
1376 {
1377  vpColVector q;
1378  bool ret;
1379 
1380  ret = this->readPosFile(filename, q);
1381 
1382  if (ret == false) {
1383  vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1384  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1385  }
1388 }
1389 
1458 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1459 {
1460 
1461  InitTry;
1462 
1463  position.resize(6);
1464 
1465  switch (frame) {
1466  case vpRobot::CAMERA_FRAME: {
1467  position = 0;
1468  return;
1469  }
1470  case vpRobot::ARTICULAR_FRAME: {
1471  Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1472  // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1473  position.deg2rad();
1474 
1475  return;
1476  }
1477  case vpRobot::REFERENCE_FRAME: {
1478  vpColVector q(njoint);
1479  Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1480 
1481  // Compute fMc
1483 
1484  // From fMc extract the pose
1485  vpRotationMatrix fRc;
1486  fMc.extract(fRc);
1487  vpRxyzVector rxyz;
1488  rxyz.buildFrom(fRc);
1489 
1490  for (unsigned int i = 0; i < 3; i++) {
1491  position[i] = fMc[i][3]; // translation x,y,z
1492  position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1493  }
1494 
1495  break;
1496  }
1497  case vpRobot::MIXT_FRAME: {
1498  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1499  "not implemented");
1500  }
1502  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1503  "not implemented");
1504  }
1505  }
1506 
1507  CatchPrint();
1508  if (TryStt < 0) {
1509  vpERROR_TRACE("Cannot get position.");
1510  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1511  }
1512 
1513  return;
1514 }
1515 
1521 {
1522  double timestamp;
1523  PrimitiveACQ_TIME_Viper850(&timestamp);
1524  return timestamp;
1525 }
1526 
1538 {
1539  double timestamp;
1540  getPosition(frame, position, timestamp);
1541 }
1542 
1554 void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1555 {
1556  vpColVector posRxyz;
1557  // recupere position en Rxyz
1558  this->getPosition(frame, posRxyz, timestamp);
1559  vpRxyzVector RxyzVect;
1560  for (unsigned int j = 0; j < 3; j++)
1561  RxyzVect[j] = posRxyz[j + 3];
1562  // recupere le vecteur thetaU correspondant
1563  vpThetaUVector RtuVect(RxyzVect);
1564 
1565  // remplit le vpPoseVector avec translation et rotation ThetaU
1566  for (unsigned int j = 0; j < 3; j++) {
1567  position[j] = posRxyz[j];
1568  position[j + 3] = RtuVect[j];
1569  }
1570 }
1571 
1583 {
1584  vpColVector posRxyz;
1585  double timestamp;
1586  // recupere position en Rxyz
1587  this->getPosition(frame, posRxyz, timestamp);
1588  vpRxyzVector RxyzVect;
1589  for (unsigned int j = 0; j < 3; j++)
1590  RxyzVect[j] = posRxyz[j + 3];
1591  // recupere le vecteur thetaU correspondant
1592  vpThetaUVector RtuVect(RxyzVect);
1593 
1594  // remplit le vpPoseVector avec translation et rotation ThetaU
1595  for (unsigned int j = 0; j < 3; j++) {
1596  position[j] = posRxyz[j];
1597  position[j + 3] = RtuVect[j];
1598  }
1599 }
1600 
1680 {
1682  vpERROR_TRACE("Cannot send a velocity to the robot "
1683  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1685  "Cannot send a velocity to the robot "
1686  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1687  }
1688 
1689  vpColVector vel_sat(6);
1690 
1691  // Velocity saturation
1692  switch (frame) {
1693  // saturation in cartesian space
1694  case vpRobot::CAMERA_FRAME:
1697  case vpRobot::MIXT_FRAME: {
1698  vpColVector vel_max(6);
1699 
1700  for (unsigned int i = 0; i < 3; i++)
1701  vel_max[i] = getMaxTranslationVelocity();
1702  for (unsigned int i = 3; i < 6; i++)
1703  vel_max[i] = getMaxRotationVelocity();
1704 
1705  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1706 
1707  break;
1708  }
1709  // saturation in joint space
1710  case vpRobot::ARTICULAR_FRAME: {
1711  vpColVector vel_max(6);
1712 
1713  // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1714  if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1715 
1716  for (unsigned int i = 0; i < 6; i++)
1717  vel_max[i] = getMaxRotationVelocity();
1718  } else {
1719  for (unsigned int i = 0; i < 5; i++)
1720  vel_max[i] = getMaxRotationVelocity();
1721  vel_max[5] = getMaxRotationVelocityJoint6();
1722  }
1723 
1724  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1725  }
1726  }
1727 
1728  InitTry;
1729 
1730  switch (frame) {
1731  case vpRobot::CAMERA_FRAME: {
1732  // Send velocities in m/s and rad/s
1733  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1734  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1735  break;
1736  }
1738  // Transform in camera frame
1739  vpHomogeneousMatrix cMe;
1740  this->get_cMe(cMe);
1741  vpVelocityTwistMatrix cVe(cMe);
1742  vpColVector v_c = cVe * vel_sat;
1743  // Send velocities in m/s and rad/s
1744  Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1745  break;
1746  }
1747  case vpRobot::ARTICULAR_FRAME: {
1748  // Convert all the velocities from rad/s into deg/s
1749  vel_sat.rad2deg();
1750  // std::cout << "Vitesse appliquee: " << vel_sat.t();
1751  // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1752  Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1753  break;
1754  }
1755  case vpRobot::REFERENCE_FRAME: {
1756  // Send velocities in m/s and rad/s
1757  //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1758  Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1759  break;
1760  }
1761  case vpRobot::MIXT_FRAME: {
1762  // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1763  break;
1764  }
1765  default: {
1766  vpERROR_TRACE("Error in spec of vpRobot. "
1767  "Case not taken in account.");
1768  return;
1769  }
1770  }
1771 
1772  Catch();
1773  if (TryStt < 0) {
1774  if (TryStt == VelStopOnJoint) {
1775  UInt32 axisInJoint[njoint];
1776  PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1777  for (unsigned int i = 0; i < njoint; i++) {
1778  if (axisInJoint[i])
1779  std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1780  }
1781  } else {
1782  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1783  if (TryString != NULL) {
1784  // The statement is in TryString, but we need to check the validity
1785  printf(" Error sentence %s\n", TryString); // Print the TryString
1786  } else {
1787  printf("\n");
1788  }
1789  }
1790  }
1791 
1792  return;
1793 }
1794 
1795 /* ------------------------------------------------------------------------ */
1796 /* --- GET ---------------------------------------------------------------- */
1797 /* ------------------------------------------------------------------------ */
1798 
1856 void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1857 {
1858 
1859  velocity.resize(6);
1860  velocity = 0;
1861 
1862  vpColVector q_cur(6);
1863  vpHomogeneousMatrix fMc_cur;
1864  vpHomogeneousMatrix cMc; // camera displacement
1865  double time_cur;
1866 
1867  InitTry;
1868 
1869  // Get the current joint position
1870  Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1871  time_cur = timestamp;
1872  q_cur.deg2rad();
1873 
1874  // Get the camera pose from the direct kinematics
1875  vpViper850::get_fMc(q_cur, fMc_cur);
1876 
1877  if (!first_time_getvel) {
1878 
1879  switch (frame) {
1880  case vpRobot::CAMERA_FRAME: {
1881  // Compute the displacement of the camera since the previous call
1882  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1883 
1884  // Compute the velocity of the camera from this displacement
1885  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1886 
1887  break;
1888  }
1889 
1890  case vpRobot::ARTICULAR_FRAME: {
1891  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1892  break;
1893  }
1894 
1895  case vpRobot::REFERENCE_FRAME: {
1896  // Compute the displacement of the camera since the previous call
1897  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1898 
1899  // Compute the velocity of the camera from this displacement
1900  vpColVector v;
1901  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1902 
1903  // Express this velocity in the reference frame
1904  vpVelocityTwistMatrix fVc(fMc_cur);
1905  velocity = fVc * v;
1906 
1907  break;
1908  }
1909 
1910  case vpRobot::MIXT_FRAME: {
1911  // Compute the displacement of the camera since the previous call
1912  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1913 
1914  // Compute the ThetaU representation for the rotation
1915  vpRotationMatrix cRc;
1916  cMc.extract(cRc);
1917  vpThetaUVector thetaU;
1918  thetaU.buildFrom(cRc);
1919 
1920  for (unsigned int i = 0; i < 3; i++) {
1921  // Compute the translation displacement in the reference frame
1922  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1923  // Update the rotation displacement in the camera frame
1924  velocity[i + 3] = thetaU[i];
1925  }
1926 
1927  // Compute the velocity
1928  velocity /= (time_cur - time_prev_getvel);
1929  break;
1930  }
1931  default: {
1933  "vpRobotViper850::getVelocity() not implemented in end-effector"));
1934  }
1935  }
1936  } else {
1937  first_time_getvel = false;
1938  }
1939 
1940  // Memorize the camera pose for the next call
1941  fMc_prev_getvel = fMc_cur;
1942 
1943  // Memorize the joint position for the next call
1944  q_prev_getvel = q_cur;
1945 
1946  // Memorize the time associated to the joint position for the next call
1947  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 
2021 {
2022  vpColVector velocity;
2023  getVelocity(frame, velocity, timestamp);
2024 
2025  return velocity;
2026 }
2027 
2037 {
2038  vpColVector velocity;
2039  double timestamp;
2040  getVelocity(frame, velocity, timestamp);
2041 
2042  return velocity;
2043 }
2044 
2111 bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2112 {
2113  std::ifstream fd(filename.c_str(), std::ios::in);
2114 
2115  if (!fd.is_open()) {
2116  return false;
2117  }
2118 
2119  std::string line;
2120  std::string key("R:");
2121  std::string id("#Viper850 - Position");
2122  bool pos_found = false;
2123  int lineNum = 0;
2124 
2125  q.resize(njoint);
2126 
2127  while (std::getline(fd, line)) {
2128  lineNum++;
2129  if (lineNum == 1) {
2130  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2131  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2132  return false;
2133  }
2134  }
2135  if ((line.compare(0, 1, "#") == 0)) { // skip comment
2136  continue;
2137  }
2138  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2139  // check if there are at least njoint values in the line
2140  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2141  if (chain.size() < njoint + 1) // try to split with tab separator
2142  chain = vpIoTools::splitChain(line, std::string("\t"));
2143  if (chain.size() < njoint + 1)
2144  continue;
2145 
2146  std::istringstream ss(line);
2147  std::string key_;
2148  ss >> key_;
2149  for (unsigned int i = 0; i < njoint; i++)
2150  ss >> q[i];
2151  pos_found = true;
2152  break;
2153  }
2154  }
2155 
2156  // converts rotations from degrees into radians
2157  q.deg2rad();
2158 
2159  fd.close();
2160 
2161  if (!pos_found) {
2162  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2163  return false;
2164  }
2165 
2166  return true;
2167 }
2168 
2192 bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2193 {
2194 
2195  FILE *fd;
2196  fd = fopen(filename.c_str(), "w");
2197  if (fd == NULL)
2198  return false;
2199 
2200  fprintf(fd, "\
2201 #Viper850 - Position - Version 1.00\n\
2202 #\n\
2203 # R: A B C D E F\n\
2204 # Joint position in degrees\n\
2205 #\n\
2206 #\n\n");
2207 
2208  // Save positions in mm and deg
2209  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2210  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2211 
2212  fclose(fd);
2213  return (true);
2214 }
2215 
2226 void vpRobotViper850::move(const std::string &filename)
2227 {
2228  vpColVector q;
2229 
2230  try {
2231  this->readPosFile(filename, q);
2233  this->setPositioningVelocity(10);
2235  } catch (...) {
2236  throw;
2237  }
2238 }
2239 
2259 {
2260  displacement.resize(6);
2261  displacement = 0;
2262 
2263  double q[6];
2264  vpColVector q_cur(6);
2265  double timestamp;
2266 
2267  InitTry;
2268 
2269  // Get the current joint position
2270  Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2271  for (unsigned int i = 0; i < njoint; i++) {
2272  q_cur[i] = q[i];
2273  }
2274 
2275  if (!first_time_getdis) {
2276  switch (frame) {
2277  case vpRobot::CAMERA_FRAME: {
2278  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2279  return;
2280  }
2281 
2282  case vpRobot::ARTICULAR_FRAME: {
2283  displacement = q_cur - q_prev_getdis;
2284  break;
2285  }
2286 
2287  case vpRobot::REFERENCE_FRAME: {
2288  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2289  return;
2290  }
2291 
2292  case vpRobot::MIXT_FRAME: {
2293  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2294  return;
2295  }
2297  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2298  return;
2299  }
2300  }
2301  } else {
2302  first_time_getdis = false;
2303  }
2304 
2305  // Memorize the joint position for the next call
2306  q_prev_getdis = q_cur;
2307 
2308  CatchPrint();
2309  if (TryStt < 0) {
2310  vpERROR_TRACE("Cannot get velocity.");
2311  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2312  }
2313 }
2314 
2323 {
2324 #if defined(USE_ATI_DAQ)
2325 #if defined(VISP_HAVE_COMEDI)
2326  ati.bias();
2327 #else
2328  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2329  "apt-get install libcomedi-dev"));
2330 #endif
2331 #else // Use serial link
2332  InitTry;
2333 
2334  Try(PrimitiveTFS_BIAS_Viper850());
2335 
2336  // Wait 500 ms to be sure the next measures take into account the bias
2337  vpTime::wait(500);
2338 
2339  CatchPrint();
2340  if (TryStt < 0) {
2341  vpERROR_TRACE("Cannot bias the force/torque sensor.");
2342  throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2343  }
2344 #endif
2345 }
2346 
2355 {
2356 #if defined(USE_ATI_DAQ)
2357 #if defined(VISP_HAVE_COMEDI)
2358  ati.unbias();
2359 #else
2360  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2361  "apt-get install libcomedi-dev"));
2362 #endif
2363 #else // Use serial link
2364 // Not implemented
2365 #endif
2366 }
2367 
2409 {
2410 #if defined(USE_ATI_DAQ)
2411 #if defined(VISP_HAVE_COMEDI)
2412  H = ati.getForceTorque();
2413 #else
2414  (void)H;
2415  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2416  "apt-get install libcomedi-dev"));
2417 #endif
2418 #else // Use serial link
2419  InitTry;
2420 
2421  H.resize(6);
2422 
2423  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2424 
2425  CatchPrint();
2426  if (TryStt < 0) {
2427  vpERROR_TRACE("Cannot get the force/torque measures.");
2428  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2429  }
2430 #endif
2431 }
2432 
2472 {
2473 #if defined(USE_ATI_DAQ)
2474 #if defined(VISP_HAVE_COMEDI)
2475  vpColVector H = ati.getForceTorque();
2476  return H;
2477 #else
2478  throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2479  "apt-get install libcomedi-dev"));
2480 #endif
2481 #else // Use serial link
2482  InitTry;
2483 
2484  vpColVector H(6);
2485 
2486  Try(PrimitiveTFS_ACQ_Viper850(H.data));
2487  return H;
2488 
2489  CatchPrint();
2490  if (TryStt < 0) {
2491  vpERROR_TRACE("Cannot get the force/torque measures.");
2492  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2493  }
2494  return H; // Here to avoid a warning, but should never be called
2495 #endif
2496 }
2497 
2505 {
2506  InitTry;
2507  Try(PrimitivePneumaticGripper_Viper850(1));
2508  std::cout << "Open the pneumatic gripper..." << std::endl;
2509  CatchPrint();
2510  if (TryStt < 0) {
2511  throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2512  }
2513 }
2514 
2523 {
2524  InitTry;
2525  Try(PrimitivePneumaticGripper_Viper850(0));
2526  std::cout << "Close the pneumatic gripper..." << std::endl;
2527  CatchPrint();
2528  if (TryStt < 0) {
2529  throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2530  }
2531 }
2532 
2539 {
2540  InitTry;
2541  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2542  std::cout << "Enable joint limits on axis 6..." << std::endl;
2543  CatchPrint();
2544  if (TryStt < 0) {
2545  vpERROR_TRACE("Cannot enable joint limits on axis 6");
2546  throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2547  }
2548 }
2549 
2561 {
2562  InitTry;
2563  Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2564  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2565  CatchPrint();
2566  if (TryStt < 0) {
2567  vpERROR_TRACE("Cannot disable joint limits on axis 6");
2568  throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2569  }
2570 }
2571 
2581 {
2584 
2585  return;
2586 }
2587 
2609 {
2610  maxRotationVelocity_joint6 = w6_max;
2611  return;
2612 }
2613 
2621 double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2622 
2623 #elif !defined(VISP_BUILD_SHARED_LIBS)
2624 // Work arround to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2625 // no symbols
2626 void dummy_vpRobotViper850(){};
2627 #endif
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocity(double w_max)
void set_eMc(const vpHomogeneousMatrix &eMc_)
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Error that can be emited by the vpRobot class and its derivates.
double getPositioningVelocity(void) const
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
void closeGripper() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:159
#define vpERROR_TRACE
Definition: vpDebug.h:393
vpColVector getForceTorque() const
void setPositioningVelocity(const double velocity)
double maxRotationVelocity
Definition: vpRobot.h:98
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void enableJoint6Limits() const
Initialize the position controller.
Definition: vpRobot.h:68
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
vpControlFrameType
Definition: vpRobot.h:75
vpRxyzVector erc
Definition: vpViper.h:160
void move(const std::string &filename)
void get_eJe(vpMatrix &eJe)
void disableJoint6Limits() const
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void deg2rad()
Definition: vpColVector.h:134
Implementation of a rotation matrix and operations on such kind of matrices.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:675
Initialize the velocity controller.
Definition: vpRobot.h:67
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:116
vpTranslationVector etc
Definition: vpViper.h:159
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpColVector joint_max
Definition: vpViper.h:172
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:258
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:103
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1771
vpColVector getForceTorque() const
bool getPowerState() const
void get_fJe(vpMatrix &fJe)
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
static double rad(double deg)
Definition: vpMath.h:102
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
static bool readPosFile(const std::string &filename, vpColVector &q)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:260
void get_cVe(vpVelocityTwistMatrix &cVe) const
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
void setMaxRotationVelocityJoint6(double w6_max)
static double deg(double rad)
Definition: vpMath.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
static const double defaultPositioningVelocity
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
double getMaxRotationVelocityJoint6() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
Emergency stop activated.
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:154
virtual ~vpRobotViper850(void)
void init(void)
Definition: vpViper850.cpp:136
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
Automatic control mode (default).
double getTime() const
Function not implemented.
Definition: vpException.h:90
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void rad2deg()
Definition: vpColVector.h:228
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_cMe(vpHomogeneousMatrix &cMe) const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
vpColVector joint_min
Definition: vpViper.h:173