Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpRobotPtu46.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  *****************************************************************************/
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  } 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 {
236  vpPtu46::get_eJe(q, eJe);
237  } catch (...) {
238  vpERROR_TRACE("catch exception ");
239  throw;
240  }
241 }
242 
251 {
252  vpColVector q(2);
254 
255  try {
256  vpPtu46::get_fJe(q, fJe);
257  } catch (...) {
258  vpERROR_TRACE("Error caught");
259  throw;
260  }
261 }
262 
269 void vpRobotPtu46::setPositioningVelocity(double velocity) { positioningVelocity = velocity; }
276 double 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;
312  case vpRobot::MIXT_FRAME:
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("Positionning error.");
332  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error.");
333  }
334 
335  return;
336 }
337 
354 void 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 
381 void 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;
417  case vpRobot::MIXT_FRAME:
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) {
485  case vpRobot::CAMERA_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) {
527  case vpRobot::CAMERA_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) {
582  case vpRobot::CAMERA_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 
651 bool 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) {
733  case vpRobot::CAMERA_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
769 void dummy_vpRobotPtu46(){};
770 #endif
unsigned int getRows() const
Definition: vpArray2D.h:288
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector & deg2rad()
Definition: vpColVector.h:197
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
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:1929
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:94
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:60
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:105
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:109
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double maxRotationVelocity
Definition: vpRobot.h:99
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