Visual Servoing Platform version 3.5.0
vpRobotPtu46.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the 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
56bool vpRobotPtu46::robotAlreadyCreated = false;
58
59/* ----------------------------------------------------------------------- */
60/* --- CONSTRUCTOR ------------------------------------------------------ */
61/* ---------------------------------------------------------------------- */
62
72vpRobotPtu46::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 {
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
109{
110
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
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
159{
160 switch (newState) {
161 case vpRobot::STATE_STOP: {
163 ptu.stop();
164 }
165 break;
166 }
169 vpDEBUG_TRACE(12, "Passage vitesse -> position.");
170 ptu.stop();
171 } else {
172 vpDEBUG_TRACE(1, "Passage arret -> position.");
173 }
174 break;
175 }
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
196{
197 ptu.stop();
199}
200
212{
214 vpPtu46::get_cMe(cMe);
215
216 cVe.buildFrom(cMe);
217}
218
229
241{
242 vpColVector q(2);
244
245 try {
247 } catch (...) {
248 vpERROR_TRACE("catch exception ");
249 throw;
250 }
251}
252
261{
262 vpColVector q(2);
264
265 try {
267 } catch (...) {
268 vpERROR_TRACE("Error caught");
269 throw;
270 }
271}
272
279void vpRobotPtu46::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
286double vpRobotPtu46::getPositioningVelocity(void) { return positioningVelocity; }
287
305{
306
308 vpERROR_TRACE("Robot was not in position-based control\n"
309 "Modification of the robot state");
311 }
312
313 switch (frame) {
315 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
316 "not implemented");
317 break;
319 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
320 "not implemented");
321 break;
323 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
324 "not implemented");
325 break;
327 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
328 "not implemented");
329 break;
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
364void 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
391void 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 }
399}
400
415{
416 vpDEBUG_TRACE(9, "# Entree.");
417
418 switch (frame) {
420 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in camera frame: "
421 "not implemented");
422 break;
424 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in reference frame: "
425 "not implemented");
426 break;
428 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
429 "not implemented");
430 break;
432 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
433 "not implemented");
434 break;
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
447
448 q[0] = artpos[0];
449 q[1] = artpos[1];
450}
451
483{
484 TPtuFrame ptuFrameInterface;
485
487 vpERROR_TRACE("Cannot send a velocity to the robot "
488 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
490 "Cannot send a velocity to the robot "
491 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
492 }
493
494 switch (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 }
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 }
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 }
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) {
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
587{
588
589 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
590
591 switch (frame) {
593 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:"
594 "functionality not implemented");
595 }
597 ptuFrameInterface = PTU_ARTICULAR_FRAME;
598 break;
599 }
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 }
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
635{
636 vpColVector q_dot;
637 getVelocity(frame, q_dot);
638
639 return q_dot;
640}
641
661bool 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
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
739{
740 double d_[6];
741
742 switch (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 }
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 }
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 }
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
779void dummy_vpRobotPtu46(){};
780#endif
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void deg2rad()
Definition: vpColVector.h:196
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of an homogeneous matrix and operations on such kind of matrices.
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:210
static const unsigned int ndof
Definition: vpPtu46.h:78
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:278
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:249
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
void setPositioningVelocity(double velocity)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
double getPositioningVelocity(void)
void get_eJe(vpMatrix &_eJe)
void get_fJe(vpMatrix &_fJe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static const double defaultPositioningVelocity
Definition: vpRobotPtu46.h:97
virtual ~vpRobotPtu46(void)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void get_cMe(vpHomogeneousMatrix &_cMe) const
bool readPositionFile(const std::string &filename, vpColVector &q)
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q)
void init(void)
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double maxRotationVelocity
Definition: vpRobot.h:98
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393