41 #include <visp3/core/vpConfig.h>
42 #ifdef VISP_HAVE_PTU46
45 #include <visp3/robot/vpPtu46.h>
46 #include <visp3/robot/vpRobotPtu46.h>
47 #include <visp3/robot/vpRobotException.h>
48 #include <visp3/core/vpDebug.h>
54 bool vpRobotPtu46::robotAlreadyCreated =
false;
71 vpRobotPtu46::vpRobotPtu46 (
const char *device)
75 this->device =
new char [FILENAME_MAX];
77 sprintf(this->device,
"%s", device);
86 delete [] this->device;
97 delete [] this->device;
120 if (0 != ptu.close())
122 vpERROR_TRACE (
"Error while closing communications with the robot ptu-46.");
125 vpRobotPtu46::robotAlreadyCreated =
false;
151 if (0 != ptu.init(device) )
155 "Cannot open connexion with ptu-46");
319 positioningVelocity = velocity;
330 return positioningVelocity;
358 "Modification of the robot state");
368 "Cannot move the robot in camera frame: "
375 "Cannot move the robot in reference frame: "
382 "Cannot move the robot in mixt frame: "
395 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
399 "Positionning error.");
424 const double &q1,
const double &q2)
460 "Cannot get ptu-46 position from file");
487 vpERROR_TRACE (
"Cannot get position in camera frame: not implemented");
489 "Cannot get position in camera frame: "
496 "Cannot get position in reference frame: "
503 "Cannot get position in mixt frame: "
512 if (0 != ptu.getCurrentPosition( artpos ) )
516 "Error when calling recup_posit_Afma4.");
559 TPtuFrame ptuFrameInterface;
564 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
566 "Cannot send a velocity to the robot "
567 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
574 ptuFrameInterface = PTU_CAMERA_FRAME;
576 vpERROR_TRACE (
"Bad dimension fo speed vector in camera frame");
578 "Bad dimension for speed vector "
585 ptuFrameInterface = PTU_ARTICULAR_FRAME;
587 vpERROR_TRACE (
"Bad dimension fo speed vector in articular frame");
589 "Bad dimension for speed vector "
590 "in articular frame");
597 "in the reference frame: "
598 "functionality not implemented");
600 "Cannot send a velocity to the robot "
601 "in the reference frame:"
602 "functionality not implemented");
608 "in the mixt frame: "
609 "functionality not implemented");
611 "Cannot send a velocity to the robot "
613 "functionality not implemented");
619 "Case not taken in account.");
621 "Cannot send a velocity to the robot ");
627 double ptuSpeedInterface[2];
632 double max =
this ->maxRotationVelocity;
633 for (
unsigned int i = 0 ; i < 2; ++ i)
635 if (fabs (v[i]) > max)
645 max =
this ->maxRotationVelocity / max;
646 for (
unsigned int i = 0 ; i < 2; ++ i)
647 ptuSpeedInterface [i] = v[i]*max;
657 vpCDEBUG(12) <<
"v: " << ptuSpeedInterface[0]
658 <<
" " << ptuSpeedInterface[1] << std::endl;
659 ptu.move(ptuSpeedInterface, ptuFrameInterface);
685 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
692 "functionality not implemented");
694 "Cannot get a velocity in the camera frame:"
695 "functionality not implemented");
700 ptuFrameInterface = PTU_ARTICULAR_FRAME;
705 vpERROR_TRACE (
"Cannot get a velocity in the reference frame: "
706 "functionality not implemented");
708 "Cannot get a velocity in the reference frame:"
709 "functionality not implemented");
716 "functionality not implemented");
718 "Cannot get a velocity in the mixt frame:"
719 "functionality not implemented");
725 double ptuSpeedInterface[2];
727 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
729 q_dot[0] = ptuSpeedInterface[0];
730 q_dot[1] = ptuSpeedInterface[1];
778 pt_f = fopen(filename,
"r") ;
781 vpERROR_TRACE (
"Can not open ptu-46 position file %s", filename);
785 char line[FILENAME_MAX];
791 if (fgets (line, 100, pt_f) != NULL) {
792 if ( strncmp (line,
"#", 1) != 0) {
794 if ( fscanf (pt_f,
"%s", line) != EOF) {
795 if ( strcmp (line, head) == 0)
807 while ( end !=
true );
811 fscanf(pt_f,
"%lf %lf", &q1, &q2);
834 vpRobotPtu46::getCameraDisplacement(
vpColVector &v)
850 void vpRobotPtu46::getArticularDisplacement(
vpColVector &d)
887 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
898 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
906 vpERROR_TRACE (
"Cannot get a displacement in the reference frame: "
907 "functionality not implemented");
909 "Cannot get a displacement in the reference frame:"
910 "functionality not implemented");
915 vpERROR_TRACE (
"Cannot get a displacement in the mixt frame: "
916 "functionality not implemented");
918 "Cannot get a displacement in the reference frame:"
919 "functionality not implemented");
926 #elif !defined(VISP_BUILD_SHARED_LIBS)
928 void dummy_vpRobotPtu46() {};
Implementation of a matrix and operations on matrices.
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
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
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
static const double defaultPositioningVelocity
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
Initialize the position controller.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q)
class that defines a generic virtual robot
void get_fJe(vpMatrix &_fJe)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void get_cMe(vpHomogeneousMatrix &_cMe) const
Initialize the velocity controller.
static const unsigned int ndof
virtual ~vpRobotPtu46(void)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
void get_eJe(vpMatrix &_eJe)
static double rad(double deg)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
void get_cVe(vpVelocityTwistMatrix &_cVe) const
double getPositioningVelocity(void)
Implementation of column vector and the associated operations.
virtual vpRobotStateType getRobotState(void) const
bool readPositionFile(const char *filename, vpColVector &q)
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)