Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotViper850.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 S850 robot.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_VIPER850
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/vpRobotViper850.h>
53
54/* ---------------------------------------------------------------------- */
55/* --- STATIC ----------------------------------------------------------- */
56/* ---------------------------------------------------------------------- */
57
58bool vpRobotViper850::m_robotAlreadyCreated = false;
59
68
69/* ---------------------------------------------------------------------- */
70/* --- EMERGENCY STOP --------------------------------------------------- */
71/* ---------------------------------------------------------------------- */
72
80void emergencyStopViper850(int signo)
81{
82 std::cout << "Stop the Viper850 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_Viper850();
104 PrimitiveSTOP_Viper850();
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
173vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
174{
175
176 /*
177 #define SIGHUP 1 // hangup
178 #define SIGINT 2 // interrupt (rubout)
179 #define SIGQUIT 3 // quit (ASCII FS)
180 #define SIGILL 4 // illegal instruction (not reset when caught)
181 #define SIGTRAP 5 // trace trap (not reset when caught)
182 #define SIGIOT 6 // IOT instruction
183 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
184 #define SIGEMT 7 // EMT instruction
185 #define SIGFPE 8 // floating point exception
186 #define SIGKILL 9 // kill (cannot be caught or ignored)
187 #define SIGBUS 10 // bus error
188 #define SIGSEGV 11 // segmentation violation
189 #define SIGSYS 12 // bad argument to system call
190 #define SIGPIPE 13 // write on a pipe with no one to read it
191 #define SIGALRM 14 // alarm clock
192 #define SIGTERM 15 // software termination signal from kill
193 */
194
195 signal(SIGINT, emergencyStopViper850);
196 signal(SIGBUS, emergencyStopViper850);
197 signal(SIGSEGV, emergencyStopViper850);
198 signal(SIGKILL, emergencyStopViper850);
199 signal(SIGQUIT, emergencyStopViper850);
200
201 setVerbose(verbose);
202 if (verbose_)
203 std::cout << "Open communication with MotionBlox.\n";
204 try {
205 this->init();
207 } catch (...) {
208 // vpERROR_TRACE("Error caught") ;
209 throw;
210 }
211 m_positioningVelocity = m_defaultPositioningVelocity;
212
213 maxRotationVelocity_joint6 = maxRotationVelocity;
214
215 vpRobotViper850::m_robotAlreadyCreated = true;
216
217 return;
218}
219
220/* ------------------------------------------------------------------------ */
221/* --- INITIALISATION ----------------------------------------------------- */
222/* ------------------------------------------------------------------------ */
223
249{
250 InitTry;
251
252 // Initialise private variables used to compute the measured velocities
253 m_q_prev_getvel.resize(6);
254 m_q_prev_getvel = 0;
255 m_time_prev_getvel = 0;
256 m_first_time_getvel = true;
257
258 // Initialise private variables used to compute the measured displacement
259 m_q_prev_getdis.resize(6);
260 m_q_prev_getdis = 0;
261 m_first_time_getdis = true;
262
263#if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
264 std::string calibfile;
265#ifdef VISP_HAVE_VIPER850_DATA
266 calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
267 if (!vpIoTools::checkFilename(calibfile))
268 throw(vpException(vpException::ioError, "ATI F/T calib file \"%s\" doesn't exist", calibfile.c_str()));
269#else
270 throw(vpException(vpException::ioError, "You don't have access to Viper850 "
271 "data to retrieve ATI F/T calib "
272 "file"));
273#endif
274 m_ati.setCalibrationFile(calibfile);
275 m_ati.open();
276#endif
277
278 // Initialize the firewire connection
279 Try(InitializeConnection(verbose_));
280
281 // Connect to the servoboard using the servo board GUID
282 Try(InitializeNode_Viper850());
283
284 Try(PrimitiveRESET_Viper850());
285
286 // Enable the joint limits on axis 6
287 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
288
289 // Update the eMc matrix in the low level controller
291
292 // Look if the power is on or off
293 UInt32 HIPowerStatus;
294 UInt32 EStopStatus;
295 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
296 CAL_Wait(0.1);
297
298 // Print the robot status
299 if (verbose_) {
300 std::cout << "Robot status: ";
301 switch (EStopStatus) {
302 case ESTOP_AUTO:
303 m_controlMode = AUTO;
304 if (HIPowerStatus == 0)
305 std::cout << "Power is OFF" << std::endl;
306 else
307 std::cout << "Power is ON" << std::endl;
308 break;
309
310 case ESTOP_MANUAL:
311 m_controlMode = MANUAL;
312 if (HIPowerStatus == 0)
313 std::cout << "Power is OFF" << std::endl;
314 else
315 std::cout << "Power is ON" << std::endl;
316 break;
317 case ESTOP_ACTIVATED:
318 m_controlMode = ESTOP;
319 std::cout << "Emergency stop is activated" << std::endl;
320 break;
321 default:
322 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
323 std::cout << "You have to call Adept for maintenance..." << std::endl;
324 // Free allocated resources
325 }
326 std::cout << std::endl;
327 }
328 // get real joint min/max from the MotionBlox
329 Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
330 // Convert units from degrees to radians
333
334 // for (unsigned int i=0; i < njoint; i++) {
335 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
336 // joint_max[i]);
337 // }
338
339 // If an error occur in the low level controller, goto here
340 // CatchPrint();
341 Catch();
342
343 // Test if an error occurs
344 if (TryStt == -20001)
345 printf("No connection detected. Check if the robot is powered on \n"
346 "and if the firewire link exist between the MotionBlox and this "
347 "computer.\n");
348 else if (TryStt == -675)
349 printf(" Timeout enabling power...\n");
350
351 if (TryStt < 0) {
352 // Power off the robot
353 PrimitivePOWEROFF_Viper850();
354 // Free allocated resources
355 ShutDownConnection();
356
357 std::cout << "Cannot open connection with the motionblox..." << std::endl;
358 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
359 }
360 return;
361}
362
420{
421 // Read the robot constants from files
422 // - joint [min,max], coupl_56, long_56
423 // - camera extrinsic parameters relative to eMc
425
426 InitTry;
427
428 // get real joint min/max from the MotionBlox
429 Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
430 // Convert units from degrees to radians
433
434 // for (unsigned int i=0; i < njoint; i++) {
435 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
436 // joint_max[i]);
437 // }
438
439 // Set the camera constant (eMc pose) in the MotionBlox
440 double eMc_pose[6];
441 for (unsigned int i = 0; i < 3; i++) {
442 eMc_pose[i] = etc[i]; // translation in meters
443 eMc_pose[i + 3] = erc[i]; // rotation in rad
444 }
445 // Update the eMc pose in the low level controller
446 Try(PrimitiveCONST_Viper850(eMc_pose));
447
448 CatchPrint();
449 return;
450}
451
502void vpRobotViper850::init(vpViper850::vpToolType tool, const std::string &filename)
503{
504 vpViper850::init(tool, filename);
505
506 InitTry;
507
508 // Get real joint min/max from the MotionBlox
509 Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
510 // Convert units from degrees to radians
513
514 // for (unsigned int i=0; i < njoint; i++) {
515 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
516 // joint_max[i]);
517 // }
518
519 // Set the camera constant (eMc pose) in the MotionBlox
520 double eMc_pose[6];
521 for (unsigned int i = 0; i < 3; i++) {
522 eMc_pose[i] = etc[i]; // translation in meters
523 eMc_pose[i + 3] = erc[i]; // rotation in rad
524 }
525 // Update the eMc pose in the low level controller
526 Try(PrimitiveCONST_Viper850(eMc_pose));
527
528 CatchPrint();
529 return;
530}
531
569{
570 vpViper850::init(tool, eMc_);
571
572 InitTry;
573
574 // Get real joint min/max from the MotionBlox
575 Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
576 // Convert units from degrees to radians
579
580 // for (unsigned int i=0; i < njoint; i++) {
581 // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
582 // joint_max[i]);
583 // }
584
585 // Set the camera constant (eMc pose) in the MotionBlox
586 double eMc_pose[6];
587 for (unsigned int i = 0; i < 3; i++) {
588 eMc_pose[i] = etc[i]; // translation in meters
589 eMc_pose[i + 3] = erc[i]; // rotation in rad
590 }
591 // Update the eMc pose in the low level controller
592 Try(PrimitiveCONST_Viper850(eMc_pose));
593
594 CatchPrint();
595 return;
596}
597
610{
611 this->vpViper850::set_eMc(eMc_);
612
613 InitTry;
614
615 // Set the camera constant (eMc pose) in the MotionBlox
616 double eMc_pose[6];
617 for (unsigned int i = 0; i < 3; i++) {
618 eMc_pose[i] = etc[i]; // translation in meters
619 eMc_pose[i + 3] = erc[i]; // rotation in rad
620 }
621 // Update the eMc pose in the low level controller
622 Try(PrimitiveCONST_Viper850(eMc_pose));
623
624 CatchPrint();
625
626 return;
627}
628
642{
643 this->vpViper850::set_eMc(etc_, erc_);
644
645 InitTry;
646
647 // Set the camera constant (eMc pose) in the MotionBlox
648 double eMc_pose[6];
649 for (unsigned int i = 0; i < 3; i++) {
650 eMc_pose[i] = etc[i]; // translation in meters
651 eMc_pose[i + 3] = erc[i]; // rotation in rad
652 }
653 // Update the eMc pose in the low level controller
654 Try(PrimitiveCONST_Viper850(eMc_pose));
655
656 CatchPrint();
657
658 return;
659}
660
661/* ------------------------------------------------------------------------ */
662/* --- DESTRUCTOR --------------------------------------------------------- */
663/* ------------------------------------------------------------------------ */
664
672{
673#if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
674 m_ati.close();
675#endif
676
677 InitTry;
678
680
681 // Look if the power is on or off
682 UInt32 HIPowerStatus;
683 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
684 CAL_Wait(0.1);
685
686 // if (HIPowerStatus == 1) {
687 // fprintf(stdout, "Power OFF the robot\n");
688 // fflush(stdout);
689
690 // Try( PrimitivePOWEROFF_Viper850() );
691 // }
692
693 // Free allocated resources
694 ShutDownConnection();
695
696 vpRobotViper850::m_robotAlreadyCreated = false;
697
698 CatchPrint();
699 return;
700}
701
709{
710 InitTry;
711
712 switch (newState) {
713 case vpRobot::STATE_STOP: {
714 // Start primitive STOP only if the current state is Velocity
716 Try(PrimitiveSTOP_Viper850());
717 vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
718 }
719 break;
720 }
723 std::cout << "Change the control mode from velocity to position control.\n";
724 Try(PrimitiveSTOP_Viper850());
725 } else {
726 // std::cout << "Change the control mode from stop to position
727 // control.\n";
728 }
729 this->powerOn();
730 break;
731 }
734 std::cout << "Change the control mode from stop to velocity control.\n";
735 }
736 this->powerOn();
737 break;
738 }
739 default:
740 break;
741 }
742
743 CatchPrint();
744
745 return vpRobot::setRobotState(newState);
746}
747
748/* ------------------------------------------------------------------------ */
749/* --- STOP --------------------------------------------------------------- */
750/* ------------------------------------------------------------------------ */
751
760{
762 return;
763
764 InitTry;
765 Try(PrimitiveSTOP_Viper850());
767
768 CatchPrint();
769 if (TryStt < 0) {
770 vpERROR_TRACE("Cannot stop robot motion");
771 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
772 }
773}
774
785{
786 InitTry;
787
788 // Look if the power is on or off
789 UInt32 HIPowerStatus;
790 UInt32 EStopStatus;
791 bool firsttime = true;
792 unsigned int nitermax = 10;
793
794 for (unsigned int i = 0; i < nitermax; i++) {
795 Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
796 if (EStopStatus == ESTOP_AUTO) {
797 m_controlMode = AUTO;
798 break; // exit for loop
799 } else if (EStopStatus == ESTOP_MANUAL) {
800 m_controlMode = MANUAL;
801 break; // exit for loop
802 } else if (EStopStatus == ESTOP_ACTIVATED) {
803 m_controlMode = ESTOP;
804 if (firsttime) {
805 std::cout << "Emergency stop is activated! \n"
806 << "Check the emergency stop button and push the yellow "
807 "button before continuing."
808 << std::endl;
809 firsttime = false;
810 }
811 fprintf(stdout, "Remaining time %us \r", nitermax - i);
812 fflush(stdout);
813 CAL_Wait(1);
814 } else {
815 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
816 std::cout << "You have to call Adept for maintenance..." << std::endl;
817 // Free allocated resources
818 ShutDownConnection();
819 throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
820 }
821 }
822
823 if (EStopStatus == ESTOP_ACTIVATED)
824 std::cout << std::endl;
825
826 if (EStopStatus == ESTOP_ACTIVATED) {
827 std::cout << "Sorry, cannot power on the robot." << std::endl;
828 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
829 }
830
831 if (HIPowerStatus == 0) {
832 fprintf(stdout, "Power ON the Viper850 robot\n");
833 fflush(stdout);
834
835 Try(PrimitivePOWERON_Viper850());
836 }
837
838 CatchPrint();
839 if (TryStt < 0) {
840 vpERROR_TRACE("Cannot power on the robot");
841 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
842 }
843}
844
855{
856 InitTry;
857
858 // Look if the power is on or off
859 UInt32 HIPowerStatus;
860 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
861 CAL_Wait(0.1);
862
863 if (HIPowerStatus == 1) {
864 fprintf(stdout, "Power OFF the Viper850 robot\n");
865 fflush(stdout);
866
867 Try(PrimitivePOWEROFF_Viper850());
868 }
869
870 CatchPrint();
871 if (TryStt < 0) {
872 vpERROR_TRACE("Cannot power off the robot");
873 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
874 }
875}
876
889{
890 InitTry;
891 bool status = false;
892 // Look if the power is on or off
893 UInt32 HIPowerStatus;
894 Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
895 CAL_Wait(0.1);
896
897 if (HIPowerStatus == 1) {
898 status = true;
899 }
900
901 CatchPrint();
902 if (TryStt < 0) {
903 vpERROR_TRACE("Cannot get the power status");
904 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
905 }
906 return status;
907}
908
919{
922
923 cVe.buildFrom(cMe);
924}
925
938
951{
952
953 double position[6];
954 double timestamp;
955
956 InitTry;
957 Try(PrimitiveACQ_POS_J_Viper850(position, &timestamp));
958 CatchPrint();
959
960 vpColVector q(6);
961 for (unsigned int i = 0; i < njoint; i++)
962 q[i] = vpMath::rad(position[i]);
963
964 try {
966 } catch (...) {
967 vpERROR_TRACE("catch exception ");
968 throw;
969 }
970}
984{
985
986 double position[6];
987 double timestamp;
988
989 InitTry;
990 Try(PrimitiveACQ_POS_Viper850(position, &timestamp));
991 CatchPrint();
992
993 vpColVector q(6);
994 for (unsigned int i = 0; i < njoint; i++)
995 q[i] = position[i];
996
997 try {
999 } catch (...) {
1000 vpERROR_TRACE("Error caught");
1001 throw;
1002 }
1003}
1004
1041void vpRobotViper850::setPositioningVelocity(double velocity) { m_positioningVelocity = velocity; }
1042
1048double vpRobotViper850::getPositioningVelocity(void) const { return m_positioningVelocity; }
1049
1127{
1128
1130 vpERROR_TRACE("Robot was not in position-based control\n"
1131 "Modification of the robot state");
1133 }
1134
1135 vpColVector destination(njoint);
1136 int error = 0;
1137 double timestamp;
1138
1139 InitTry;
1140 switch (frame) {
1141 case vpRobot::CAMERA_FRAME: {
1143 Try(PrimitiveACQ_POS_Viper850(q.data, &timestamp));
1144
1145 // Convert degrees into rad
1146 q.deg2rad();
1147
1148 // Get fMc from the inverse kinematics
1150 vpViper850::get_fMc(q, fMc);
1151
1152 // Set cMc from the input position
1154 vpRxyzVector rxyz;
1155 for (unsigned int i = 0; i < 3; i++) {
1156 txyz[i] = position[i];
1157 rxyz[i] = position[i + 3];
1158 }
1159
1160 // Compute cMc2
1161 vpRotationMatrix cRc2(rxyz);
1162 vpHomogeneousMatrix cMc2(txyz, cRc2);
1163
1164 // Compute the new position to reach: fMc*cMc2
1165 vpHomogeneousMatrix fMc2 = fMc * cMc2;
1166
1167 // Compute the corresponding joint position from the inverse kinematics
1168 unsigned int solution = this->getInverseKinematics(fMc2, q);
1169 if (solution) { // Position is reachable
1170 destination = q;
1171 // convert rad to deg requested for the low level controller
1172 destination.rad2deg();
1173 Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1174 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1175 } else {
1176 // Cartesian position is out of range
1177 error = -1;
1178 }
1179
1180 break;
1181 }
1183 destination = position;
1184 // convert rad to deg requested for the low level controller
1185 destination.rad2deg();
1186
1187 // std::cout << "Joint destination (deg): " << destination.t() <<
1188 // std::endl;
1189 Try(PrimitiveMOVE_J_Viper850(destination.data, m_positioningVelocity));
1190 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1191 break;
1192 }
1194 // Convert angles from Rxyz representation to Rzyz representation
1195 vpRxyzVector rxyz(position[3], position[4], position[5]);
1196 vpRotationMatrix R(rxyz);
1197 vpRzyzVector rzyz(R);
1198
1199 for (unsigned int i = 0; i < 3; i++) {
1200 destination[i] = position[i];
1201 destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
1202 }
1203 int configuration = 0; // keep the actual configuration
1204
1205 // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
1206 // << std::endl;
1207 Try(PrimitiveMOVE_C_Viper850(destination.data, configuration, m_positioningVelocity));
1208 Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
1209
1210 break;
1211 }
1212 case vpRobot::MIXT_FRAME: {
1213 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1214 "Mixt frame not implemented.");
1215 }
1217 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1218 "End-effector frame not implemented.");
1219 }
1220 }
1221
1222 CatchPrint();
1223 if (TryStt == InvalidPosition || TryStt == -1023)
1224 std::cout << " : Position out of range.\n";
1225 else if (TryStt == -3019) {
1226 if (frame == vpRobot::ARTICULAR_FRAME)
1227 std::cout << " : Joint position out of range.\n";
1228 else
1229 std::cout << " : Cartesian position leads to a joint position out of "
1230 "range.\n";
1231 } else if (TryStt < 0)
1232 std::cout << " : Unknown error (see Fabien).\n";
1233 else if (error == -1)
1234 std::cout << "Position out of range.\n";
1235
1236 if (TryStt < 0 || error < 0) {
1237 vpERROR_TRACE("Positioning error.");
1238 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1239 }
1240
1241 return;
1242}
1243
1308void vpRobotViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1309 double pos4, double pos5, double pos6)
1310{
1311 try {
1312 vpColVector position(6);
1313 position[0] = pos1;
1314 position[1] = pos2;
1315 position[2] = pos3;
1316 position[3] = pos4;
1317 position[4] = pos5;
1318 position[5] = pos6;
1319
1320 setPosition(frame, position);
1321 } catch (...) {
1322 vpERROR_TRACE("Error caught");
1323 throw;
1324 }
1325}
1326
1365void vpRobotViper850::setPosition(const std::string &filename)
1366{
1367 vpColVector q;
1368 bool ret;
1369
1370 ret = this->readPosFile(filename, q);
1371
1372 if (ret == false) {
1373 vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1374 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1375 }
1378}
1379
1447void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1448{
1449
1450 InitTry;
1451
1452 position.resize(6);
1453
1454 switch (frame) {
1455 case vpRobot::CAMERA_FRAME: {
1456 position = 0;
1457 return;
1458 }
1460 Try(PrimitiveACQ_POS_J_Viper850(position.data, &timestamp));
1461 // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
1462 position.deg2rad();
1463
1464 return;
1465 }
1468 Try(PrimitiveACQ_POS_J_Viper850(q.data, &timestamp));
1469
1470 // Compute fMc
1472
1473 // From fMc extract the pose
1474 vpRotationMatrix fRc;
1475 fMc.extract(fRc);
1476 vpRxyzVector rxyz;
1477 rxyz.buildFrom(fRc);
1478
1479 for (unsigned int i = 0; i < 3; i++) {
1480 position[i] = fMc[i][3]; // translation x,y,z
1481 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1482 }
1483
1484 break;
1485 }
1486 case vpRobot::MIXT_FRAME: {
1487 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1488 "not implemented");
1489 }
1491 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1492 "not implemented");
1493 }
1494 }
1495
1496 CatchPrint();
1497 if (TryStt < 0) {
1498 vpERROR_TRACE("Cannot get position.");
1499 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1500 }
1501
1502 return;
1503}
1504
1510{
1511 double timestamp;
1512 PrimitiveACQ_TIME_Viper850(&timestamp);
1513 return timestamp;
1514}
1515
1527{
1528 double timestamp;
1529 getPosition(frame, position, timestamp);
1530}
1531
1543void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1544{
1545 vpColVector posRxyz;
1546 // recupere position en Rxyz
1547 this->getPosition(frame, posRxyz, timestamp);
1548 vpRxyzVector RxyzVect;
1549 for (unsigned int j = 0; j < 3; j++)
1550 RxyzVect[j] = posRxyz[j + 3];
1551 // recupere le vecteur thetaU correspondant
1552 vpThetaUVector RtuVect(RxyzVect);
1553
1554 // remplit le vpPoseVector avec translation et rotation ThetaU
1555 for (unsigned int j = 0; j < 3; j++) {
1556 position[j] = posRxyz[j];
1557 position[j + 3] = RtuVect[j];
1558 }
1559}
1560
1572{
1573 vpColVector posRxyz;
1574 double timestamp;
1575 // recupere position en Rxyz
1576 this->getPosition(frame, posRxyz, timestamp);
1577 vpRxyzVector RxyzVect;
1578 for (unsigned int j = 0; j < 3; j++)
1579 RxyzVect[j] = posRxyz[j + 3];
1580 // recupere le vecteur thetaU correspondant
1581 vpThetaUVector RtuVect(RxyzVect);
1582
1583 // remplit le vpPoseVector avec translation et rotation ThetaU
1584 for (unsigned int j = 0; j < 3; j++) {
1585 position[j] = posRxyz[j];
1586 position[j + 3] = RtuVect[j];
1587 }
1588}
1589
1668{
1670 vpERROR_TRACE("Cannot send a velocity to the robot "
1671 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1673 "Cannot send a velocity to the robot "
1674 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1675 }
1676
1677 vpColVector vel_sat(6);
1678
1679 // Velocity saturation
1680 switch (frame) {
1681 // saturation in cartesian space
1685 case vpRobot::MIXT_FRAME: {
1686 vpColVector vel_max(6);
1687
1688 for (unsigned int i = 0; i < 3; i++)
1689 vel_max[i] = getMaxTranslationVelocity();
1690 for (unsigned int i = 3; i < 6; i++)
1691 vel_max[i] = getMaxRotationVelocity();
1692
1693 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1694
1695 break;
1696 }
1697 // saturation in joint space
1699 vpColVector vel_max(6);
1700
1701 // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
1702 if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
1703
1704 for (unsigned int i = 0; i < 6; i++)
1705 vel_max[i] = getMaxRotationVelocity();
1706 } else {
1707 for (unsigned int i = 0; i < 5; i++)
1708 vel_max[i] = getMaxRotationVelocity();
1709 vel_max[5] = getMaxRotationVelocityJoint6();
1710 }
1711
1712 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1713 }
1714 }
1715
1716 InitTry;
1717
1718 switch (frame) {
1719 case vpRobot::CAMERA_FRAME: {
1720 // Send velocities in m/s and rad/s
1721 // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
1722 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
1723 break;
1724 }
1726 // Transform in camera frame
1728 this->get_cMe(cMe);
1729 vpVelocityTwistMatrix cVe(cMe);
1730 vpColVector v_c = cVe * vel_sat;
1731 // Send velocities in m/s and rad/s
1732 Try(PrimitiveMOVESPEED_CART_Viper850(v_c.data, REPCAM_VIPER850));
1733 break;
1734 }
1736 // Convert all the velocities from rad/s into deg/s
1737 vel_sat.rad2deg();
1738 // std::cout << "Vitesse appliquee: " << vel_sat.t();
1739 // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
1740 Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
1741 break;
1742 }
1744 // Send velocities in m/s and rad/s
1745 // std::cout << "Vitesse ref appliquee: " << vel_sat.t();
1746 Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
1747 break;
1748 }
1749 case vpRobot::MIXT_FRAME: {
1750 // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
1751 break;
1752 }
1753 default: {
1754 vpERROR_TRACE("Error in spec of vpRobot. "
1755 "Case not taken in account.");
1756 return;
1757 }
1758 }
1759
1760 Catch();
1761 if (TryStt < 0) {
1762 if (TryStt == VelStopOnJoint) {
1763 UInt32 axisInJoint[njoint];
1764 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1765 for (unsigned int i = 0; i < njoint; i++) {
1766 if (axisInJoint[i])
1767 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1768 }
1769 } else {
1770 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1771 if (TryString != NULL) {
1772 // The statement is in TryString, but we need to check the validity
1773 printf(" Error sentence %s\n", TryString); // Print the TryString
1774 } else {
1775 printf("\n");
1776 }
1777 }
1778 }
1779
1780 return;
1781}
1782
1783/* ------------------------------------------------------------------------ */
1784/* --- GET ---------------------------------------------------------------- */
1785/* ------------------------------------------------------------------------ */
1786
1843void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1844{
1845 velocity.resize(6);
1846 velocity = 0;
1847
1848 vpColVector q_cur(6);
1849 vpHomogeneousMatrix fMe_cur, fMc_cur;
1850 vpHomogeneousMatrix eMe, cMc; // camera displacement
1851 double time_cur;
1852
1853 InitTry;
1854
1855 // Get the current joint position
1856 Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, &timestamp));
1857 time_cur = timestamp;
1858 q_cur.deg2rad();
1859
1860 // Get the end-effector pose from the direct kinematics
1861 vpViper850::get_fMe(q_cur, fMe_cur);
1862 // Get the camera pose from the direct kinematics
1863 vpViper850::get_fMc(q_cur, fMc_cur);
1864
1865 if (!m_first_time_getvel) {
1866
1867 switch (frame) {
1869 // Compute the displacement of the end-effector since the previous call
1870 eMe = m_fMe_prev_getvel.inverse() * fMe_cur;
1871
1872 // Compute the velocity of the end-effector from this displacement
1873 velocity = vpExponentialMap::inverse(eMe, time_cur - m_time_prev_getvel);
1874
1875 break;
1876 }
1877
1878 case vpRobot::CAMERA_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 velocity of the camera from this displacement
1883 velocity = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1884
1885 break;
1886 }
1887
1889 velocity = (q_cur - m_q_prev_getvel) / (time_cur - m_time_prev_getvel);
1890 break;
1891 }
1892
1894 // Compute the displacement of the camera since the previous call
1895 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1896
1897 // Compute the velocity of the camera from this displacement
1898 vpColVector v;
1899 v = vpExponentialMap::inverse(cMc, time_cur - m_time_prev_getvel);
1900
1901 // Express this velocity in the reference frame
1902 vpVelocityTwistMatrix fVc(fMc_cur);
1903 velocity = fVc * v;
1904
1905 break;
1906 }
1907
1908 case vpRobot::MIXT_FRAME: {
1909 // Compute the displacement of the camera since the previous call
1910 cMc = m_fMc_prev_getvel.inverse() * fMc_cur;
1911
1912 // Compute the ThetaU representation for the rotation
1913 vpRotationMatrix cRc;
1914 cMc.extract(cRc);
1915 vpThetaUVector thetaU;
1916 thetaU.buildFrom(cRc);
1917
1918 for (unsigned int i = 0; i < 3; i++) {
1919 // Compute the translation displacement in the reference frame
1920 velocity[i] = m_fMc_prev_getvel[i][3] - fMc_cur[i][3];
1921 // Update the rotation displacement in the camera frame
1922 velocity[i + 3] = thetaU[i];
1923 }
1924
1925 // Compute the velocity
1926 velocity /= (time_cur - m_time_prev_getvel);
1927 break;
1928 }
1929 default: {
1931 "vpRobotViper850::getVelocity() not implemented in end-effector"));
1932 }
1933 }
1934 } else {
1935 m_first_time_getvel = false;
1936 }
1937
1938 // Memorize the end-effector pose for the next call
1939 m_fMe_prev_getvel = fMe_cur;
1940 // Memorize the camera pose for the next call
1941 m_fMc_prev_getvel = fMc_cur;
1942
1943 // Memorize the joint position for the next call
1944 m_q_prev_getvel = q_cur;
1945
1946 // Memorize the time associated to the joint position for the next call
1947 m_time_prev_getvel = time_cur;
1948
1949 CatchPrint();
1950 if (TryStt < 0) {
1951 vpERROR_TRACE("Cannot get velocity.");
1952 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1953 }
1954}
1955
1965{
1966 double timestamp;
1967 getVelocity(frame, velocity, timestamp);
1968}
1969
2020{
2021 vpColVector velocity;
2022 getVelocity(frame, velocity, timestamp);
2023
2024 return velocity;
2025}
2026
2036{
2037 vpColVector velocity;
2038 double timestamp;
2039 getVelocity(frame, velocity, timestamp);
2040
2041 return velocity;
2042}
2043
2109bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
2110{
2111 std::ifstream fd(filename.c_str(), std::ios::in);
2112
2113 if (!fd.is_open()) {
2114 return false;
2115 }
2116
2117 std::string line;
2118 std::string key("R:");
2119 std::string id("#Viper850 - Position");
2120 bool pos_found = false;
2121 int lineNum = 0;
2122
2123 q.resize(njoint);
2124
2125 while (std::getline(fd, line)) {
2126 lineNum++;
2127 if (lineNum == 1) {
2128 if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
2129 std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
2130 return false;
2131 }
2132 }
2133 if ((line.compare(0, 1, "#") == 0)) { // skip comment
2134 continue;
2135 }
2136 if ((line.compare(0, key.size(), key) == 0)) { // decode position
2137 // check if there are at least njoint values in the line
2138 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2139 if (chain.size() < njoint + 1) // try to split with tab separator
2140 chain = vpIoTools::splitChain(line, std::string("\t"));
2141 if (chain.size() < njoint + 1)
2142 continue;
2143
2144 std::istringstream ss(line);
2145 std::string key_;
2146 ss >> key_;
2147 for (unsigned int i = 0; i < njoint; i++)
2148 ss >> q[i];
2149 pos_found = true;
2150 break;
2151 }
2152 }
2153
2154 // converts rotations from degrees into radians
2155 q.deg2rad();
2156
2157 fd.close();
2158
2159 if (!pos_found) {
2160 std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2161 return false;
2162 }
2163
2164 return true;
2165}
2166
2190bool vpRobotViper850::savePosFile(const std::string &filename, const vpColVector &q)
2191{
2192
2193 FILE *fd;
2194 fd = fopen(filename.c_str(), "w");
2195 if (fd == NULL)
2196 return false;
2197
2198 fprintf(fd, "\
2199#Viper850 - Position - Version 1.00\n\
2200#\n\
2201# R: A B C D E F\n\
2202# Joint position in degrees\n\
2203#\n\
2204#\n\n");
2205
2206 // Save positions in mm and deg
2207 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2208 vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2209
2210 fclose(fd);
2211 return (true);
2212}
2213
2224void vpRobotViper850::move(const std::string &filename)
2225{
2226 vpColVector q;
2227
2228 try {
2229 this->readPosFile(filename, q);
2231 this->setPositioningVelocity(10);
2233 } catch (...) {
2234 throw;
2235 }
2236}
2237
2257{
2258 displacement.resize(6);
2259 displacement = 0;
2260
2261 double q[6];
2262 vpColVector q_cur(6);
2263 double timestamp;
2264
2265 InitTry;
2266
2267 // Get the current joint position
2268 Try(PrimitiveACQ_POS_Viper850(q, &timestamp));
2269 for (unsigned int i = 0; i < njoint; i++) {
2270 q_cur[i] = q[i];
2271 }
2272
2273 if (!m_first_time_getdis) {
2274 switch (frame) {
2275 case vpRobot::CAMERA_FRAME: {
2276 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
2277 return;
2278 }
2279
2281 displacement = q_cur - m_q_prev_getdis;
2282 break;
2283 }
2284
2286 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2287 return;
2288 }
2289
2290 case vpRobot::MIXT_FRAME: {
2291 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2292 return;
2293 }
2295 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2296 return;
2297 }
2298 }
2299 } else {
2300 m_first_time_getdis = false;
2301 }
2302
2303 // Memorize the joint position for the next call
2304 m_q_prev_getdis = q_cur;
2305
2306 CatchPrint();
2307 if (TryStt < 0) {
2308 vpERROR_TRACE("Cannot get velocity.");
2309 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2310 }
2311}
2312
2321{
2322#if defined(USE_ATI_DAQ)
2323#if defined(VISP_HAVE_COMEDI)
2324 m_ati.bias();
2325#else
2326 throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2327 "apt-get install libcomedi-dev"));
2328#endif
2329#else // Use serial link
2330 InitTry;
2331
2332 Try(PrimitiveTFS_BIAS_Viper850());
2333
2334 // Wait 500 ms to be sure the next measures take into account the bias
2335 vpTime::wait(500);
2336
2337 CatchPrint();
2338 if (TryStt < 0) {
2339 vpERROR_TRACE("Cannot bias the force/torque sensor.");
2340 throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
2341 }
2342#endif
2343}
2344
2353{
2354#if defined(USE_ATI_DAQ)
2355#if defined(VISP_HAVE_COMEDI)
2356 m_ati.unbias();
2357#else
2358 throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2359 "apt-get install libcomedi-dev"));
2360#endif
2361#else // Use serial link
2362// Not implemented
2363#endif
2364}
2365
2406{
2407#if defined(USE_ATI_DAQ)
2408#if defined(VISP_HAVE_COMEDI)
2409 H = m_ati.getForceTorque();
2410#else
2411 (void)H;
2412 throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2413 "apt-get install libcomedi-dev"));
2414#endif
2415#else // Use serial link
2416 InitTry;
2417
2418 H.resize(6);
2419
2420 Try(PrimitiveTFS_ACQ_Viper850(H.data));
2421
2422 CatchPrint();
2423 if (TryStt < 0) {
2424 vpERROR_TRACE("Cannot get the force/torque measures.");
2425 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2426 }
2427#endif
2428}
2429
2468{
2469#if defined(USE_ATI_DAQ)
2470#if defined(VISP_HAVE_COMEDI)
2471 vpColVector H = m_ati.getForceTorque();
2472 return H;
2473#else
2474 throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo "
2475 "apt-get install libcomedi-dev"));
2476#endif
2477#else // Use serial link
2478 InitTry;
2479
2480 vpColVector H(6);
2481
2482 Try(PrimitiveTFS_ACQ_Viper850(H.data));
2483 return H;
2484
2485 CatchPrint();
2486 if (TryStt < 0) {
2487 vpERROR_TRACE("Cannot get the force/torque measures.");
2488 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
2489 }
2490 return H; // Here to avoid a warning, but should never be called
2491#endif
2492}
2493
2501{
2502 InitTry;
2503 Try(PrimitivePneumaticGripper_Viper850(1));
2504 std::cout << "Open the pneumatic gripper..." << std::endl;
2505 CatchPrint();
2506 if (TryStt < 0) {
2507 throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2508 }
2509}
2510
2519{
2520 InitTry;
2521 Try(PrimitivePneumaticGripper_Viper850(0));
2522 std::cout << "Close the pneumatic gripper..." << std::endl;
2523 CatchPrint();
2524 if (TryStt < 0) {
2525 throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2526 }
2527}
2528
2535{
2536 InitTry;
2537 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
2538 std::cout << "Enable joint limits on axis 6..." << std::endl;
2539 CatchPrint();
2540 if (TryStt < 0) {
2541 vpERROR_TRACE("Cannot enable joint limits on axis 6");
2542 throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
2543 }
2544}
2545
2557{
2558 InitTry;
2559 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
2560 std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
2561 CatchPrint();
2562 if (TryStt < 0) {
2563 vpERROR_TRACE("Cannot disable joint limits on axis 6");
2564 throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
2565 }
2566}
2567
2577{
2580
2581 return;
2582}
2583
2605{
2606 maxRotationVelocity_joint6 = w6_max;
2607 return;
2608}
2609
2617double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
2618
2619#elif !defined(VISP_BUILD_SHARED_LIBS)
2620// Work around to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
2621// no symbols
2622void dummy_vpRobotViper850(){};
2623#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
@ ioError
I/O error.
Definition vpException.h:79
@ functionNotImplementedError
Function not implemented.
Definition vpException.h:78
@ fatalError
Fatal error.
Definition vpException.h:84
static vpColVector inverse(const vpHomogeneousMatrix &M)
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)
static bool checkFilename(const std::string &filename)
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.
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: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 850 robot.
Definition vpViper850.h:101
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper850.h:134
vpCameraParameters::vpCameraParametersProjType projModel
Definition vpViper850.h:174
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition vpViper850.h:125
void init(void)
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)