Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotViper650.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Irisa's Viper S650 robot.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_VIPER650
39
40#include <signal.h>
41#include <stdlib.h>
42#include <sys/types.h>
43#include <unistd.h>
44
45#include <visp3/core/vpDebug.h>
46#include <visp3/core/vpExponentialMap.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpThetaUVector.h>
49#include <visp3/core/vpVelocityTwistMatrix.h>
50#include <visp3/robot/vpRobot.h>
51#include <visp3/robot/vpRobotException.h>
52#include <visp3/robot/vpRobotViper650.h>
53
54/* ---------------------------------------------------------------------- */
55/* --- STATIC ----------------------------------------------------------- */
56/* ---------------------------------------------------------------------- */
57
58bool vpRobotViper650::m_robotAlreadyCreated = false;
59
68
69/* ---------------------------------------------------------------------- */
70/* --- EMERGENCY STOP --------------------------------------------------- */
71/* ---------------------------------------------------------------------- */
72
80void emergencyStopViper650(int signo)
81{
82 std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7;
83 switch (signo) {
84 case SIGINT:
85 std::cout << "SIGINT (stop by ^C) " << std::endl;
86 break;
87 case SIGBUS:
88 std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
89 break;
90 case SIGSEGV:
91 std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
92 break;
93 case SIGKILL:
94 std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
95 break;
96 case SIGQUIT:
97 std::cout << "SIGQUIT " << std::endl;
98 break;
99 default:
100 std::cout << signo << std::endl;
101 }
102 // std::cout << "Emergency stop called\n";
103 // PrimitiveESTOP_Viper650();
104 PrimitiveSTOP_Viper650();
105 std::cout << "Robot was stopped\n";
106
107 // Free allocated resources
108 // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
109
110 fprintf(stdout, "Application ");
111 fflush(stdout);
112 kill(getpid(), SIGKILL);
113 exit(1);
114}
115
116/* ---------------------------------------------------------------------- */
117/* --- CONSTRUCTOR ------------------------------------------------------ */
118/* ---------------------------------------------------------------------- */
119
174vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
175{
176
177 /*
178 #define SIGHUP 1 // hangup
179 #define SIGINT 2 // interrupt (rubout)
180 #define SIGQUIT 3 // quit (ASCII FS)
181 #define SIGILL 4 // illegal instruction (not reset when caught)
182 #define SIGTRAP 5 // trace trap (not reset when caught)
183 #define SIGIOT 6 // IOT instruction
184 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
185 #define SIGEMT 7 // EMT instruction
186 #define SIGFPE 8 // floating point exception
187 #define SIGKILL 9 // kill (cannot be caught or ignored)
188 #define SIGBUS 10 // bus error
189 #define SIGSEGV 11 // segmentation violation
190 #define SIGSYS 12 // bad argument to system call
191 #define SIGPIPE 13 // write on a pipe with no one to read it
192 #define SIGALRM 14 // alarm clock
193 #define SIGTERM 15 // software termination signal from kill
194 */
195
196 signal(SIGINT, emergencyStopViper650);
197 signal(SIGBUS, emergencyStopViper650);
198 signal(SIGSEGV, emergencyStopViper650);
199 signal(SIGKILL, emergencyStopViper650);
200 signal(SIGQUIT, emergencyStopViper650);
201
202 setVerbose(verbose);
203 if (verbose_)
204 std::cout << "Open communication with MotionBlox.\n";
205 try {
206 this->init();
208 } catch (...) {
209 // vpERROR_TRACE("Error caught") ;
210 throw;
211 }
212 m_positioningVelocity = m_defaultPositioningVelocity;
213
214 m_maxRotationVelocity_joint6 = maxRotationVelocity;
215
216 vpRobotViper650::m_robotAlreadyCreated = true;
217
218 return;
219}
220
221/* ------------------------------------------------------------------------ */
222/* --- INITIALISATION ----------------------------------------------------- */
223/* ------------------------------------------------------------------------ */
224
248{
249 InitTry;
250
251 // Initialise private variables used to compute the measured velocities
252 m_q_prev_getvel.resize(6);
253 m_q_prev_getvel = 0;
254 m_time_prev_getvel = 0;
255 m_first_time_getvel = true;
256
257 // Initialise private variables used to compute the measured displacement
258 m_q_prev_getdis.resize(6);
259 m_q_prev_getdis = 0;
260 m_first_time_getdis = true;
261
262 // Initialize the firewire connection
263 Try(InitializeConnection(verbose_));
264
265 // Connect to the servoboard using the servo board GUID
266 Try(InitializeNode_Viper650());
267
268 Try(PrimitiveRESET_Viper650());
269
270 // Enable the joint limits on axis 6
271 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
272
273 // Update the eMc matrix in the low level controller
275
276 // Look if the power is on or off
277 UInt32 HIPowerStatus;
278 UInt32 EStopStatus;
279 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
280 CAL_Wait(0.1);
281
282 // Print the robot status
283 if (verbose_) {
284 std::cout << "Robot status: ";
285 switch (EStopStatus) {
286 case ESTOP_AUTO:
287 m_controlMode = AUTO;
288 if (HIPowerStatus == 0)
289 std::cout << "Power is OFF" << std::endl;
290 else
291 std::cout << "Power is ON" << std::endl;
292 break;
293
294 case ESTOP_MANUAL:
295 m_controlMode = MANUAL;
296 if (HIPowerStatus == 0)
297 std::cout << "Power is OFF" << std::endl;
298 else
299 std::cout << "Power is ON" << std::endl;
300 break;
301 case ESTOP_ACTIVATED:
302 m_controlMode = ESTOP;
303 std::cout << "Emergency stop is activated" << std::endl;
304 break;
305 default:
306 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
307 std::cout << "You have to call Adept for maintenance..." << std::endl;
308 // Free allocated resources
309 }
310 std::cout << std::endl;
311 }
312 // get real joint min/max from the MotionBlox
313 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
314 // Convert units from degrees to radians
317
318 // for (unsigned int i=0; i < njoint; i++) {
319 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
320 // joint_max[i]);
321 // }
322
323 // If an error occur in the low level controller, goto here
324 // CatchPrint();
325 Catch();
326
327 // Test if an error occurs
328 if (TryStt == -20001)
329 printf("No connection detected. Check if the robot is powered on \n"
330 "and if the firewire link exist between the MotionBlox and this "
331 "computer.\n");
332 else if (TryStt == -675)
333 printf(" Timeout enabling power...\n");
334
335 if (TryStt < 0) {
336 // Power off the robot
337 PrimitivePOWEROFF_Viper650();
338 // Free allocated resources
339 ShutDownConnection();
340
341 std::cout << "Cannot open connection with the motionblox..." << std::endl;
342 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
343 }
344 return;
345}
346
404{
405 // Read the robot constants from files
406 // - joint [min,max], coupl_56, long_56
407 // - camera extrinsic parameters relative to eMc
409
410 InitTry;
411
412 // Get real joint min/max from the MotionBlox
413 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
414 // Convert units from degrees to radians
417
418 // for (unsigned int i=0; i < njoint; i++) {
419 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
420 // joint_max[i]);
421 // }
422
423 // Set the camera constant (eMc pose) in the MotionBlox
424 double eMc_pose[6];
425 for (unsigned int i = 0; i < 3; i++) {
426 eMc_pose[i] = etc[i]; // translation in meters
427 eMc_pose[i + 3] = erc[i]; // rotation in rad
428 }
429 // Update the eMc pose in the low level controller
430 Try(PrimitiveCONST_Viper650(eMc_pose));
431
432 CatchPrint();
433 return;
434}
435
486void vpRobotViper650::init(vpViper650::vpToolType tool, 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
497
498 // for (unsigned int i=0; i < njoint; i++) {
499 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
500 // joint_max[i]);
501 // }
502
503 // Set the camera constant (eMc pose) in the MotionBlox
504 double eMc_pose[6];
505 for (unsigned int i = 0; i < 3; i++) {
506 eMc_pose[i] = etc[i]; // translation in meters
507 eMc_pose[i + 3] = erc[i]; // rotation in rad
508 }
509 // Update the eMc pose in the low level controller
510 Try(PrimitiveCONST_Viper650(eMc_pose));
511
512 CatchPrint();
513 return;
514}
515
553{
554 vpViper650::init(tool, eMc_);
555
556 InitTry;
557
558 // Get real joint min/max from the MotionBlox
559 Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
560 // Convert units from degrees to radians
563
564 // for (unsigned int i=0; i < njoint; i++) {
565 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
566 // joint_max[i]);
567 // }
568
569 // Set the camera constant (eMc pose) in the MotionBlox
570 double eMc_pose[6];
571 for (unsigned int i = 0; i < 3; i++) {
572 eMc_pose[i] = etc[i]; // translation in meters
573 eMc_pose[i + 3] = erc[i]; // rotation in rad
574 }
575 // Update the eMc pose in the low level controller
576 Try(PrimitiveCONST_Viper650(eMc_pose));
577
578 CatchPrint();
579 return;
580}
581
594{
595 this->vpViper650::set_eMc(eMc_);
596
597 InitTry;
598
599 // Set the camera constant (eMc pose) in the MotionBlox
600 double eMc_pose[6];
601 for (unsigned int i = 0; i < 3; i++) {
602 eMc_pose[i] = etc[i]; // translation in meters
603 eMc_pose[i + 3] = erc[i]; // rotation in rad
604 }
605 // Update the eMc pose in the low level controller
606 Try(PrimitiveCONST_Viper650(eMc_pose));
607
608 CatchPrint();
609
610 return;
611}
612
626{
627 this->vpViper650::set_eMc(etc_, erc_);
628
629 InitTry;
630
631 // Set the camera constant (eMc pose) in the MotionBlox
632 double eMc_pose[6];
633 for (unsigned int i = 0; i < 3; i++) {
634 eMc_pose[i] = etc[i]; // translation in meters
635 eMc_pose[i + 3] = erc[i]; // rotation in rad
636 }
637 // Update the eMc pose in the low level controller
638 Try(PrimitiveCONST_Viper650(eMc_pose));
639
640 CatchPrint();
641
642 return;
643}
644
645/* ------------------------------------------------------------------------ */
646/* --- DESTRUCTOR --------------------------------------------------------- */
647/* ------------------------------------------------------------------------ */
648
656{
657 InitTry;
658
660
661 // Look if the power is on or off
662 UInt32 HIPowerStatus;
663 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
664 CAL_Wait(0.1);
665
666 // if (HIPowerStatus == 1) {
667 // fprintf(stdout, "Power OFF the robot\n");
668 // fflush(stdout);
669
670 // Try( PrimitivePOWEROFF_Viper650() );
671 // }
672
673 // Free allocated resources
674 ShutDownConnection();
675
676 vpRobotViper650::m_robotAlreadyCreated = false;
677
678 CatchPrint();
679 return;
680}
681
689{
690 InitTry;
691
692 switch (newState) {
693 case vpRobot::STATE_STOP: {
694 // Start primitive STOP only if the current state is Velocity
696 Try(PrimitiveSTOP_Viper650());
697 vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
698 }
699 break;
700 }
703 std::cout << "Change the control mode from velocity to position control.\n";
704 Try(PrimitiveSTOP_Viper650());
705 } else {
706 // std::cout << "Change the control mode from stop to position
707 // control.\n";
708 }
709 this->powerOn();
710 break;
711 }
714 std::cout << "Change the control mode from stop to velocity control.\n";
715 }
716 this->powerOn();
717 break;
718 }
719 default:
720 break;
721 }
722
723 CatchPrint();
724
725 return vpRobot::setRobotState(newState);
726}
727
728/* ------------------------------------------------------------------------ */
729/* --- STOP --------------------------------------------------------------- */
730/* ------------------------------------------------------------------------ */
731
740{
742 return;
743
744 InitTry;
745 Try(PrimitiveSTOP_Viper650());
747
748 CatchPrint();
749 if (TryStt < 0) {
750 vpERROR_TRACE("Cannot stop robot motion");
751 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
752 }
753}
754
765{
766 InitTry;
767
768 // Look if the power is on or off
769 UInt32 HIPowerStatus;
770 UInt32 EStopStatus;
771 bool firsttime = true;
772 unsigned int nitermax = 10;
773
774 for (unsigned int i = 0; i < nitermax; i++) {
775 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
776 if (EStopStatus == ESTOP_AUTO) {
777 m_controlMode = AUTO;
778 break; // exit for loop
779 } else if (EStopStatus == ESTOP_MANUAL) {
780 m_controlMode = MANUAL;
781 break; // exit for loop
782 } else if (EStopStatus == ESTOP_ACTIVATED) {
783 m_controlMode = ESTOP;
784 if (firsttime) {
785 std::cout << "Emergency stop is activated! \n"
786 << "Check the emergency stop button and push the yellow "
787 "button before continuing."
788 << std::endl;
789 firsttime = false;
790 }
791 fprintf(stdout, "Remaining time %us \r", nitermax - i);
792 fflush(stdout);
793 CAL_Wait(1);
794 } else {
795 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
796 std::cout << "You have to call Adept for maintenance..." << std::endl;
797 // Free allocated resources
798 ShutDownConnection();
799 throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
800 }
801 }
802
803 if (EStopStatus == ESTOP_ACTIVATED)
804 std::cout << std::endl;
805
806 if (EStopStatus == ESTOP_ACTIVATED) {
807 std::cout << "Sorry, cannot power on the robot." << std::endl;
808 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
809 }
810
811 if (HIPowerStatus == 0) {
812 fprintf(stdout, "Power ON the Viper650 robot\n");
813 fflush(stdout);
814
815 Try(PrimitivePOWERON_Viper650());
816 }
817
818 CatchPrint();
819 if (TryStt < 0) {
820 vpERROR_TRACE("Cannot power on the robot");
821 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
822 }
823}
824
835{
836 InitTry;
837
838 // Look if the power is on or off
839 UInt32 HIPowerStatus;
840 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
841 CAL_Wait(0.1);
842
843 if (HIPowerStatus == 1) {
844 fprintf(stdout, "Power OFF the Viper650 robot\n");
845 fflush(stdout);
846
847 Try(PrimitivePOWEROFF_Viper650());
848 }
849
850 CatchPrint();
851 if (TryStt < 0) {
852 vpERROR_TRACE("Cannot power off the robot");
853 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
854 }
855}
856
869{
870 InitTry;
871 bool status = false;
872 // Look if the power is on or off
873 UInt32 HIPowerStatus;
874 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
875 CAL_Wait(0.1);
876
877 if (HIPowerStatus == 1) {
878 status = true;
879 }
880
881 CatchPrint();
882 if (TryStt < 0) {
883 vpERROR_TRACE("Cannot get the power status");
884 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
885 }
886 return status;
887}
888
899{
902
903 cVe.buildFrom(cMe);
904}
905
918
931{
932
933 double position[6];
934 double timestamp;
935
936 InitTry;
937 Try(PrimitiveACQ_POS_J_Viper650(position, &timestamp));
938 CatchPrint();
939
940 vpColVector q(6);
941 for (unsigned int i = 0; i < njoint; i++)
942 q[i] = vpMath::rad(position[i]);
943
944 try {
946 } catch (...) {
947 vpERROR_TRACE("catch exception ");
948 throw;
949 }
950}
964{
965
966 double position[6];
967 double timestamp;
968
969 InitTry;
970 Try(PrimitiveACQ_POS_Viper650(position, &timestamp));
971 CatchPrint();
972
973 vpColVector q(6);
974 for (unsigned int i = 0; i < njoint; i++)
975 q[i] = position[i];
976
977 try {
979 } catch (...) {
980 vpERROR_TRACE("Error caught");
981 throw;
982 }
983}
984
1021void vpRobotViper650::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1022
1028double vpRobotViper650::getPositioningVelocity(void) const { return m_positioningVelocity; }
1029
1108{
1109
1111 vpERROR_TRACE("Robot was not in position-based control\n"
1112 "Modification of the robot state");
1114 }
1115
1116 vpColVector destination(njoint);
1117 int error = 0;
1118 double timestamp;
1119
1120 InitTry;
1121 switch (frame) {
1122 case vpRobot::CAMERA_FRAME: {
1124 Try(PrimitiveACQ_POS_Viper650(q.data, &timestamp));
1125
1126 // Convert degrees into rad
1127 q.deg2rad();
1128
1129 // Get fMc from the inverse kinematics
1131 vpViper650::get_fMc(q, fMc);
1132
1133 // Set cMc from the input position
1135 vpRxyzVector rxyz;
1136 for (unsigned int i = 0; i < 3; i++) {
1137 txyz[i] = position[i];
1138 rxyz[i] = position[i + 3];
1139 }
1140
1141 // Compute cMc2
1142 vpRotationMatrix cRc2(rxyz);
1143 vpHomogeneousMatrix cMc2(txyz, cRc2);
1144
1145 // Compute the new position to reach: fMc*cMc2
1146 vpHomogeneousMatrix fMc2 = fMc * cMc2;
1147
1148 // Compute the corresponding joint position from the inverse kinematics
1149 unsigned int solution = this->getInverseKinematics(fMc2, q);
1150 if (solution) { // Position is reachable
1151 destination = q;
1152 // convert rad to deg requested for the low level controller
1153 destination.rad2deg();
1154 Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1155 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1156 } else {
1157 // Cartesian position is out of range
1158 error = -1;
1159 }
1160
1161 break;
1162 }
1164 destination = position;
1165 // convert rad to deg requested for the low level controller
1166 destination.rad2deg();
1167
1168 // std::cout << "Joint destination (deg): " << destination.t() <<
1169 // std::endl;
1170 Try(PrimitiveMOVE_J_Viper650(destination.data, m_positioningVelocity));
1171 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1172 break;
1173 }
1175 // Convert angles from Rxyz representation to Rzyz representation
1176 vpRxyzVector rxyz(position[3], position[4], position[5]);
1177 vpRotationMatrix R(rxyz);
1178 vpRzyzVector rzyz(R);
1179
1180 for (unsigned int i = 0; i < 3; i++) {
1181 destination[i] = position[i];
1182 destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1183 }
1184 int configuration = 0; // keep the actual configuration
1185
1186 // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1187 // << std::endl;
1188 Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, m_positioningVelocity));
1189 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1190
1191 break;
1192 }
1193 case vpRobot::MIXT_FRAME: {
1194 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1195 "Mixt frame not implemented.");
1196 }
1198 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1199 "Mixt frame not implemented.");
1200 }
1201 }
1202
1203 CatchPrint();
1204 if (TryStt == InvalidPosition || TryStt == -1023)
1205 std::cout << " : Position out of range.\n";
1206
1207 else if (TryStt == -3019) {
1208 if (frame == vpRobot::ARTICULAR_FRAME)
1209 std::cout << " : Joint position out of range.\n";
1210 else
1211 std::cout << " : Cartesian position leads to a joint position out of "
1212 "range.\n";
1213 } else if (TryStt < 0)
1214 std::cout << " : Unknown error (see Fabien).\n";
1215 else if (error == -1)
1216 std::cout << "Position out of range.\n";
1217
1218 if (TryStt < 0 || error < 0) {
1219 vpERROR_TRACE("Positioning error.");
1220 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1221 }
1222
1223 return;
1224}
1225
1290void vpRobotViper650::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1291 double pos4, double pos5, double pos6)
1292{
1293 try {
1294 vpColVector position(6);
1295 position[0] = pos1;
1296 position[1] = pos2;
1297 position[2] = pos3;
1298 position[3] = pos4;
1299 position[4] = pos5;
1300 position[5] = pos6;
1301
1302 setPosition(frame, position);
1303 } catch (...) {
1304 vpERROR_TRACE("Error caught");
1305 throw;
1306 }
1307}
1308
1347void vpRobotViper650::setPosition(const std::string &filename)
1348{
1349 vpColVector q;
1350 bool ret;
1351
1352 ret = this->readPosFile(filename, q);
1353
1354 if (ret == false) {
1355 vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1356 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1357 }
1360}
1361
1429void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1430{
1431
1432 InitTry;
1433
1434 position.resize(6);
1435
1436 switch (frame) {
1437 case vpRobot::CAMERA_FRAME: {
1438 position = 0;
1439 return;
1440 }
1442 Try(PrimitiveACQ_POS_J_Viper650(position.data, &timestamp));
1443 // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1444 position.deg2rad();
1445
1446 return;
1447 }
1450 Try(PrimitiveACQ_POS_J_Viper650(q.data, &timestamp));
1451
1452 // Compute fMc
1454
1455 // From fMc extract the pose
1456 vpRotationMatrix fRc;
1457 fMc.extract(fRc);
1458 vpRxyzVector rxyz;
1459 rxyz.buildFrom(fRc);
1460
1461 for (unsigned int i = 0; i < 3; i++) {
1462 position[i] = fMc[i][3]; // translation x,y,z
1463 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1464 }
1465
1466 break;
1467 }
1468 case vpRobot::MIXT_FRAME: {
1469 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1470 "not implemented");
1471 }
1473 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1474 "not implemented");
1475 }
1476 }
1477
1478 CatchPrint();
1479 if (TryStt < 0) {
1480 vpERROR_TRACE("Cannot get position.");
1481 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1482 }
1483
1484 return;
1485}
1486
1498{
1499 double timestamp;
1500 getPosition(frame, position, timestamp);
1501}
1502
1516void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1517{
1518 vpColVector posRxyz;
1519 // recupere position en Rxyz
1520 this->getPosition(frame, posRxyz, timestamp);
1521 vpRxyzVector RxyzVect;
1522 for (unsigned int j = 0; j < 3; j++)
1523 RxyzVect[j] = posRxyz[j + 3];
1524 // recupere le vecteur thetaU correspondant
1525 vpThetaUVector RtuVect(RxyzVect);
1526
1527 // remplit le vpPoseVector avec translation et rotation ThetaU
1528 for (unsigned int j = 0; j < 3; j++) {
1529 position[j] = posRxyz[j];
1530 position[j + 3] = RtuVect[j];
1531 }
1532}
1533
1545{
1546 double timestamp;
1547 getPosition(frame, position, timestamp);
1548}
1549
1555{
1556 double timestamp;
1557 PrimitiveACQ_TIME_Viper650(&timestamp);
1558 return timestamp;
1559}
1560
1639{
1641 vpERROR_TRACE("Cannot send a velocity to the robot "
1642 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1644 "Cannot send a velocity to the robot "
1645 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1646 }
1647
1648 vpColVector vel_sat(6);
1649
1650 // Velocity saturation
1651 switch (frame) {
1652 // saturation in cartesian space
1656 case vpRobot::MIXT_FRAME: {
1657 vpColVector vel_max(6);
1658
1659 for (unsigned int i = 0; i < 3; i++)
1660 vel_max[i] = getMaxTranslationVelocity();
1661 for (unsigned int i = 3; i < 6; i++)
1662 vel_max[i] = getMaxRotationVelocity();
1663
1664 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1665
1666 break;
1667 }
1668 // saturation in joint space
1670 vpColVector vel_max(6);
1671
1672 // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1673 if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1674 for (unsigned int i = 0; i < 6; i++)
1675 vel_max[i] = getMaxRotationVelocity();
1676 } else {
1677 for (unsigned int i = 0; i < 5; i++)
1678 vel_max[i] = getMaxRotationVelocity();
1679 vel_max[5] = getMaxRotationVelocityJoint6();
1680 }
1681
1682 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1683 }
1684 }
1685
1686 InitTry;
1687
1688 switch (frame) {
1689 case vpRobot::CAMERA_FRAME: {
1690 // Send velocities in m/s and rad/s
1691 // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1692 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
1693 break;
1694 }
1696 // Transform in camera frame
1698 this->get_cMe(cMe);
1699 vpVelocityTwistMatrix cVe(cMe);
1700 vpColVector v_c = cVe * vel_sat;
1701 // Send velocities in m/s and rad/s
1702 Try(PrimitiveMOVESPEED_CART_Viper650(v_c.data, REPCAM_VIPER650));
1703 break;
1704 }
1706 // Convert all the velocities from rad/s into deg/s
1707 vel_sat.rad2deg();
1708 // std::cout << "Vitesse appliquee: " << vel_sat.t();
1709 // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
1710 Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
1711 break;
1712 }
1714 // Send velocities in m/s and rad/s
1715 // std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1716 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
1717 break;
1718 }
1719 case vpRobot::MIXT_FRAME: {
1720 // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
1721 break;
1722 }
1723 default: {
1724 vpERROR_TRACE("Error in spec of vpRobot. "
1725 "Case not taken in account.");
1726 return;
1727 }
1728 }
1729
1730 Catch();
1731 if (TryStt < 0) {
1732 if (TryStt == VelStopOnJoint) {
1733 UInt32 axisInJoint[njoint];
1734 PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1735 for (unsigned int i = 0; i < njoint; i++) {
1736 if (axisInJoint[i])
1737 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1738 }
1739 } else {
1740 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1741 if (TryString != NULL) {
1742 // The statement is in TryString, but we need to check the validity
1743 printf(" Error sentence %s\n", TryString); // Print the TryString
1744 } else {
1745 printf("\n");
1746 }
1747 }
1748 }
1749
1750 return;
1751}
1752
1753/* ------------------------------------------------------------------------ */
1754/* --- GET ---------------------------------------------------------------- */
1755/* ------------------------------------------------------------------------ */
1756
1813void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1814{
1815 velocity.resize(6);
1816 velocity = 0;
1817
1818 vpColVector q_cur(6);
1819 vpHomogeneousMatrix fMe_cur, fMc_cur;
1820 vpHomogeneousMatrix eMe, cMc; // camera displacement
1821 double time_cur;
1822
1823 InitTry;
1824
1825 // Get the current joint position
1826 Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, &timestamp));
1827 time_cur = timestamp;
1828 q_cur.deg2rad();
1829
1830 // Get the end-effector pose from the direct kinematics
1831 vpViper650::get_fMe(q_cur, fMe_cur);
1832 // Get the camera pose from the direct kinematics
1833 vpViper650::get_fMc(q_cur, fMc_cur);
1834
1835 if (!m_first_time_getvel) {
1836
1837 switch (frame) {
1839 // Compute the displacement of the end-effector since the previous call
1840 eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1841
1842 // Compute the velocity of the end-effector from this displacement
1843 velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1844
1845 break;
1846 }
1847
1848 case vpRobot::CAMERA_FRAME: {
1849 // Compute the displacement of the camera since the previous call
1850 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1851
1852 // Compute the velocity of the camera from this displacement
1853 velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1854
1855 break;
1856 }
1857
1859 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1860 break;
1861 }
1862
1864 // Compute the displacement of the camera since the previous call
1865 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1866
1867 // Compute the velocity of the camera from this displacement
1868 vpColVector v;
1869 v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1870
1871 // Express this velocity in the reference frame
1872 vpVelocityTwistMatrix fVc(fMc_cur);
1873 velocity = fVc * v;
1874
1875 break;
1876 }
1877
1878 case vpRobot::MIXT_FRAME: {
1879 // Compute the displacement of the camera since the previous call
1880 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1881
1882 // Compute the ThetaU representation for the rotation
1883 vpRotationMatrix cRc;
1884 cMc.extract(cRc);
1885 vpThetaUVector thetaU;
1886 thetaU.buildFrom(cRc);
1887
1888 for (unsigned int i = 0; i < 3; i++) {
1889 // Compute the translation displacement in the reference frame
1890 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1891 // Update the rotation displacement in the camera frame
1892 velocity[i + 3] = thetaU[i];
1893 }
1894
1895 // Compute the velocity
1896 velocity /= (time_cur - m_time_prev_getvel);
1897 break;
1898 }
1899 default: {
1901 "vpRobotViper650::getVelocity() not implemented in end-effector"));
1902 }
1903 }
1904 } else {
1905 m_first_time_getvel = false;
1906 }
1907
1908 // Memorize the end-effector pose for the next call
1909 m_fMe_prev_getvel = fMe_cur;
1910 // Memorize the camera pose for the next call
1911 m_fMc_prev_getvel = fMc_cur;
1912
1913 // Memorize the joint position for the next call
1914 m_q_prev_getvel = q_cur;
1915
1916 // Memorize the time associated to the joint position for the next call
1917 m_time_prev_getvel = time_cur;
1918
1919 CatchPrint();
1920 if (TryStt < 0) {
1921 vpERROR_TRACE("Cannot get velocity.");
1922 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1923 }
1924}
1925
1935{
1936 double timestamp;
1937 getVelocity(frame, velocity, timestamp);
1938}
1939
1990{
1991 vpColVector velocity;
1992 getVelocity(frame, velocity, timestamp);
1993
1994 return velocity;
1995}
1996
2006{
2007 vpColVector velocity;
2008 double timestamp;
2009 getVelocity(frame, velocity, timestamp);
2010
2011 return velocity;
2012}
2013
2079bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
2080{
2081 std::ifstream fd(filename.c_str(), std::ios::in);
2082
2083 if (!fd.is_open()) {
2084 return false;
2085 }
2086
2087 std::string line;
2088 std::string key("R:");
2089 std::string id("#Viper650 - Position");
2090 bool pos_found = false;
2091 int lineNum = 0;
2092
2093 q.resize(njoint);
2094
2095 while (std::getline(fd, line)) {
2096 lineNum++;
2097 if (lineNum == 1) {
2098 if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
2099 std::cout << "Error: this position file " << filename << " is not for Viper650 robot" << std::endl;
2100 return false;
2101 }
2102 }
2103 if ((line.compare(0, 1, "#") == 0)) { // skip comment
2104 continue;
2105 }
2106 if ((line.compare(0, key.size(), key) == 0)) { // decode position
2107 // check if there are at least njoint values in the line
2108 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2109 if (chain.size() < njoint + 1) // try to split with tab separator
2110 chain = vpIoTools::splitChain(line, std::string("\t"));
2111 if (chain.size() < njoint + 1)
2112 continue;
2113
2114 std::istringstream ss(line);
2115 std::string key_;
2116 ss >> key_;
2117 for (unsigned int i = 0; i < njoint; i++)
2118 ss >> q[i];
2119 pos_found = true;
2120 break;
2121 }
2122 }
2123
2124 // converts rotations from degrees into radians
2125 q.deg2rad();
2126
2127 fd.close();
2128
2129 if (!pos_found) {
2130 std::cout << "Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2131 return false;
2132 }
2133
2134 return true;
2135}
2136
2160bool vpRobotViper650::savePosFile(const std::string &filename, const vpColVector &q)
2161{
2162
2163 FILE *fd;
2164 fd = fopen(filename.c_str(), "w");
2165 if (fd == NULL)
2166 return false;
2167
2168 fprintf(fd, "\
2169#Viper650 - Position - Version 1.00\n\
2170#\n\
2171# R: A B C D E F\n\
2172# Joint position in degrees\n\
2173#\n\
2174#\n\n");
2175
2176 // Save positions in mm and deg
2177 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2178 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2179
2180 fclose(fd);
2181 return (true);
2182}
2183
2194void vpRobotViper650::move(const std::string &filename)
2195{
2196 vpColVector q;
2197
2198 try {
2199 this->readPosFile(filename, q);
2201 this->setPositioningVelocity(10);
2203 } catch (...) {
2204 throw;
2205 }
2206}
2207
2227{
2228 displacement.resize(6);
2229 displacement = 0;
2230
2231 double q[6];
2232 vpColVector q_cur(6);
2233 double timestamp;
2234
2235 InitTry;
2236
2237 // Get the current joint position
2238 Try(PrimitiveACQ_POS_Viper650(q, &timestamp));
2239 for (unsigned int i = 0; i < njoint; i++) {
2240 q_cur[i] = q[i];
2241 }
2242
2243 if (!m_first_time_getdis) {
2244 switch (frame) {
2245 case vpRobot::CAMERA_FRAME: {
2246 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2247 return;
2248 }
2249
2251 displacement = q_cur - m_q_prev_getdis;
2252 break;
2253 }
2254
2256 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2257 return;
2258 }
2259
2260 case vpRobot::MIXT_FRAME: {
2261 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2262 return;
2263 }
2265 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2266 return;
2267 }
2268 }
2269 } else {
2270 m_first_time_getdis = false;
2271 }
2272
2273 // Memorize the joint position for the next call
2274 m_q_prev_getdis = q_cur;
2275
2276 CatchPrint();
2277 if (TryStt < 0) {
2278 vpERROR_TRACE("Cannot get velocity.");
2279 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2280 }
2281}
2282
2297{
2298 InitTry;
2299
2300 Try(PrimitiveTFS_BIAS_Viper650());
2301
2302 // Wait 500 ms to be sure the next measures take into account the bias
2303 vpTime::wait(500);
2304
2305 CatchPrint();
2306 if (TryStt < 0) {
2307 vpERROR_TRACE("Cannot bias the force/torque sensor.");
2308 throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2309 }
2310}
2311
2352{
2353 InitTry;
2354
2355 H.resize(6);
2356
2357 Try(PrimitiveTFS_ACQ_Viper650(H.data));
2358
2359 CatchPrint();
2360 if (TryStt < 0) {
2361 vpERROR_TRACE("Cannot get the force/torque measures.");
2362 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2363 }
2364}
2365
2404{
2405 InitTry;
2406
2407 vpColVector H(6);
2408
2409 Try(PrimitiveTFS_ACQ_Viper650(H.data));
2410
2411 return H;
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 return H; // Here to avoid a warning, but should never be called
2419}
2420
2427{
2428 InitTry;
2429 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2430 std::cout << "Enable joint limits on axis 6..." << std::endl;
2431 CatchPrint();
2432 if (TryStt < 0) {
2433 vpERROR_TRACE("Cannot enable joint limits on axis 6");
2434 throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2435 }
2436}
2437
2449{
2450 InitTry;
2451 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2452 std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2453 CatchPrint();
2454 if (TryStt < 0) {
2455 vpERROR_TRACE("Cannot disable joint limits on axis 6");
2456 throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2457 }
2458}
2459
2469{
2472
2473 return;
2474}
2475
2496{
2497 m_maxRotationVelocity_joint6 = w6_max;
2498 return;
2499}
2500
2508double vpRobotViper650::getMaxRotationVelocityJoint6() const { return m_maxRotationVelocity_joint6; }
2509
2517{
2518 InitTry;
2519 Try(PrimitivePneumaticGripper_Viper650(1));
2520 std::cout << "Open the pneumatic gripper..." << std::endl;
2521 CatchPrint();
2522 if (TryStt < 0) {
2523 throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2524 }
2525}
2526
2535{
2536 InitTry;
2537 Try(PrimitivePneumaticGripper_Viper650(0));
2538 std::cout << "Close the pneumatic gripper..." << std::endl;
2539 CatchPrint();
2540 if (TryStt < 0) {
2541 throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2542 }
2543}
2544
2545#elif !defined(VISP_BUILD_SHARED_LIBS)
2546// Work around to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
2547// no symbols
2548void dummy_vpRobotViper650(){};
2549#endif
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
Implementation of column vector and the associated operations.
vpColVector & rad2deg()
vpColVector & deg2rad()
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition vpException.h:78
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)
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of a pose vector and operations on poses.
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
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:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition vpRobot.h:102
void setVerbose(bool verbose)
Definition vpRobot.h:157
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition vpRobot.h:106
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
double maxRotationVelocity
Definition vpRobot.h:96
void setMaxRotationVelocity(double maxVr)
Definition vpRobot.cpp:257
bool verbose_
Definition vpRobot.h:114
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
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:100
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpViper650.h:173
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper650.h:133
void init(void)
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper650.h:124
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
Definition vpViper.cpp:1212
vpTranslationVector etc
Definition vpViper.h:156
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpViper.cpp:904
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpViper.cpp:1141
vpColVector joint_max
Definition vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition vpViper.cpp:555
vpRxyzVector erc
Definition vpViper.h:157
vpColVector joint_min
Definition vpViper.h:170
static const unsigned int njoint
Number of joint.
Definition vpViper.h:151
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpViper.cpp:952
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition vpViper.cpp:707
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpViper.cpp:592
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)