Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
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 
53 bool vpRobotPtu46::robotAlreadyCreated = false;
55 
56 /* ----------------------------------------------------------------------- */
57 /* --- CONSTRUCTOR ------------------------------------------------------ */
58 /* ---------------------------------------------------------------------- */
59 
69 vpRobotPtu46::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  }
77  catch (...) {
78  vpERROR_TRACE("Error caught");
79  throw;
80  }
81 
82  try {
84  }
85  catch (...) {
86  vpERROR_TRACE("Error caught");
87  throw;
88  }
89  positioningVelocity = defaultPositioningVelocity;
90  return;
91 }
92 
93 /* ------------------------------------------------------------------------ */
94 /* --- DESTRUCTOR -------------------------------------------------------- */
95 /* ------------------------------------------------------------------------ */
96 
104 {
105 
107 
108  if (0 != ptu.close()) {
109  vpERROR_TRACE("Error while closing communications with the robot ptu-46.");
110  }
111 
112  vpRobotPtu46::robotAlreadyCreated = false;
113 
114  return;
115 }
116 
117 /* --------------------------------------------------------------------------
118  */
119 /* --- INITIALISATION -------------------------------------------------------
120  */
121 /* --------------------------------------------------------------------------
122  */
123 
134 {
135  vpDEBUG_TRACE(12, "Open connection Ptu-46.");
136  if (0 != ptu.init(device.c_str())) {
137  vpERROR_TRACE("Cannot open connection with ptu-46.");
138  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with ptu-46");
139  }
140 
141  return;
142 }
143 
151 {
152  switch (newState) {
153  case vpRobot::STATE_STOP: {
155  ptu.stop();
156  }
157  break;
158  }
161  vpDEBUG_TRACE(12, "Passage vitesse -> position.");
162  ptu.stop();
163  }
164  else {
165  vpDEBUG_TRACE(1, "Passage arret -> position.");
166  }
167  break;
168  }
171  vpDEBUG_TRACE(10, "Arret du robot...");
172  ptu.stop();
173  }
174  break;
175  }
176  default:
177  break;
178  }
179 
180  return vpRobot::setRobotState(newState);
181 }
182 
189 {
190  ptu.stop();
192 }
193 
205 {
207  vpPtu46::get_cMe(cMe);
208 
209  cVe.buildFrom(cMe);
210 }
211 
222 
234 {
235  vpColVector q(2);
237 
238  try {
239  vpPtu46::get_eJe(q, eJe);
240  }
241  catch (...) {
242  vpERROR_TRACE("catch exception ");
243  throw;
244  }
245 }
246 
255 {
256  vpColVector q(2);
258 
259  try {
260  vpPtu46::get_fJe(q, fJe);
261  }
262  catch (...) {
263  vpERROR_TRACE("Error caught");
264  throw;
265  }
266 }
267 
274 void vpRobotPtu46::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
281 double vpRobotPtu46::getPositioningVelocity(void) { return positioningVelocity; }
282 
300 {
301 
303  vpERROR_TRACE("Robot was not in position-based control\n"
304  "Modification of the robot state");
306  }
307 
308  switch (frame) {
310  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
311  "not implemented");
312  break;
314  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
315  "not implemented");
316  break;
317  case vpRobot::MIXT_FRAME:
318  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
319  "not implemented");
320  break;
322  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
323  "not implemented");
324  break;
326  break;
327  }
328 
329  // Interface for the controller
330  double artpos[2];
331 
332  artpos[0] = q[0];
333  artpos[1] = q[1];
334 
335  if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
336  vpERROR_TRACE("Positioning error.");
337  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error.");
338  }
339 
340  return;
341 }
342 
359 void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
360 {
361  try {
362  vpColVector q(2);
363  q[0] = q1;
364  q[1] = q2;
365 
366  setPosition(frame, q);
367  }
368  catch (...) {
369  vpERROR_TRACE("Error caught");
370  throw;
371  }
372 }
373 
387 void vpRobotPtu46::setPosition(const char *filename)
388 {
389  vpColVector q;
390  if (readPositionFile(filename, q) == false) {
391  vpERROR_TRACE("Cannot get ptu-46 position from file");
392  throw vpRobotException(vpRobotException::readingParametersError, "Cannot get ptu-46 position from file");
393  }
395 }
396 
411 {
412  vpDEBUG_TRACE(9, "# Entree.");
413 
414  switch (frame) {
416  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in camera frame: "
417  "not implemented");
418  break;
420  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in reference frame: "
421  "not implemented");
422  break;
423  case vpRobot::MIXT_FRAME:
424  throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
425  "not implemented");
426  break;
428  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
429  "not implemented");
430  break;
432  break;
433  }
434 
435  double artpos[2];
436 
437  if (0 != ptu.getCurrentPosition(artpos)) {
438  vpERROR_TRACE("Error when calling recup_posit_Afma4.");
439  throw vpRobotException(vpRobotException::lowLevelError, "Error when calling recup_posit_Afma4.");
440  }
441 
443 
444  q[0] = artpos[0];
445  q[1] = artpos[1];
446 }
447 
479 {
480  TPtuFrame ptuFrameInterface;
481 
483  vpERROR_TRACE("Cannot send a velocity to the robot "
484  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
486  "Cannot send a velocity to the robot "
487  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
488  }
489 
490  switch (frame) {
491  case vpRobot::CAMERA_FRAME: {
492  ptuFrameInterface = PTU_CAMERA_FRAME;
493  if (v.getRows() != 2) {
494  vpERROR_TRACE("Bad dimension fo speed vector in camera frame");
495  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
496  "in camera frame");
497  }
498  break;
499  }
501  ptuFrameInterface = PTU_ARTICULAR_FRAME;
502  if (v.getRows() != 2) {
503  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
504  "in articular frame");
505  }
506  break;
507  }
509  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
510  "in the reference frame:"
511  "functionality not implemented");
512  }
513  case vpRobot::MIXT_FRAME: {
514  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
515  "in the mixt frame:"
516  "functionality not implemented");
517  }
519  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
520  "in the end-effector frame:"
521  "functionality not implemented");
522  }
523  default: {
524  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
525  }
526  }
527 
528  vpDEBUG_TRACE(12, "Velocity limitation.");
529  double ptuSpeedInterface[2];
530 
531  switch (frame) {
533  case vpRobot::CAMERA_FRAME: {
534  double max = this->maxRotationVelocity;
535  bool norm = false; // Flag to indicate when velocities need to be normalized
536  for (unsigned int i = 0; i < 2; ++i) // rx and ry of the camera
537  {
538  if (fabs(v[i]) > max) {
539  norm = true;
540  max = fabs(v[i]);
541  vpERROR_TRACE("Excess velocity: ROTATION "
542  "(axe nr.%d).",
543  i);
544  }
545  }
546  // Rotations velocities normalization
547  if (norm == true) {
548  max = this->maxRotationVelocity / max;
549  for (unsigned int i = 0; i < 2; ++i)
550  ptuSpeedInterface[i] = v[i] * max;
551  }
552  break;
553  }
554  default:
555  // Should never occur
556  break;
557  }
558 
559  vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] << std::endl;
560  ptu.move(ptuSpeedInterface, ptuFrameInterface);
561  return;
562 }
563 
564 /* -------------------------------------------------------------------------
565  */
566 /* --- GET -----------------------------------------------------------------
567  */
568 /* -------------------------------------------------------------------------
569  */
570 
583 {
584 
585  TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
586 
587  switch (frame) {
588  case vpRobot::CAMERA_FRAME: {
589  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:"
590  "functionality not implemented");
591  }
593  ptuFrameInterface = PTU_ARTICULAR_FRAME;
594  break;
595  }
597  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
598  "functionality not implemented");
599  }
600  case vpRobot::MIXT_FRAME: {
601  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
602  "functionality not implemented");
603  }
605  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
606  "functionality not implemented");
607  }
608  }
609 
610  q_dot.resize(vpPtu46::ndof);
611  double ptuSpeedInterface[2];
612 
613  ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
614 
615  q_dot[0] = ptuSpeedInterface[0];
616  q_dot[1] = ptuSpeedInterface[1];
617 }
618 
631 {
632  vpColVector q_dot;
633  getVelocity(frame, q_dot);
634 
635  return q_dot;
636 }
637 
657 bool vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q)
658 {
659  std::ifstream fd(filename.c_str(), std::ios::in);
660 
661  if (!fd.is_open()) {
662  return false;
663  }
664 
665  std::string line;
666  std::string key("R:");
667  std::string id("#PTU-EVI - Position");
668  bool pos_found = false;
669  int lineNum = 0;
670 
672 
673  while (std::getline(fd, line)) {
674  lineNum++;
675  if (lineNum == 1) {
676  if (!(line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file
677  std::cout << "Error: this position file " << filename << " is not for Ptu-46 robot" << std::endl;
678  return false;
679  }
680  }
681  if ((line.compare(0, 1, "#") == 0)) { // skip comment
682  continue;
683  }
684  if ((line.compare(0, key.size(), key) == 0)) { // decode position
685  // check if there are at least njoint values in the line
686  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
687  if (chain.size() < vpPtu46::ndof + 1) // try to split with tab separator
688  chain = vpIoTools::splitChain(line, std::string("\t"));
689  if (chain.size() < vpPtu46::ndof + 1)
690  continue;
691 
692  std::istringstream ss(line);
693  std::string key_;
694  ss >> key_;
695  for (unsigned int i = 0; i < vpPtu46::ndof; i++)
696  ss >> q[i];
697  pos_found = true;
698  break;
699  }
700  }
701 
702  // converts rotations from degrees into radians
703  q.deg2rad();
704 
705  fd.close();
706 
707  if (!pos_found) {
708  std::cout << "Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
709  return false;
710  }
711 
712  return true;
713 }
714 
735 {
736  double d_[6];
737 
738  switch (frame) {
739  case vpRobot::CAMERA_FRAME: {
740  d.resize(6);
741  ptu.measureDpl(d_, PTU_CAMERA_FRAME);
742  d[0] = d_[0];
743  d[1] = d_[1];
744  d[2] = d_[2];
745  d[3] = d_[3];
746  d[4] = d_[4];
747  d[5] = d_[5];
748  break;
749  }
751  ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
752  d.resize(vpPtu46::ndof);
753  d[0] = d_[0]; // pan
754  d[1] = d_[1]; // tilt
755  break;
756  }
758  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
759  "functionality not implemented");
760  }
761  case vpRobot::MIXT_FRAME: {
762  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
763  "functionality not implemented");
764  }
766  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the end-effector frame:"
767  "functionality not implemented");
768  }
769  }
770 }
771 
772 #elif !defined(VISP_BUILD_SHARED_LIBS)
773 // Work around to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no symbols
774 void dummy_vpRobotPtu46() { };
775 #endif
unsigned int getRows() const
Definition: vpArray2D.h:324
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & deg2rad()
Definition: vpColVector.h:342
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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:2425
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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) vp_override
void setPositioningVelocity(double velocity)
void get_eJe(vpMatrix &_eJe) vp_override
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
double getPositioningVelocity(void)
void get_fJe(vpMatrix &_fJe) vp_override
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
static const double defaultPositioningVelocity
Definition: vpRobotPtu46.h:94
virtual ~vpRobotPtu46(void)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
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:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
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:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
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:198
double maxRotationVelocity
Definition: vpRobot.h:98
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:497
#define vpDEBUG_TRACE
Definition: vpDebug.h:473
#define vpERROR_TRACE
Definition: vpDebug.h:382