Visual Servoing Platform  version 3.0.0
vpRobotViper650.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface for the Irisa's Viper S650 robot.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 #include <visp3/core/vpConfig.h>
39 
40 #ifdef VISP_HAVE_VIPER650
41 
42 #include <signal.h>
43 #include <stdlib.h>
44 #include <sys/types.h>
45 #include <unistd.h>
46 
47 #include <visp3/robot/vpRobotException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpDebug.h>
50 #include <visp3/core/vpVelocityTwistMatrix.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/robot/vpRobot.h>
53 #include <visp3/robot/vpRobotViper650.h>
54 
55 /* ---------------------------------------------------------------------- */
56 /* --- STATIC ----------------------------------------------------------- */
57 /* ---------------------------------------------------------------------- */
58 
59 bool vpRobotViper650::robotAlreadyCreated = false;
60 
69 
70 /* ---------------------------------------------------------------------- */
71 /* --- EMERGENCY STOP --------------------------------------------------- */
72 /* ---------------------------------------------------------------------- */
73 
81 void emergencyStopViper650(int signo)
82 {
83  std::cout << "Stop the Viper650 application by signal ("
84  << signo << "): " << (char)7 ;
85  switch(signo)
86  {
87  case SIGINT:
88  std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
89  case SIGBUS:
90  std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
91  case SIGSEGV:
92  std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
93  case SIGKILL:
94  std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
95  case SIGQUIT:
96  std::cout <<"SIGQUIT " << std::endl ; break ;
97  default :
98  std::cout << signo << std::endl ;
99 }
100  //std::cout << "Emergency stop called\n";
101  // PrimitiveESTOP_Viper650();
102  PrimitiveSTOP_Viper650();
103  std::cout << "Robot was stopped\n";
104 
105  // Free allocated resources
106  // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
107 
108  fprintf(stdout, "Application ");
109  fflush(stdout);
110  kill(getpid(), SIGKILL);
111  exit(1) ;
112 }
113 
114 /* ---------------------------------------------------------------------- */
115 /* --- CONSTRUCTOR ------------------------------------------------------ */
116 /* ---------------------------------------------------------------------- */
117 
173 vpRobotViper650::vpRobotViper650 (bool verbose)
174  :
175  vpViper650 (),
176  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, emergencyStopViper650);
199  signal(SIGBUS, emergencyStopViper650) ;
200  signal(SIGSEGV, emergencyStopViper650) ;
201  signal(SIGKILL, emergencyStopViper650);
202  signal(SIGQUIT, emergencyStopViper650);
203 
204  setVerbose(verbose);
205  if (verbose_)
206  std::cout << "Open communication with MotionBlox.\n";
207  try {
208  this->init();
210  }
211  catch(...) {
212  // vpERROR_TRACE("Error caught") ;
213  throw ;
214  }
215  positioningVelocity = defaultPositioningVelocity ;
216 
217  maxRotationVelocity_joint6 = maxRotationVelocity;
218 
219  vpRobotViper650::robotAlreadyCreated = true;
220 
221  return ;
222 }
223 
224 
225 /* ------------------------------------------------------------------------ */
226 /* --- INITIALISATION ----------------------------------------------------- */
227 /* ------------------------------------------------------------------------ */
228 
252 void
254 {
255  InitTry;
256 
257  // Initialise private variables used to compute the measured velocities
258  q_prev_getvel.resize(6);
259  q_prev_getvel = 0;
260  time_prev_getvel = 0;
261  first_time_getvel = true;
262 
263  // Initialise private variables used to compute the measured displacement
264  q_prev_getdis.resize(6);
265  q_prev_getdis = 0;
266  first_time_getdis = true;
267 
268  // Initialize the firewire connection
269  Try( InitializeConnection(verbose_) );
270 
271  // Connect to the servoboard using the servo board GUID
272  Try( InitializeNode_Viper650() );
273 
274  Try( PrimitiveRESET_Viper650() );
275 
276  // Enable the joint limits on axis 6
277  Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
278 
279  // Update the eMc matrix in the low level controller
281 
282  // Look if the power is on or off
283  UInt32 HIPowerStatus;
284  UInt32 EStopStatus;
285  Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
286  &HIPowerStatus));
287  CAL_Wait(0.1);
288 
289  // Print the robot status
290  if (verbose_) {
291  std::cout << "Robot status: ";
292  switch(EStopStatus) {
293  case ESTOP_AUTO:
294  controlMode = AUTO;
295  if (HIPowerStatus == 0)
296  std::cout << "Power is OFF" << std::endl;
297  else
298  std::cout << "Power is ON" << std::endl;
299  break;
300 
301  case ESTOP_MANUAL:
302  controlMode = MANUAL;
303  if (HIPowerStatus == 0)
304  std::cout << "Power is OFF" << std::endl;
305  else
306  std::cout << "Power is ON" << std::endl;
307  break;
308  case ESTOP_ACTIVATED:
309  controlMode = ESTOP;
310  std::cout << "Emergency stop is activated" << std::endl;
311  break;
312  default:
313  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
314  std::cout << "You have to call Adept for maintenance..." << std::endl;
315  // Free allocated resources
316  }
317  std::cout << std::endl;
318  }
319  // get real joint min/max from the MotionBlox
320  Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
321  // Convert units from degrees to radians
322  joint_min.deg2rad();
323  joint_max.deg2rad();
324 
325  // for (unsigned int i=0; i < njoint; i++) {
326  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
327  // }
328 
329  // If an error occur in the low level controller, goto here
330  //CatchPrint();
331  Catch();
332 
333  // Test if an error occurs
334  if (TryStt == -20001)
335  printf("No connection detected. Check if the robot is powered on \n"
336  "and if the firewire link exist between the MotionBlox and this computer.\n");
337  else if (TryStt == -675)
338  printf(" Timeout enabling power...\n");
339 
340  if (TryStt < 0) {
341  // Power off the robot
342  PrimitivePOWEROFF_Viper650();
343  // Free allocated resources
344  ShutDownConnection();
345 
346  std::cout << "Cannot open connexion with the motionblox..." << std::endl;
348  "Cannot open connexion with the motionblox");
349  }
350  return ;
351 }
352 
411 void
414 {
415  vpViper650::init(tool, projModel);
416 
417  InitTry;
418 
419  // Get real joint min/max from the MotionBlox
420  Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
421  // Convert units from degrees to radians
422  joint_min.deg2rad();
423  joint_max.deg2rad();
424 
425  // for (unsigned int i=0; i < njoint; i++) {
426  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
427  // }
428 
429  CatchPrint();
430  return ;
431 }
432 
484 void
486  const std::string &filename)
487 {
488  vpViper650::init(tool, filename);
489 
490  InitTry;
491 
492  // Get real joint min/max from the MotionBlox
493  Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
494  // Convert units from degrees to radians
495  joint_min.deg2rad();
496  joint_max.deg2rad();
497 
498  // for (unsigned int i=0; i < njoint; i++) {
499  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
500  // }
501 
502  CatchPrint();
503  return ;
504 }
505 
543 void
545  const vpHomogeneousMatrix &eMc_)
546 {
547  vpViper650::init(tool, eMc_);
548 
549  InitTry;
550 
551  // Get real joint min/max from the MotionBlox
552  Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
553  // Convert units from degrees to radians
554  joint_min.deg2rad();
555  joint_max.deg2rad();
556 
557  // for (unsigned int i=0; i < njoint; i++) {
558  // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
559  // }
560 
561  CatchPrint();
562  return ;
563 }
564 
576 void
578 {
579  this->vpViper650::set_eMc(eMc_);
580 
581  InitTry;
582 
583  // Set the camera constant (eMc pose) in the MotionBlox
584  double eMc_pose[6];
585  for (unsigned int i=0; i < 3; i ++) {
586  eMc_pose[i] = etc[i]; // translation in meters
587  eMc_pose[i+3] = erc[i]; // rotation in rad
588  }
589  // Update the eMc pose in the low level controller
590  Try( PrimitiveCONST_Viper650(eMc_pose) );
591 
592  CatchPrint();
593 
594  return ;
595 }
596 
609 void
611 {
612  this->vpViper650::set_eMc(etc_,erc_);
613 
614  InitTry;
615 
616  // Set the camera constant (eMc pose) in the MotionBlox
617  double eMc_pose[6];
618  for (unsigned int i=0; i < 3; i ++) {
619  eMc_pose[i] = etc[i]; // translation in meters
620  eMc_pose[i+3] = erc[i]; // rotation in rad
621  }
622  // Update the eMc pose in the low level controller
623  Try( PrimitiveCONST_Viper650(eMc_pose) );
624 
625  CatchPrint();
626 
627  return ;
628 }
629 
630 /* ------------------------------------------------------------------------ */
631 /* --- DESTRUCTOR --------------------------------------------------------- */
632 /* ------------------------------------------------------------------------ */
633 
641 {
642  InitTry;
643 
645 
646  // Look if the power is on or off
647  UInt32 HIPowerStatus;
648  Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
649  &HIPowerStatus));
650  CAL_Wait(0.1);
651 
652  // if (HIPowerStatus == 1) {
653  // fprintf(stdout, "Power OFF the robot\n");
654  // fflush(stdout);
655 
656  // Try( PrimitivePOWEROFF_Viper650() );
657  // }
658 
659  // Free allocated resources
660  ShutDownConnection();
661 
662  vpRobotViper650::robotAlreadyCreated = false;
663 
664  CatchPrint();
665  return;
666 }
667 
668 
669 
670 
679 {
680  InitTry;
681 
682  switch (newState) {
683  case vpRobot::STATE_STOP: {
684  // Start primitive STOP only if the current state is Velocity
686  Try( PrimitiveSTOP_Viper650() );
687  }
688  break;
689  }
692  std::cout << "Change the control mode from velocity to position control.\n";
693  Try( PrimitiveSTOP_Viper650() );
694  }
695  else {
696  //std::cout << "Change the control mode from stop to position control.\n";
697  }
698  this->powerOn();
699  break;
700  }
703  std::cout << "Change the control mode from stop to velocity control.\n";
704  }
705  this->powerOn();
706  break;
707  }
708  default:
709  break ;
710  }
711 
712  CatchPrint();
713 
714  return vpRobot::setRobotState (newState);
715 }
716 
717 
718 /* ------------------------------------------------------------------------ */
719 /* --- STOP --------------------------------------------------------------- */
720 /* ------------------------------------------------------------------------ */
721 
729 void
731 {
733  return;
734 
735  InitTry;
736  Try( PrimitiveSTOP_Viper650() );
738 
739  CatchPrint();
740  if (TryStt < 0) {
741  vpERROR_TRACE ("Cannot stop robot motion");
743  "Cannot stop robot motion.");
744  }
745 }
746 
756 void
758 {
759  InitTry;
760 
761  // Look if the power is on or off
762  UInt32 HIPowerStatus;
763  UInt32 EStopStatus;
764  bool firsttime = true;
765  unsigned int nitermax = 10;
766 
767  for (unsigned int i=0; i<nitermax; i++) {
768  Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
769  &HIPowerStatus));
770  if (EStopStatus == ESTOP_AUTO) {
771  controlMode = AUTO;
772  break; // exit for loop
773  }
774  else if (EStopStatus == ESTOP_MANUAL) {
775  controlMode = MANUAL;
776  break; // exit for loop
777  }
778  else if (EStopStatus == ESTOP_ACTIVATED) {
779  controlMode = ESTOP;
780  if (firsttime) {
781  std::cout << "Emergency stop is activated! \n"
782  << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
783  firsttime = false;
784  }
785  fprintf(stdout, "Remaining time %ds \r", nitermax-i);
786  fflush(stdout);
787  CAL_Wait(1);
788  }
789  else {
790  std::cout << "Sorry there is an error on the emergency chain." << std::endl;
791  std::cout << "You have to call Adept for maintenance..." << std::endl;
792  // Free allocated resources
793  ShutDownConnection();
794  exit(0);
795  }
796  }
797 
798  if (EStopStatus == ESTOP_ACTIVATED)
799  std::cout << std::endl;
800 
801  if (EStopStatus == ESTOP_ACTIVATED) {
802  std::cout << "Sorry, cannot power on the robot." << std::endl;
804  "Cannot power on the robot.");
805  }
806 
807  if (HIPowerStatus == 0) {
808  fprintf(stdout, "Power ON the Viper650 robot\n");
809  fflush(stdout);
810 
811  Try( PrimitivePOWERON_Viper650() );
812  }
813 
814  CatchPrint();
815  if (TryStt < 0) {
816  vpERROR_TRACE ("Cannot power on the robot");
818  "Cannot power off the robot.");
819  }
820 }
821 
831 void
833 {
834  InitTry;
835 
836  // Look if the power is on or off
837  UInt32 HIPowerStatus;
838  Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
839  &HIPowerStatus));
840  CAL_Wait(0.1);
841 
842  if (HIPowerStatus == 1) {
843  fprintf(stdout, "Power OFF the Viper650 robot\n");
844  fflush(stdout);
845 
846  Try( PrimitivePOWEROFF_Viper650() );
847  }
848 
849  CatchPrint();
850  if (TryStt < 0) {
851  vpERROR_TRACE ("Cannot power off the robot");
853  "Cannot power off the robot.");
854  }
855 }
856 
868 bool
870 {
871  InitTry;
872  bool status = false;
873  // Look if the power is on or off
874  UInt32 HIPowerStatus;
875  Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
876  &HIPowerStatus));
877  CAL_Wait(0.1);
878 
879  if (HIPowerStatus == 1) {
880  status = true;
881  }
882 
883  CatchPrint();
884  if (TryStt < 0) {
885  vpERROR_TRACE ("Cannot get the power status");
887  "Cannot get the power status.");
888  }
889  return status;
890 }
891 
901 void
903 {
904  vpHomogeneousMatrix cMe ;
905  vpViper650::get_cMe(cMe) ;
906 
907  cVe.buildFrom(cMe) ;
908 }
909 
921 void
923 {
924  vpViper650::get_cMe(cMe) ;
925 }
926 
927 
939 void
941 {
942 
943  double position[6];
944  double timestamp;
945 
946  InitTry;
947  Try( PrimitiveACQ_POS_J_Viper650(position, &timestamp) );
948  CatchPrint();
949 
950  vpColVector q(6);
951  for (unsigned int i=0; i < njoint; i++)
952  q[i] = vpMath::rad(position[i]);
953 
954  try
955  {
956  vpViper650::get_eJe(q, eJe) ;
957  }
958  catch(...)
959  {
960  vpERROR_TRACE("catch exception ") ;
961  throw ;
962  }
963 }
976 void
978 {
979 
980  double position[6];
981  double timestamp;
982 
983  InitTry;
984  Try( PrimitiveACQ_POS_Viper650(position, &timestamp) );
985  CatchPrint();
986 
987  vpColVector q(6);
988  for (unsigned int i=0; i < njoint; i++)
989  q[i] = position[i];
990 
991  try
992  {
993  vpViper650::get_fJe(q, fJe) ;
994  }
995  catch(...)
996  {
997  vpERROR_TRACE("Error caught");
998  throw ;
999  }
1000 }
1001 
1039 void
1041 {
1042  positioningVelocity = velocity;
1043 }
1044 
1050 double
1052 {
1053  return positioningVelocity;
1054 }
1055 
1056 
1134 void
1136  const vpColVector & position )
1137 {
1138 
1140  {
1141  vpERROR_TRACE ("Robot was not in position-based control\n"
1142  "Modification of the robot state");
1144  }
1145 
1146  vpColVector destination(njoint);
1147  int error = 0;
1148  double timestamp;
1149 
1150  InitTry;
1151  switch(frame) {
1152  case vpRobot::CAMERA_FRAME : {
1153  vpColVector q(njoint);
1154  Try( PrimitiveACQ_POS_Viper650(q.data, &timestamp) );
1155 
1156  // Convert degrees into rad
1157  q.deg2rad();
1158 
1159  // Get fMc from the inverse kinematics
1160  vpHomogeneousMatrix fMc;
1161  vpViper650::get_fMc(q, fMc);
1162 
1163  // Set cMc from the input position
1164  vpTranslationVector txyz;
1165  vpRxyzVector rxyz;
1166  for (unsigned int i=0; i < 3; i++) {
1167  txyz[i] = position[i];
1168  rxyz[i] = position[i+3];
1169  }
1170 
1171  // Compute cMc2
1172  vpRotationMatrix cRc2(rxyz);
1173  vpHomogeneousMatrix cMc2(txyz, cRc2);
1174 
1175  // Compute the new position to reach: fMc*cMc2
1176  vpHomogeneousMatrix fMc2 = fMc * cMc2;
1177 
1178  // Compute the corresponding joint position from the inverse kinematics
1179  unsigned int solution = this->getInverseKinematics(fMc2, q);
1180  if (solution) { // Position is reachable
1181  destination = q;
1182  // convert rad to deg requested for the low level controller
1183  destination.rad2deg();
1184  Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
1185  Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
1186  }
1187  else {
1188  // Cartesian position is out of range
1189  error = -1;
1190  }
1191 
1192  break ;
1193  }
1194  case vpRobot::ARTICULAR_FRAME: {
1195  destination = position;
1196  // convert rad to deg requested for the low level controller
1197  destination.rad2deg();
1198 
1199  //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
1200  Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
1201  Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
1202  break ;
1203 
1204  }
1205  case vpRobot::REFERENCE_FRAME: {
1206  // Convert angles from Rxyz representation to Rzyz representation
1207  vpRxyzVector rxyz(position[3],position[4],position[5]);
1208  vpRotationMatrix R(rxyz);
1209  vpRzyzVector rzyz(R);
1210 
1211  for (unsigned int i=0; i <3; i++) {
1212  destination[i] = position[i];
1213  destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1214  }
1215  int configuration = 0; // keep the actual configuration
1216 
1217  //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
1218  Try( PrimitiveMOVE_C_Viper650(destination.data, configuration,
1219  positioningVelocity) );
1220  Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
1221 
1222  break ;
1223  }
1224  case vpRobot::MIXT_FRAME:
1225  {
1226  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1228  "Positionning error: "
1229  "Mixt frame not implemented.");
1230  break ;
1231  }
1232  }
1233 
1234  CatchPrint();
1235  if (TryStt == InvalidPosition || TryStt == -1023)
1236  std::cout << " : Position out of range.\n";
1237 
1238  else if (TryStt == -3019) {
1239  if (frame == vpRobot::ARTICULAR_FRAME)
1240  std::cout << " : Joint position out of range.\n";
1241  else
1242  std::cout << " : Cartesian position leads to a joint position out of range.\n";
1243  }
1244  else if (TryStt < 0)
1245  std::cout << " : Unknown error (see Fabien).\n";
1246  else if (error == -1)
1247  std::cout << "Position out of range.\n";
1248 
1249  if (TryStt < 0 || error < 0) {
1250  vpERROR_TRACE ("Positionning error.");
1252  "Position out of range.");
1253  }
1254 
1255  return ;
1256 }
1257 
1324  const double pos1,
1325  const double pos2,
1326  const double pos3,
1327  const double pos4,
1328  const double pos5,
1329  const double pos6)
1330 {
1331  try{
1332  vpColVector position(6) ;
1333  position[0] = pos1 ;
1334  position[1] = pos2 ;
1335  position[2] = pos3 ;
1336  position[3] = pos4 ;
1337  position[4] = pos5 ;
1338  position[5] = pos6 ;
1339 
1340  setPosition(frame, position) ;
1341  }
1342  catch(...)
1343  {
1344  vpERROR_TRACE("Error caught");
1345  throw ;
1346  }
1347 }
1348 
1388 void vpRobotViper650::setPosition(const char *filename)
1389 {
1390  vpColVector q;
1391  bool ret;
1392 
1393  ret = this->readPosFile(filename, q);
1394 
1395  if (ret == false) {
1396  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1398  "Bad position in filename.");
1399  }
1402 }
1403 
1473  vpColVector &position,
1474  double &timestamp)
1475 {
1476 
1477  InitTry;
1478 
1479  position.resize (6);
1480 
1481  switch (frame) {
1482  case vpRobot::CAMERA_FRAME : {
1483  position = 0;
1484  return;
1485  }
1486  case vpRobot::ARTICULAR_FRAME : {
1487  Try( PrimitiveACQ_POS_J_Viper650(position.data, &timestamp) );
1488  //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1489  position.deg2rad();
1490 
1491  return;
1492  }
1493  case vpRobot::REFERENCE_FRAME : {
1494  Try( PrimitiveACQ_POS_C_Viper650(position.data, &timestamp) );
1495  // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
1496  // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
1497  // Convert Euler Rzyz angles from deg to rad
1498  for (unsigned int i=3; i <6; i++)
1499  position[i] = vpMath::rad(position[i]);
1500  // Convert Rzyz angles into Rxyz representation
1501  vpRzyzVector rzyz(position[3], position[4], position[5]);
1502  vpRotationMatrix R(rzyz);
1503  vpRxyzVector rxyz(R);
1504 
1505  // Update the position using Rxyz representation
1506  for (unsigned int i=0; i <3; i++)
1507  position[i+3] = rxyz[i];
1508  // vpCTRACE << "Cartesian position Rxyz (deg)"
1509  // << position[0] << " " << position[1] << " " << position[2] << " "
1510  // << vpMath::deg(position[3]) << " "
1511  // << vpMath::deg(position[4]) << " "
1512  // << vpMath::deg(position[5]) << std::endl;
1513 
1514  break ;
1515  }
1516  case vpRobot::MIXT_FRAME: {
1517  vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
1519  "Cannot get position in mixt frame: "
1520  "not implemented");
1521  break ;
1522  }
1523  }
1524 
1525  CatchPrint();
1526  if (TryStt < 0) {
1527  vpERROR_TRACE ("Cannot get position.");
1529  "Cannot get position.");
1530  }
1531 
1532  return;
1533 }
1534 
1545  vpColVector &position)
1546 {
1547  double timestamp;
1548  getPosition(frame, position, timestamp);
1549 }
1550 
1563  vpPoseVector &position,
1564  double &timestamp)
1565 {
1566  vpColVector posRxyz;
1567  //recupere position en Rxyz
1568  this->getPosition(frame, posRxyz, timestamp);
1569  vpRxyzVector RxyzVect;
1570  for (unsigned int j=0;j<3;j++)
1571  RxyzVect[j]=posRxyz[j+3];
1572  //recupere le vecteur thetaU correspondant
1573  vpThetaUVector RtuVect(RxyzVect);
1574 
1575  //remplit le vpPoseVector avec translation et rotation ThetaU
1576  for (unsigned int j=0;j<3;j++)
1577  {
1578  position[j]=posRxyz[j];
1579  position[j+3]=RtuVect[j];
1580  }
1581 }
1582 
1593  vpPoseVector &position)
1594 {
1595  double timestamp;
1596  getPosition(frame, position, timestamp);
1597 }
1598 
1603 {
1604  double timestamp;
1605  PrimitiveACQ_TIME_Viper650(&timestamp);
1606  return timestamp;
1607 }
1608 
1681 void
1683  const vpColVector & vel)
1684 {
1686  vpERROR_TRACE ("Cannot send a velocity to the robot "
1687  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1689  "Cannot send a velocity to the robot "
1690  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1691  }
1692 
1693  vpColVector vel_sat(6);
1694 
1695  // Velocity saturation
1696  switch(frame) {
1697  // saturation in cartesian space
1698  case vpRobot::CAMERA_FRAME :
1700  case vpRobot::MIXT_FRAME : {
1701  vpColVector vel_max(6);
1702 
1703  for (unsigned int i=0; i<3; i++)
1704  vel_max[i] = getMaxTranslationVelocity();
1705  for (unsigned int i=3; i<6; i++)
1706  vel_max[i] = getMaxRotationVelocity();
1707 
1708  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1709 
1710  break;
1711  }
1712  // saturation in joint space
1713  case vpRobot::ARTICULAR_FRAME : {
1714  vpColVector vel_max(6);
1715 
1717  for (unsigned int i=0; i<6; i++)
1718  vel_max[i] = getMaxRotationVelocity();
1719  }
1720  else {
1721  for (unsigned int i=0; i<5; i++)
1722  vel_max[i] = getMaxRotationVelocity();
1723  vel_max[5] = getMaxRotationVelocityJoint6();
1724  }
1725 
1726  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1727 
1728  }
1729  }
1730 
1731  InitTry;
1732 
1733  switch(frame) {
1734  case vpRobot::CAMERA_FRAME : {
1735  // Send velocities in m/s and rad/s
1736  // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1737  Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650) );
1738  break ;
1739  }
1740  case vpRobot::ARTICULAR_FRAME : {
1741  // Convert all the velocities from rad/s into deg/s
1742  vel_sat.rad2deg();
1743  //std::cout << "Vitesse appliquee: " << vel_sat.t();
1744  //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
1745  Try( PrimitiveMOVESPEED_Viper650(vel_sat.data) );
1746  break ;
1747  }
1748  case vpRobot::REFERENCE_FRAME : {
1749  // Send velocities in m/s and rad/s
1750  std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1751  Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650) );
1752  break ;
1753  }
1754  case vpRobot::MIXT_FRAME : {
1755  //Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
1756  break ;
1757  }
1758  default: {
1759  vpERROR_TRACE ("Error in spec of vpRobot. "
1760  "Case not taken in account.");
1761  return;
1762  }
1763  }
1764 
1765  Catch();
1766  if (TryStt < 0) {
1767  if (TryStt == VelStopOnJoint) {
1768  UInt32 axisInJoint[njoint];
1769  PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1770  for (unsigned int i=0; i < njoint; i ++) {
1771  if (axisInJoint[i])
1772  std::cout << "\nWarning: Velocity control stopped: axis "
1773  << i+1 << " on joint limit!" <<std::endl;
1774  }
1775  }
1776  else {
1777  printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1778  if (TryString != NULL) {
1779  // The statement is in TryString, but we need to check the validity
1780  printf(" Error sentence %s\n", TryString); // Print the TryString
1781  }
1782  else {
1783  printf("\n");
1784  }
1785  }
1786  }
1787 
1788  return;
1789 }
1790 
1791 
1792 
1793 
1794 
1795 
1796 /* ------------------------------------------------------------------------ */
1797 /* --- GET ---------------------------------------------------------------- */
1798 /* ------------------------------------------------------------------------ */
1799 
1800 
1858  vpColVector & velocity, double &timestamp)
1859 {
1860  velocity.resize (6);
1861  velocity = 0;
1862 
1863  vpColVector q_cur(6);
1864  vpHomogeneousMatrix fMc_cur;
1865  vpHomogeneousMatrix cMc; // camera displacement
1866  double time_cur;
1867 
1868  InitTry;
1869 
1870  // Get the current joint position
1871  Try( PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp) );
1872  time_cur = timestamp;
1873  q_cur.deg2rad();
1874 
1875  // Get the camera pose from the direct kinematics
1876  vpViper650::get_fMc(q_cur, fMc_cur);
1877 
1878  if ( ! first_time_getvel ) {
1879 
1880  switch (frame) {
1881  case vpRobot::CAMERA_FRAME: {
1882  // Compute the displacement of the camera since the previous call
1883  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1884 
1885  // Compute the velocity of the camera from this displacement
1886  velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1887 
1888  break ;
1889  }
1890 
1891  case vpRobot::ARTICULAR_FRAME: {
1892  velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1893  break ;
1894  }
1895 
1896  case vpRobot::REFERENCE_FRAME: {
1897  // Compute the displacement of the camera since the previous call
1898  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1899 
1900  // Compute the velocity of the camera from this displacement
1901  vpColVector v;
1902  v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1903 
1904  // Express this velocity in the reference frame
1905  vpVelocityTwistMatrix fVc(fMc_cur);
1906  velocity = fVc * v;
1907 
1908  break ;
1909  }
1910 
1911  case vpRobot::MIXT_FRAME: {
1912  // Compute the displacement of the camera since the previous call
1913  cMc = fMc_prev_getvel.inverse() * fMc_cur;
1914 
1915  // Compute the ThetaU representation for the rotation
1916  vpRotationMatrix cRc;
1917  cMc.extract(cRc);
1918  vpThetaUVector thetaU;
1919  thetaU.buildFrom(cRc);
1920 
1921  for (unsigned int i=0; i < 3; i++) {
1922  // Compute the translation displacement in the reference frame
1923  velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1924  // Update the rotation displacement in the camera frame
1925  velocity[i+3] = thetaU[i];
1926  }
1927 
1928  // Compute the velocity
1929  velocity /= (time_cur - time_prev_getvel);
1930  break ;
1931  }
1932  }
1933  }
1934  else {
1935  first_time_getvel = false;
1936  }
1937 
1938  // Memorize the camera pose for the next call
1939  fMc_prev_getvel = fMc_cur;
1940 
1941  // Memorize the joint position for the next call
1942  q_prev_getvel = q_cur;
1943 
1944  // Memorize the time associated to the joint position for the next call
1945  time_prev_getvel = time_cur;
1946 
1947 
1948  CatchPrint();
1949  if (TryStt < 0) {
1950  vpERROR_TRACE ("Cannot get velocity.");
1952  "Cannot get velocity.");
1953  }
1954 }
1955 
1965  vpColVector & velocity)
1966 {
1967  double timestamp;
1968  getVelocity(frame, velocity, timestamp);
1969 }
1970 
2022 {
2023  vpColVector velocity;
2024  getVelocity (frame, velocity, timestamp);
2025 
2026  return velocity;
2027 }
2028 
2038 {
2039  vpColVector velocity;
2040  double timestamp;
2041  getVelocity (frame, velocity, timestamp);
2042 
2043  return velocity;
2044 }
2045 
2111 bool vpRobotViper650::readPosFile(const char *filename, vpColVector &q)
2112 {
2113 
2114  FILE * fd ;
2115  fd = fopen(filename, "r") ;
2116  if (fd == NULL)
2117  return false;
2118 
2119  char line[FILENAME_MAX];
2120  char dummy[FILENAME_MAX];
2121  char head[] = "R:";
2122  bool sortie = false;
2123 
2124  do {
2125  // Saut des lignes commencant par #
2126  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2127  if ( strncmp (line, "#", 1) != 0) {
2128  // La ligne n'est pas un commentaire
2129  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2130  sortie = true; // Position robot trouvee.
2131  }
2132  // else
2133  // return (false); // fin fichier sans position robot.
2134  }
2135  }
2136  else {
2137  return (false); /* fin fichier */
2138  }
2139 
2140  }
2141  while ( sortie != true );
2142 
2143  // Lecture des positions
2144  q.resize(njoint);
2145  sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2146  dummy,
2147  &q[0], &q[1], &q[2],
2148  &q[3], &q[4], &q[5]);
2149 
2150  // converts rotations from degrees into radians
2151  q.deg2rad();
2152 
2153  fclose(fd) ;
2154  return (true);
2155 }
2156 
2180 bool
2181  vpRobotViper650::savePosFile(const char *filename, const vpColVector &q)
2182 {
2183 
2184  FILE * fd ;
2185  fd = fopen(filename, "w") ;
2186  if (fd == NULL)
2187  return false;
2188 
2189  fprintf(fd, "\
2190 #Viper650 - 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",
2199  vpMath::deg(q[0]),
2200  vpMath::deg(q[1]),
2201  vpMath::deg(q[2]),
2202  vpMath::deg(q[3]),
2203  vpMath::deg(q[4]),
2204  vpMath::deg(q[5]));
2205 
2206  fclose(fd) ;
2207  return (true);
2208 }
2209 
2220 void
2221  vpRobotViper650::move(const char *filename)
2222 {
2223  vpColVector q;
2224 
2225  try {
2226  this->readPosFile(filename, q) ;
2228  this->setPositioningVelocity(10);
2230  }
2231  catch(...) {
2232  throw;
2233  }
2234 }
2235 
2249 void
2250  vpRobotViper650::getCameraDisplacement(vpColVector &displacement)
2251 {
2252  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
2253 }
2265 void
2266  vpRobotViper650::getArticularDisplacement(vpColVector &displacement)
2267 {
2269 }
2270 
2291 void
2293  vpColVector &displacement)
2294 {
2295  displacement.resize (6);
2296  displacement = 0;
2297 
2298  double q[6];
2299  vpColVector q_cur(6);
2300  double timestamp;
2301 
2302  InitTry;
2303 
2304  // Get the current joint position
2305  Try( PrimitiveACQ_POS_Viper650(q, &timestamp) );
2306  for (unsigned int i=0; i < njoint; i ++) {
2307  q_cur[i] = q[i];
2308  }
2309 
2310  if ( ! first_time_getdis ) {
2311  switch (frame) {
2312  case vpRobot::CAMERA_FRAME: {
2313  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2314  return;
2315  break ;
2316  }
2317 
2318  case vpRobot::ARTICULAR_FRAME: {
2319  displacement = q_cur - q_prev_getdis;
2320  break ;
2321  }
2322 
2323  case vpRobot::REFERENCE_FRAME: {
2324  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2325  return;
2326  break ;
2327  }
2328 
2329  case vpRobot::MIXT_FRAME: {
2330  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2331  return;
2332  break ;
2333  }
2334  }
2335  }
2336  else {
2337  first_time_getdis = false;
2338  }
2339 
2340  // Memorize the joint position for the next call
2341  q_prev_getdis = q_cur;
2342 
2343  CatchPrint();
2344  if (TryStt < 0) {
2345  vpERROR_TRACE ("Cannot get velocity.");
2347  "Cannot get velocity.");
2348  }
2349 }
2350 
2364 void
2366 {
2367  InitTry;
2368 
2369  Try( PrimitiveTFS_BIAS_Viper650() );
2370 
2371  // Wait 500 ms to be sure the next measures take into account the bias
2372  vpTime::wait(500);
2373 
2374  CatchPrint();
2375  if (TryStt < 0) {
2376  vpERROR_TRACE ("Cannot bias the force/torque sensor.");
2378  "Cannot bias the force/torque sensor.");
2379  }
2380 }
2381 
2421 void
2423 {
2424  InitTry;
2425 
2426  H.resize (6);
2427 
2428  Try( PrimitiveTFS_ACQ_Viper650(H.data) );
2429 
2430  CatchPrint();
2431  if (TryStt < 0) {
2432  vpERROR_TRACE ("Cannot get the force/torque measures.");
2434  "Cannot get force/torque measures.");
2435  }
2436 }
2437 
2444 {
2445  InitTry;
2446  Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
2447  std::cout << "Enable joint limits on axis 6..." << std::endl;
2448  CatchPrint();
2449  if (TryStt < 0) {
2450  vpERROR_TRACE ("Cannot enable joint limits on axis 6");
2452  "Cannot enable joint limits on axis 6.");
2453  }
2454 }
2455 
2466 {
2467  InitTry;
2468  Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1) );
2469  std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2470  CatchPrint();
2471  if (TryStt < 0) {
2472  vpERROR_TRACE ("Cannot disable joint limits on axis 6");
2474  "Cannot disable joint limits on axis 6.");
2475  }
2476 }
2477 
2485 void
2487 {
2490 
2491  return;
2492 }
2493 
2513 void
2515 {
2516  maxRotationVelocity_joint6 = w6_max;
2517  return;
2518 }
2519 
2526 double
2528 {
2529  return maxRotationVelocity_joint6;
2530 }
2531 
2532 #elif !defined(VISP_BUILD_SHARED_LIBS)
2533 // Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has no symbols
2534 void dummy_vpRobotViper650() {};
2535 #endif
static const double defaultPositioningVelocity
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:981
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotViper650(void)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
Definition: vpRobot.h:156
#define vpERROR_TRACE
Definition: vpDebug.h:391
void setPositioningVelocity(const double velocity)
double maxRotationVelocity
Definition: vpRobot.h:95
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:250
void setMaxRotationVelocityJoint6(double w6_max)
Initialize the position controller.
Definition: vpRobot.h:69
class that defines a generic virtual robot
Definition: vpRobot.h:58
Automatic control mode (default).
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
void get_cVe(vpVelocityTwistMatrix &cVe) const
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
vpControlFrameType
Definition: vpRobot.h:76
vpRxyzVector erc
Definition: vpViper.h:148
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void move(const char *filename)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:275
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void deg2rad()
Definition: vpColVector.h:124
double getTime() const
Implementation of a rotation matrix and operations on such kind of matrices.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:96
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
Definition: vpRobot.h:68
Emergency stop activated.
vpRobotStateType
Definition: vpRobot.h:64
bool verbose_
Definition: vpRobot.h:113
vpTranslationVector etc
Definition: vpViper.h:147
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1258
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRowVector t() const
vpColVector joint_max
Definition: vpViper.h:159
Modelisation of the ADEPT Viper 650 robot.
Definition: vpViper650.h:62
void enableJoint6Limits() const
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:611
double getPositioningVelocity(void) const
Manual control mode activated when the dead man switch is in use.
void init(void)
Definition: vpViper650.cpp:171
Implementation of a velocity twist matrix and operations on such kind of matrices.
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:87
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:573
static double rad(double deg)
Definition: vpMath.h:104
void get_fJe(vpMatrix &fJe)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1173
static bool savePosFile(const char *filename, const vpColVector &q)
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:262
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:927
static double deg(double rad)
Definition: vpMath.h:97
void biasForceTorqueSensor() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
static bool readPosFile(const char *filename, vpColVector &q)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:138
void disableJoint6Limits() const
double getMaxRotationVelocityJoint6() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:154
void setMaxRotationVelocity(double w_max)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:151
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_cMe(vpHomogeneousMatrix &cMe) const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void rad2deg()
Definition: vpColVector.h:202
void getForceTorque(vpColVector &H) const
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:142
void get_eJe(vpMatrix &eJe)
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217
vpColVector joint_min
Definition: vpViper.h:160