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 ptu-46 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39 #include <signal.h>
40 #include <string.h>
41
42 #include <visp3/core/vpConfig.h>
43 #ifdef VISP_HAVE_PTU46
44
45 /* Headers des fonctions implementees. */
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/robot/vpPtu46.h>
49 #include <visp3/robot/vpRobotException.h>
50 #include <visp3/robot/vpRobotPtu46.h>
51
52 /* ---------------------------------------------------------------------- */
53 /* --- STATIC ------------------------------------------------------------ */
54 /* ------------------------------------------------------------------------ */
55
56 bool vpRobotPtu46::robotAlreadyCreated = false;
57 const double vpRobotPtu46::defaultPositioningVelocity = 10.0;
58
59 /* ----------------------------------------------------------------------- */
60 /* --- CONSTRUCTOR ------------------------------------------------------ */
61 /* ---------------------------------------------------------------------- */
62
63 /*!
64
65 Default constructor.
66
67 Initialize the ptu-46 pan, tilt head by opening the serial port.
68
69 \sa init()
70
71 */
vpRobotPtu46(const char * device)72 vpRobotPtu46::vpRobotPtu46(const char *device) : vpRobot()
73 {
74 this->device = new char[FILENAME_MAX];
75
76 sprintf(this->device, "%s", device);
77
78 vpDEBUG_TRACE(12, "Open communication with Ptu-46.");
79 try {
80 init();
81 } catch (...) {
82 delete[] this->device;
83 vpERROR_TRACE("Error caught");
84 throw;
85 }
86
87 try {
88 setRobotState(vpRobot::STATE_STOP);
89 } catch (...) {
90 delete[] this->device;
91 vpERROR_TRACE("Error caught");
92 throw;
93 }
94 positioningVelocity = defaultPositioningVelocity;
95 return;
96 }
97
98 /* ------------------------------------------------------------------------ */
99 /* --- DESTRUCTOR -------------------------------------------------------- */
100 /* ------------------------------------------------------------------------ */
101
102 /*!
103
104 Destructor.
105 Close the serial connection with the head.
106
107 */
~vpRobotPtu46(void)108 vpRobotPtu46::~vpRobotPtu46(void)
109 {
110
111 setRobotState(vpRobot::STATE_STOP);
112
113 if (0 != ptu.close()) {
114 vpERROR_TRACE("Error while closing communications with the robot ptu-46.");
115 }
116
117 vpRobotPtu46::robotAlreadyCreated = false;
118
119 delete[] device;
120
121 return;
122 }
123
124 /* --------------------------------------------------------------------------
125 */
126 /* --- INITIALISATION -------------------------------------------------------
127 */
128 /* --------------------------------------------------------------------------
129 */
130
131 /*!
132
133 Open the serial port.
134
135
136 \exception vpRobotException::constructionError : If the device cannot be
137 oppened.
138
139 */
init()140 void vpRobotPtu46::init()
141 {
142
143 vpDEBUG_TRACE(12, "Open connection Ptu-46.");
144 if (0 != ptu.init(device)) {
145 vpERROR_TRACE("Cannot open connection with ptu-46.");
146 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with ptu-46");
147 }
148
149 return;
150 }
151
152 /*!
153
154 Change the state of the robot either to stop them, or to set position or
155 speed control.
156
157 */
setRobotState(vpRobot::vpRobotStateType newState)158 vpRobot::vpRobotStateType vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
159 {
160 switch (newState) {
161 case vpRobot::STATE_STOP: {
162 if (vpRobot::STATE_STOP != getRobotState()) {
163 ptu.stop();
164 }
165 break;
166 }
167 case vpRobot::STATE_POSITION_CONTROL: {
168 if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
169 vpDEBUG_TRACE(12, "Passage vitesse -> position.");
170 ptu.stop();
171 } else {
172 vpDEBUG_TRACE(1, "Passage arret -> position.");
173 }
174 break;
175 }
176 case vpRobot::STATE_VELOCITY_CONTROL: {
177 if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
178 vpDEBUG_TRACE(10, "Arret du robot...");
179 ptu.stop();
180 }
181 break;
182 }
183 default:
184 break;
185 }
186
187 return vpRobot::setRobotState(newState);
188 }
189
190 /*!
191
192 Halt all the axis.
193
194 */
stopMotion(void)195 void vpRobotPtu46::stopMotion(void)
196 {
197 ptu.stop();
198 setRobotState(vpRobot::STATE_STOP);
199 }
200
201 /*!
202
203 Get the twist matrix corresponding to the transformation between the
204 camera frame and the end effector frame. The end effector frame is located
205 on the tilt axis.
206
207 \param cVe : Twist transformation between camera and end effector frame to
208 expess a velocity skew from end effector frame in camera frame.
209
210 */
get_cVe(vpVelocityTwistMatrix & cVe) const211 void vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
212 {
213 vpHomogeneousMatrix cMe;
214 vpPtu46::get_cMe(cMe);
215
216 cVe.buildFrom(cMe);
217 }
218
219 /*!
220
221 Get the homogeneous matrix corresponding to the transformation between the
222 camera frame and the end effector frame. The end effector frame is located
223 on the tilt axis.
224
225 \param cMe : Homogeneous matrix between camera and end effector frame.
226
227 */
get_cMe(vpHomogeneousMatrix & cMe) const228 void vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const { vpPtu46::get_cMe(cMe); }
229
230 /*!
231 Get the robot jacobian expressed in the end-effector frame.
232
233 \warning Re is not the embedded camera frame. It corresponds to the frame
234 associated to the tilt axis (see also get_cMe).
235
236 \param eJe : Jacobian between end effector frame and end effector frame (on
237 tilt axis).
238
239 */
get_eJe(vpMatrix & eJe)240 void vpRobotPtu46::get_eJe(vpMatrix &eJe)
241 {
242 vpColVector q(2);
243 getPosition(vpRobot::ARTICULAR_FRAME, q);
244
245 try {
246 vpPtu46::get_eJe(q, eJe);
247 } catch (...) {
248 vpERROR_TRACE("catch exception ");
249 throw;
250 }
251 }
252
253 /*!
254 Get the robot jacobian expressed in the robot reference frame
255
256 \param fJe : Jacobian between reference frame (or fix frame) and end
257 effector frame (on tilt axis).
258
259 */
get_fJe(vpMatrix & fJe)260 void vpRobotPtu46::get_fJe(vpMatrix &fJe)
261 {
262 vpColVector q(2);
263 getPosition(vpRobot::ARTICULAR_FRAME, q);
264
265 try {
266 vpPtu46::get_fJe(q, fJe);
267 } catch (...) {
268 vpERROR_TRACE("Error caught");
269 throw;
270 }
271 }
272
273 /*!
274
275 Set the velocity used for a position control.
276
277 \param velocity : Velocity in % of the maximum velocity between [0,100].
278 */
setPositioningVelocity(double velocity)279 void vpRobotPtu46::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
280 /*!
281 Get the velocity in % used for a position control.
282
283 \return Positionning velocity in [0, 100]
284
285 */
getPositioningVelocity(void)286 double vpRobotPtu46::getPositioningVelocity(void) { return positioningVelocity; }
287
288 /*!
289 Move the robot in position control.
290
291 \warning This method is blocking. That mean that it waits the end of the
292 positionning.
293
294 \param frame : Control frame. This head can only be controlled in
295 articular.
296
297 \param q : The position to set for each axis.
298
299 \exception vpRobotException::wrongStateError : If a not supported frame
300 type is given.
301
302 */
303
setPosition(const vpRobot::vpControlFrameType frame,const vpColVector & q)304 void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
305 {
306
307 if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
308 vpERROR_TRACE("Robot was not in position-based control\n"
309 "Modification of the robot state");
310 setRobotState(vpRobot::STATE_POSITION_CONTROL);
311 }
312
313 switch (frame) {
314 case vpRobot::CAMERA_FRAME:
315 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
316 "not implemented");
317 break;
318 case vpRobot::REFERENCE_FRAME:
319 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
320 "not implemented");
321 break;
322 case vpRobot::MIXT_FRAME:
323 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
324 "not implemented");
325 break;
326 case vpRobot::END_EFFECTOR_FRAME:
327 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
328 "not implemented");
329 break;
330 case vpRobot::ARTICULAR_FRAME:
331 break;
332 }
333
334 // Interface for the controller
335 double artpos[2];
336
337 artpos[0] = q[0];
338 artpos[1] = q[1];
339
340 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
341 vpERROR_TRACE("Positionning error.");
342 throw vpRobotException(vpRobotException::lowLevelError, "Positionning error.");
343 }
344
345 return;
346 }
347
348 /*!
349 Move the robot in position control.
350
351 \warning This method is blocking. That mean that it wait the end of the
352 positionning.
353
354 \param frame : Control frame. This head can only be controlled in
355 articular.
356
357 \param q1 : The pan position to set.
358 \param q2 : The tilt position to set.
359
360 \exception vpRobotException::wrongStateError : If a not supported frame
361 type is given.
362
363 */
setPosition(const vpRobot::vpControlFrameType frame,const double & q1,const double & q2)364 void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
365 {
366 try {
367 vpColVector q(2);
368 q[0] = q1;
369 q[1] = q2;
370
371 setPosition(frame, q);
372 } catch (...) {
373 vpERROR_TRACE("Error caught");
374 throw;
375 }
376 }
377
378 /*!
379
380 Read the content of the position file and moves to head to articular
381 position.
382
383 \param filename : Position filename
384
385 \exception vpRobotException::readingParametersError : If the articular
386 position cannot be read from file.
387
388 \sa readPositionFile()
389
390 */
setPosition(const char * filename)391 void vpRobotPtu46::setPosition(const char *filename)
392 {
393 vpColVector q;
394 if (readPositionFile(filename, q) == false) {
395 vpERROR_TRACE("Cannot get ptu-46 position from file");
396 throw vpRobotException(vpRobotException::readingParametersError, "Cannot get ptu-46 position from file");
397 }
398 setPosition(vpRobot::ARTICULAR_FRAME, q);
399 }
400
401 /*!
402
403 Return the position of each axis.
404
405 \param frame : Control frame. This head can only be controlled in
406 articular.
407
408 \param q : The position of the axis.
409
410 \exception vpRobotException::wrongStateError : If a not supported frame type
411 is given.
412
413 */
getPosition(const vpRobot::vpControlFrameType frame,vpColVector & q)414 void vpRobotPtu46::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
415 {
416 vpDEBUG_TRACE(9, "# Entree.");
417
418 switch (frame) {
419 case vpRobot::CAMERA_FRAME:
420 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in camera frame: "
421 "not implemented");
422 break;
423 case vpRobot::REFERENCE_FRAME:
424 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in reference frame: "
425 "not implemented");
426 break;
427 case vpRobot::MIXT_FRAME:
428 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
429 "not implemented");
430 break;
431 case vpRobot::END_EFFECTOR_FRAME:
432 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
433 "not implemented");
434 break;
435 case vpRobot::ARTICULAR_FRAME:
436 break;
437 }
438
439 double artpos[2];
440
441 if (0 != ptu.getCurrentPosition(artpos)) {
442 vpERROR_TRACE("Error when calling recup_posit_Afma4.");
443 throw vpRobotException(vpRobotException::lowLevelError, "Error when calling recup_posit_Afma4.");
444 }
445
446 q.resize(vpPtu46::ndof);
447
448 q[0] = artpos[0];
449 q[1] = artpos[1];
450 }
451
452 /*!
453
454 Send a velocity on each axis.
455
456 \param frame : Control frame. This head can only be controlled in articular
457 and camera frame. Be aware, the reference frame (vpRobot::REFERENCE_FRAME)
458 end-effector frame (vpRobot::END_EFFECTOR_FRAME) and the mixt frame (vpRobot::MIXT_FRAME)
459 are not implemented.
460
461 \param v : The desired velocity of the axis. The size of this vector is
462 always 2. Velocities are expressed in rad/s.
463
464 - In camera frame, \f$ v = [\omega_x, \omega_y]^t \f$ expressed in rad/s.
465
466 - In articular, we control the 2 dof, \f$ v = [\dot{q}_1, \dot{q}_2]^t \f$
467 with \f$ \dot{q}_1 \f$ the pan of the camera and \f$ \dot{q}_2\f$ the tilt
468 of the camera in rad/s.
469
470 \exception vpRobotException::wrongStateError : If a the robot is not
471 configured to handle a velocity. The robot can handle a velocity only if the
472 velocity control mode is set. For that, call setRobotState(
473 vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
474
475 \exception vpRobotException::wrongStateError : If a non supported frame type
476 (vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::MIXT_FRAME) is given.
477
478 \warning Velocities could be saturated if one of them exceed the maximal
479 autorized speed (see vpRobot::maxRotationVelocity).
480 */
481
setVelocity(const vpRobot::vpControlFrameType frame,const vpColVector & v)482 void vpRobotPtu46::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
483 {
484 TPtuFrame ptuFrameInterface;
485
486 if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
487 vpERROR_TRACE("Cannot send a velocity to the robot "
488 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
489 throw vpRobotException(vpRobotException::wrongStateError,
490 "Cannot send a velocity to the robot "
491 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
492 }
493
494 switch (frame) {
495 case vpRobot::CAMERA_FRAME: {
496 ptuFrameInterface = PTU_CAMERA_FRAME;
497 if (v.getRows() != 2) {
498 vpERROR_TRACE("Bad dimension fo speed vector in camera frame");
499 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
500 "in camera frame");
501 }
502 break;
503 }
504 case vpRobot::ARTICULAR_FRAME: {
505 ptuFrameInterface = PTU_ARTICULAR_FRAME;
506 if (v.getRows() != 2) {
507 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
508 "in articular frame");
509 }
510 break;
511 }
512 case vpRobot::REFERENCE_FRAME: {
513 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
514 "in the reference frame:"
515 "functionality not implemented");
516 }
517 case vpRobot::MIXT_FRAME: {
518 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
519 "in the mixt frame:"
520 "functionality not implemented");
521 }
522 case vpRobot::END_EFFECTOR_FRAME: {
523 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
524 "in the end-effector frame:"
525 "functionality not implemented");
526 }
527 default: {
528 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
529 }
530 }
531
532 vpDEBUG_TRACE(12, "Velocity limitation.");
533 double ptuSpeedInterface[2];
534
535 switch (frame) {
536 case vpRobot::ARTICULAR_FRAME:
537 case vpRobot::CAMERA_FRAME: {
538 double max = this->maxRotationVelocity;
539 bool norm = false; // Flag to indicate when velocities need to be nomalized
540 for (unsigned int i = 0; i < 2; ++i) // rx and ry of the camera
541 {
542 if (fabs(v[i]) > max) {
543 norm = true;
544 max = fabs(v[i]);
545 vpERROR_TRACE("Excess velocity: ROTATION "
546 "(axe nr.%d).",
547 i);
548 }
549 }
550 // Rotations velocities normalisation
551 if (norm == true) {
552 max = this->maxRotationVelocity / max;
553 for (unsigned int i = 0; i < 2; ++i)
554 ptuSpeedInterface[i] = v[i] * max;
555 }
556 break;
557 }
558 default:
559 // Should never occur
560 break;
561 }
562
563 vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] << std::endl;
564 ptu.move(ptuSpeedInterface, ptuFrameInterface);
565 return;
566 }
567
568 /* -------------------------------------------------------------------------
569 */
570 /* --- GET -----------------------------------------------------------------
571 */
572 /* -------------------------------------------------------------------------
573 */
574
575 /*!
576
577 Get the articular velocity.
578
579 \param frame : Control frame. This head can only be controlled in articular.
580
581 \param q_dot : The measured articular velocity in rad/s.
582
583 \exception vpRobotException::wrongStateError : If a not supported frame type
584 is given.
585 */
getVelocity(const vpRobot::vpControlFrameType frame,vpColVector & q_dot)586 void vpRobotPtu46::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
587 {
588
589 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
590
591 switch (frame) {
592 case vpRobot::CAMERA_FRAME: {
593 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:"
594 "functionality not implemented");
595 }
596 case vpRobot::ARTICULAR_FRAME: {
597 ptuFrameInterface = PTU_ARTICULAR_FRAME;
598 break;
599 }
600 case vpRobot::REFERENCE_FRAME: {
601 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
602 "functionality not implemented");
603 }
604 case vpRobot::MIXT_FRAME: {
605 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
606 "functionality not implemented");
607 }
608 case vpRobot::END_EFFECTOR_FRAME: {
609 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
610 "functionality not implemented");
611 }
612 }
613
614 q_dot.resize(vpPtu46::ndof);
615 double ptuSpeedInterface[2];
616
617 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
618
619 q_dot[0] = ptuSpeedInterface[0];
620 q_dot[1] = ptuSpeedInterface[1];
621 }
622
623 /*!
624
625 Return the articular velocity.
626
627 \param frame : Control frame. This head can only be controlled in articular.
628
629 \return The measured articular velocity in rad/s.
630
631 \exception vpRobotException::wrongStateError : If a not supported frame type
632 is given.
633 */
getVelocity(vpRobot::vpControlFrameType frame)634 vpColVector vpRobotPtu46::getVelocity(vpRobot::vpControlFrameType frame)
635 {
636 vpColVector q_dot;
637 getVelocity(frame, q_dot);
638
639 return q_dot;
640 }
641
642 /*!
643
644 Get an articular position from the position file.
645
646 \param filename : Position file.
647
648 \param q : The articular position read in the file.
649
650 \code
651 # Example of ptu-46 position file
652 # The axis positions must be preceed by R:
653 # First value : pan articular position in degrees
654 # Second value: tilt articular position in degrees
655 R: 15.0 5.0
656 \endcode
657
658 \return true if a position was found, false otherwise.
659
660 */
readPositionFile(const std::string & filename,vpColVector & q)661 bool vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q)
662 {
663 std::ifstream fd(filename.c_str(), std::ios::in);
664
665 if (!fd.is_open()) {
666 return false;
667 }
668
669 std::string line;
670 std::string key("R:");
671 std::string id("#PTU-EVI - Position");
672 bool pos_found = false;
673 int lineNum = 0;
674
675 q.resize(vpPtu46::ndof);
676
677 while (std::getline(fd, line)) {
678 lineNum++;
679 if (lineNum == 1) {
680 if (!(line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file
681 std::cout << "Error: this position file " << filename << " is not for Ptu-46 robot" << std::endl;
682 return false;
683 }
684 }
685 if ((line.compare(0, 1, "#") == 0)) { // skip comment
686 continue;
687 }
688 if ((line.compare(0, key.size(), key) == 0)) { // decode position
689 // check if there are at least njoint values in the line
690 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
691 if (chain.size() < vpPtu46::ndof + 1) // try to split with tab separator
692 chain = vpIoTools::splitChain(line, std::string("\t"));
693 if (chain.size() < vpPtu46::ndof + 1)
694 continue;
695
696 std::istringstream ss(line);
697 std::string key_;
698 ss >> key_;
699 for (unsigned int i = 0; i < vpPtu46::ndof; i++)
700 ss >> q[i];
701 pos_found = true;
702 break;
703 }
704 }
705
706 // converts rotations from degrees into radians
707 q.deg2rad();
708
709 fd.close();
710
711 if (!pos_found) {
712 std::cout << "Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
713 return false;
714 }
715
716 return true;
717 }
718
719 /*!
720
721 Get the robot displacement since the last call of this method.
722
723 \warning The first call of this method gives not a good value for the
724 displacement.
725
726 \param frame The frame in which the measured displacement is expressed.
727
728 \param d The displacement:
729 - In articular, the dimension of q is 2 (the number of axis of the robot)
730 with respectively d[0] (pan displacement), d[1] (tilt displacement).
731 - In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
732 rz). Translations are expressed in meters, rotations in radians.
733
734 \exception vpRobotException::wrongStateError If a not supported frame type
735 is given.
736
737 */
getDisplacement(vpRobot::vpControlFrameType frame,vpColVector & d)738 void vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d)
739 {
740 double d_[6];
741
742 switch (frame) {
743 case vpRobot::CAMERA_FRAME: {
744 d.resize(6);
745 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
746 d[0] = d_[0];
747 d[1] = d_[1];
748 d[2] = d_[2];
749 d[3] = d_[3];
750 d[4] = d_[4];
751 d[5] = d_[5];
752 break;
753 }
754 case vpRobot::ARTICULAR_FRAME: {
755 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
756 d.resize(vpPtu46::ndof);
757 d[0] = d_[0]; // pan
758 d[1] = d_[1]; // tilt
759 break;
760 }
761 case vpRobot::REFERENCE_FRAME: {
762 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
763 "functionality not implemented");
764 }
765 case vpRobot::MIXT_FRAME: {
766 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
767 "functionality not implemented");
768 }
769 case vpRobot::END_EFFECTOR_FRAME: {
770 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
771 "functionality not implemented");
772 }
773 }
774 }
775
776 #elif !defined(VISP_BUILD_SHARED_LIBS)
777 // Work arround to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no
778 // symbols
dummy_vpRobotPtu46()779 void dummy_vpRobotPtu46(){};
780 #endif
781