Visual Servoing Platform version 3.5.0
vpRobotAfma6.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 Afma6 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_AFMA6
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/vpRotationMatrix.h>
52#include <visp3/core/vpThetaUVector.h>
53#include <visp3/core/vpVelocityTwistMatrix.h>
54#include <visp3/robot/vpRobotAfma6.h>
55#include <visp3/robot/vpRobotException.h>
56
57/* ---------------------------------------------------------------------- */
58/* --- STATIC ----------------------------------------------------------- */
59/* ---------------------------------------------------------------------- */
60
61bool vpRobotAfma6::robotAlreadyCreated = false;
62
71
72/* ---------------------------------------------------------------------- */
73/* --- EMERGENCY STOP --------------------------------------------------- */
74/* ---------------------------------------------------------------------- */
75
83void emergencyStopAfma6(int signo)
84{
85 std::cout << "Stop the Afma6 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_Afma6();
107 PrimitiveSTOP_Afma6();
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
159vpRobotAfma6::vpRobotAfma6(bool verbose) : vpAfma6(), vpRobot()
160{
161
162 /*
163 #define SIGHUP 1 // hangup
164 #define SIGINT 2 // interrupt (rubout)
165 #define SIGQUIT 3 // quit (ASCII FS)
166 #define SIGILL 4 // illegal instruction (not reset when caught)
167 #define SIGTRAP 5 // trace trap (not reset when caught)
168 #define SIGIOT 6 // IOT instruction
169 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
170 #define SIGEMT 7 // EMT instruction
171 #define SIGFPE 8 // floating point exception
172 #define SIGKILL 9 // kill (cannot be caught or ignored)
173 #define SIGBUS 10 // bus error
174 #define SIGSEGV 11 // segmentation violation
175 #define SIGSYS 12 // bad argument to system call
176 #define SIGPIPE 13 // write on a pipe with no one to read it
177 #define SIGALRM 14 // alarm clock
178 #define SIGTERM 15 // software termination signal from kill
179 */
180
181 signal(SIGINT, emergencyStopAfma6);
182 signal(SIGBUS, emergencyStopAfma6);
183 signal(SIGSEGV, emergencyStopAfma6);
184 signal(SIGKILL, emergencyStopAfma6);
185 signal(SIGQUIT, emergencyStopAfma6);
186
187 setVerbose(verbose);
188 if (verbose_)
189 std::cout << "Open communication with MotionBlox.\n";
190 try {
191 this->init();
193 } catch (...) {
194 // vpERROR_TRACE("Error caught") ;
195 throw;
196 }
197 positioningVelocity = defaultPositioningVelocity;
198
199 vpRobotAfma6::robotAlreadyCreated = true;
200
201 return;
202}
203
204/* ------------------------------------------------------------------------ */
205/* --- INITIALISATION ----------------------------------------------------- */
206/* ------------------------------------------------------------------------ */
207
228{
229 InitTry;
230
231 // Initialise private variables used to compute the measured velocities
232 q_prev_getvel.resize(6);
233 q_prev_getvel = 0;
234 time_prev_getvel = 0;
235 first_time_getvel = true;
236
237 // Initialise private variables used to compute the measured displacement
238 q_prev_getdis.resize(6);
239 q_prev_getdis = 0;
240 first_time_getdis = true;
241
242 // Initialize the firewire connection
243 Try(InitializeConnection(verbose_));
244
245 // Connect to the servoboard using the servo board GUID
246 Try(InitializeNode_Afma6());
247
248 Try(PrimitiveRESET_Afma6());
249
250 // Update the eMc matrix in the low level controller
252
253 // Look if the power is on or off
254 UInt32 HIPowerStatus;
255 UInt32 EStopStatus;
256 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
257 CAL_Wait(0.1);
258
259 // Print the robot status
260 if (verbose_) {
261 std::cout << "Robot status: ";
262 switch (EStopStatus) {
263 case ESTOP_AUTO:
264 case ESTOP_MANUAL:
265 if (HIPowerStatus == 0)
266 std::cout << "Power is OFF" << std::endl;
267 else
268 std::cout << "Power is ON" << std::endl;
269 break;
270 case ESTOP_ACTIVATED:
271 std::cout << "Emergency stop is activated" << std::endl;
272 break;
273 default:
274 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
275 std::cout << "You have to call Adept for maintenance..." << std::endl;
276 // Free allocated resources
277 }
278 std::cout << std::endl;
279 }
280 // get real joint min/max from the MotionBlox
281 Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
282 // for (unsigned int i=0; i < njoint; i++) {
283 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
284 // _joint_max[i]);
285 // }
286
287 // If an error occur in the low level controller, goto here
288 // CatchPrint();
289 Catch();
290
291 // Test if an error occurs
292 if (TryStt == -20001)
293 printf("No connection detected. Check if the robot is powered on \n"
294 "and if the firewire link exist between the MotionBlox and this "
295 "computer.\n");
296 else if (TryStt == -675)
297 printf(" Timeout enabling power...\n");
298
299 if (TryStt < 0) {
300 // Power off the robot
301 PrimitivePOWEROFF_Afma6();
302 // Free allocated resources
303 ShutDownConnection();
304
305 std::cout << "Cannot open connection with the motionblox..." << std::endl;
306 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
307 }
308 return;
309}
310
348{
349 InitTry;
350 // Read the robot constants from files
351 // - joint [min,max], coupl_56, long_56
352 // - camera extrinsic parameters relative to eMc
354
355 // Set the robot constant (coupl_56, long_56) in the MotionBlox
356 Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
357
358 // Set the camera constant (eMc pose) in the MotionBlox
359 double eMc_pose[6];
360 for (unsigned int i = 0; i < 3; i++) {
361 eMc_pose[i] = _etc[i]; // translation in meters
362 eMc_pose[i + 3] = _erc[i]; // rotation in rad
363 }
364 // Update the eMc pose in the low level controller
365 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
366
367 // get real joint min/max from the MotionBlox
368 Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
369 // for (unsigned int i=0; i < njoint; i++) {
370 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
371 // _joint_max[i]);
372 // }
373
374 setToolType(tool);
375
376 CatchPrint();
377 return;
378}
379
392{
393 InitTry;
394 // Set camera extrinsic parameters equal to eMc
395 this->vpAfma6::set_eMc(eMc);
396
397 // Set the camera constant (eMc pose) in the MotionBlox
398 double eMc_pose[6];
399 for (unsigned int i = 0; i < 3; i++) {
400 eMc_pose[i] = _etc[i]; // translation in meters
401 eMc_pose[i + 3] = _erc[i]; // rotation in rad
402 }
403 // Update the eMc pose in the low level controller
404 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
405
406 CatchPrint();
407}
408
445{
446 InitTry;
447 // Read the robot constants from files
448 // - joint [min,max], coupl_56, long_56
449 // ans set camera extrinsic parameters equal to eMc
450 vpAfma6::init(tool, eMc);
451
452 // Set the robot constant (coupl_56, long_56) in the MotionBlox
453 Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
454
455 // Set the camera constant (eMc pose) in the MotionBlox
456 double eMc_pose[6];
457 for (unsigned int i = 0; i < 3; i++) {
458 eMc_pose[i] = _etc[i]; // translation in meters
459 eMc_pose[i + 3] = _erc[i]; // rotation in rad
460 }
461 // Update the eMc pose in the low level controller
462 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
463
464 // get real joint min/max from the MotionBlox
465 Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
466
467 setToolType(tool);
468
469 CatchPrint();
470}
471
522void vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
523{
524 InitTry;
525 // Read the robot constants from files
526 // - joint [min,max], coupl_56, long_56
527 // ans set camera extrinsic parameters from file name
528 vpAfma6::init(tool, filename);
529
530 // Set the robot constant (coupl_56, long_56) in the MotionBlox
531 Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
532
533 // Set the camera constant (eMc pose) in the MotionBlox
534 double eMc_pose[6];
535 for (unsigned int i = 0; i < 3; i++) {
536 eMc_pose[i] = _etc[i]; // translation in meters
537 eMc_pose[i + 3] = _erc[i]; // rotation in rad
538 }
539 // Update the eMc pose in the low level controller
540 Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
541
542 // get real joint min/max from the MotionBlox
543 Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
544
545 setToolType(tool);
546
547 CatchPrint();
548}
549
550/* ------------------------------------------------------------------------ */
551/* --- DESTRUCTOR --------------------------------------------------------- */
552/* ------------------------------------------------------------------------ */
553
561{
562 InitTry;
563
565
566 // Look if the power is on or off
567 UInt32 HIPowerStatus;
568 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
569 CAL_Wait(0.1);
570
571 // if (HIPowerStatus == 1) {
572 // fprintf(stdout, "Power OFF the robot\n");
573 // fflush(stdout);
574
575 // Try( PrimitivePOWEROFF_Afma6() );
576 // }
577
578 // Free allocated resources
579 ShutDownConnection();
580
581 vpRobotAfma6::robotAlreadyCreated = false;
582
583 CatchPrint();
584 return;
585}
586
594{
595 InitTry;
596
597 switch (newState) {
598 case vpRobot::STATE_STOP: {
600 Try(PrimitiveSTOP_Afma6());
601 }
602 break;
603 }
606 std::cout << "Change the control mode from velocity to position control.\n";
607 Try(PrimitiveSTOP_Afma6());
608 } else {
609 // std::cout << "Change the control mode from stop to position
610 // control.\n";
611 }
612 this->powerOn();
613 break;
614 }
617 std::cout << "Change the control mode from stop to velocity control.\n";
618 }
619 this->powerOn();
620 break;
621 }
622 default:
623 break;
624 }
625
626 CatchPrint();
627
628 return vpRobot::setRobotState(newState);
629}
630
631/* ------------------------------------------------------------------------ */
632/* --- STOP --------------------------------------------------------------- */
633/* ------------------------------------------------------------------------ */
634
643{
644 InitTry;
645 Try(PrimitiveSTOP_Afma6());
647
648 CatchPrint();
649 if (TryStt < 0) {
650 vpERROR_TRACE("Cannot stop robot motion");
651 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
652 }
653}
654
665{
666 InitTry;
667
668 // Look if the power is on or off
669 UInt32 HIPowerStatus;
670 UInt32 EStopStatus;
671 bool firsttime = true;
672 unsigned int nitermax = 10;
673
674 for (unsigned int i = 0; i < nitermax; i++) {
675 Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
676 if (EStopStatus == ESTOP_AUTO) {
677 break; // exit for loop
678 } else if (EStopStatus == ESTOP_MANUAL) {
679 break; // exit for loop
680 } else if (EStopStatus == ESTOP_ACTIVATED) {
681 if (firsttime) {
682 std::cout << "Emergency stop is activated! \n"
683 << "Check the emergency stop button and push the yellow "
684 "button before continuing."
685 << std::endl;
686 firsttime = false;
687 }
688 fprintf(stdout, "Remaining time %us \r", nitermax - i);
689 fflush(stdout);
690 CAL_Wait(1);
691 } else {
692 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
693 std::cout << "You have to call Adept for maintenance..." << std::endl;
694 // Free allocated resources
695 ShutDownConnection();
696 throw(vpRobotException(vpRobotException::lowLevelError, "Error on the emergency chain"));
697 }
698 }
699
700 if (EStopStatus == ESTOP_ACTIVATED)
701 std::cout << std::endl;
702
703 if (EStopStatus == ESTOP_ACTIVATED) {
704 std::cout << "Sorry, cannot power on the robot." << std::endl;
705 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
706 }
707
708 if (HIPowerStatus == 0) {
709 fprintf(stdout, "Power ON the Afma6 robot\n");
710 fflush(stdout);
711
712 Try(PrimitivePOWERON_Afma6());
713 }
714
715 CatchPrint();
716 if (TryStt < 0) {
717 vpERROR_TRACE("Cannot power on the robot");
718 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
719 }
720}
721
732{
733 InitTry;
734
735 // Look if the power is on or off
736 UInt32 HIPowerStatus;
737 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
738 CAL_Wait(0.1);
739
740 if (HIPowerStatus == 1) {
741 fprintf(stdout, "Power OFF the Afma6 robot\n");
742 fflush(stdout);
743
744 Try(PrimitivePOWEROFF_Afma6());
745 }
746
747 CatchPrint();
748 if (TryStt < 0) {
749 vpERROR_TRACE("Cannot power off the robot");
750 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
751 }
752}
753
766{
767 InitTry;
768 bool status = false;
769 // Look if the power is on or off
770 UInt32 HIPowerStatus;
771 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
772 CAL_Wait(0.1);
773
774 if (HIPowerStatus == 1) {
775 status = true;
776 }
777
778 CatchPrint();
779 if (TryStt < 0) {
780 vpERROR_TRACE("Cannot get the power status");
781 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
782 }
783 return status;
784}
785
796{
798 vpAfma6::get_cMe(cMe);
799
800 cVe.buildFrom(cMe);
801}
802
814
826{
827
828 double position[6];
829 double timestamp;
830
831 InitTry;
832 Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
833 CatchPrint();
834
835 vpColVector q(6);
836 for (unsigned int i = 0; i < njoint; i++)
837 q[i] = position[i];
838
839 try {
841 } catch (...) {
842 vpERROR_TRACE("catch exception ");
843 throw;
844 }
845}
870{
871
872 double position[6];
873 double timestamp;
874
875 InitTry;
876 Try(PrimitiveACQ_POS_Afma6(position, &timestamp));
877 CatchPrint();
878
879 vpColVector q(6);
880 for (unsigned int i = 0; i < njoint; i++)
881 q[i] = position[i];
882
883 try {
885 } catch (...) {
886 vpERROR_TRACE("Error caught");
887 throw;
888 }
889}
890
919void vpRobotAfma6::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
920
926double vpRobotAfma6::getPositioningVelocity(void) { return positioningVelocity; }
927
1003
1004{
1005 vpColVector position(6);
1006 vpRxyzVector rxyz;
1008
1009 R.buildFrom(pose[3], pose[4], pose[5]); // thetau
1010 rxyz.buildFrom(R);
1011
1012 for (unsigned int i = 0; i < 3; i++) {
1013 position[i] = pose[i];
1014 position[i + 3] = rxyz[i];
1015 }
1016 if (frame == vpRobot::ARTICULAR_FRAME) {
1017 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1018 "Joint frame not implemented for pose positionning.");
1019 }
1020 setPosition(frame, position);
1021}
1105{
1106
1108 vpERROR_TRACE("Robot was not in position-based control\n"
1109 "Modification of the robot state");
1111 }
1112
1113 double _destination[6];
1114 int error = 0;
1115 double timestamp;
1116
1117 InitTry;
1118 switch (frame) {
1119 case vpRobot::CAMERA_FRAME: {
1120 double _q[njoint];
1121 Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1122
1124 for (unsigned int i = 0; i < njoint; i++)
1125 q[i] = _q[i];
1126
1127 // Get fMc from the inverse kinematics
1129 vpAfma6::get_fMc(q, fMc);
1130
1131 // Set cMc from the input position
1133 vpRxyzVector rxyz;
1134 for (unsigned int i = 0; i < 3; i++) {
1135 txyz[i] = position[i];
1136 rxyz[i] = position[i + 3];
1137 }
1138
1139 // Compute cMc2
1140 vpRotationMatrix cRc2(rxyz);
1141 vpHomogeneousMatrix cMc2(txyz, cRc2);
1142
1143 // Compute the new position to reach: fMc*cMc2
1144 vpHomogeneousMatrix fMc2 = fMc * cMc2;
1145
1146 // Compute the corresponding joint position from the inverse kinematics
1147 bool nearest = true;
1148 int solution = this->getInverseKinematics(fMc2, q, nearest);
1149 if (solution) { // Position is reachable
1150 for (unsigned int i = 0; i < njoint; i++) {
1151 _destination[i] = q[i];
1152 }
1153 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1154 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1155 } else {
1156 // Cartesian position is out of range
1157 error = -1;
1158 }
1159
1160 break;
1161 }
1163 for (unsigned int i = 0; i < njoint; i++) {
1164 _destination[i] = position[i];
1165 }
1166 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1167 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1168 break;
1169 }
1171 // Set fMc from the input position
1173 vpRxyzVector rxyz;
1174 for (unsigned int i = 0; i < 3; i++) {
1175 txyz[i] = position[i];
1176 rxyz[i] = position[i + 3];
1177 }
1178 // Compute fMc from the input position
1179 vpRotationMatrix fRc(rxyz);
1180 vpHomogeneousMatrix fMc(txyz, fRc);
1181
1182 // Compute the corresponding joint position from the inverse kinematics
1183 vpColVector q(6);
1184 bool nearest = true;
1185 int solution = this->getInverseKinematics(fMc, q, nearest);
1186 if (solution) { // Position is reachable
1187 for (unsigned int i = 0; i < njoint; i++) {
1188 _destination[i] = q[i];
1189 }
1190 Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
1191 Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
1192 } else {
1193 // Cartesian position is out of range
1194 error = -1;
1195 }
1196
1197 break;
1198 }
1199 case vpRobot::MIXT_FRAME: {
1200 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1201 "Mixt frame not implemented.");
1202 }
1204 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1205 "end-effector frame not implemented.");
1206 }
1207 }
1208
1209 CatchPrint();
1210 if (TryStt == InvalidPosition || TryStt == -1023)
1211 std::cout << " : Position out of range.\n";
1212 else if (TryStt < 0)
1213 std::cout << " : Unknown error (see Fabien).\n";
1214 else if (error == -1)
1215 std::cout << "Position out of range.\n";
1216
1217 if (TryStt < 0 || error < 0) {
1218 vpERROR_TRACE("Positionning error.");
1219 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1220 }
1221
1222 return;
1223}
1224
1291void vpRobotAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1292 double pos3, double pos4, double pos5, double pos6)
1293{
1294 try {
1295 vpColVector position(6);
1296 position[0] = pos1;
1297 position[1] = pos2;
1298 position[2] = pos3;
1299 position[3] = pos4;
1300 position[4] = pos5;
1301 position[5] = pos6;
1302
1303 setPosition(frame, position);
1304 } catch (...) {
1305 vpERROR_TRACE("Error caught");
1306 throw;
1307 }
1308}
1309
1350void vpRobotAfma6::setPosition(const std::string &filename)
1351{
1352 vpColVector q;
1353 bool ret;
1354
1355 ret = this->readPosFile(filename, q);
1356
1357 if (ret == false) {
1358 vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
1359 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1360 }
1363}
1364
1419void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
1420{
1421
1422 InitTry;
1423
1424 position.resize(6);
1425
1426 switch (frame) {
1427 case vpRobot::CAMERA_FRAME: {
1428 position = 0;
1429 return;
1430 }
1432 double _q[njoint];
1433 Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1434 for (unsigned int i = 0; i < njoint; i++) {
1435 position[i] = _q[i];
1436 }
1437
1438 return;
1439 }
1441 double _q[njoint];
1442 Try(PrimitiveACQ_POS_Afma6(_q, &timestamp));
1443
1445 for (unsigned int i = 0; i < njoint; i++)
1446 q[i] = _q[i];
1447
1448 // Compute fMc
1450 vpAfma6::get_fMc(q, fMc);
1451
1452 // From fMc extract the pose
1453 vpRotationMatrix fRc;
1454 fMc.extract(fRc);
1455 vpRxyzVector rxyz;
1456 rxyz.buildFrom(fRc);
1457
1458 for (unsigned int i = 0; i < 3; i++) {
1459 position[i] = fMc[i][3]; // translation x,y,z
1460 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1461 }
1462 break;
1463 }
1464 case vpRobot::MIXT_FRAME: {
1465 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1466 "not implemented");
1467 }
1469 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1470 "not implemented");
1471 }
1472 }
1473
1474 CatchPrint();
1475 if (TryStt < 0) {
1476 vpERROR_TRACE("Cannot get position.");
1477 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1478 }
1479
1480 return;
1481}
1487{
1488 double timestamp;
1489 PrimitiveACQ_TIME_Afma6(&timestamp);
1490 return timestamp;
1491}
1492
1504{
1505 double timestamp;
1506 getPosition(frame, position, timestamp);
1507}
1508
1518void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1519{
1520 vpColVector posRxyz;
1521 // recupere position en Rxyz
1522 this->getPosition(frame, posRxyz, timestamp);
1523 vpRxyzVector RxyzVect;
1524 for (unsigned int j = 0; j < 3; j++)
1525 RxyzVect[j] = posRxyz[j + 3];
1526 // recupere le vecteur thetaU correspondant
1527 vpThetaUVector RtuVect(RxyzVect);
1528
1529 // remplit le vpPoseVector avec translation et rotation ThetaU
1530 for (unsigned int j = 0; j < 3; j++) {
1531 position[j] = posRxyz[j];
1532 position[j + 3] = RtuVect[j];
1533 }
1534}
1535
1547{
1548 double timestamp;
1549 getPosition(frame, position, timestamp);
1550}
1551
1616{
1618 vpERROR_TRACE("Cannot send a velocity to the robot "
1619 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1621 "Cannot send a velocity to the robot "
1622 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1623 }
1624
1625 vpColVector vel_max(6);
1626
1627 for (unsigned int i = 0; i < 3; i++)
1628 vel_max[i] = getMaxTranslationVelocity();
1629 for (unsigned int i = 3; i < 6; i++)
1630 vel_max[i] = getMaxRotationVelocity();
1631
1632 vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
1633
1634 InitTry;
1635
1636 switch (frame) {
1637 case vpRobot::CAMERA_FRAME: {
1638 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
1639 break;
1640 }
1642 // Tranform in camera frame
1644 this->get_cMe(cMe);
1645 vpVelocityTwistMatrix cVe(cMe);
1646 vpColVector v_c = cVe * vel_sat;
1647 // Send velocities in m/s and rad/s
1648 Try(PrimitiveMOVESPEED_CART_Afma6(v_c.data, REPCAM_AFMA6));
1649 break;
1650 }
1652 // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
1653 Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
1654 break;
1655 }
1657 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
1658 break;
1659 }
1660 case vpRobot::MIXT_FRAME: {
1661 Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
1662 break;
1663 }
1664 default: {
1665 vpERROR_TRACE("Error in spec of vpRobot. "
1666 "Case not taken in account.");
1667 return;
1668 }
1669 }
1670
1671 Catch();
1672 if (TryStt < 0) {
1673 if (TryStt == VelStopOnJoint) {
1674 Int32 axisInJoint[njoint];
1675 PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1676 for (unsigned int i = 0; i < njoint; i++) {
1677 if (axisInJoint[i])
1678 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1679 }
1680 } else {
1681 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1682 if (TryString != NULL) {
1683 // The statement is in TryString, but we need to check the validity
1684 printf(" Error sentence %s\n", TryString); // Print the TryString
1685 } else {
1686 printf("\n");
1687 }
1688 }
1689 }
1690
1691 return;
1692}
1693
1694/* ------------------------------------------------------------------------ */
1695/* --- GET ---------------------------------------------------------------- */
1696/* ------------------------------------------------------------------------ */
1697
1747void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1748{
1749 velocity.resize(6);
1750 velocity = 0;
1751
1752 double q[6];
1753 vpColVector q_cur(6);
1754 vpHomogeneousMatrix fMc_cur;
1755 vpHomogeneousMatrix cMc; // camera displacement
1756 double time_cur;
1757
1758 InitTry;
1759
1760 // Get the current joint position
1761 Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
1762 time_cur = timestamp;
1763 for (unsigned int i = 0; i < njoint; i++) {
1764 q_cur[i] = q[i];
1765 }
1766
1767 // Get the camera pose from the direct kinematics
1768 vpAfma6::get_fMc(q_cur, fMc_cur);
1769
1770 if (!first_time_getvel) {
1771
1772 switch (frame) {
1773 case vpRobot::CAMERA_FRAME: {
1774 // Compute the displacement of the camera since the previous call
1775 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1776
1777 // Compute the velocity of the camera from this displacement
1778 velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1779
1780 break;
1781 }
1782
1784 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1785 break;
1786 }
1787
1789 // Compute the displacement of the camera since the previous call
1790 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1791
1792 // Compute the velocity of the camera from this displacement
1793 vpColVector v;
1794 v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1795
1796 // Express this velocity in the reference frame
1797 vpVelocityTwistMatrix fVc(fMc_cur);
1798 velocity = fVc * v;
1799
1800 break;
1801 }
1802
1803 case vpRobot::MIXT_FRAME: {
1804 // Compute the displacement of the camera since the previous call
1805 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1806
1807 // Compute the ThetaU representation for the rotation
1808 vpRotationMatrix cRc;
1809 cMc.extract(cRc);
1810 vpThetaUVector thetaU;
1811 thetaU.buildFrom(cRc);
1812
1813 for (unsigned int i = 0; i < 3; i++) {
1814 // Compute the translation displacement in the reference frame
1815 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1816 // Update the rotation displacement in the camera frame
1817 velocity[i + 3] = thetaU[i];
1818 }
1819
1820 // Compute the velocity
1821 velocity /= (time_cur - time_prev_getvel);
1822 break;
1823 }
1824 default: {
1826 "vpRobotAfma6::getVelocity() not implemented in end-effector"));
1827 }
1828 }
1829 } else {
1830 first_time_getvel = false;
1831 }
1832
1833 // Memorize the camera pose for the next call
1834 fMc_prev_getvel = fMc_cur;
1835
1836 // Memorize the joint position for the next call
1837 q_prev_getvel = q_cur;
1838
1839 // Memorize the time associated to the joint position for the next call
1840 time_prev_getvel = time_cur;
1841
1842 CatchPrint();
1843 if (TryStt < 0) {
1844 vpERROR_TRACE("Cannot get velocity.");
1845 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1846 }
1847}
1848
1858{
1859 double timestamp;
1860 getVelocity(frame, velocity, timestamp);
1861}
1862
1905{
1906 vpColVector velocity;
1907 getVelocity(frame, velocity, timestamp);
1908
1909 return velocity;
1910}
1911
1921{
1922 vpColVector velocity;
1923 double timestamp;
1924 getVelocity(frame, velocity, timestamp);
1925
1926 return velocity;
1927}
1928
1978bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
1979{
1980 std::ifstream fd(filename.c_str(), std::ios::in);
1981
1982 if (!fd.is_open()) {
1983 return false;
1984 }
1985
1986 std::string line;
1987 std::string key("R:");
1988 std::string id("#AFMA6 - Position");
1989 bool pos_found = false;
1990 int lineNum = 0;
1991
1992 q.resize(njoint);
1993
1994 while (std::getline(fd, line)) {
1995 lineNum++;
1996 if (lineNum == 1) {
1997 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1998 std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1999 return false;
2000 }
2001 }
2002 if ((line.compare(0, 1, "#") == 0)) { // skip comment
2003 continue;
2004 }
2005 if ((line.compare(0, key.size(), key) == 0)) { // decode position
2006 // check if there are at least njoint values in the line
2007 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2008 if (chain.size() < njoint + 1) // try to split with tab separator
2009 chain = vpIoTools::splitChain(line, std::string("\t"));
2010 if (chain.size() < njoint + 1)
2011 continue;
2012
2013 std::istringstream ss(line);
2014 std::string key_;
2015 ss >> key_;
2016 for (unsigned int i = 0; i < njoint; i++)
2017 ss >> q[i];
2018 pos_found = true;
2019 break;
2020 }
2021 }
2022
2023 // converts rotations from degrees into radians
2024 q[3] = vpMath::rad(q[3]);
2025 q[4] = vpMath::rad(q[4]);
2026 q[5] = vpMath::rad(q[5]);
2027
2028 fd.close();
2029
2030 if (!pos_found) {
2031 std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2032 return false;
2033 }
2034
2035 return true;
2036}
2037
2062bool vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2063{
2064
2065 FILE *fd;
2066 fd = fopen(filename.c_str(), "w");
2067 if (fd == NULL)
2068 return false;
2069
2070 fprintf(fd, "\
2071#AFMA6 - Position - Version 2.01\n\
2072#\n\
2073# R: X Y Z A B C\n\
2074# Joint position: X, Y, Z: translations in meters\n\
2075# A, B, C: rotations in degrees\n\
2076#\n\
2077#\n\n");
2078
2079 // Save positions in mm and deg
2080 fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2081 vpMath::deg(q[5]));
2082
2083 fclose(fd);
2084 return (true);
2085}
2086
2097void vpRobotAfma6::move(const std::string &filename)
2098{
2099 vpColVector q;
2100
2101 this->readPosFile(filename, q);
2103 this->setPositioningVelocity(10);
2105}
2106
2119void vpRobotAfma6::move(const std::string &filename, double velocity)
2120{
2121 vpColVector q;
2122
2123 this->readPosFile(filename, q);
2125 this->setPositioningVelocity(velocity);
2127}
2128
2136{
2137 InitTry;
2138 Try(PrimitiveGripper_Afma6(1));
2139 std::cout << "Open the gripper..." << std::endl;
2140 CatchPrint();
2141 if (TryStt < 0) {
2142 vpERROR_TRACE("Cannot open the gripper");
2143 throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
2144 }
2145}
2146
2155{
2156 InitTry;
2157 Try(PrimitiveGripper_Afma6(0));
2158 std::cout << "Close the gripper..." << std::endl;
2159 CatchPrint();
2160 if (TryStt < 0) {
2161 vpERROR_TRACE("Cannot close the gripper");
2162 throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
2163 }
2164}
2165
2185{
2186 displacement.resize(6);
2187 displacement = 0;
2188
2189 double q[6];
2190 vpColVector q_cur(6);
2191 vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
2192 double timestamp;
2193
2194 InitTry;
2195
2196 // Get the current joint position
2197 Try(PrimitiveACQ_POS_Afma6(q, &timestamp));
2198 for (unsigned int i = 0; i < njoint; i++) {
2199 q_cur[i] = q[i];
2200 }
2201
2202 // Compute the camera pose in the reference frame
2203 fMc_cur = get_fMc(q_cur);
2204
2205 if (!first_time_getdis) {
2206 switch (frame) {
2207 case vpRobot::CAMERA_FRAME: {
2208 // Compute the camera dispacement from the previous pose
2209 c_prevMc_cur = fMc_prev_getdis.inverse() * fMc_cur;
2210
2213 c_prevMc_cur.extract(t);
2214 c_prevMc_cur.extract(R);
2215
2216 vpRxyzVector rxyz;
2217 rxyz.buildFrom(R);
2218
2219 for (unsigned int i = 0; i < 3; i++) {
2220 displacement[i] = t[i];
2221 displacement[i + 3] = rxyz[i];
2222 }
2223 break;
2224 }
2225
2227 displacement = q_cur - q_prev_getdis;
2228 break;
2229 }
2230
2232 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
2233 return;
2234 }
2235
2236 case vpRobot::MIXT_FRAME: {
2237 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2238 return;
2239 }
2241 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2242 return;
2243 }
2244 }
2245 } else {
2246 first_time_getdis = false;
2247 }
2248
2249 // Memorize the joint position for the next call
2250 q_prev_getdis = q_cur;
2251
2252 // Memorize the pose for the next call
2253 fMc_prev_getdis = fMc_cur;
2254
2255 CatchPrint();
2256 if (TryStt < 0) {
2257 vpERROR_TRACE("Cannot get velocity.");
2258 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
2259 }
2260}
2261
2273{
2274 Int32 axisInJoint[njoint];
2275 bool status = true;
2276 jointsStatus.resize(6);
2277 InitTry;
2278
2279 Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
2280 for (unsigned int i = 0; i < njoint; i++) {
2281 if (axisInJoint[i]) {
2282 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
2283 jointsStatus[i] = axisInJoint[i];
2284 status = false;
2285 } else {
2286 jointsStatus[i] = 0;
2287 }
2288 }
2289
2290 Catch();
2291 if (TryStt < 0) {
2292 vpERROR_TRACE("Cannot check joint limits.");
2293 throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
2294 }
2295
2296 return status;
2297}
2298
2299#elif !defined(VISP_BUILD_SHARED_LIBS)
2300// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no
2301// symbols
2302void dummy_vpRobotAfma6(){};
2303#endif
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:79
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1187
double _joint_min[6]
Definition: vpAfma6.h:204
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:198
void init(void)
Definition: vpAfma6.cpp:160
vpRxyzVector _erc
Definition: vpAfma6.h:207
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
double _coupl_56
Definition: vpAfma6.h:201
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
double _long_56
Definition: vpAfma6.h:202
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
double _joint_max[6]
Definition: vpAfma6.h:203
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
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 resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_fJe(vpMatrix &_fJe)
bool checkJointLimits(vpColVector &jointsStatus)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)
void get_eJe(vpMatrix &_eJe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
double getTime() const
static const double defaultPositioningVelocity
Definition: vpRobotAfma6.h:252
void init(void)
void get_cMe(vpHomogeneousMatrix &_cMe) const
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double getPositioningVelocity(void)
void set_eMc(const vpHomogeneousMatrix &eMc)
virtual ~vpRobotAfma6(void)
void move(const std::string &filename)
bool getPowerState()
void setPositioningVelocity(double velocity)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static bool readPosFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Error that can be emited by the vpRobot class and its derivates.
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
bool verbose_
Definition: vpRobot.h:116
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
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 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)
#define vpERROR_TRACE
Definition: vpDebug.h:393