Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotPtu46.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the ptu-46 robot.
33 *
34*****************************************************************************/
35
36#include <signal.h>
37#include <string.h>
38
39#include <visp3/core/vpConfig.h>
40#ifdef VISP_HAVE_PTU46
41
42/* Headers des fonctions implementees. */
43#include <visp3/core/vpDebug.h>
44#include <visp3/core/vpIoTools.h>
45#include <visp3/robot/vpPtu46.h>
46#include <visp3/robot/vpRobotException.h>
47#include <visp3/robot/vpRobotPtu46.h>
48
49/* ---------------------------------------------------------------------- */
50/* --- STATIC ------------------------------------------------------------ */
51/* ------------------------------------------------------------------------ */
52
53bool vpRobotPtu46::robotAlreadyCreated = false;
55
56/* ----------------------------------------------------------------------- */
57/* --- CONSTRUCTOR ------------------------------------------------------ */
58/* ---------------------------------------------------------------------- */
59
69vpRobotPtu46::vpRobotPtu46(const std::string &device) : vpRobot()
70{
71 this->device = device;
72
73 vpDEBUG_TRACE(12, "Open communication with Ptu-46.");
74 try {
75 init();
76 } catch (...) {
77 vpERROR_TRACE("Error caught");
78 throw;
79 }
80
81 try {
83 } catch (...) {
84 vpERROR_TRACE("Error caught");
85 throw;
86 }
87 positioningVelocity = defaultPositioningVelocity;
88 return;
89}
90
91/* ------------------------------------------------------------------------ */
92/* --- DESTRUCTOR -------------------------------------------------------- */
93/* ------------------------------------------------------------------------ */
94
102{
103
105
106 if (0 != ptu.close()) {
107 vpERROR_TRACE("Error while closing communications with the robot ptu-46.");
108 }
109
110 vpRobotPtu46::robotAlreadyCreated = false;
111
112 return;
113}
114
115/* --------------------------------------------------------------------------
116 */
117/* --- INITIALISATION -------------------------------------------------------
118 */
119/* --------------------------------------------------------------------------
120 */
121
132{
133 vpDEBUG_TRACE(12, "Open connection Ptu-46.");
134 if (0 != ptu.init(device.c_str())) {
135 vpERROR_TRACE("Cannot open connection with ptu-46.");
136 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with ptu-46");
137 }
138
139 return;
140}
141
149{
150 switch (newState) {
151 case vpRobot::STATE_STOP: {
153 ptu.stop();
154 }
155 break;
156 }
159 vpDEBUG_TRACE(12, "Passage vitesse -> position.");
160 ptu.stop();
161 } else {
162 vpDEBUG_TRACE(1, "Passage arret -> position.");
163 }
164 break;
165 }
168 vpDEBUG_TRACE(10, "Arret du robot...");
169 ptu.stop();
170 }
171 break;
172 }
173 default:
174 break;
175 }
176
177 return vpRobot::setRobotState(newState);
178}
179
186{
187 ptu.stop();
189}
190
202{
204 vpPtu46::get_cMe(cMe);
205
206 cVe.buildFrom(cMe);
207}
208
219
231{
232 vpColVector q(2);
234
235 try {
237 } catch (...) {
238 vpERROR_TRACE("catch exception ");
239 throw;
240 }
241}
242
251{
252 vpColVector q(2);
254
255 try {
257 } catch (...) {
258 vpERROR_TRACE("Error caught");
259 throw;
260 }
261}
262
269void vpRobotPtu46::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
276double vpRobotPtu46::getPositioningVelocity(void) { return positioningVelocity; }
277
295{
296
298 vpERROR_TRACE("Robot was not in position-based control\n"
299 "Modification of the robot state");
301 }
302
303 switch (frame) {
305 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
306 "not implemented");
307 break;
309 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
310 "not implemented");
311 break;
313 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
314 "not implemented");
315 break;
317 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
318 "not implemented");
319 break;
321 break;
322 }
323
324 // Interface for the controller
325 double artpos[2];
326
327 artpos[0] = q[0];
328 artpos[1] = q[1];
329
330 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
331 vpERROR_TRACE("Positioning error.");
332 throw vpRobotException(vpRobotException::lowLevelError, "Positioning error.");
333 }
334
335 return;
336}
337
354void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
355{
356 try {
357 vpColVector q(2);
358 q[0] = q1;
359 q[1] = q2;
360
361 setPosition(frame, q);
362 } catch (...) {
363 vpERROR_TRACE("Error caught");
364 throw;
365 }
366}
367
381void vpRobotPtu46::setPosition(const char *filename)
382{
383 vpColVector q;
384 if (readPositionFile(filename, q) == false) {
385 vpERROR_TRACE("Cannot get ptu-46 position from file");
386 throw vpRobotException(vpRobotException::readingParametersError, "Cannot get ptu-46 position from file");
387 }
389}
390
405{
406 vpDEBUG_TRACE(9, "# Entree.");
407
408 switch (frame) {
410 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in camera frame: "
411 "not implemented");
412 break;
414 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in reference frame: "
415 "not implemented");
416 break;
418 throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
419 "not implemented");
420 break;
422 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
423 "not implemented");
424 break;
426 break;
427 }
428
429 double artpos[2];
430
431 if (0 != ptu.getCurrentPosition(artpos)) {
432 vpERROR_TRACE("Error when calling recup_posit_Afma4.");
433 throw vpRobotException(vpRobotException::lowLevelError, "Error when calling recup_posit_Afma4.");
434 }
435
437
438 q[0] = artpos[0];
439 q[1] = artpos[1];
440}
441
473{
474 TPtuFrame ptuFrameInterface;
475
477 vpERROR_TRACE("Cannot send a velocity to the robot "
478 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
480 "Cannot send a velocity to the robot "
481 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
482 }
483
484 switch (frame) {
486 ptuFrameInterface = PTU_CAMERA_FRAME;
487 if (v.getRows() != 2) {
488 vpERROR_TRACE("Bad dimension fo speed vector in camera frame");
489 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
490 "in camera frame");
491 }
492 break;
493 }
495 ptuFrameInterface = PTU_ARTICULAR_FRAME;
496 if (v.getRows() != 2) {
497 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
498 "in articular frame");
499 }
500 break;
501 }
503 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
504 "in the reference frame:"
505 "functionality not implemented");
506 }
507 case vpRobot::MIXT_FRAME: {
508 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
509 "in the mixt frame:"
510 "functionality not implemented");
511 }
513 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
514 "in the end-effector frame:"
515 "functionality not implemented");
516 }
517 default: {
518 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
519 }
520 }
521
522 vpDEBUG_TRACE(12, "Velocity limitation.");
523 double ptuSpeedInterface[2];
524
525 switch (frame) {
528 double max = this->maxRotationVelocity;
529 bool norm = false; // Flag to indicate when velocities need to be nomalized
530 for (unsigned int i = 0; i < 2; ++i) // rx and ry of the camera
531 {
532 if (fabs(v[i]) > max) {
533 norm = true;
534 max = fabs(v[i]);
535 vpERROR_TRACE("Excess velocity: ROTATION "
536 "(axe nr.%d).",
537 i);
538 }
539 }
540 // Rotations velocities normalisation
541 if (norm == true) {
542 max = this->maxRotationVelocity / max;
543 for (unsigned int i = 0; i < 2; ++i)
544 ptuSpeedInterface[i] = v[i] * max;
545 }
546 break;
547 }
548 default:
549 // Should never occur
550 break;
551 }
552
553 vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] << std::endl;
554 ptu.move(ptuSpeedInterface, ptuFrameInterface);
555 return;
556}
557
558/* -------------------------------------------------------------------------
559 */
560/* --- GET -----------------------------------------------------------------
561 */
562/* -------------------------------------------------------------------------
563 */
564
577{
578
579 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
580
581 switch (frame) {
583 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:"
584 "functionality not implemented");
585 }
587 ptuFrameInterface = PTU_ARTICULAR_FRAME;
588 break;
589 }
591 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
592 "functionality not implemented");
593 }
594 case vpRobot::MIXT_FRAME: {
595 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
596 "functionality not implemented");
597 }
599 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
600 "functionality not implemented");
601 }
602 }
603
604 q_dot.resize(vpPtu46::ndof);
605 double ptuSpeedInterface[2];
606
607 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
608
609 q_dot[0] = ptuSpeedInterface[0];
610 q_dot[1] = ptuSpeedInterface[1];
611}
612
625{
626 vpColVector q_dot;
627 getVelocity(frame, q_dot);
628
629 return q_dot;
630}
631
651bool vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q)
652{
653 std::ifstream fd(filename.c_str(), std::ios::in);
654
655 if (!fd.is_open()) {
656 return false;
657 }
658
659 std::string line;
660 std::string key("R:");
661 std::string id("#PTU-EVI - Position");
662 bool pos_found = false;
663 int lineNum = 0;
664
666
667 while (std::getline(fd, line)) {
668 lineNum++;
669 if (lineNum == 1) {
670 if (!(line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file
671 std::cout << "Error: this position file " << filename << " is not for Ptu-46 robot" << std::endl;
672 return false;
673 }
674 }
675 if ((line.compare(0, 1, "#") == 0)) { // skip comment
676 continue;
677 }
678 if ((line.compare(0, key.size(), key) == 0)) { // decode position
679 // check if there are at least njoint values in the line
680 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
681 if (chain.size() < vpPtu46::ndof + 1) // try to split with tab separator
682 chain = vpIoTools::splitChain(line, std::string("\t"));
683 if (chain.size() < vpPtu46::ndof + 1)
684 continue;
685
686 std::istringstream ss(line);
687 std::string key_;
688 ss >> key_;
689 for (unsigned int i = 0; i < vpPtu46::ndof; i++)
690 ss >> q[i];
691 pos_found = true;
692 break;
693 }
694 }
695
696 // converts rotations from degrees into radians
697 q.deg2rad();
698
699 fd.close();
700
701 if (!pos_found) {
702 std::cout << "Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
703 return false;
704 }
705
706 return true;
707}
708
729{
730 double d_[6];
731
732 switch (frame) {
734 d.resize(6);
735 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
736 d[0] = d_[0];
737 d[1] = d_[1];
738 d[2] = d_[2];
739 d[3] = d_[3];
740 d[4] = d_[4];
741 d[5] = d_[5];
742 break;
743 }
745 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
746 d.resize(vpPtu46::ndof);
747 d[0] = d_[0]; // pan
748 d[1] = d_[1]; // tilt
749 break;
750 }
752 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
753 "functionality not implemented");
754 }
755 case vpRobot::MIXT_FRAME: {
756 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
757 "functionality not implemented");
758 }
760 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
761 "functionality not implemented");
762 }
763 }
764}
765
766#elif !defined(VISP_BUILD_SHARED_LIBS)
767// Work around to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no
768// symbols
769void dummy_vpRobotPtu46(){};
770#endif
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
vpColVector & deg2rad()
void resize(unsigned int i, bool flagNullify=true)
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)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition vpPtu46.cpp:207
static const unsigned int ndof
Definition vpPtu46.h:75
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpPtu46.cpp:275
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpPtu46.cpp:246
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ readingParametersError
Cannot parse parameters.
@ lowLevelError
Error thrown by the low level sdk.
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
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:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition vpRobot.h:102
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition vpRobot.h:106
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double maxRotationVelocity
Definition vpRobot.h:96
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition vpDebug.h:506
#define vpDEBUG_TRACE
Definition vpDebug.h:482
#define vpERROR_TRACE
Definition vpDebug.h:388