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