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