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