Visual Servoing Platform version 3.5.0
vpRobotViper650.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 S650 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_VIPER650
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/vpRobotViper650.h>
56
57/* ---------------------------------------------------------------------- */
58/* --- STATIC ----------------------------------------------------------- */
59/* ---------------------------------------------------------------------- */
60
61bool vpRobotViper650::m_robotAlreadyCreated = false;
62
71
72/* ---------------------------------------------------------------------- */
73/* --- EMERGENCY STOP --------------------------------------------------- */
74/* ---------------------------------------------------------------------- */
75
83void emergencyStopViper650(int signo)
84{
85 std::cout << "Stop the Viper650 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_Viper650();
107 PrimitiveSTOP_Viper650();
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
177vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), 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, emergencyStopViper650);
200 signal(SIGBUS, emergencyStopViper650);
201 signal(SIGSEGV, emergencyStopViper650);
202 signal(SIGKILL, emergencyStopViper650);
203 signal(SIGQUIT, emergencyStopViper650);
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 m_positioningVelocity = m_defaultPositioningVelocity;
216
217 m_maxRotationVelocity_joint6 = maxRotationVelocity;
218
219 vpRobotViper650::m_robotAlreadyCreated = true;
220
221 return;
222}
223
224/* ------------------------------------------------------------------------ */
225/* --- INITIALISATION ----------------------------------------------------- */
226/* ------------------------------------------------------------------------ */
227
251{
252 InitTry;
253
254 // Initialise private variables used to compute the measured velocities
255 m_q_prev_getvel.resize(6);
256 m_q_prev_getvel = 0;
257 m_time_prev_getvel = 0;
258 m_first_time_getvel = true;
259
260 // Initialise private variables used to compute the measured displacement
261 m_q_prev_getdis.resize(6);
262 m_q_prev_getdis = 0;
263 m_first_time_getdis = true;
264
265 // Initialize the firewire connection
266 Try(InitializeConnection(verbose_));
267
268 // Connect to the servoboard using the servo board GUID
269 Try(InitializeNode_Viper650());
270
271 Try(PrimitiveRESET_Viper650());
272
273 // Enable the joint limits on axis 6
274 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
275
276 // Update the eMc matrix in the low level controller
278
279 // Look if the power is on or off
280 UInt32 HIPowerStatus;
281 UInt32 EStopStatus;
282 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
283 CAL_Wait(0.1);
284
285 // Print the robot status
286 if (verbose_) {
287 std::cout << "Robot status: ";
288 switch (EStopStatus) {
289 case ESTOP_AUTO:
290 m_controlMode = AUTO;
291 if (HIPowerStatus == 0)
292 std::cout << "Power is OFF" << std::endl;
293 else
294 std::cout << "Power is ON" << std::endl;
295 break;
296
297 case ESTOP_MANUAL:
298 m_controlMode = MANUAL;
299 if (HIPowerStatus == 0)
300 std::cout << "Power is OFF" << std::endl;
301 else
302 std::cout << "Power is ON" << std::endl;
303 break;
304 case ESTOP_ACTIVATED:
305 m_controlMode = ESTOP;
306 std::cout << "Emergency stop is activated" << std::endl;
307 break;
308 default:
309 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
310 std::cout << "You have to call Adept for maintenance..." << std::endl;
311 // Free allocated resources
312 }
313 std::cout << std::endl;
314 }
315 // get real joint min/max from the MotionBlox
316 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
317 // Convert units from degrees to radians
320
321 // for (unsigned int i=0; i < njoint; i++) {
322 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
323 // joint_max[i]);
324 // }
325
326 // If an error occur in the low level controller, goto here
327 // CatchPrint();
328 Catch();
329
330 // Test if an error occurs
331 if (TryStt == -20001)
332 printf("No connection detected. Check if the robot is powered on \n"
333 "and if the firewire link exist between the MotionBlox and this "
334 "computer.\n");
335 else if (TryStt == -675)
336 printf(" Timeout enabling power...\n");
337
338 if (TryStt < 0) {
339 // Power off the robot
340 PrimitivePOWEROFF_Viper650();
341 // Free allocated resources
342 ShutDownConnection();
343
344 std::cout << "Cannot open connection with the motionblox..." << std::endl;
345 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
346 }
347 return;
348}
349
407{
408 // Read the robot constants from files
409 // - joint [min,max], coupl_56, long_56
410 // - camera extrinsic parameters relative to eMc
412
413 InitTry;
414
415 // Get real joint min/max from the MotionBlox
416 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
417 // Convert units from degrees to radians
420
421 // for (unsigned int i=0; i < njoint; i++) {
422 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
423 // joint_max[i]);
424 // }
425
426 // Set the camera constant (eMc pose) in the MotionBlox
427 double eMc_pose[6];
428 for (unsigned int i = 0; i < 3; i++) {
429 eMc_pose[i] = etc[i]; // translation in meters
430 eMc_pose[i + 3] = erc[i]; // rotation in rad
431 }
432 // Update the eMc pose in the low level controller
433 Try(PrimitiveCONST_Viper650(eMc_pose));
434
435 CatchPrint();
436 return;
437}
438
489void vpRobotViper650::init(vpViper650::vpToolType tool, const std::string &filename)
490{
491 vpViper650::init(tool, filename);
492
493 InitTry;
494
495 // Get real joint min/max from the MotionBlox
496 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
497 // Convert units from degrees to radians
500
501 // for (unsigned int i=0; i < njoint; i++) {
502 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
503 // joint_max[i]);
504 // }
505
506 // Set the camera constant (eMc pose) in the MotionBlox
507 double eMc_pose[6];
508 for (unsigned int i = 0; i < 3; i++) {
509 eMc_pose[i] = etc[i]; // translation in meters
510 eMc_pose[i + 3] = erc[i]; // rotation in rad
511 }
512 // Update the eMc pose in the low level controller
513 Try(PrimitiveCONST_Viper650(eMc_pose));
514
515 CatchPrint();
516 return;
517}
518
556{
557 vpViper650::init(tool, eMc_);
558
559 InitTry;
560
561 // Get real joint min/max from the MotionBlox
562 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
563 // Convert units from degrees to radians
566
567 // for (unsigned int i=0; i < njoint; i++) {
568 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
569 // joint_max[i]);
570 // }
571
572 // Set the camera constant (eMc pose) in the MotionBlox
573 double eMc_pose[6];
574 for (unsigned int i = 0; i < 3; i++) {
575 eMc_pose[i] = etc[i]; // translation in meters
576 eMc_pose[i + 3] = erc[i]; // rotation in rad
577 }
578 // Update the eMc pose in the low level controller
579 Try(PrimitiveCONST_Viper650(eMc_pose));
580
581 CatchPrint();
582 return;
583}
584
597{
598 this->vpViper650::set_eMc(eMc_);
599
600 InitTry;
601
602 // Set the camera constant (eMc pose) in the MotionBlox
603 double eMc_pose[6];
604 for (unsigned int i = 0; i < 3; i++) {
605 eMc_pose[i] = etc[i]; // translation in meters
606 eMc_pose[i + 3] = erc[i]; // rotation in rad
607 }
608 // Update the eMc pose in the low level controller
609 Try(PrimitiveCONST_Viper650(eMc_pose));
610
611 CatchPrint();
612
613 return;
614}
615
629{
630 this->vpViper650::set_eMc(etc_, erc_);
631
632 InitTry;
633
634 // Set the camera constant (eMc pose) in the MotionBlox
635 double eMc_pose[6];
636 for (unsigned int i = 0; i < 3; i++) {
637 eMc_pose[i] = etc[i]; // translation in meters
638 eMc_pose[i + 3] = erc[i]; // rotation in rad
639 }
640 // Update the eMc pose in the low level controller
641 Try(PrimitiveCONST_Viper650(eMc_pose));
642
643 CatchPrint();
644
645 return;
646}
647
648/* ------------------------------------------------------------------------ */
649/* --- DESTRUCTOR --------------------------------------------------------- */
650/* ------------------------------------------------------------------------ */
651
659{
660 InitTry;
661
663
664 // Look if the power is on or off
665 UInt32 HIPowerStatus;
666 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
667 CAL_Wait(0.1);
668
669 // if (HIPowerStatus == 1) {
670 // fprintf(stdout, "Power OFF the robot\n");
671 // fflush(stdout);
672
673 // Try( PrimitivePOWEROFF_Viper650() );
674 // }
675
676 // Free allocated resources
677 ShutDownConnection();
678
679 vpRobotViper650::m_robotAlreadyCreated = false;
680
681 CatchPrint();
682 return;
683}
684
692{
693 InitTry;
694
695 switch (newState) {
696 case vpRobot::STATE_STOP: {
697 // Start primitive STOP only if the current state is Velocity
699 Try(PrimitiveSTOP_Viper650());
700 vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
701 }
702 break;
703 }
706 std::cout << "Change the control mode from velocity to position control.\n";
707 Try(PrimitiveSTOP_Viper650());
708 } else {
709 // std::cout << "Change the control mode from stop to position
710 // control.\n";
711 }
712 this->powerOn();
713 break;
714 }
717 std::cout << "Change the control mode from stop to velocity control.\n";
718 }
719 this->powerOn();
720 break;
721 }
722 default:
723 break;
724 }
725
726 CatchPrint();
727
728 return vpRobot::setRobotState(newState);
729}
730
731/* ------------------------------------------------------------------------ */
732/* --- STOP --------------------------------------------------------------- */
733/* ------------------------------------------------------------------------ */
734
743{
745 return;
746
747 InitTry;
748 Try(PrimitiveSTOP_Viper650());
750
751 CatchPrint();
752 if (TryStt < 0) {
753 vpERROR_TRACE("Cannot stop robot motion");
754 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
755 }
756}
757
768{
769 InitTry;
770
771 // Look if the power is on or off
772 UInt32 HIPowerStatus;
773 UInt32 EStopStatus;
774 bool firsttime = true;
775 unsigned int nitermax = 10;
776
777 for (unsigned int i = 0; i < nitermax; i++) {
778 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
779 if (EStopStatus == ESTOP_AUTO) {
780 m_controlMode = AUTO;
781 break; // exit for loop
782 } else if (EStopStatus == ESTOP_MANUAL) {
783 m_controlMode = MANUAL;
784 break; // exit for loop
785 } else if (EStopStatus == ESTOP_ACTIVATED) {
786 m_controlMode = ESTOP;
787 if (firsttime) {
788 std::cout << "Emergency stop is activated! \n"
789 << "Check the emergency stop button and push the yellow "
790 "button before continuing."
791 << std::endl;
792 firsttime = false;
793 }
794 fprintf(stdout, "Remaining time %us \r", nitermax - i);
795 fflush(stdout);
796 CAL_Wait(1);
797 } else {
798 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
799 std::cout << "You have to call Adept for maintenance..." << std::endl;
800 // Free allocated resources
801 ShutDownConnection();
802 throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
803 }
804 }
805
806 if (EStopStatus == ESTOP_ACTIVATED)
807 std::cout << std::endl;
808
809 if (EStopStatus == ESTOP_ACTIVATED) {
810 std::cout << "Sorry, cannot power on the robot." << std::endl;
811 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
812 }
813
814 if (HIPowerStatus == 0) {
815 fprintf(stdout, "Power ON the Viper650 robot\n");
816 fflush(stdout);
817
818 Try(PrimitivePOWERON_Viper650());
819 }
820
821 CatchPrint();
822 if (TryStt < 0) {
823 vpERROR_TRACE("Cannot power on the robot");
824 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
825 }
826}
827
838{
839 InitTry;
840
841 // Look if the power is on or off
842 UInt32 HIPowerStatus;
843 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
844 CAL_Wait(0.1);
845
846 if (HIPowerStatus == 1) {
847 fprintf(stdout, "Power OFF the Viper650 robot\n");
848 fflush(stdout);
849
850 Try(PrimitivePOWEROFF_Viper650());
851 }
852
853 CatchPrint();
854 if (TryStt < 0) {
855 vpERROR_TRACE("Cannot power off the robot");
856 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
857 }
858}
859
872{
873 InitTry;
874 bool status = false;
875 // Look if the power is on or off
876 UInt32 HIPowerStatus;
877 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
878 CAL_Wait(0.1);
879
880 if (HIPowerStatus == 1) {
881 status = true;
882 }
883
884 CatchPrint();
885 if (TryStt < 0) {
886 vpERROR_TRACE("Cannot get the power status");
887 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
888 }
889 return status;
890}
891
902{
905
906 cVe.buildFrom(cMe);
907}
908
921
934{
935
936 double position[6];
937 double timestamp;
938
939 InitTry;
940 Try(PrimitiveACQ_POS_J_Viper650(position, &timestamp));
941 CatchPrint();
942
943 vpColVector q(6);
944 for (unsigned int i = 0; i < njoint; i++)
945 q[i] = vpMath::rad(position[i]);
946
947 try {
949 } catch (...) {
950 vpERROR_TRACE("catch exception ");
951 throw;
952 }
953}
967{
968
969 double position[6];
970 double timestamp;
971
972 InitTry;
973 Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
974 CatchPrint();
975
976 vpColVector q(6);
977 for (unsigned int i = 0; i < njoint; i++)
978 q[i] = position[i];
979
980 try {
982 } catch (...) {
983 vpERROR_TRACE("Error caught");
984 throw;
985 }
986}
987
1024void vpRobotViper650::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1025
1031double vpRobotViper650::getPositioningVelocity(void) const { return m_positioningVelocity; }
1032
1111{
1112
1114 vpERROR_TRACE("Robot was not in position-based control\n"
1115 "Modification of the robot state");
1117 }
1118
1119 vpColVector destination(njoint);
1120 int error = 0;
1121 double timestamp;
1122
1123 InitTry;
1124 switch (frame) {
1125 case vpRobot::CAMERA_FRAME: {
1127 Try(PrimitiveACQ_POS_Viper650(q.data, &timestamp));
1128
1129 // Convert degrees into rad
1130 q.deg2rad();
1131
1132 // Get fMc from the inverse kinematics
1134 vpViper650::get_fMc(q, fMc);
1135
1136 // Set cMc from the input position
1138 vpRxyzVector rxyz;
1139 for (unsigned int i = 0; i < 3; i++) {
1140 txyz[i] = position[i];
1141 rxyz[i] = position[i + 3];
1142 }
1143
1144 // Compute cMc2
1145 vpRotationMatrix cRc2(rxyz);
1146 vpHomogeneousMatrix cMc2(txyz, cRc2);
1147
1148 // Compute the new position to reach: fMc*cMc2
1149 vpHomogeneousMatrix fMc2 = fMc * cMc2;
1150
1151 // Compute the corresponding joint position from the inverse kinematics
1152 unsigned int solution = this->getInverseKinematics(fMc2, q);
1153 if (solution) { // Position is reachable
1154 destination = q;
1155 // convert rad to deg requested for the low level controller
1156 destination.rad2deg();
1157 Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1158 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1159 } else {
1160 // Cartesian position is out of range
1161 error = -1;
1162 }
1163
1164 break;
1165 }
1167 destination = position;
1168 // convert rad to deg requested for the low level controller
1169 destination.rad2deg();
1170
1171 // std::cout << "Joint destination (deg): " << destination.t() <<
1172 // std::endl;
1173 Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1174 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1175 break;
1176 }
1178 // Convert angles from Rxyz representation to Rzyz representation
1179 vpRxyzVector rxyz(position[3], position[4], position[5]);
1180 vpRotationMatrix R(rxyz);
1181 vpRzyzVector rzyz(R);
1182
1183 for (unsigned int i = 0; i < 3; i++) {
1184 destination[i] = position[i];
1185 destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1186 }
1187 int configuration = 0; // keep the actual configuration
1188
1189 // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1190 // << std::endl;
1191 Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, m_positioningVelocity));
1192 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1193
1194 break;
1195 }
1196 case vpRobot::MIXT_FRAME: {
1197 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1198 "Mixt frame not implemented.");
1199 }
1201 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1202 "Mixt frame not implemented.");
1203 }
1204 }
1205
1206 CatchPrint();
1207 if (TryStt == InvalidPosition || TryStt == -1023)
1208 std::cout << " : Position out of range.\n";
1209
1210 else if (TryStt == -3019) {
1211 if (frame == vpRobot::ARTICULAR_FRAME)
1212 std::cout << " : Joint position out of range.\n";
1213 else
1214 std::cout << " : Cartesian position leads to a joint position out of "
1215 "range.\n";
1216 } else if (TryStt < 0)
1217 std::cout << " : Unknown error (see Fabien).\n";
1218 else if (error == -1)
1219 std::cout << "Position out of range.\n";
1220
1221 if (TryStt < 0 || error < 0) {
1222 vpERROR_TRACE("Positionning error.");
1223 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1224 }
1225
1226 return;
1227}
1228
1293void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1294 double pos3, double pos4, double pos5, double pos6)
1295{
1296 try {
1297 vpColVector position(6);
1298 position[0] = pos1;
1299 position[1] = pos2;
1300 position[2] = pos3;
1301 position[3] = pos4;
1302 position[4] = pos5;
1303 position[5] = pos6;
1304
1305 setPosition(frame, position);
1306 } catch (...) {
1307 vpERROR_TRACE("Error caught");
1308 throw;
1309 }
1310}
1311
1350void vpRobotViper650::setPosition(const std::string &filename)
1351{
1352 vpColVector q;
1353 bool ret;
1354
1355 ret = this->readPosFile(filename, q);
1356
1357 if (ret == false) {
1358 vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1359 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1360 }
1363}
1364
1432void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1433{
1434
1435 InitTry;
1436
1437 position.resize(6);
1438
1439 switch (frame) {
1440 case vpRobot::CAMERA_FRAME: {
1441 position = 0;
1442 return;
1443 }
1445 Try(PrimitiveACQ_POS_J_Viper650(position.data, &timestamp));
1446 // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1447 position.deg2rad();
1448
1449 return;
1450 }
1453 Try(PrimitiveACQ_POS_J_Viper650(q.data, &timestamp));
1454
1455 // Compute fMc
1457
1458 // From fMc extract the pose
1459 vpRotationMatrix fRc;
1460 fMc.extract(fRc);
1461 vpRxyzVector rxyz;
1462 rxyz.buildFrom(fRc);
1463
1464 for (unsigned int i = 0; i < 3; i++) {
1465 position[i] = fMc[i][3]; // translation x,y,z
1466 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1467 }
1468
1469 break;
1470 }
1471 case vpRobot::MIXT_FRAME: {
1472 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1473 "not implemented");
1474 }
1476 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1477 "not implemented");
1478 }
1479 }
1480
1481 CatchPrint();
1482 if (TryStt < 0) {
1483 vpERROR_TRACE("Cannot get position.");
1484 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1485 }
1486
1487 return;
1488}
1489
1501{
1502 double timestamp;
1503 getPosition(frame, position, timestamp);
1504}
1505
1519void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1520{
1521 vpColVector posRxyz;
1522 // recupere position en Rxyz
1523 this->getPosition(frame, posRxyz, timestamp);
1524 vpRxyzVector RxyzVect;
1525 for (unsigned int j = 0; j < 3; j++)
1526 RxyzVect[j] = posRxyz[j + 3];
1527 // recupere le vecteur thetaU correspondant
1528 vpThetaUVector RtuVect(RxyzVect);
1529
1530 // remplit le vpPoseVector avec translation et rotation ThetaU
1531 for (unsigned int j = 0; j < 3; j++) {
1532 position[j] = posRxyz[j];
1533 position[j + 3] = RtuVect[j];
1534 }
1535}
1536
1548{
1549 double timestamp;
1550 getPosition(frame, position, timestamp);
1551}
1552
1558{
1559 double timestamp;
1560 PrimitiveACQ_TIME_Viper650(&timestamp);
1561 return timestamp;
1562}
1563
1642{
1644 vpERROR_TRACE("Cannot send a velocity to the robot "
1645 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1647 "Cannot send a velocity to the robot "
1648 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1649 }
1650
1651 vpColVector vel_sat(6);
1652
1653 // Velocity saturation
1654 switch (frame) {
1655 // saturation in cartesian space
1659 case vpRobot::MIXT_FRAME: {
1660 vpColVector vel_max(6);
1661
1662 for (unsigned int i = 0; i < 3; i++)
1663 vel_max[i] = getMaxTranslationVelocity();
1664 for (unsigned int i = 3; i < 6; i++)
1665 vel_max[i] = getMaxRotationVelocity();
1666
1667 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1668
1669 break;
1670 }
1671 // saturation in joint space
1673 vpColVector vel_max(6);
1674
1675 // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1676 if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1677 for (unsigned int i = 0; i < 6; i++)
1678 vel_max[i] = getMaxRotationVelocity();
1679 } else {
1680 for (unsigned int i = 0; i < 5; i++)
1681 vel_max[i] = getMaxRotationVelocity();
1682 vel_max[5] = getMaxRotationVelocityJoint6();
1683 }
1684
1685 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1686 }
1687 }
1688
1689 InitTry;
1690
1691 switch (frame) {
1692 case vpRobot::CAMERA_FRAME: {
1693 // Send velocities in m/s and rad/s
1694 // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1695 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
1696 break;
1697 }
1699 // Transform in camera frame
1701 this->get_cMe(cMe);
1702 vpVelocityTwistMatrix cVe(cMe);
1703 vpColVector v_c = cVe * vel_sat;
1704 // Send velocities in m/s and rad/s
1705 Try(PrimitiveMOVESPEED_CART_Viper650(v_c.data, REPCAM_VIPER650));
1706 break;
1707 }
1709 // Convert all the velocities from rad/s into deg/s
1710 vel_sat.rad2deg();
1711 // std::cout << "Vitesse appliquee: " << vel_sat.t();
1712 // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
1713 Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
1714 break;
1715 }
1717 // Send velocities in m/s and rad/s
1718 //std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1719 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
1720 break;
1721 }
1722 case vpRobot::MIXT_FRAME: {
1723 // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
1724 break;
1725 }
1726 default: {
1727 vpERROR_TRACE("Error in spec of vpRobot. "
1728 "Case not taken in account.");
1729 return;
1730 }
1731 }
1732
1733 Catch();
1734 if (TryStt < 0) {
1735 if (TryStt == VelStopOnJoint) {
1736 UInt32 axisInJoint[njoint];
1737 PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1738 for (unsigned int i = 0; i < njoint; i++) {
1739 if (axisInJoint[i])
1740 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1741 }
1742 } else {
1743 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1744 if (TryString != NULL) {
1745 // The statement is in TryString, but we need to check the validity
1746 printf(" Error sentence %s\n", TryString); // Print the TryString
1747 } else {
1748 printf("\n");
1749 }
1750 }
1751 }
1752
1753 return;
1754}
1755
1756/* ------------------------------------------------------------------------ */
1757/* --- GET ---------------------------------------------------------------- */
1758/* ------------------------------------------------------------------------ */
1759
1816void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1817{
1818 velocity.resize(6);
1819 velocity = 0;
1820
1821 vpColVector q_cur(6);
1822 vpHomogeneousMatrix fMe_cur, fMc_cur;
1823 vpHomogeneousMatrix eMe, cMc; // camera displacement
1824 double time_cur;
1825
1826 InitTry;
1827
1828 // Get the current joint position
1829 Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
1830 time_cur = timestamp;
1831 q_cur.deg2rad();
1832
1833 // Get the end-effector pose from the direct kinematics
1834 vpViper650::get_fMe(q_cur, fMe_cur);
1835 // Get the camera pose from the direct kinematics
1836 vpViper650::get_fMc(q_cur, fMc_cur);
1837
1838 if (!m_first_time_getvel) {
1839
1840 switch (frame) {
1842 // Compute the displacement of the end-effector since the previous call
1843 eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1844
1845 // Compute the velocity of the end-effector from this displacement
1846 velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1847
1848 break;
1849 }
1850
1851 case vpRobot::CAMERA_FRAME: {
1852 // Compute the displacement of the camera since the previous call
1853 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1854
1855 // Compute the velocity of the camera from this displacement
1856 velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1857
1858 break;
1859 }
1860
1862 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1863 break;
1864 }
1865
1867 // Compute the displacement of the camera since the previous call
1868 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1869
1870 // Compute the velocity of the camera from this displacement
1871 vpColVector v;
1872 v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1873
1874 // Express this velocity in the reference frame
1875 vpVelocityTwistMatrix fVc(fMc_cur);
1876 velocity = fVc * v;
1877
1878 break;
1879 }
1880
1881 case vpRobot::MIXT_FRAME: {
1882 // Compute the displacement of the camera since the previous call
1883 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1884
1885 // Compute the ThetaU representation for the rotation
1886 vpRotationMatrix cRc;
1887 cMc.extract(cRc);
1888 vpThetaUVector thetaU;
1889 thetaU.buildFrom(cRc);
1890
1891 for (unsigned int i = 0; i < 3; i++) {
1892 // Compute the translation displacement in the reference frame
1893 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1894 // Update the rotation displacement in the camera frame
1895 velocity[i + 3] = thetaU[i];
1896 }
1897
1898 // Compute the velocity
1899 velocity /= (time_cur - m_time_prev_getvel);
1900 break;
1901 }
1902 default: {
1904 "vpRobotViper650::getVelocity() not implemented in end-effector"));
1905 }
1906 }
1907 } else {
1908 m_first_time_getvel = false;
1909 }
1910
1911 // Memorize the end-effector pose for the next call
1912 m_fMe_prev_getvel = fMe_cur;
1913 // Memorize the camera pose for the next call
1914 m_fMc_prev_getvel = fMc_cur;
1915
1916 // Memorize the joint position for the next call
1917 m_q_prev_getvel = q_cur;
1918
1919 // Memorize the time associated to the joint position for the next call
1920 m_time_prev_getvel = time_cur;
1921
1922 CatchPrint();
1923 if (TryStt < 0) {
1924 vpERROR_TRACE("Cannot get velocity.");
1925 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1926 }
1927}
1928
1938{
1939 double timestamp;
1940 getVelocity(frame, velocity, timestamp);
1941}
1942
1993{
1994 vpColVector velocity;
1995 getVelocity(frame, velocity, timestamp);
1996
1997 return velocity;
1998}
1999
2009{
2010 vpColVector velocity;
2011 double timestamp;
2012 getVelocity(frame, velocity, timestamp);
2013
2014 return velocity;
2015}
2016
2082bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
2083{
2084 std::ifstream fd(filename.c_str(), std::ios::in);
2085
2086 if (!fd.is_open()) {
2087 return false;
2088 }
2089
2090 std::string line;
2091 std::string key("R:");
2092 std::string id("#Viper650 - Position");
2093 bool pos_found = false;
2094 int lineNum = 0;
2095
2096 q.resize(njoint);
2097
2098 while (std::getline(fd, line)) {
2099 lineNum++;
2100 if (lineNum == 1) {
2101 if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
2102 std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl;
2103 return false;
2104 }
2105 }
2106 if ((line.compare(0, 1, "#") == 0)) { // skip comment
2107 continue;
2108 }
2109 if ((line.compare(0, key.size(), key) == 0)) { // decode position
2110 // check if there are at least njoint values in the line
2111 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2112 if (chain.size() < njoint + 1) // try to split with tab separator
2113 chain = vpIoTools::splitChain(line, std::string("\t"));
2114 if (chain.size() < njoint + 1)
2115 continue;
2116
2117 std::istringstream ss(line);
2118 std::string key_;
2119 ss >> key_;
2120 for (unsigned int i = 0; i < njoint; i++)
2121 ss >> q[i];
2122 pos_found = true;
2123 break;
2124 }
2125 }
2126
2127 // converts rotations from degrees into radians
2128 q.deg2rad();
2129
2130 fd.close();
2131
2132 if (!pos_found) {
2133 std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2134 return false;
2135 }
2136
2137 return true;
2138}
2139
2163bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
2164{
2165
2166 FILE *fd;
2167 fd = fopen(filename.c_str(), "w");
2168 if (fd == NULL)
2169 return false;
2170
2171 fprintf(fd, "\
2172#Viper650 - Position - Version 1.00\n\
2173#\n\
2174# R: A B C D E F\n\
2175# Joint position in degrees\n\
2176#\n\
2177#\n\n");
2178
2179 // Save positions in mm and deg
2180 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2181 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2182
2183 fclose(fd);
2184 return (true);
2185}
2186
2197void vpRobotViper650::move(const std::string &filename)
2198{
2199 vpColVector q;
2200
2201 try {
2202 this->readPosFile(filename, q);
2204 this->setPositioningVelocity(10);
2206 } catch (...) {
2207 throw;
2208 }
2209}
2210
2230{
2231 displacement.resize(6);
2232 displacement = 0;
2233
2234 double q[6];
2235 vpColVector q_cur(6);
2236 double timestamp;
2237
2238 InitTry;
2239
2240 // Get the current joint position
2241 Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
2242 for (unsigned int i = 0; i < njoint; i++) {
2243 q_cur[i] = q[i];
2244 }
2245
2246 if (!m_first_time_getdis) {
2247 switch (frame) {
2248 case vpRobot::CAMERA_FRAME: {
2249 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2250 return;
2251 }
2252
2254 displacement = q_cur - m_q_prev_getdis;
2255 break;
2256 }
2257
2259 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2260 return;
2261 }
2262
2263 case vpRobot::MIXT_FRAME: {
2264 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2265 return;
2266 }
2268 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2269 return;
2270 }
2271 }
2272 } else {
2273 m_first_time_getdis = false;
2274 }
2275
2276 // Memorize the joint position for the next call
2277 m_q_prev_getdis = q_cur;
2278
2279 CatchPrint();
2280 if (TryStt < 0) {
2281 vpERROR_TRACE("Cannot get velocity.");
2282 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2283 }
2284}
2285
2300{
2301 InitTry;
2302
2303 Try(PrimitiveTFS_BIAS_Viper650());
2304
2305 // Wait 500 ms to be sure the next measures take into account the bias
2306 vpTime::wait(500);
2307
2308 CatchPrint();
2309 if (TryStt < 0) {
2310 vpERROR_TRACE("Cannot bias the force/torque sensor.");
2311 throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2312 }
2313}
2314
2355{
2356 InitTry;
2357
2358 H.resize(6);
2359
2360 Try(PrimitiveTFS_ACQ_Viper650(H.data));
2361
2362 CatchPrint();
2363 if (TryStt < 0) {
2364 vpERROR_TRACE("Cannot get the force/torque measures.");
2365 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2366 }
2367}
2368
2407{
2408 InitTry;
2409
2410 vpColVector H(6);
2411
2412 Try(PrimitiveTFS_ACQ_Viper650(H.data));
2413
2414 return H;
2415
2416 CatchPrint();
2417 if (TryStt < 0) {
2418 vpERROR_TRACE("Cannot get the force/torque measures.");
2419 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2420 }
2421 return H; // Here to avoid a warning, but should never be called
2422}
2423
2430{
2431 InitTry;
2432 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2433 std::cout << "Enable joint limits on axis 6..." << std::endl;
2434 CatchPrint();
2435 if (TryStt < 0) {
2436 vpERROR_TRACE("Cannot enable joint limits on axis 6");
2437 throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2438 }
2439}
2440
2452{
2453 InitTry;
2454 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2455 std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2456 CatchPrint();
2457 if (TryStt < 0) {
2458 vpERROR_TRACE("Cannot disable joint limits on axis 6");
2459 throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2460 }
2461}
2462
2472{
2475
2476 return;
2477}
2478
2499{
2500 m_maxRotationVelocity_joint6 = w6_max;
2501 return;
2502}
2503
2511double vpRobotViper650::getMaxRotationVelocityJoint6() const { return m_maxRotationVelocity_joint6; }
2512
2520{
2521 InitTry;
2522 Try(PrimitivePneumaticGripper_Viper650(1));
2523 std::cout << "Open the pneumatic gripper..." << std::endl;
2524 CatchPrint();
2525 if (TryStt < 0) {
2526 throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2527 }
2528}
2529
2538{
2539 InitTry;
2540 Try(PrimitivePneumaticGripper_Viper650(0));
2541 std::cout << "Close the pneumatic gripper..." << std::endl;
2542 CatchPrint();
2543 if (TryStt < 0) {
2544 throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2545 }
2546}
2547
2548#elif !defined(VISP_BUILD_SHARED_LIBS)
2549// Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
2550// no symbols
2551void dummy_vpRobotViper650(){};
2552#endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void deg2rad()
Definition: vpColVector.h:196
void rad2deg()
Definition: vpColVector.h:294
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Error that can be emited by the vpRobot class and its derivates.
void get_eJe(vpMatrix &eJe)
void move(const std::string &filename)
void set_eMc(const vpHomogeneousMatrix &eMc_)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
static bool savePosFile(const std::string &filename, const vpColVector &q)
static const double m_defaultPositioningVelocity
virtual ~vpRobotViper650(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPositioningVelocity(double velocity)
void get_fJe(vpMatrix &fJe)
void enableJoint6Limits() const
void get_cVe(vpVelocityTwistMatrix &cVe) const
vpColVector getForceTorque() const
bool getPowerState() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setMaxRotationVelocityJoint6(double w6_max)
void disableJoint6Limits() const
void biasForceTorqueSensor() const
void get_cMe(vpHomogeneousMatrix &cMe) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
@ AUTO
Automatic control mode (default).
@ ESTOP
Emergency stop activated.
double getMaxRotationVelocityJoint6() const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void setMaxRotationVelocity(double w_max)
double getTime() const
static bool readPosFile(const std::string &filename, vpColVector &q)
double getPositioningVelocity(void) const
void closeGripper() const
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
void setVerbose(bool verbose)
Definition: vpRobot.h:159
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
double maxRotationVelocity
Definition: vpRobot.h:98
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:183
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 650 robot.
Definition: vpViper650.h:103
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper650.h:176
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper650.h:136
void init(void)
Definition: vpViper650.cpp:135
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper650.h:127
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition: vpViper.cpp:1230
vpTranslationVector etc
Definition: vpViper.h:159
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector joint_max
Definition: vpViper.h:172
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector joint_min
Definition: vpViper.h:173
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpViper.cpp:715
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)