ViSP  2.8.0
vpRobotPtu46.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotPtu46.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the ptu-46 robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <signal.h>
43 #include <string.h>
44 
45 #include <visp/vpConfig.h>
46 #ifdef VISP_HAVE_PTU46
47 
48 /* Headers des fonctions implementees. */
49 #include <visp/vpPtu46.h>
50 #include <visp/vpRobotPtu46.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpDebug.h>
53 
54 /* ---------------------------------------------------------------------- */
55 /* --- STATIC ------------------------------------------------------------ */
56 /* ------------------------------------------------------------------------ */
57 
58 bool vpRobotPtu46::robotAlreadyCreated = false;
60 
61 /* ----------------------------------------------------------------------- */
62 /* --- CONSTRUCTOR ------------------------------------------------------ */
63 /* ---------------------------------------------------------------------- */
64 
65 
75 vpRobotPtu46::vpRobotPtu46 (const char *device)
76  :
77  vpRobot ()
78 {
79  this->device = new char [FILENAME_MAX];
80 
81  sprintf(this->device, "%s", device);
82 
83  vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
84  try
85  {
86  init();
87  }
88  catch(...)
89  {
90  delete [] this->device;
91  vpERROR_TRACE("Error caught") ;
92  throw ;
93  }
94 
95  try
96  {
98  }
99  catch(...)
100  {
101  delete [] this->device;
102  vpERROR_TRACE("Error caught") ;
103  throw ;
104  }
105  positioningVelocity = defaultPositioningVelocity ;
106  return ;
107 }
108 
109 /* ------------------------------------------------------------------------ */
110 /* --- DESTRUCTOR -------------------------------------------------------- */
111 /* ------------------------------------------------------------------------ */
112 
120 {
121 
123 
124  if (0 != ptu.close())
125  {
126  vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
127  }
128 
129  vpRobotPtu46::robotAlreadyCreated = false;
130 
131  delete [] device;
132 
133  return;
134 }
135 
136 
137 /* -------------------------------------------------------------------------- */
138 /* --- INITIALISATION ------------------------------------------------------- */
139 /* -------------------------------------------------------------------------- */
140 
150 void
152 {
153 
154  vpDEBUG_TRACE (12, "Open connection Ptu-46.");
155  if (0 != ptu.init(device) )
156  {
157  vpERROR_TRACE ("Cannot open connexion with ptu-46.");
159  "Cannot open connexion with ptu-46");
160  }
161 
162  return ;
163 }
164 
165 
174 {
175  switch (newState)
176  {
177  case vpRobot::STATE_STOP:
178  {
180  {
181  ptu.stop();
182  }
183  break;
184  }
186  {
188  {
189  vpDEBUG_TRACE (12, "Passage vitesse -> position.");
190  ptu.stop();
191  }
192  else
193  {
194  vpDEBUG_TRACE (1, "Passage arret -> position.");
195  }
196  break;
197  }
199  {
201  {
202  vpDEBUG_TRACE (10, "Arret du robot...");
203  ptu.stop();
204  }
205  break;
206  }
207  default:
208  break ;
209  }
210 
211  return vpRobot::setRobotState (newState);
212 }
213 
219 void
221 {
222  ptu.stop();
224 }
225 
226 
237 void
239 {
240  vpHomogeneousMatrix cMe ;
241  vpPtu46::get_cMe(cMe) ;
242 
243  cVe.buildFrom(cMe) ;
244 }
245 
255 void
257 {
258  vpPtu46::get_cMe(cMe) ;
259 }
260 
271 void
273 {
274  vpColVector q(2) ;
276 
277  try
278  {
279  vpPtu46::get_eJe(q,eJe) ;
280  }
281  catch(...)
282  {
283  vpERROR_TRACE("catch exception ") ;
284  throw ;
285  }
286 }
287 
295 void
297 {
298  vpColVector q(2) ;
300 
301  try
302  {
303  vpPtu46::get_fJe(q,fJe) ;
304  }
305  catch(...)
306  {
307  vpERROR_TRACE("Error caught");
308  throw ;
309  }
310 
311 }
312 
313 
320 void
322 {
323  positioningVelocity = velocity;
324 }
331 double
333 {
334  return positioningVelocity;
335 }
336 
337 
354 void
356  const vpColVector & q )
357 {
358 
360  {
361  vpERROR_TRACE ("Robot was not in position-based control\n"
362  "Modification of the robot state");
364  }
365 
366  switch(frame)
367  {
369  vpERROR_TRACE ("Cannot move the robot in camera frame: "
370  "not implemented");
372  "Cannot move the robot in camera frame: "
373  "not implemented");
374  break;
376  vpERROR_TRACE ("Cannot move the robot in reference frame: "
377  "not implemented");
379  "Cannot move the robot in reference frame: "
380  "not implemented");
381  break;
382  case vpRobot::MIXT_FRAME:
383  vpERROR_TRACE ("Cannot move the robot in mixt frame: "
384  "not implemented");
386  "Cannot move the robot in mixt frame: "
387  "not implemented");
388  break;
390  break ;
391  }
392 
393  // Interface for the controller
394  double artpos[2];
395 
396  artpos[0] = q[0];
397  artpos[1] = q[1];
398 
399  if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
400  {
401  vpERROR_TRACE ("Positionning error.");
403  "Positionning error.");
404  }
405 
406  return ;
407 }
408 
409 
426 void
428  const double &q1, const double &q2)
429 {
430  try{
431  vpColVector q(2) ;
432  q[0] = q1 ;
433  q[1] = q2 ;
434 
435  setPosition(frame,q) ;
436  }
437  catch(...)
438  {
439  vpERROR_TRACE("Error caught");
440  throw ;
441  }
442 }
443 
457 void
458 vpRobotPtu46::setPosition(const char *filename)
459 {
460  vpColVector q ;
461  if (readPositionFile(filename, q) == false) {
462  vpERROR_TRACE ("Cannot get ptu-46 position from file");
464  "Cannot get ptu-46 position from file");
465  }
467 }
468 
482 void
484  vpColVector & q)
485 {
486  vpDEBUG_TRACE (9, "# Entree.");
487 
488  switch(frame)
489  {
490  case vpRobot::CAMERA_FRAME :
491  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
493  "Cannot get position in camera frame: "
494  "not implemented");
495  break;
497  vpERROR_TRACE ("Cannot get position in reference frame: "
498  "not implemented");
500  "Cannot get position in reference frame: "
501  "not implemented");
502  break;
503  case vpRobot::MIXT_FRAME:
504  vpERROR_TRACE ("Cannot get position in mixt frame: "
505  "not implemented");
507  "Cannot get position in mixt frame: "
508  "not implemented");
509  break;
511  break ;
512  }
513 
514  double artpos[2];
515 
516  if (0 != ptu.getCurrentPosition( artpos ) )
517  {
518  vpERROR_TRACE ("Error when calling recup_posit_Afma4.");
520  "Error when calling recup_posit_Afma4.");
521  }
522 
523  q.resize (vpPtu46::ndof);
524 
525  q[0] = artpos[0];
526  q[1] = artpos[1];
527 }
528 
529 
559 void
561  const vpColVector & v)
562 {
563  TPtuFrame ptuFrameInterface;
564 
566  {
567  vpERROR_TRACE ("Cannot send a velocity to the robot "
568  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
570  "Cannot send a velocity to the robot "
571  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
572  }
573 
574  switch(frame)
575  {
576  case vpRobot::CAMERA_FRAME :
577  {
578  ptuFrameInterface = PTU_CAMERA_FRAME;
579  if ( v.getRows() != 2) {
580  vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
582  "Bad dimension for speed vector "
583  "in camera frame");
584  }
585  break ;
586  }
588  {
589  ptuFrameInterface = PTU_ARTICULAR_FRAME;
590  if ( v.getRows() != 2) {
591  vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
593  "Bad dimension for speed vector "
594  "in articular frame");
595  }
596  break ;
597  }
599  {
600  vpERROR_TRACE ("Cannot send a velocity to the robot "
601  "in the reference frame: "
602  "functionality not implemented");
604  "Cannot send a velocity to the robot "
605  "in the reference frame:"
606  "functionality not implemented");
607  break ;
608  }
609  case vpRobot::MIXT_FRAME :
610  {
611  vpERROR_TRACE ("Cannot send a velocity to the robot "
612  "in the mixt frame: "
613  "functionality not implemented");
615  "Cannot send a velocity to the robot "
616  "in the mixt frame:"
617  "functionality not implemented");
618  break ;
619  }
620  default:
621  {
622  vpERROR_TRACE ("Error in spec of vpRobot. "
623  "Case not taken in account.");
625  "Cannot send a velocity to the robot ");
626  }
627  }
628 
629  vpDEBUG_TRACE (12, "Velocity limitation.");
630  bool norm = false; // Flag to indicate when velocities need to be nomalized
631  double ptuSpeedInterface[2];
632 
633  switch(frame) {
635  case vpRobot::CAMERA_FRAME : {
636  double max = this ->maxRotationVelocity;
637  for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
638  {
639  if (fabs (v[i]) > max)
640  {
641  norm = true;
642  max = fabs (v[i]);
643  vpERROR_TRACE ("Excess velocity: ROTATION "
644  "(axe nr.%d).", i);
645  }
646  }
647  // Rotations velocities normalisation
648  if (norm == true) {
649  max = this ->maxRotationVelocity / max;
650  for (unsigned int i = 0 ; i < 2; ++ i)
651  ptuSpeedInterface [i] = v[i]*max;
652  }
653  break;
654  }
655  default:
656  // Should never occur
657  break;
658 
659  }
660 
661  vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
662  << " " << ptuSpeedInterface[1] << std::endl;
663  ptu.move(ptuSpeedInterface, ptuFrameInterface);
664  return;
665 }
666 
667 
668 /* ------------------------------------------------------------------------- */
669 /* --- GET ----------------------------------------------------------------- */
670 /* ------------------------------------------------------------------------- */
671 
672 
684 void
686  vpColVector & q_dot)
687 {
688 
689  TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
690 
691  switch (frame)
692  {
694  {
695  vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
696  "functionality not implemented");
698  "Cannot get a velocity in the camera frame:"
699  "functionality not implemented");
700  break ;
701  }
703  {
704  ptuFrameInterface = PTU_ARTICULAR_FRAME;
705  break ;
706  }
708  {
709  vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
710  "functionality not implemented");
712  "Cannot get a velocity in the reference frame:"
713  "functionality not implemented");
714  break ;
715  }
716  case vpRobot::MIXT_FRAME:
717  {
718 
719  vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
720  "functionality not implemented");
722  "Cannot get a velocity in the mixt frame:"
723  "functionality not implemented");
724  break ;
725  }
726  }
727 
728  q_dot.resize(vpPtu46::ndof);
729  double ptuSpeedInterface[2];
730 
731  ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
732 
733  q_dot[0] = ptuSpeedInterface[0];
734  q_dot[1] = ptuSpeedInterface[1];
735 
736 }
737 
738 
752 {
753  vpColVector q_dot;
754  getVelocity (frame, q_dot);
755 
756  return q_dot;
757 }
758 
778 bool
780 {
781  FILE * pt_f ;
782  pt_f = fopen(filename,"r") ;
783 
784  if (pt_f == NULL) {
785  vpERROR_TRACE ("Can not open ptu-46 position file %s", filename);
786  return false;
787  }
788 
789  char line[FILENAME_MAX];
790  char head[] = "R:";
791  bool end = false;
792 
793  do {
794  // skip lines begining with # for comments
795  if (fgets (line, 100, pt_f) != NULL) {
796  if ( strncmp (line, "#", 1) != 0) {
797  // this line is not a comment
798  if ( fscanf (pt_f, "%s", line) != EOF) {
799  if ( strcmp (line, head) == 0)
800  end = true; // robot position was found
801  }
802  else
803  return (false); // end of file without position
804  }
805  }
806  else {
807  return (false);// end of file
808  }
809 
810  }
811  while ( end != true );
812 
813  double q1,q2;
814  // Read positions
815  fscanf(pt_f, "%lf %lf", &q1, &q2);
816  q.resize(vpPtu46::ndof) ;
817 
818  q[0] = vpMath::rad(q1) ; // Rot tourelle
819  q[1] = vpMath::rad(q2) ;
820 
821  fclose(pt_f) ;
822  return (true);
823 }
824 
837 void
838 vpRobotPtu46::getCameraDisplacement(vpColVector &v)
839 {
841 
842 }
854 void vpRobotPtu46::getArticularDisplacement(vpColVector &d)
855 {
857 }
858 
880 void
882  vpColVector &d)
883 {
884  double d_[6];
885 
886  switch (frame)
887  {
889  {
890  d.resize (6);
891  ptu.measureDpl(d_, PTU_CAMERA_FRAME);
892  d[0]=d_[0];
893  d[1]=d_[1];
894  d[2]=d_[2];
895  d[3]=d_[3];
896  d[4]=d_[4];
897  d[5]=d_[5];
898  break ;
899  }
901  {
902  ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
903  d.resize (vpPtu46::ndof);
904  d[0]=d_[0]; // pan
905  d[1]=d_[1]; // tilt
906  break ;
907  }
909  {
910  vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
911  "functionality not implemented");
913  "Cannot get a displacement in the reference frame:"
914  "functionality not implemented");
915  break ;
916  }
917  case vpRobot::MIXT_FRAME:
918  {
919  vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
920  "functionality not implemented");
922  "Cannot get a displacement in the reference frame:"
923  "functionality not implemented");
924 
925  break ;
926  }
927  }
928 }
929 
930 /*
931  * Local variables:
932  * c-basic-offset: 2
933  * End:
934  */
935 
936 #endif
937 
void get_cMe(vpHomogeneousMatrix &_cMe)
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
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)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
virtual vpRobotStateType getRobotState(void)
Definition: vpRobot.h:139
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
static const double defaultPositioningVelocity
Definition: vpRobotPtu46.h:109
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
Initialize the position controller.
Definition: vpRobot.h:71
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q)
class that defines a generic virtual robot
Definition: vpRobot.h:60
vpControlFrameType
Definition: vpRobot.h:78
void get_fJe(vpMatrix &_fJe)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:154
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_cMe(vpHomogeneousMatrix &_cMe)
Definition: vpPtu46.cpp:242
Initialize the velocity controller.
Definition: vpRobot.h:70
static const unsigned int ndof
Definition: vpPtu46.h:86
vpRobotStateType
Definition: vpRobot.h:66
virtual ~vpRobotPtu46(void)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void init(void)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void get_eJe(vpMatrix &_eJe)
#define vpCDEBUG(niv)
Definition: vpDebug.h:478
static double rad(double deg)
Definition: vpMath.h:100
double getPositioningVelocity(void)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void get_fJe(const vpColVector &q, vpMatrix &fJe)
Definition: vpPtu46.cpp:314
void get_eJe(const vpColVector &q, vpMatrix &eJe)
Definition: vpPtu46.cpp:282
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
void get_cVe(vpVelocityTwistMatrix &_cVe)
bool readPositionFile(const char *filename, vpColVector &q)
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94