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 Afma4 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39 #include <visp3/core/vpConfig.h>
40
41 #ifdef VISP_HAVE_AFMA4
42
43 #include <signal.h>
44 #include <stdlib.h>
45 #include <sys/types.h>
46 #include <unistd.h>
47
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpExponentialMap.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpThetaUVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/robot/vpRobotAfma4.h>
54 #include <visp3/robot/vpRobotException.h>
55
56 /* ---------------------------------------------------------------------- */
57 /* --- STATIC ----------------------------------------------------------- */
58 /* ---------------------------------------------------------------------- */
59
60 bool vpRobotAfma4::robotAlreadyCreated = false;
61
62 /*!
63
64 Default positioning velocity in percentage of the maximum
65 velocity. This value is set to 15. The member function
66 setPositioningVelocity() allows to change this value.
67
68 */
69 const double vpRobotAfma4::defaultPositioningVelocity = 15.0;
70
71 /* ---------------------------------------------------------------------- */
72 /* --- EMERGENCY STOP --------------------------------------------------- */
73 /* ---------------------------------------------------------------------- */
74
75 /*!
76
77 Emergency stops the robot if the program is interrupted by a SIGINT
78 (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL
79 or SIGQUIT signal.
80
81 */
emergencyStopAfma4(int signo)82 void emergencyStopAfma4(int signo)
83 {
84 std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
85 switch (signo) {
86 case SIGINT:
87 std::cout << "SIGINT (stop by ^C) " << std::endl;
88 break;
89 case SIGBUS:
90 std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
91 break;
92 case SIGSEGV:
93 std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
94 break;
95 case SIGKILL:
96 std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
97 break;
98 case SIGQUIT:
99 std::cout << "SIGQUIT " << std::endl;
100 break;
101 default:
102 std::cout << signo << std::endl;
103 }
104 // std::cout << "Emergency stop called\n";
105 // PrimitiveESTOP_Afma4();
106 PrimitiveSTOP_Afma4();
107 std::cout << "Robot was stopped\n";
108
109 // Free allocated resources
110 // ShutDownConnection(); // Some times cannot exit here when Ctrl-C
111
112 fprintf(stdout, "Application ");
113 fflush(stdout);
114 kill(getpid(), SIGKILL);
115 exit(1);
116 }
117
118 /* ---------------------------------------------------------------------- */
119 /* --- CONSTRUCTOR ------------------------------------------------------ */
120 /* ---------------------------------------------------------------------- */
121
122 /*!
123
124 The only available constructor.
125
126 This contructor calls init() to initialise the connection with the MotionBox
127 or low level controller, power on the robot and wait 1 sec before returning
128 to be sure the initialisation is done.
129
130 It also set the robot state to vpRobot::STATE_STOP.
131
132 */
vpRobotAfma4(bool verbose)133 vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
134 {
135
136 /*
137 #define SIGHUP 1 // hangup
138 #define SIGINT 2 // interrupt (rubout)
139 #define SIGQUIT 3 // quit (ASCII FS)
140 #define SIGILL 4 // illegal instruction (not reset when caught)
141 #define SIGTRAP 5 // trace trap (not reset when caught)
142 #define SIGIOT 6 // IOT instruction
143 #define SIGABRT 6 // used by abort, replace SIGIOT in the future
144 #define SIGEMT 7 // EMT instruction
145 #define SIGFPE 8 // floating point exception
146 #define SIGKILL 9 // kill (cannot be caught or ignored)
147 #define SIGBUS 10 // bus error
148 #define SIGSEGV 11 // segmentation violation
149 #define SIGSYS 12 // bad argument to system call
150 #define SIGPIPE 13 // write on a pipe with no one to read it
151 #define SIGALRM 14 // alarm clock
152 #define SIGTERM 15 // software termination signal from kill
153 */
154
155 signal(SIGINT, emergencyStopAfma4);
156 signal(SIGBUS, emergencyStopAfma4);
157 signal(SIGSEGV, emergencyStopAfma4);
158 signal(SIGKILL, emergencyStopAfma4);
159 signal(SIGQUIT, emergencyStopAfma4);
160
161 setVerbose(verbose);
162 if (verbose_)
163 std::cout << "Open communication with MotionBlox.\n";
164 try {
165 this->init();
166 this->setRobotState(vpRobot::STATE_STOP);
167 } catch (...) {
168 // vpERROR_TRACE("Error caught") ;
169 throw;
170 }
171 positioningVelocity = defaultPositioningVelocity;
172
173 vpRobotAfma4::robotAlreadyCreated = true;
174
175 return;
176 }
177
178 /* ------------------------------------------------------------------------ */
179 /* --- INITIALISATION ----------------------------------------------------- */
180 /* ------------------------------------------------------------------------ */
181
182 /*!
183
184 Initialise the connection with the MotionBox or low level
185 controller, power on the
186 robot and wait 1 sec before returning to be sure the initialisation
187 is done.
188
189 */
init(void)190 void vpRobotAfma4::init(void)
191 {
192 int stt;
193 InitTry;
194
195 // Initialise private variables used to compute the measured velocities
196 q_prev_getvel.resize(4);
197 q_prev_getvel = 0;
198 time_prev_getvel = 0;
199 first_time_getvel = true;
200
201 // Initialise private variables used to compute the measured displacement
202 q_prev_getdis.resize(4);
203 q_prev_getdis = 0;
204 first_time_getdis = true;
205
206 // Initialize the firewire connection
207 Try(stt = InitializeConnection(verbose_));
208
209 if (stt != SUCCESS) {
210 vpERROR_TRACE("Cannot open connection with the motionblox.");
211 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
212 }
213
214 // Connect to the servoboard using the servo board GUID
215 Try(stt = InitializeNode_Afma4());
216
217 if (stt != SUCCESS) {
218 vpERROR_TRACE("Cannot open connection with the motionblox.");
219 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
220 }
221 Try(PrimitiveRESET_Afma4());
222
223 // Look if the power is on or off
224 UInt32 HIPowerStatus;
225 UInt32 EStopStatus;
226 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
227 CAL_Wait(0.1);
228
229 // Print the robot status
230 if (verbose_) {
231 std::cout << "Robot status: ";
232 switch (EStopStatus) {
233 case ESTOP_AUTO:
234 case ESTOP_MANUAL:
235 if (HIPowerStatus == 0)
236 std::cout << "Power is OFF" << std::endl;
237 else
238 std::cout << "Power is ON" << std::endl;
239 break;
240 case ESTOP_ACTIVATED:
241 std::cout << "Emergency stop is activated" << std::endl;
242 break;
243 default:
244 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
245 std::cout << "You have to call Adept for maintenance..." << std::endl;
246 // Free allocated resources
247 }
248 std::cout << std::endl;
249 }
250 // get real joint min/max from the MotionBlox
251 Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
252 // for (unsigned int i=0; i < njoint; i++) {
253 // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
254 // _joint_max[i]);
255 // }
256
257 // If an error occur in the low level controller, goto here
258 // CatchPrint();
259 Catch();
260
261 // Test if an error occurs
262 if (TryStt == -20001)
263 printf("No connection detected. Check if the robot is powered on \n"
264 "and if the firewire link exist between the MotionBlox and this "
265 "computer.\n");
266 else if (TryStt == -675)
267 printf(" Timeout enabling power...\n");
268
269 if (TryStt < 0) {
270 // Power off the robot
271 PrimitivePOWEROFF_Afma4();
272 // Free allocated resources
273 ShutDownConnection();
274
275 std::cout << "Cannot open connection with the motionblox..." << std::endl;
276 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
277 }
278 return;
279 }
280
281 /* ------------------------------------------------------------------------ */
282 /* --- DESTRUCTOR --------------------------------------------------------- */
283 /* ------------------------------------------------------------------------ */
284
285 /*!
286
287 Destructor.
288
289 Free allocated resources.
290 */
~vpRobotAfma4(void)291 vpRobotAfma4::~vpRobotAfma4(void)
292 {
293 InitTry;
294
295 setRobotState(vpRobot::STATE_STOP);
296
297 // Look if the power is on or off
298 UInt32 HIPowerStatus;
299 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
300 CAL_Wait(0.1);
301
302 // if (HIPowerStatus == 1) {
303 // fprintf(stdout, "Power OFF the robot\n");
304 // fflush(stdout);
305
306 // Try( PrimitivePOWEROFF_Afma4() );
307 // }
308
309 // Free allocated resources
310 ShutDownConnection();
311
312 vpRobotAfma4::robotAlreadyCreated = false;
313
314 CatchPrint();
315 return;
316 }
317
318 /*!
319
320 Change the robot state.
321
322 \param newState : New requested robot state.
323 */
setRobotState(vpRobot::vpRobotStateType newState)324 vpRobot::vpRobotStateType vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState)
325 {
326 InitTry;
327
328 switch (newState) {
329 case vpRobot::STATE_STOP: {
330 if (vpRobot::STATE_STOP != getRobotState()) {
331 Try(PrimitiveSTOP_Afma4());
332 }
333 break;
334 }
335 case vpRobot::STATE_POSITION_CONTROL: {
336 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
337 std::cout << "Change the control mode from velocity to position control.\n";
338 Try(PrimitiveSTOP_Afma4());
339 } else {
340 // std::cout << "Change the control mode from stop to position
341 // control.\n";
342 }
343 this->powerOn();
344 break;
345 }
346 case vpRobot::STATE_VELOCITY_CONTROL: {
347 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
348 std::cout << "Change the control mode from stop to velocity control.\n";
349 }
350 this->powerOn();
351 break;
352 }
353 default:
354 break;
355 }
356
357 CatchPrint();
358
359 return vpRobot::setRobotState(newState);
360 }
361
362 /* ------------------------------------------------------------------------ */
363 /* --- STOP --------------------------------------------------------------- */
364 /* ------------------------------------------------------------------------ */
365
366 /*!
367
368 Stop the robot and set the robot state to vpRobot::STATE_STOP.
369
370 \exception vpRobotException::lowLevelError : If the low level
371 controller returns an error during robot stopping.
372 */
stopMotion(void)373 void vpRobotAfma4::stopMotion(void)
374 {
375 InitTry;
376 Try(PrimitiveSTOP_Afma4());
377 setRobotState(vpRobot::STATE_STOP);
378
379 CatchPrint();
380 if (TryStt < 0) {
381 vpERROR_TRACE("Cannot stop robot motion");
382 throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
383 }
384 }
385
386 /*!
387
388 Power on the robot.
389
390 \exception vpRobotException::lowLevelError : If the low level
391 controller returns an error during robot power on.
392
393 \sa powerOff(), getPowerState()
394 */
powerOn(void)395 void vpRobotAfma4::powerOn(void)
396 {
397 InitTry;
398
399 // Look if the power is on or off
400 UInt32 HIPowerStatus;
401 UInt32 EStopStatus;
402 bool firsttime = true;
403 unsigned int nitermax = 10;
404
405 for (unsigned int i = 0; i < nitermax; i++) {
406 Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
407 if (EStopStatus == ESTOP_AUTO) {
408 break; // exit for loop
409 } else if (EStopStatus == ESTOP_MANUAL) {
410 break; // exit for loop
411 } else if (EStopStatus == ESTOP_ACTIVATED) {
412 if (firsttime) {
413 std::cout << "Emergency stop is activated! \n"
414 << "Check the emergency stop button and push the yellow "
415 "button before continuing."
416 << std::endl;
417 firsttime = false;
418 }
419 fprintf(stdout, "Remaining time %us \r", nitermax - i);
420 fflush(stdout);
421 CAL_Wait(1);
422 } else {
423 std::cout << "Sorry there is an error on the emergency chain." << std::endl;
424 std::cout << "You have to call Adept for maintenance..." << std::endl;
425 // Free allocated resources
426 ShutDownConnection();
427 exit(0);
428 }
429 }
430
431 if (EStopStatus == ESTOP_ACTIVATED)
432 std::cout << std::endl;
433
434 if (EStopStatus == ESTOP_ACTIVATED) {
435 std::cout << "Sorry, cannot power on the robot." << std::endl;
436 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power on the robot.");
437 }
438
439 if (HIPowerStatus == 0) {
440 fprintf(stdout, "Power ON the Afma4 robot\n");
441 fflush(stdout);
442
443 Try(PrimitivePOWERON_Afma4());
444 }
445
446 CatchPrint();
447 if (TryStt < 0) {
448 vpERROR_TRACE("Cannot power on the robot");
449 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
450 }
451 }
452
453 /*!
454
455 Power off the robot.
456
457 \exception vpRobotException::lowLevelError : If the low level
458 controller returns an error during robot stopping.
459
460 \sa powerOn(), getPowerState()
461 */
powerOff(void)462 void vpRobotAfma4::powerOff(void)
463 {
464 InitTry;
465
466 // Look if the power is on or off
467 UInt32 HIPowerStatus;
468 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
469 CAL_Wait(0.1);
470
471 if (HIPowerStatus == 1) {
472 fprintf(stdout, "Power OFF the Afma4 robot\n");
473 fflush(stdout);
474
475 Try(PrimitivePOWEROFF_Afma4());
476 }
477
478 CatchPrint();
479 if (TryStt < 0) {
480 vpERROR_TRACE("Cannot power off the robot");
481 throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
482 }
483 }
484
485 /*!
486
487 Get the robot power state indication if power is on or off.
488
489 \return true if power is on. false if power is off
490
491 \exception vpRobotException::lowLevelError : If the low level
492 controller returns an error.
493
494 \sa powerOn(), powerOff()
495 */
getPowerState(void)496 bool vpRobotAfma4::getPowerState(void)
497 {
498 InitTry;
499 bool status = false;
500 // Look if the power is on or off
501 UInt32 HIPowerStatus;
502 Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
503 CAL_Wait(0.1);
504
505 if (HIPowerStatus == 1) {
506 status = true;
507 }
508
509 CatchPrint();
510 if (TryStt < 0) {
511 vpERROR_TRACE("Cannot get the power status");
512 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
513 }
514 return status;
515 }
516
517 /*!
518
519 Get the twist transformation from camera frame to end-effector
520 frame. This transformation allows to compute a velocity expressed
521 in the end-effector frame into the camera frame.
522
523 \param cVe : Twist transformation.
524
525 */
get_cVe(vpVelocityTwistMatrix & cVe) const526 void vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
527 {
528 vpHomogeneousMatrix cMe;
529 vpAfma4::get_cMe(cMe);
530
531 cVe.buildFrom(cMe);
532 }
533
534 /*!
535
536 Get the twist transformation from camera frame to the reference
537 frame. This transformation allows to compute a velocity expressed
538 in the reference frame into the camera frame.
539
540 \param cVf : Twist transformation.
541
542 */
get_cVf(vpVelocityTwistMatrix & cVf) const543 void vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const
544 {
545 double position[this->njoint];
546 double timestamp;
547
548 InitTry;
549 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
550 CatchPrint();
551
552 vpColVector q(this->njoint);
553 for (unsigned int i = 0; i < njoint; i++)
554 q[i] = position[i];
555
556 try {
557 vpAfma4::get_cVf(q, cVf);
558 } catch (...) {
559 vpERROR_TRACE("catch exception ");
560 throw;
561 }
562 }
563
564 /*!
565
566 Get the geometric transformation between the camera frame and the
567 end-effector frame. This transformation is constant and correspond
568 to the extrinsic camera parameters estimated by calibration.
569
570 \param cMe : Transformation between the camera frame and the
571 end-effector frame.
572
573 */
get_cMe(vpHomogeneousMatrix & cMe) const574 void vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { vpAfma4::get_cMe(cMe); }
575
576 /*!
577
578 Get the robot jacobian expressed in the end-effector frame. To have
579 acces to the analytic form of this jacobian see vpAfma4::get_eJe().
580
581 To compute eJe, we communicate with the low level controller to get
582 the articular joint position of the robot.
583
584 \param eJe : Robot jacobian expressed in the end-effector frame.
585
586 \sa vpAfma4::get_eJe()
587 */
get_eJe(vpMatrix & eJe)588 void vpRobotAfma4::get_eJe(vpMatrix &eJe)
589 {
590
591 double position[this->njoint];
592 double timestamp;
593
594 InitTry;
595 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
596 CatchPrint();
597
598 vpColVector q(this->njoint);
599 for (unsigned int i = 0; i < njoint; i++)
600 q[i] = position[i];
601
602 try {
603 vpAfma4::get_eJe(q, eJe);
604 } catch (...) {
605 vpERROR_TRACE("catch exception ");
606 throw;
607 }
608 }
609
610 /*!
611
612 Get the robot jacobian expressed in the robot reference frame also
613 called fix frame. To have acces to the analytic form of this
614 jacobian see vpAfma4::get_fJe().
615
616 To compute fJe, we communicate with the low level controller to get
617 the articular joint position of the robot.
618
619 \param fJe : Robot jacobian expressed in the reference frame.
620
621 \sa vpAfma4::get_fJe()
622 */
623
get_fJe(vpMatrix & fJe)624 void vpRobotAfma4::get_fJe(vpMatrix &fJe)
625 {
626 double position[6];
627 double timestamp;
628
629 InitTry;
630 Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
631 CatchPrint();
632
633 vpColVector q(6);
634 for (unsigned int i = 0; i < njoint; i++)
635 q[i] = position[i];
636
637 try {
638 vpAfma4::get_fJe(q, fJe);
639 } catch (...) {
640 vpERROR_TRACE("Error caught");
641 throw;
642 }
643 }
644 /*!
645
646 Set the maximal velocity percentage to use for a position control.
647
648 The default positioning velocity is defined by
649 vpRobotAfma4::defaultPositioningVelocity. This method allows to
650 change this default positioning velocity
651
652 \param velocity : Percentage of the maximal velocity. Values should
653 be in ]0:100].
654
655 \code
656 vpColVector q[4]);
657 q = 0; // position in meter and rad
658
659 vpRobotAfma4 robot;
660
661 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
662
663 // Set the max velocity to 20%
664 robot.setPositioningVelocity(20);
665
666 // Moves the robot to the joint position [0,0,0,0]
667 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
668 \endcode
669
670 \sa getPositioningVelocity()
671 */
setPositioningVelocity(double velocity)672 void vpRobotAfma4::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
673
674 /*!
675 Get the maximal velocity percentage used for a position control.
676
677 \sa setPositioningVelocity()
678 */
getPositioningVelocity(void)679 double vpRobotAfma4::getPositioningVelocity(void) { return positioningVelocity; }
680
681 /*!
682
683 Move to an absolute position with a given percent of max velocity.
684 The percent of max velocity is to set with setPositioningVelocity().
685
686 \warning The position to reach can only be specified in joint space
687 coordinates.
688
689 This method owerloads setPosition(const
690 vpRobot::vpControlFrameType, const vpColVector &).
691
692 \warning This method is blocking. It returns only when the position
693 is reached by the robot.
694
695 \param position : The joint positions to reach. position[0]
696 corresponds to the first rotation of the turret around the vertical
697 axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
698 to the vertical translation (joint 2 with value \f$q_2\f$), while
699 position[2] and position[3] correspond to the pan and tilt of the
700 camera (respectively joint 4 and 5 with values \f$q_4\f$ and
701 \f$q_5\f$). Rotations position[0], position[2] and position[3] are
702 expressed in radians. The translation q[1] is expressed in meters.
703
704 \param frame : Frame in which the position is expressed.
705
706 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME and
707 vpRobot::END_EFFECTOR_FRAME not implemented.
708
709 \exception vpRobotException::positionOutOfRangeError : The requested
710 position is out of range.
711
712 \code
713 // Set positions in the joint space
714 vpColVector q[4];
715 double q[0] = M_PI/8; // Joint 1, in radian
716 double q[1] = 0.2; // Joint 2, in meter
717 double q[2] = M_PI/4; // Joint 4, in radian
718 double q[3] = M_PI/8; // Joint 5, in radian
719
720 vpRobotAfma4 robot;
721
722 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
723
724 // Set the max velocity to 20%
725 robot.setPositioningVelocity(20);
726
727 // Moves the robot in the camera frame
728 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
729 \endcode
730
731 \exception vpRobotException::lowLevelError : If the requested frame
732 (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME
733 or vpRobot::MIXT_FRAME) are requested since they are not implemented.
734
735 \exception vpRobotException::positionOutOfRangeError : The requested
736 position is out of range.
737
738 To catch the exception if the position is out of range, modify the code
739 like:
740
741 \code
742 try {
743 robot.setPosition(vpRobot::CAMERA_FRAME, q);
744 }
745 catch (vpRobotException &e) {
746 if (e.getCode() == vpRobotException::positionOutOfRangeError) {
747 std::cout << "The position is out of range" << std::endl;
748 }
749 \endcode
750
751 */
752
setPosition(const vpRobot::vpControlFrameType frame,const vpColVector & position)753 void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
754 {
755
756 if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
757 vpERROR_TRACE("Robot was not in position-based control\n"
758 "Modification of the robot state");
759 setRobotState(vpRobot::STATE_POSITION_CONTROL);
760 }
761
762 int error = 0;
763
764 switch (frame) {
765 case vpRobot::REFERENCE_FRAME:
766 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
767 "Reference frame not implemented.");
768 break;
769 case vpRobot::CAMERA_FRAME:
770 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
771 "Camera frame not implemented.");
772 break;
773 case vpRobot::MIXT_FRAME:
774 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
775 "Mixt frame not implemented.");
776 break;
777 case vpRobot::END_EFFECTOR_FRAME:
778 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
779 "End-effector frame not implemented.");
780 break;
781
782 case vpRobot::ARTICULAR_FRAME: {
783 break;
784 }
785 }
786 if (position.getRows() != this->njoint) {
787 vpERROR_TRACE("Positionning error: bad vector dimension.");
788 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension.");
789 }
790
791 InitTry;
792
793 Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
794 Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
795
796 CatchPrint();
797 if (TryStt == InvalidPosition || TryStt == -1023)
798 std::cout << " : Position out of range.\n";
799 else if (TryStt < 0)
800 std::cout << " : Unknown error (see Fabien).\n";
801 else if (error == -1)
802 std::cout << "Position out of range.\n";
803
804 if (TryStt < 0 || error < 0) {
805 vpERROR_TRACE("Positionning error.");
806 throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
807 }
808
809 return;
810 }
811
812 /*!
813 Move to an absolute position with a given percent of max velocity.
814 The percent of max velocity is to set with setPositioningVelocity().
815
816 \warning The position to reach can only be specified in joint space
817 coordinates.
818
819 This method owerloads setPosition(const
820 vpRobot::vpControlFrameType, const vpColVector &).
821
822 \warning This method is blocking. It returns only when the position
823 is reached by the robot.
824
825 \param q1, q2, q4, q5 : The four joint positions to reach. q1 corresponds to
826 the first rotation (joint 1 with value \f$q_1\f$) of the turret
827 around the vertical axis, while q2 corresponds to the vertical
828 translation (joint 2 with value \f$q_2\f$), while q4 and q5
829 correspond to the pan and tilt of the camera (respectively joint 4
830 and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q1, q4 and
831 q5 are expressed in radians. The translation q2 is expressed in
832 meters.
833
834 \param frame : Frame in which the position is expressed.
835
836 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
837 and vpRobot::END_EFFECTOR_FRAME not implemented.
838
839 \exception vpRobotException::positionOutOfRangeError : The requested
840 position is out of range.
841
842 \code
843 // Set positions in the camera frame
844 double q1 = M_PI/8; // Joint 1, in radian
845 double q2 = 0.2; // Joint 2, in meter
846 double q4 = M_PI/4; // Joint 4, in radian
847 double q5 = M_PI/8; // Joint 5, in radian
848
849 vpRobotAfma4 robot;
850
851 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
852
853 // Set the max velocity to 20%
854 robot.setPositioningVelocity(20);
855
856 // Moves the robot in the camera frame
857 robot.setPosition(vpRobot::ARTICULAR_FRAME, q1, q2, q4, q5);
858 \endcode
859
860 \sa setPosition()
861 */
setPosition(const vpRobot::vpControlFrameType frame,const double q1,const double q2,const double q4,const double q5)862 void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
863 const double q4, const double q5)
864 {
865 try {
866 vpColVector position(this->njoint);
867 position[0] = q1;
868 position[1] = q2;
869 position[2] = q4;
870 position[3] = q5;
871
872 setPosition(frame, position);
873 } catch (...) {
874 vpERROR_TRACE("Error caught");
875 throw;
876 }
877 }
878
879 /*!
880
881 Move to an absolute joint position with a given percent of max
882 velocity. The robot state is set to position control. The percent
883 of max velocity is to set with setPositioningVelocity(). The
884 position to reach is defined in the position file.
885
886 \param filename : Name of the position file to read. The
887 readPosFile() documentation shows a typical content of such a
888 position file.
889
890 This method has the same behavior than the sample code given below;
891 \code
892 vpColVector q;
893
894 robot.readPosFile("MyPositionFilename.pos", q);
895 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL)
896 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
897 \endcode
898
899 \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
900 and vpRobot::END_EFFECTOR_FRAME not implemented.
901
902 \exception vpRobotException::positionOutOfRangeError : The requested
903 position is out of range.
904
905 \sa setPositioningVelocity()
906
907 */
setPosition(const char * filename)908 void vpRobotAfma4::setPosition(const char *filename)
909 {
910 vpColVector q;
911 bool ret;
912
913 ret = this->readPosFile(filename, q);
914
915 if (ret == false) {
916 vpERROR_TRACE("Bad position in \"%s\"", filename);
917 throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
918 }
919 this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
920 this->setPosition(vpRobot::ARTICULAR_FRAME, q);
921 }
922
923 /*!
924 Returns the robot controller current time (in second) since last robot power
925 on.
926 */
getTime() const927 double vpRobotAfma4::getTime() const
928 {
929 double timestamp;
930 PrimitiveACQ_TIME_Afma4(×tamp);
931 return timestamp;
932 }
933
934 /*!
935
936 Get the current position of the robot.
937
938 \param frame : Control frame type in which to get the position, either :
939 - in the camera cartesien frame,
940 - joint (articular) coordinates of each axes
941 - in a reference or fixed cartesien frame attached to the robot base
942 - in a mixt cartesien frame (translation in reference
943 frame, and rotation in camera frame)
944
945 \param position : Measured position of the robot:
946 - in camera cartesien frame, a 6 dimension vector, set to 0.
947
948 - in articular, a 4 dimension vector corresponding to the joint position of
949 each dof. position[0]
950 corresponds to the first rotation of the turret around the vertical
951 axis (joint 1 with value \f$q_1\f$), while position[1] corresponds
952 to the vertical translation (joint 2 with value \f$q_2\f$), while
953 position[2] and position[3] correspond to the pan and tilt of the
954 camera (respectively joint 4 and 5 with values \f$q_4\f$ and
955 \f$q_5\f$). Rotations position[0], position[2] and position[3] are
956 expressed in radians. The translation q[1] is expressed in meters.
957
958 - in reference frame, a 6 dimension vector, the first 3 values correspond to
959 the translation tx, ty, tz in meters (like a vpTranslationVector), and the
960 last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code
961 below show how to convert this position into a vpHomogeneousMatrix:
962
963 \param timestamp : Time in second since last robot power on.
964
965 \code
966 vpRobotAfma4 robot;
967 vpColVector r;
968 robot.getPosition(vpRobot::REFERENCE_FRAME, r);
969
970 vpTranslationVector ftc; // reference frame to camera frame translations
971 vpRxyzVector frc; // reference frame to camera frame rotations
972
973 // Update the transformation between reference frame and camera frame
974 for (int i=0; i < 3; i++) {
975 ftc[i] = position[i]; // tx, ty, tz
976 frc[i] = position[i+3]; // ry, ry, rz
977 }
978
979 // Create a rotation matrix from the Rxyz rotation angles
980 vpRotationMatrix fRc(frc); // reference frame to camera frame rotation
981 matrix
982
983 // Create the camera to fix frame pose in terms of a homogeneous matrix
984 vpHomogeneousMatrix fMc(fRc, ftc);
985 \endcode
986
987 \exception vpRobotException::lowLevelError : If the position cannot
988 be get from the low level controller.
989
990 \sa setPosition(const vpRobot::vpControlFrameType frame, const
991 vpColVector & r)
992
993 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & position,double & timestamp)994 void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp)
995 {
996
997 InitTry;
998
999 position.resize(this->njoint);
1000
1001 switch (frame) {
1002 case vpRobot::CAMERA_FRAME: {
1003 position = 0;
1004 return;
1005 }
1006 case vpRobot::ARTICULAR_FRAME: {
1007 double _q[njoint];
1008 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1009 for (unsigned int i = 0; i < this->njoint; i++) {
1010 position[i] = _q[i];
1011 }
1012
1013 return;
1014 }
1015 case vpRobot::REFERENCE_FRAME: {
1016 double _q[njoint];
1017 Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
1018
1019 vpColVector q(this->njoint);
1020 for (unsigned int i = 0; i < this->njoint; i++)
1021 q[i] = _q[i];
1022
1023 // Compute fMc
1024 vpHomogeneousMatrix fMc;
1025 vpAfma4::get_fMc(q, fMc);
1026
1027 // From fMc extract the pose
1028 vpRotationMatrix fRc;
1029 fMc.extract(fRc);
1030 vpRxyzVector rxyz;
1031 rxyz.buildFrom(fRc);
1032
1033 for (unsigned int i = 0; i < 3; i++) {
1034 position[i] = fMc[i][3]; // translation x,y,z
1035 position[i + 3] = rxyz[i]; // Euler rotation x,y,z
1036 }
1037 break;
1038 }
1039 case vpRobot::MIXT_FRAME: {
1040 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
1041 "not implemented");
1042 }
1043 case vpRobot::END_EFFECTOR_FRAME: {
1044 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in end-effector frame: "
1045 "not implemented");
1046 }
1047 }
1048
1049 CatchPrint();
1050 if (TryStt < 0) {
1051 vpERROR_TRACE("Cannot get position.");
1052 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
1053 }
1054
1055 return;
1056 }
1057
1058 /*!
1059
1060 Get the current position of the robot.
1061
1062 Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
1063 &, double &).
1064
1065 The difference is here that the timestamp is not used.
1066
1067 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & position)1068 void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
1069 {
1070 double timestamp;
1071 getPosition(frame, position, timestamp);
1072 }
1073
1074 /*!
1075 Apply a velocity to the robot.
1076
1077 \param frame : Control frame in which the velocity is expressed. Velocities
1078 could be expressed in the joint space or in the camera frame. The reference
1079 frame and mixt frame are not implemented.
1080
1081 \param vel : Velocity vector. Translation velocities are expressed in m/s
1082 while rotation velocities in rad/s. The size of this vector may be 4 (the
1083 number of dof) if frame is set to vpRobot::ARTICULAR_FRAME. The size of this
1084 vector is 2 when the control is in the camera frame (frame is than set to
1085 vpRobot::CAMERA_FRAME).
1086
1087 - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t \f$
1088 where \f$\dot{q}_1, \dot{q}_3, \dot{q}_4 \f$ correspond to joint
1089 velocities in rad/s and \f$\dot{q}_2\f$ to the vertical translation velocity
1090 in m/s.
1091
1092 - In camera frame, \f$ vel = [^{c} t_x, ^{c} t_y, ^{c} t_z,^{c}
1093 \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t\f$ is a velocity twist
1094 expressed in the camera frame, with translations velocities
1095 \f$ ^{c} v_x, ^{c} v_y, ^{c} v_z \f$ in m/s
1096 and rotation velocities \f$ ^{c}\omega_x, ^{c} \omega_y, ^{c} \omega_z \f$
1097 in rad/s. Since only four dof are available, in camera frame we control only
1098 the camera pan and tilt. In other words we apply only \f$ ^{c}\omega_x, ^{c}
1099 \omega_y\f$ to the robot, even if \f$ vel \f$ is a 6-dimension vector.
1100
1101 \exception vpRobotException::wrongStateError : If a the robot is not
1102 configured to handle a velocity. The robot can handle a velocity only if the
1103 velocity control mode is set. For that, call setRobotState(
1104 vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
1105
1106 \warning Velocities could be saturated if one of them exceed the
1107 maximal autorized speed (see vpRobot::maxTranslationVelocity and
1108 vpRobot::maxRotationVelocity). To change these values use
1109 setMaxTranslationVelocity() and setMaxRotationVelocity().
1110
1111 \code
1112 // Set joint velocities
1113 vpColVector q_dot(4);
1114 q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
1115 q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
1116 q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
1117 q_dot[3] = M_PI/8; // Joint 5 velocity, in rad/s
1118
1119 vpRobotAfma4 robot;
1120
1121 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
1122
1123 // Moves the joint in velocity
1124 robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
1125 \endcode
1126 */
setVelocity(const vpRobot::vpControlFrameType frame,const vpColVector & vel)1127 void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
1128 {
1129
1130 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
1131 vpERROR_TRACE("Cannot send a velocity to the robot "
1132 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1133 throw vpRobotException(vpRobotException::wrongStateError,
1134 "Cannot send a velocity to the robot "
1135 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1136 }
1137
1138 // Check the dimension of the velocity vector to see if it is
1139 // compatible with the requested frame
1140 switch (frame) {
1141 case vpRobot::CAMERA_FRAME: {
1142 // if (vel.getRows() != 2) {
1143 if (vel.getRows() != 6) {
1144 vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
1145 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
1146 "in camera frame");
1147 }
1148 break;
1149 }
1150 case vpRobot::ARTICULAR_FRAME: {
1151 if (vel.getRows() != this->njoint) {
1152 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
1153 "velocity vector ");
1154 }
1155 break;
1156 }
1157 case vpRobot::REFERENCE_FRAME: {
1158 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1159 "in the reference frame:"
1160 "functionality not implemented");
1161 }
1162 case vpRobot::MIXT_FRAME: {
1163 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1164 "in the mixt frame:"
1165 "functionality not implemented");
1166 }
1167 case vpRobot::END_EFFECTOR_FRAME: {
1168 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
1169 "in the end-effector frame:"
1170 "functionality not implemented");
1171 }
1172 default: {
1173 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
1174 }
1175 }
1176
1177 //
1178 // Velocities saturation with normalization
1179 //
1180 vpColVector joint_vel(this->njoint);
1181
1182 // Case of the camera frame where we control only 2 dof
1183 if (frame == vpRobot::CAMERA_FRAME) {
1184 vpColVector vel_max(6);
1185
1186 for (unsigned int i = 0; i < 3; i++) {
1187 vel_max[i] = getMaxTranslationVelocity();
1188 vel_max[i + 3] = getMaxRotationVelocity();
1189 }
1190
1191 vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
1192
1193 #if 0 // ok
1194 vpMatrix eJe(4,6);
1195 eJe = 0;
1196 eJe[2][4] = -1;
1197 eJe[3][3] = 1;
1198
1199 joint_vel = eJe * velocity; // Compute the articular velocity
1200 #endif
1201 vpColVector q;
1202 getPosition(vpRobot::ARTICULAR_FRAME, q);
1203 vpMatrix fJe_inverse;
1204 get_fJe_inverse(q, fJe_inverse);
1205 vpHomogeneousMatrix fMe;
1206 get_fMe(q, fMe);
1207 vpTranslationVector t;
1208 t = 0;
1209 vpRotationMatrix fRe;
1210 fMe.extract(fRe);
1211 vpVelocityTwistMatrix fVe(t, fRe);
1212 // compute the inverse jacobian in the end-effector frame
1213 vpMatrix eJe_inverse = fJe_inverse * fVe;
1214
1215 // Transform the velocities from camera to end-effector frame
1216 vpVelocityTwistMatrix eVc;
1217 eVc.buildFrom(this->_eMc);
1218 joint_vel = eJe_inverse * eVc * velocity;
1219
1220 // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1],
1221 // joint_vel[2], joint_vel[3]);
1222 }
1223
1224 // Case of the joint control where we control all the joints
1225 else if (frame == vpRobot::ARTICULAR_FRAME) {
1226
1227 vpColVector vel_max(4);
1228
1229 vel_max[0] = getMaxRotationVelocity();
1230 vel_max[1] = getMaxTranslationVelocity();
1231 vel_max[2] = getMaxRotationVelocity();
1232 vel_max[3] = getMaxRotationVelocity();
1233
1234 joint_vel = vpRobot::saturateVelocities(vel, vel_max, true);
1235 }
1236
1237 InitTry;
1238
1239 // Send a joint velocity to the low level controller
1240 Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
1241
1242 Catch();
1243 if (TryStt < 0) {
1244 if (TryStt == VelStopOnJoint) {
1245 UInt32 axisInJoint[njoint];
1246 PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1247 for (unsigned int i = 0; i < njoint; i++) {
1248 if (axisInJoint[i])
1249 std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
1250 }
1251 } else {
1252 printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1253 if (TryString != NULL) {
1254 // The statement is in TryString, but we need to check the validity
1255 printf(" Error sentence %s\n", TryString); // Print the TryString
1256 } else {
1257 printf("\n");
1258 }
1259 }
1260 }
1261
1262 return;
1263 }
1264
1265 /* ------------------------------------------------------------------------ */
1266 /* --- GET ---------------------------------------------------------------- */
1267 /* ------------------------------------------------------------------------ */
1268
1269 /*!
1270
1271 Get the robot velocities.
1272
1273 \param frame : Frame in wich velocities are mesured.
1274
1275 \param velocity : Measured velocities. Translations are expressed in m/s
1276 and rotations in rad/s.
1277
1278 \param timestamp : Time in second since last robot power on.
1279
1280 \warning In camera frame, reference frame and mixt frame, the representation
1281 of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
1282 \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
1283
1284 \warning The first time this method is called, \e velocity is set to 0. The
1285 first call is used to intialise the velocity computation for the next call.
1286
1287 \code
1288 // Set joint velocities
1289 vpColVector q_dot(4);
1290 q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
1291 q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
1292 q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
1293 q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
1294
1295 vpRobotAfma4 robot;
1296
1297 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
1298
1299 // Moves the joint in velocity
1300 robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
1301
1302 vpColVector q_dot_mes; // Measured velocities
1303
1304 // Initialisation of the velocity measurement
1305 robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
1306 // q_dot_mes is resized to 4, the number of joint
1307
1308 double timestamp;
1309 while (1) {
1310 robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
1311 vpTime::wait(40); // wait 40 ms
1312 // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
1313 }
1314 \endcode
1315 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & velocity,double & timestamp)1316 void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp)
1317 {
1318
1319 switch (frame) {
1320 case vpRobot::ARTICULAR_FRAME:
1321 velocity.resize(this->njoint);
1322 break;
1323 default:
1324 velocity.resize(6);
1325 }
1326
1327 velocity = 0;
1328
1329 double q[4];
1330 vpColVector q_cur(4);
1331 vpHomogeneousMatrix fMc_cur;
1332 vpHomogeneousMatrix cMc; // camera displacement
1333 double time_cur;
1334
1335 InitTry;
1336
1337 // Get the current joint position
1338 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1339 time_cur = timestamp;
1340
1341 for (unsigned int i = 0; i < this->njoint; i++) {
1342 q_cur[i] = q[i];
1343 }
1344
1345 // Get the camera pose from the direct kinematics
1346 vpAfma4::get_fMc(q_cur, fMc_cur);
1347
1348 if (!first_time_getvel) {
1349
1350 switch (frame) {
1351 case vpRobot::CAMERA_FRAME: {
1352 // Compute the displacement of the camera since the previous call
1353 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1354
1355 // Compute the velocity of the camera from this displacement
1356 velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1357
1358 break;
1359 }
1360
1361 case vpRobot::ARTICULAR_FRAME: {
1362 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1363 break;
1364 }
1365
1366 case vpRobot::REFERENCE_FRAME: {
1367 // Compute the displacement of the camera since the previous call
1368 cMc = fMc_prev_getvel.inverse() * fMc_cur;
1369
1370 // Compute the velocity of the camera from this displacement
1371 vpColVector v;
1372 v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
1373
1374 // Express this velocity in the reference frame
1375 vpVelocityTwistMatrix fVc(fMc_cur);
1376 velocity = fVc * v;
1377
1378 break;
1379 }
1380
1381 case vpRobot::MIXT_FRAME: {
1382 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
1383 "functionality not implemented");
1384
1385 break;
1386 }
1387 case vpRobot::END_EFFECTOR_FRAME: {
1388 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
1389 "functionality not implemented");
1390
1391 break;
1392 }
1393 }
1394 } else {
1395 first_time_getvel = false;
1396 }
1397
1398 // Memorize the camera pose for the next call
1399 fMc_prev_getvel = fMc_cur;
1400
1401 // Memorize the joint position for the next call
1402 q_prev_getvel = q_cur;
1403
1404 // Memorize the time associated to the joint position for the next call
1405 time_prev_getvel = time_cur;
1406
1407 CatchPrint();
1408 if (TryStt < 0) {
1409 vpERROR_TRACE("Cannot get velocity.");
1410 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1411 }
1412 }
1413
1414 /*!
1415
1416 Get robot velocities.
1417
1418 The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
1419 vpColVector &, double &) except that the timestamp is not returned.
1420
1421 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & velocity)1422 void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
1423 {
1424 double timestamp;
1425 getVelocity(frame, velocity, timestamp);
1426 }
1427
1428 /*!
1429
1430 Get the robot velocities.
1431
1432 \param frame : Frame in wich velocities are mesured.
1433
1434 \param timestamp : Time in second since last robot power on.
1435
1436 \return Measured velocities. Translations are expressed in m/s
1437 and rotations in rad/s.
1438
1439 \code
1440 // Set joint velocities
1441 vpColVector q_dot(4);
1442 q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
1443 q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
1444 q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
1445 q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
1446
1447 vpRobotAfma4 robot;
1448
1449 robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
1450
1451 // Moves the joint in velocity
1452 robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
1453
1454 // Initialisation of the velocity measurement
1455 robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
1456 // q_dot_mes is resized to 4, the number of joint
1457
1458 vpColVector q_dot_mes; // Measured velocities
1459 double timestamp;
1460 while (1) {
1461 q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
1462 vpTime::wait(40); // wait 40 ms
1463 // here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
1464 }
1465 \endcode
1466 */
getVelocity(vpRobot::vpControlFrameType frame,double & timestamp)1467 vpColVector vpRobotAfma4::getVelocity(vpRobot::vpControlFrameType frame, double ×tamp)
1468 {
1469 vpColVector velocity;
1470 getVelocity(frame, velocity, timestamp);
1471
1472 return velocity;
1473 }
1474
1475 /*!
1476
1477 Get robot velocities.
1478
1479 The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
1480 double &) except that the timestamp is not returned.
1481
1482 */
getVelocity(vpRobot::vpControlFrameType frame)1483 vpColVector vpRobotAfma4::getVelocity(vpRobot::vpControlFrameType frame)
1484 {
1485 vpColVector velocity;
1486 double timestamp;
1487 getVelocity(frame, velocity, timestamp);
1488
1489 return velocity;
1490 }
1491
1492 /*!
1493
1494 Read joint positions in a specific Afma4 position file.
1495
1496 This position file has to start with a header. The six joint positions
1497 are given after the "R:" keyword. The first 3 values correspond to the
1498 joint translations X,Y,Z expressed in meters. The 3 last values
1499 correspond to the joint rotations A,B,C expressed in degres to be more
1500 representative for the user. Theses values are then converted in
1501 radians in \e q. The character "#" starting a line indicates a
1502 comment.
1503
1504 A typical content of such a file is given below:
1505
1506 \code
1507 #AFMA4 - Position - Version 2.01
1508 # file: "myposition.pos "
1509 #
1510 # R: X Y A B
1511 # Joint position: X : rotation of the turret in degrees (joint 1)
1512 # Y : vertical translation in meters (joint 2)
1513 # A : pan rotation of the camera in degrees (joint 4)
1514 # B : tilt rotation of the camera in degrees (joint 5)
1515 #
1516
1517 R: 45 0.3 -20 30
1518 \endcode
1519
1520 \param filename : Name of the position file to read.
1521
1522 \param q : Joint positions: X,Y,A,B. Translations Y is
1523 expressed in meters, while joint rotations X,A,B in radians.
1524
1525 \return true if the positions were successfully readen in the file. false, if
1526 an error occurs.
1527
1528 The code below shows how to read a position from a file and move the robot to
1529 this position.
1530 \code
1531 vpRobotAfma4 robot;
1532 vpColVector q; // Joint position robot.
1533 readPosFile("myposition.pos", q); // Set the joint position from the file
1534 robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
1535
1536 robot.setPositioningVelocity(5); // Positioning velocity set to 5%
1537 robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
1538 \endcode
1539
1540 \sa savePosFile()
1541 */
1542
readPosFile(const std::string & filename,vpColVector & q)1543 bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
1544 {
1545 std::ifstream fd(filename.c_str(), std::ios::in);
1546
1547 if (!fd.is_open()) {
1548 return false;
1549 }
1550
1551 std::string line;
1552 std::string key("R:");
1553 std::string id("#AFMA4 - Position");
1554 bool pos_found = false;
1555 int lineNum = 0;
1556
1557 q.resize(njoint);
1558
1559 while (std::getline(fd, line)) {
1560 lineNum++;
1561 if (lineNum == 1) {
1562 if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
1563 std::cout << "Error: this position file " << filename << " is not for Afma4 robot" << std::endl;
1564 return false;
1565 }
1566 }
1567 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1568 continue;
1569 }
1570 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1571 // check if there are at least njoint values in the line
1572 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1573 if (chain.size() < njoint + 1) // try to split with tab separator
1574 chain = vpIoTools::splitChain(line, std::string("\t"));
1575 if (chain.size() < njoint + 1)
1576 continue;
1577
1578 std::istringstream ss(line);
1579 std::string key_;
1580 ss >> key_;
1581 for (unsigned int i = 0; i < njoint; i++)
1582 ss >> q[i];
1583 pos_found = true;
1584 break;
1585 }
1586 }
1587
1588 // converts rotations from degrees into radians
1589 q[0] = vpMath::rad(q[0]);
1590 q[2] = vpMath::rad(q[2]);
1591 q[3] = vpMath::rad(q[3]);
1592
1593 fd.close();
1594
1595 if (!pos_found) {
1596 std::cout << "Error: unable to find a position for Afma4 robot in " << filename << std::endl;
1597 return false;
1598 }
1599
1600 return true;
1601 }
1602
1603 /*!
1604
1605 Save joint (articular) positions in a specific Afma4 position file.
1606
1607 This position file starts with a header on the first line. After
1608 convertion of the rotations in degrees, the joint position \e q is
1609 written on a line starting with the keyword "R: ". See readPosFile()
1610 documentation for an example of such a file.
1611
1612 \param filename : Name of the position file to create.
1613
1614 \param q : Joint positions: X,Y,A,B. Translations Y is
1615 expressed in meters, while joint rotations X,A,B in radians.
1616
1617 \warning The joint rotations X,A,B written in the file are converted
1618 in degrees to be more representative for the user.
1619
1620 \return true if the positions were successfully saved in the file. false, if
1621 an error occurs.
1622
1623 \sa readPosFile()
1624 */
1625
savePosFile(const std::string & filename,const vpColVector & q)1626 bool vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
1627 {
1628
1629 FILE *fd;
1630 fd = fopen(filename.c_str(), "w");
1631 if (fd == NULL)
1632 return false;
1633
1634 fprintf(fd, "\
1635 #AFMA4 - Position - Version 2.01\n\
1636 #\n\
1637 # R: X Y A B\n\
1638 # Joint position: X : rotation of the turret in degrees (joint 1)\n\
1639 # Y : vertical translation in meters (joint 2)\n\
1640 # A : pan rotation of the camera in degrees (joint 4)\n\
1641 # B : tilt rotation of the camera in degrees (joint 5)\n\
1642 #\n\n");
1643
1644 // Save positions in mm and deg
1645 fprintf(fd, "R: %lf %lf %lf %lf\n", vpMath::deg(q[0]), q[1], vpMath::deg(q[2]), vpMath::deg(q[3]));
1646
1647 fclose(fd);
1648 return (true);
1649 }
1650
1651 /*!
1652
1653 Moves the robot to the joint position specified in the filename. The
1654 positioning velocity is set to 10% of the robot maximal velocity.
1655
1656 \param filename: File containing a joint position.
1657
1658 \sa readPosFile
1659
1660 */
move(const char * filename)1661 void vpRobotAfma4::move(const char *filename)
1662 {
1663 vpColVector q;
1664
1665 this->readPosFile(filename, q);
1666 this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
1667 this->setPositioningVelocity(10);
1668 this->setPosition(vpRobot::ARTICULAR_FRAME, q);
1669 }
1670
1671 /*!
1672
1673 Get the robot displacement since the last call of this method.
1674
1675 \warning This functionnality is not implemented for the moment in the
1676 cartesian space. It is only available in the joint space
1677 (vpRobot::ARTICULAR_FRAME).
1678
1679 \param frame : The frame in which the measured displacement is expressed.
1680
1681 \param displacement : The measured displacement since the last call of this
1682 method. The dimension of \e displacement is always 4, the number of
1683 joints. Translations are expressed in meters, rotations in radians.
1684
1685 In camera or reference frame, rotations are expressed with the
1686 Euler Rxyz representation.
1687
1688 */
getDisplacement(vpRobot::vpControlFrameType frame,vpColVector & displacement)1689 void vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
1690 {
1691 displacement.resize(6);
1692 displacement = 0;
1693
1694 double q[6];
1695 vpColVector q_cur(6);
1696 double timestamp;
1697
1698 InitTry;
1699
1700 // Get the current joint position
1701 Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
1702 for (unsigned int i = 0; i < njoint; i++) {
1703 q_cur[i] = q[i];
1704 }
1705
1706 if (!first_time_getdis) {
1707 switch (frame) {
1708 case vpRobot::CAMERA_FRAME: {
1709 std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1710 return;
1711 }
1712
1713 case vpRobot::ARTICULAR_FRAME: {
1714 displacement = q_cur - q_prev_getdis;
1715 break;
1716 }
1717
1718 case vpRobot::REFERENCE_FRAME: {
1719 std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1720 return;
1721 }
1722
1723 case vpRobot::MIXT_FRAME: {
1724 std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1725 return;
1726 }
1727 case vpRobot::END_EFFECTOR_FRAME: {
1728 std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1729 return;
1730 }
1731 }
1732 } else {
1733 first_time_getdis = false;
1734 }
1735
1736 // Memorize the joint position for the next call
1737 q_prev_getdis = q_cur;
1738
1739 CatchPrint();
1740 if (TryStt < 0) {
1741 vpERROR_TRACE("Cannot get velocity.");
1742 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
1743 }
1744 }
1745
1746 #elif !defined(VISP_BUILD_SHARED_LIBS)
1747 // Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
1748 // symbols
dummy_vpRobotAfma4()1749 void dummy_vpRobotAfma4(){};
1750 #endif
1751