Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotAfma4.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 Afma4 robot.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_AFMA4
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/vpRobotAfma4.h>
51#include <visp3/robot/vpRobotException.h>
52
53/* ---------------------------------------------------------------------- */
54/* --- STATIC ----------------------------------------------------------- */
55/* ---------------------------------------------------------------------- */
56
57bool vpRobotAfma4::robotAlreadyCreated = false;
58
67
68/* ---------------------------------------------------------------------- */
69/* --- EMERGENCY STOP --------------------------------------------------- */
70/* ---------------------------------------------------------------------- */
71
79void emergencyStopAfma4(int signo)
80{
81 std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
82 switch (signo) {
83 case SIGINT:
84 std::cout << "SIGINT (stop by ^C) " << std::endl;
85 break;
86 case SIGBUS:
87 std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
88 break;
89 case SIGSEGV:
90 std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
91 break;
92 case SIGKILL:
93 std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
94 break;
95 case SIGQUIT:
96 std::cout << "SIGQUIT " << std::endl;
97 break;
98 default:
99 std::cout << signo << std::endl;
100 }
101 // std::cout << "Emergency stop called\n";
102 // PrimitiveESTOP_Afma4();
103 PrimitiveSTOP_Afma4();
104 std::cout << "Robot was stopped\n";
105
106 // Free allocated resources
107 // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
108
109 fprintf(stdout, "Application ");
110 fflush(stdout);
111 kill(getpid(), SIGKILL);
112 exit(1);
113}
114
115/* ---------------------------------------------------------------------- */
116/* --- CONSTRUCTOR ------------------------------------------------------ */
117/* ---------------------------------------------------------------------- */
118
130vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
131{
132
133 /*
134 #define SIGHUP 1 // hangup
135 #define SIGINT 2 // interrupt (rubout)
136 #define SIGQUIT 3 // quit (ASCII FS)
137 #define SIGILL 4 // illegal instruction (not reset when caught)
138 #define SIGTRAP 5 // trace trap (not reset when caught)
139 #define SIGIOT 6 // IOT instruction
140 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
141 #define SIGEMT 7 // EMT instruction
142 #define SIGFPE 8 // floating point exception
143 #define SIGKILL 9 // kill (cannot be caught or ignored)
144 #define SIGBUS 10 // bus error
145 #define SIGSEGV 11 // segmentation violation
146 #define SIGSYS 12 // bad argument to system call
147 #define SIGPIPE 13 // write on a pipe with no one to read it
148 #define SIGALRM 14 // alarm clock
149 #define SIGTERM 15 // software termination signal from kill
150 */
151
152 signal(SIGINT, emergencyStopAfma4);
153 signal(SIGBUS, emergencyStopAfma4);
154 signal(SIGSEGV, emergencyStopAfma4);
155 signal(SIGKILL, emergencyStopAfma4);
156 signal(SIGQUIT, emergencyStopAfma4);
157
158 setVerbose(verbose);
159 if (verbose_)
160 std::cout << "Open communication with MotionBlox.\n";
161 try {
162 this->init();
164 } catch (...) {
165 // vpERROR_TRACE("Error caught") ;
166 throw;
167 }
168 positioningVelocity = defaultPositioningVelocity;
169
170 vpRobotAfma4::robotAlreadyCreated = true;
171
172 return;
173}
174
175/* ------------------------------------------------------------------------ */
176/* --- INITIALISATION ----------------------------------------------------- */
177/* ------------------------------------------------------------------------ */
178
188{
189 int stt;
190 InitTry;
191
192 // Initialise private variables used to compute the measured velocities
193 q_prev_getvel.resize(4);
194 q_prev_getvel = 0;
195 time_prev_getvel = 0;
196 first_time_getvel = true;
197
198 // Initialise private variables used to compute the measured displacement
199 q_prev_getdis.resize(4);
200 q_prev_getdis = 0;
201 first_time_getdis = true;
202
203 // Initialize the firewire connection
204 Try(stt = InitializeConnection(verbose_));
205
206 if (stt != SUCCESS) {
207 vpERROR_TRACE("Cannot open connection with the motionblox.");
208 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
209 }
210
211 // Connect to the servoboard using the servo board GUID
212 Try(stt = InitializeNode_Afma4());
213
214 if (stt != SUCCESS) {
215 vpERROR_TRACE("Cannot open connection with the motionblox.");
216 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
217 }
218 Try(PrimitiveRESET_Afma4());
219
220 // Look if the power is on or off
221 UInt32 HIPowerStatus;
222 UInt32 EStopStatus;
223 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
224 CAL_Wait(0.1);
225
226 // Print the robot status
227 if (verbose_) {
228 std::cout << "Robot status: ";
229 switch (EStopStatus) {
230 case ESTOP_AUTO:
231 case ESTOP_MANUAL:
232 if (HIPowerStatus == 0)
233 std::cout << "Power is OFF" << std::endl;
234 else
235 std::cout << "Power is ON" << std::endl;
236 break;
237 case ESTOP_ACTIVATED:
238 std::cout << "Emergency stop is activated" << std::endl;
239 break;
240 default:
241 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
242 std::cout << "You have to call Adept for maintenance..." << std::endl;
243 // Free allocated resources
244 }
245 std::cout << std::endl;
246 }
247 // get real joint min/max from the MotionBlox
248 Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
249 // for (unsigned int i=0; i < njoint; i++) {
250 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
251 // _joint_max[i]);
252 // }
253
254 // If an error occur in the low level controller, goto here
255 // CatchPrint();
256 Catch();
257
258 // Test if an error occurs
259 if (TryStt == -20001)
260 printf("No connection detected. Check if the robot is powered on \n"
261 "and if the firewire link exist between the MotionBlox and this "
262 "computer.\n");
263 else if (TryStt == -675)
264 printf(" Timeout enabling power...\n");
265
266 if (TryStt < 0) {
267 // Power off the robot
268 PrimitivePOWEROFF_Afma4();
269 // Free allocated resources
270 ShutDownConnection();
271
272 std::cout << "Cannot open connection with the motionblox..." << std::endl;
273 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
274 }
275 return;
276}
277
278/* ------------------------------------------------------------------------ */
279/* --- DESTRUCTOR --------------------------------------------------------- */
280/* ------------------------------------------------------------------------ */
281
289{
290 InitTry;
291
293
294 // Look if the power is on or off
295 UInt32 HIPowerStatus;
296 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
297 CAL_Wait(0.1);
298
299 // if (HIPowerStatus == 1) {
300 // fprintf(stdout, "Power OFF the robot\n");
301 // fflush(stdout);
302
303 // Try( PrimitivePOWEROFF_Afma4() );
304 // }
305
306 // Free allocated resources
307 ShutDownConnection();
308
309 vpRobotAfma4::robotAlreadyCreated = false;
310
311 CatchPrint();
312 return;
313}
314
322{
323 InitTry;
324
325 switch (newState) {
326 case vpRobot::STATE_STOP: {
328 Try(PrimitiveSTOP_Afma4());
329 }
330 break;
331 }
334 std::cout << "Change the control mode from velocity to position control.\n";
335 Try(PrimitiveSTOP_Afma4());
336 } else {
337 // std::cout << "Change the control mode from stop to position
338 // control.\n";
339 }
340 this->powerOn();
341 break;
342 }
345 std::cout << "Change the control mode from stop to velocity control.\n";
346 }
347 this->powerOn();
348 break;
349 }
350 default:
351 break;
352 }
353
354 CatchPrint();
355
356 return vpRobot::setRobotState(newState);
357}
358
359/* ------------------------------------------------------------------------ */
360/* --- STOP --------------------------------------------------------------- */
361/* ------------------------------------------------------------------------ */
362
371{
372 InitTry;
373 Try(PrimitiveSTOP_Afma4());
375
376 CatchPrint();
377 if (TryStt < 0) {
378 vpERROR_TRACE("Cannot stop robot motion");
379 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
380 }
381}
382
393{
394 InitTry;
395
396 // Look if the power is on or off
397 UInt32 HIPowerStatus;
398 UInt32 EStopStatus;
399 bool firsttime = true;
400 unsigned int nitermax = 10;
401
402 for (unsigned int i = 0; i < nitermax; i++) {
403 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
404 if (EStopStatus == ESTOP_AUTO) {
405 break; // exit for loop
406 } else if (EStopStatus == ESTOP_MANUAL) {
407 break; // exit for loop
408 } else if (EStopStatus == ESTOP_ACTIVATED) {
409 if (firsttime) {
410 std::cout << "Emergency stop is activated! \n"
411 << "Check the emergency stop button and push the yellow "
412 "button before continuing."
413 << std::endl;
414 firsttime = false;
415 }
416 fprintf(stdout, "Remaining time %us \r", nitermax - i);
417 fflush(stdout);
418 CAL_Wait(1);
419 } else {
420 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
421 std::cout << "You have to call Adept for maintenance..." << std::endl;
422 // Free allocated resources
423 ShutDownConnection();
424 throw(vpRobotException(vpRobotException::lowLevelError, "Error in the emergency chain"));
425 }
426 }
427
428 if (EStopStatus == ESTOP_ACTIVATED)
429 std::cout << std::endl;
430
431 if (EStopStatus == ESTOP_ACTIVATED) {
432 std::cout << "Sorry, cannot power on the robot." << std::endl;
433 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot."));
434 }
435
436 if (HIPowerStatus == 0) {
437 fprintf(stdout, "Power ON the Afma4 robot\n");
438 fflush(stdout);
439
440 Try(PrimitivePOWERON_Afma4());
441 }
442
443 CatchPrint();
444 if (TryStt < 0) {
445 vpERROR_TRACE("Cannot power on the robot");
446 throw(vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot."));
447 }
448}
449
460{
461 InitTry;
462
463 // Look if the power is on or off
464 UInt32 HIPowerStatus;
465 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
466 CAL_Wait(0.1);
467
468 if (HIPowerStatus == 1) {
469 fprintf(stdout, "Power OFF the Afma4 robot\n");
470 fflush(stdout);
471
472 Try(PrimitivePOWEROFF_Afma4());
473 }
474
475 CatchPrint();
476 if (TryStt < 0) {
477 vpERROR_TRACE("Cannot power off the robot");
478 throw vpRobotException(vpRobotException::communicationError, "Cannot power off the robot.");
479 }
480}
481
494{
495 InitTry;
496 bool status = false;
497 // Look if the power is on or off
498 UInt32 HIPowerStatus;
499 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
500 CAL_Wait(0.1);
501
502 if (HIPowerStatus == 1) {
503 status = true;
504 }
505
506 CatchPrint();
507 if (TryStt < 0) {
508 vpERROR_TRACE("Cannot get the power status");
509 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
510 }
511 return status;
512}
513
524{
526 vpAfma4::get_cMe(cMe);
527
528 cVe.buildFrom(cMe);
529}
530
541{
542 double position[this->njoint];
543 double timestamp;
544
545 InitTry;
546 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
547 CatchPrint();
548
549 vpColVector q(this->njoint);
550 for (unsigned int i = 0; i < njoint; i++)
551 q[i] = position[i];
552
553 try {
554 vpAfma4::get_cVf(q, cVf);
555 } catch (...) {
556 vpERROR_TRACE("catch exception ");
557 throw;
558 }
559}
560
572
586{
587
588 double position[this->njoint];
589 double timestamp;
590
591 InitTry;
592 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
593 CatchPrint();
594
595 vpColVector q(this->njoint);
596 for (unsigned int i = 0; i < njoint; i++)
597 q[i] = position[i];
598
599 try {
601 } catch (...) {
602 vpERROR_TRACE("catch exception ");
603 throw;
604 }
605}
606
622{
623 double position[6];
624 double timestamp;
625
626 InitTry;
627 Try(PrimitiveACQ_POS_Afma4(position, &timestamp));
628 CatchPrint();
629
630 vpColVector q(6);
631 for (unsigned int i = 0; i < njoint; i++)
632 q[i] = position[i];
633
634 try {
636 } catch (...) {
637 vpERROR_TRACE("Error caught");
638 throw;
639 }
640}
669void vpRobotAfma4::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
670
676double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; }
677
751{
752
754 vpERROR_TRACE("Robot was not in position-based control\n"
755 "Modification of the robot state");
757 }
758
759 int error = 0;
760
761 switch (frame) {
763 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
764 "Reference frame not implemented.");
765 break;
767 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
768 "Camera frame not implemented.");
769 break;
771 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
772 "Mixt frame not implemented.");
773 break;
775 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
776 "End-effector frame not implemented.");
777 break;
778
780 break;
781 }
782 }
783 if (position.getRows() != this->njoint) {
784 vpERROR_TRACE("Positioning error: bad vector dimension.");
785 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positioning error: bad vector dimension.");
786 }
787
788 InitTry;
789
790 Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
791 Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
792
793 CatchPrint();
794 if (TryStt == InvalidPosition || TryStt == -1023)
795 std::cout << " : Position out of range.\n";
796 else if (TryStt < 0)
797 std::cout << " : Unknown error (see Fabien).\n";
798 else if (error == -1)
799 std::cout << "Position out of range.\n";
800
801 if (TryStt < 0 || error < 0) {
802 vpERROR_TRACE("Positioning error.");
803 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
804 }
805
806 return;
807}
808
859void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
860 const double q4, const double q5)
861{
862 try {
863 vpColVector position(this->njoint);
864 position[0] = q1;
865 position[1] = q2;
866 position[2] = q4;
867 position[3] = q5;
868
869 setPosition(frame, position);
870 } catch (...) {
871 vpERROR_TRACE("Error caught");
872 throw;
873 }
874}
875
905void vpRobotAfma4::setPosition(const char *filename)
906{
907 vpColVector q;
908 bool ret;
909
910 ret = this->readPosFile(filename, q);
911
912 if (ret == false) {
913 vpERROR_TRACE("Bad position in \"%s\"", filename);
914 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
915 }
918}
919
925{
926 double timestamp;
927 PrimitiveACQ_TIME_Afma4(&timestamp);
928 return timestamp;
929}
930
991void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
992{
993
994 InitTry;
995
996 position.resize(this->njoint);
997
998 switch (frame) {
1000 position = 0;
1001 return;
1002 }
1004 double _q[njoint];
1005 Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1006 for (unsigned int i = 0; i < this->njoint; i++) {
1007 position[i] = _q[i];
1008 }
1009
1010 return;
1011 }
1013 double _q[njoint];
1014 Try(PrimitiveACQ_POS_Afma4(_q, &timestamp));
1015
1016 vpColVector q(this->njoint);
1017 for (unsigned int i = 0; i < this->njoint; i++)
1018 q[i] = _q[i];
1019
1020 // Compute fMc
1022 vpAfma4::get_fMc(q, fMc);
1023
1024 // From fMc extract the pose
1025 vpRotationMatrix fRc;
1026 fMc.extract(fRc);
1027 vpRxyzVector rxyz;
1028 rxyz.buildFrom(fRc);
1029
1030 for (unsigned int i = 0; i < 3; i++) {
1031 position[i] = fMc[i][3]; // translation x,y,z
1032 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1033 }
1034 break;
1035 }
1036 case vpRobot::MIXT_FRAME: {
1037 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1038 "not implemented");
1039 }
1041 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1042 "not implemented");
1043 }
1044 }
1045
1046 CatchPrint();
1047 if (TryStt < 0) {
1048 vpERROR_TRACE("Cannot get position.");
1049 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1050 }
1051
1052 return;
1053}
1054
1066{
1067 double timestamp;
1068 getPosition(frame, position, timestamp);
1069}
1070
1125{
1126
1128 vpERROR_TRACE("Cannot send a velocity to the robot "
1129 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1131 "Cannot send a velocity to the robot "
1132 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1133 }
1134
1135 // Check the dimension of the velocity vector to see if it is
1136 // compatible with the requested frame
1137 switch (frame) {
1138 case vpRobot::CAMERA_FRAME: {
1139 // if (vel.getRows() != 2) {
1140 if (vel.getRows() != 6) {
1141 vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1142 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1143 "in camera frame");
1144 }
1145 break;
1146 }
1148 if (vel.getRows() != this->njoint) {
1149 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1150 "velocity vector ");
1151 }
1152 break;
1153 }
1155 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1156 "in the reference frame:"
1157 "functionality not implemented");
1158 }
1159 case vpRobot::MIXT_FRAME: {
1160 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1161 "in the mixt frame:"
1162 "functionality not implemented");
1163 }
1165 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1166 "in the end-effector frame:"
1167 "functionality not implemented");
1168 }
1169 default: {
1170 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
1171 }
1172 }
1173
1174 //
1175 // Velocities saturation with normalization
1176 //
1177 vpColVector joint_vel(this->njoint);
1178
1179 // Case of the camera frame where we control only 2 dof
1180 if (frame == vpRobot::CAMERA_FRAME) {
1181 vpColVector vel_max(6);
1182
1183 for (unsigned int i = 0; i < 3; i++) {
1184 vel_max[i] = getMaxTranslationVelocity();
1185 vel_max[i + 3] = getMaxRotationVelocity();
1186 }
1187
1188 vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1189
1190#if 0 // ok
1191 vpMatrix eJe(4,6);
1192 eJe = 0;
1193 eJe[2][4] = -1;
1194 eJe[3][3] = 1;
1195
1196 joint_vel = eJe * velocity; // Compute the articular velocity
1197#endif
1198 vpColVector q;
1200 vpMatrix fJe_inverse;
1201 get_fJe_inverse(q, fJe_inverse);
1203 get_fMe(q, fMe);
1205 t = 0;
1206 vpRotationMatrix fRe;
1207 fMe.extract(fRe);
1208 vpVelocityTwistMatrix fVe(t, fRe);
1209 // compute the inverse jacobian in the end-effector frame
1210 vpMatrix eJe_inverse = fJe_inverse * fVe;
1211
1212 // Transform the velocities from camera to end-effector frame
1214 eVc.buildFrom(this->_eMc);
1215 joint_vel = eJe_inverse * eVc * velocity;
1216
1217 // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1218 // joint_vel[2], joint_vel[3]);
1219 }
1220
1221 // Case of the joint control where we control all the joints
1222 else if (frame == vpRobot::ARTICULAR_FRAME) {
1223
1224 vpColVector vel_max(4);
1225
1226 vel_max[0] = getMaxRotationVelocity();
1227 vel_max[1] = getMaxTranslationVelocity();
1228 vel_max[2] = getMaxRotationVelocity();
1229 vel_max[3] = getMaxRotationVelocity();
1230
1231 joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1232 }
1233
1234 InitTry;
1235
1236 // Send a joint velocity to the low level controller
1237 Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
1238
1239 Catch();
1240 if (TryStt < 0) {
1241 if (TryStt == VelStopOnJoint) {
1242 UInt32 axisInJoint[njoint];
1243 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1244 for (unsigned int i = 0; i < njoint; i++) {
1245 if (axisInJoint[i])
1246 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1247 }
1248 } else {
1249 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1250 if (TryString != NULL) {
1251 // The statement is in TryString, but we need to check the validity
1252 printf(" Error sentence %s\n", TryString); // Print the TryString
1253 } else {
1254 printf("\n");
1255 }
1256 }
1257 }
1258
1259 return;
1260}
1261
1262/* ------------------------------------------------------------------------ */
1263/* --- GET ---------------------------------------------------------------- */
1264/* ------------------------------------------------------------------------ */
1265
1313void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
1314{
1315
1316 switch (frame) {
1318 velocity.resize(this->njoint);
1319 break;
1320 default:
1321 velocity.resize(6);
1322 }
1323
1324 velocity = 0;
1325
1326 double q[4];
1327 vpColVector q_cur(4);
1328 vpHomogeneousMatrix fMc_cur;
1329 vpHomogeneousMatrix cMc; // camera displacement
1330 double time_cur;
1331
1332 InitTry;
1333
1334 // Get the current joint position
1335 Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1336 time_cur = timestamp;
1337
1338 for (unsigned int i = 0; i < this->njoint; i++) {
1339 q_cur[i] = q[i];
1340 }
1341
1342 // Get the camera pose from the direct kinematics
1343 vpAfma4::get_fMc(q_cur, fMc_cur);
1344
1345 if (!first_time_getvel) {
1346
1347 switch (frame) {
1348 case vpRobot::CAMERA_FRAME: {
1349 // Compute the displacement of the camera since the previous call
1350 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1351
1352 // Compute the velocity of the camera from this displacement
1353 velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1354
1355 break;
1356 }
1357
1359 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1360 break;
1361 }
1362
1364 // Compute the displacement of the camera since the previous call
1365 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1366
1367 // Compute the velocity of the camera from this displacement
1368 vpColVector v;
1369 v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1370
1371 // Express this velocity in the reference frame
1372 vpVelocityTwistMatrix fVc(fMc_cur);
1373 velocity = fVc * v;
1374
1375 break;
1376 }
1377
1378 case vpRobot::MIXT_FRAME: {
1379 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1380 "functionality not implemented");
1381
1382 break;
1383 }
1385 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
1386 "functionality not implemented");
1387
1388 break;
1389 }
1390 }
1391 } else {
1392 first_time_getvel = false;
1393 }
1394
1395 // Memorize the camera pose for the next call
1396 fMc_prev_getvel = fMc_cur;
1397
1398 // Memorize the joint position for the next call
1399 q_prev_getvel = q_cur;
1400
1401 // Memorize the time associated to the joint position for the next call
1402 time_prev_getvel = time_cur;
1403
1404 CatchPrint();
1405 if (TryStt < 0) {
1406 vpERROR_TRACE("Cannot get velocity.");
1407 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1408 }
1409}
1410
1420{
1421 double timestamp;
1422 getVelocity(frame, velocity, timestamp);
1423}
1424
1465{
1466 vpColVector velocity;
1467 getVelocity(frame, velocity, timestamp);
1468
1469 return velocity;
1470}
1471
1481{
1482 vpColVector velocity;
1483 double timestamp;
1484 getVelocity(frame, velocity, timestamp);
1485
1486 return velocity;
1487}
1488
1540bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1541{
1542 std::ifstream fd(filename.c_str(), std::ios::in);
1543
1544 if (!fd.is_open()) {
1545 return false;
1546 }
1547
1548 std::string line;
1549 std::string key("R:");
1550 std::string id("#AFMA4 - Position");
1551 bool pos_found = false;
1552 int lineNum = 0;
1553
1554 q.resize(njoint);
1555
1556 while (std::getline(fd, line)) {
1557 lineNum++;
1558 if (lineNum == 1) {
1559 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1560 std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1561 return false;
1562 }
1563 }
1564 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1565 continue;
1566 }
1567 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1568 // check if there are at least njoint values in the line
1569 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1570 if (chain.size() < njoint + 1) // try to split with tab separator
1571 chain = vpIoTools::splitChain(line, std::string("\t"));
1572 if (chain.size() < njoint + 1)
1573 continue;
1574
1575 std::istringstream ss(line);
1576 std::string key_;
1577 ss >> key_;
1578 for (unsigned int i = 0; i < njoint; i++)
1579 ss >> q[i];
1580 pos_found = true;
1581 break;
1582 }
1583 }
1584
1585 // converts rotations from degrees into radians
1586 q[0] = vpMath::rad(q[0]);
1587 q[2] = vpMath::rad(q[2]);
1588 q[3] = vpMath::rad(q[3]);
1589
1590 fd.close();
1591
1592 if (!pos_found) {
1593 std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1594 return false;
1595 }
1596
1597 return true;
1598}
1599
1623bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1624{
1625
1626 FILE *fd;
1627 fd = fopen(filename.c_str(), "w");
1628 if (fd == NULL)
1629 return false;
1630
1631 fprintf(fd, "\
1632#AFMA4 - Position - Version 2.01\n\
1633#\n\
1634# R: X Y A B\n\
1635# Joint position: X : rotation of the turret in degrees (joint 1)\n\
1636# Y : vertical translation in meters (joint 2)\n\
1637# A : pan rotation of the camera in degrees (joint 4)\n\
1638# B : tilt rotation of the camera in degrees (joint 5)\n\
1639#\n\n");
1640
1641 // Save positions in mm and deg
1642 fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1643
1644 fclose(fd);
1645 return (true);
1646}
1647
1658void vpRobotAfma4::move(const char *filename)
1659{
1660 vpColVector q;
1661
1662 this->readPosFile(filename, q);
1664 this->setPositioningVelocity(10);
1666}
1667
1687{
1688 displacement.resize(6);
1689 displacement = 0;
1690
1691 double q[6];
1692 vpColVector q_cur(6);
1693 double timestamp;
1694
1695 InitTry;
1696
1697 // Get the current joint position
1698 Try(PrimitiveACQ_POS_Afma4(q, &timestamp));
1699 for (unsigned int i = 0; i < njoint; i++) {
1700 q_cur[i] = q[i];
1701 }
1702
1703 if (!first_time_getdis) {
1704 switch (frame) {
1705 case vpRobot::CAMERA_FRAME: {
1706 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1707 return;
1708 }
1709
1711 displacement = q_cur - q_prev_getdis;
1712 break;
1713 }
1714
1716 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1717 return;
1718 }
1719
1720 case vpRobot::MIXT_FRAME: {
1721 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1722 return;
1723 }
1725 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1726 return;
1727 }
1728 }
1729 } else {
1730 first_time_getdis = false;
1731 }
1732
1733 // Memorize the joint position for the next call
1734 q_prev_getdis = q_cur;
1735
1736 CatchPrint();
1737 if (TryStt < 0) {
1738 vpERROR_TRACE("Cannot get velocity.");
1739 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1740 }
1741}
1742
1743#elif !defined(VISP_BUILD_SHARED_LIBS)
1744// Work around to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
1745// symbols
1746void dummy_vpRobotAfma4(){};
1747#endif
Modelisation of Irisa's cylindrical robot named Afma4.
Definition vpAfma4.h:108
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpAfma4.cpp:447
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpAfma4.cpp:173
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpAfma4.cpp:306
static const unsigned int njoint
Number of joint.
Definition vpAfma4.h:137
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpAfma4.cpp:392
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition vpAfma4.cpp:503
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition vpAfma4.cpp:345
vpHomogeneousMatrix _eMc
Definition vpAfma4.h:151
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition vpAfma4.cpp:256
double _joint_min[4]
Definition vpAfma4.h:145
double _joint_max[4]
Definition vpAfma4.h:144
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
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
void get_cMe(vpHomogeneousMatrix &cMe) const
void get_cVf(vpVelocityTwistMatrix &cVf) const
virtual ~vpRobotAfma4(void)
double getPositioningVelocity(void)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void setPositioningVelocity(double velocity)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_fJe(vpMatrix &fJe)
void move(const char *filename)
static const double defaultPositioningVelocity
void get_eJe(vpMatrix &eJe)
static bool readPosFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void init(void)
double getTime() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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.
@ communicationError
Unable to communicate.
@ 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.
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition vpDebug.h:388