Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
56 bool vpRobotPtu46::robotAlreadyCreated = false;
58 
59 /* ----------------------------------------------------------------------- */
60 /* --- CONSTRUCTOR ------------------------------------------------------ */
61 /* ---------------------------------------------------------------------- */
62 
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 {
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 {
246  vpPtu46::get_eJe(q, eJe);
247  } catch (...) {
248  vpERROR_TRACE("catch exception ");
249  throw;
250  }
251 }
252 
261 {
262  vpColVector q(2);
264 
265  try {
266  vpPtu46::get_fJe(q, fJe);
267  } catch (...) {
268  vpERROR_TRACE("Error caught");
269  throw;
270  }
271 }
272 
279 void vpRobotPtu46::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
286 double 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;
322  case vpRobot::MIXT_FRAME:
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 
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 
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  }
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;
427  case vpRobot::MIXT_FRAME:
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) {
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  }
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) {
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 
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  }
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 
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 
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) {
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  }
755  ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
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
779 void dummy_vpRobotPtu46(){};
780 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpPtu46.cpp:278
Error that can be emited by the vpRobot class and its derivates.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void get_cMe(vpHomogeneousMatrix &_cMe) const
#define vpERROR_TRACE
Definition: vpDebug.h:393
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
static const double defaultPositioningVelocity
Definition: vpRobotPtu46.h:97
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
double maxRotationVelocity
Definition: vpRobot.h:98
Initialize the position controller.
Definition: vpRobot.h:68
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q)
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
vpControlFrameType
Definition: vpRobot.h:75
void get_fJe(vpMatrix &_fJe)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void deg2rad()
Definition: vpColVector.h:134
void get_cMe(vpHomogeneousMatrix &_cMe) const
Definition: vpPtu46.cpp:210
Initialize the velocity controller.
Definition: vpRobot.h:67
static const unsigned int ndof
Definition: vpPtu46.h:80
vpRobotStateType
Definition: vpRobot.h:64
virtual ~vpRobotPtu46(void)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
void init(void)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1771
unsigned int getRows() const
Definition: vpArray2D.h:156
void get_eJe(vpMatrix &_eJe)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpPtu46.cpp:249
void get_cVe(vpVelocityTwistMatrix &_cVe) const
double getPositioningVelocity(void)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
bool readPositionFile(const std::string &filename, vpColVector &q)
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244