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> 49 #include <visp3/core/vpIoTools.h> 55 bool vpRobotPtu46::robotAlreadyCreated =
false;
72 vpRobotPtu46::vpRobotPtu46 (
const char *device)
76 this->device =
new char [FILENAME_MAX];
78 sprintf(this->device,
"%s", device);
87 delete [] this->device;
98 delete [] this->device;
121 if (0 != ptu.close())
123 vpERROR_TRACE (
"Error while closing communications with the robot ptu-46.");
126 vpRobotPtu46::robotAlreadyCreated =
false;
152 if (0 != ptu.init(device) )
156 "Cannot open connection with ptu-46");
320 positioningVelocity = velocity;
331 return positioningVelocity;
359 "Modification of the robot state");
369 "Cannot move the robot in camera frame: " 376 "Cannot move the robot in reference frame: " 383 "Cannot move the robot in mixt frame: " 396 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
400 "Positionning error.");
425 const double &q1,
const double &q2)
461 "Cannot get ptu-46 position from file");
488 vpERROR_TRACE (
"Cannot get position in camera frame: not implemented");
490 "Cannot get position in camera frame: " 497 "Cannot get position in reference frame: " 504 "Cannot get position in mixt frame: " 513 if (0 != ptu.getCurrentPosition( artpos ) )
517 "Error when calling recup_posit_Afma4.");
560 TPtuFrame ptuFrameInterface;
565 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
567 "Cannot send a velocity to the robot " 568 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
575 ptuFrameInterface = PTU_CAMERA_FRAME;
577 vpERROR_TRACE (
"Bad dimension fo speed vector in camera frame");
579 "Bad dimension for speed vector " 586 ptuFrameInterface = PTU_ARTICULAR_FRAME;
588 vpERROR_TRACE (
"Bad dimension fo speed vector in articular frame");
590 "Bad dimension for speed vector " 591 "in articular frame");
598 "in the reference frame: " 599 "functionality not implemented");
601 "Cannot send a velocity to the robot " 602 "in the reference frame:" 603 "functionality not implemented");
608 "in the mixt frame: " 609 "functionality not implemented");
611 "Cannot send a velocity to the robot " 613 "functionality not implemented");
618 "Case not taken in account.");
620 "Cannot send a velocity to the robot ");
625 double ptuSpeedInterface[2];
630 double max =
this ->maxRotationVelocity;
632 for (
unsigned int i = 0 ; i < 2; ++ i)
634 if (fabs (v[i]) > max)
644 max =
this ->maxRotationVelocity / max;
645 for (
unsigned int i = 0 ; i < 2; ++ i)
646 ptuSpeedInterface [i] = v[i]*max;
656 vpCDEBUG(12) <<
"v: " << ptuSpeedInterface[0]
657 <<
" " << ptuSpeedInterface[1] << std::endl;
658 ptu.move(ptuSpeedInterface, ptuFrameInterface);
684 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
691 "functionality not implemented");
693 "Cannot get a velocity in the camera frame:" 694 "functionality not implemented");
698 ptuFrameInterface = PTU_ARTICULAR_FRAME;
703 vpERROR_TRACE (
"Cannot get a velocity in the reference frame: " 704 "functionality not implemented");
706 "Cannot get a velocity in the reference frame:" 707 "functionality not implemented");
713 "functionality not implemented");
715 "Cannot get a velocity in the mixt frame:" 716 "functionality not implemented");
721 double ptuSpeedInterface[2];
723 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
725 q_dot[0] = ptuSpeedInterface[0];
726 q_dot[1] = ptuSpeedInterface[1];
773 std::ifstream fd(filename.c_str(), std::ios::in);
780 std::string key(
"R:");
781 std::string id(
"#PTU-EVI - Position");
782 bool pos_found =
false;
787 while(std::getline(fd, line)) {
790 if(! (line.compare(0,
id.size(), id) == 0)) {
791 std::cout <<
"Error: this position file " << filename <<
" is not for Ptu-46 robot" << std::endl;
795 if((line.compare(0, 1,
"#") == 0)) {
798 if((line.compare(0, key.size(), key) == 0)) {
806 std::istringstream ss(line);
822 std::cout <<
"Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
842 vpRobotPtu46::getCameraDisplacement(
vpColVector &v)
858 void vpRobotPtu46::getArticularDisplacement(
vpColVector &d)
895 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
906 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
914 vpERROR_TRACE (
"Cannot get a displacement in the reference frame: " 915 "functionality not implemented");
917 "Cannot get a displacement in the reference frame:" 918 "functionality not implemented");
922 vpERROR_TRACE (
"Cannot get a displacement in the mixt frame: " 923 "functionality not implemented");
925 "Cannot get a displacement in the reference frame:" 926 "functionality not implemented");
931 #elif !defined(VISP_BUILD_SHARED_LIBS) 933 void dummy_vpRobotPtu46() {};
Implementation of a matrix and operations on matrices.
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)
unsigned int getRows() const
Return the number of rows of the 2D array.
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_cVe(vpVelocityTwistMatrix &_cVe) const
Initialize the velocity controller.
static const unsigned int ndof
virtual vpRobotStateType getRobotState(void) const
virtual ~vpRobotPtu46(void)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a velocity twist matrix and operations on such kind of matrices.
void get_eJe(vpMatrix &_eJe)
double getPositioningVelocity(void)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Implementation of column vector and the associated operations.
void get_cMe(vpHomogeneousMatrix &_cMe) const
bool readPositionFile(const std::string &filename, vpColVector &q)
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)