39 #include <visp3/core/vpConfig.h>
40 #ifdef VISP_HAVE_PTU46
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>
53 bool vpRobotPtu46::robotAlreadyCreated =
false;
69 vpRobotPtu46::vpRobotPtu46(
const std::string &device) :
vpRobot()
71 this->device = device;
106 if (0 != ptu.close()) {
107 vpERROR_TRACE(
"Error while closing communications with the robot ptu-46.");
110 vpRobotPtu46::robotAlreadyCreated =
false;
134 if (0 != ptu.init(device.c_str())) {
299 "Modification of the robot state");
330 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
431 if (0 != ptu.getCurrentPosition(artpos)) {
474 TPtuFrame ptuFrameInterface;
478 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
480 "Cannot send a velocity to the robot "
481 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
486 ptuFrameInterface = PTU_CAMERA_FRAME;
488 vpERROR_TRACE(
"Bad dimension fo speed vector in camera frame");
495 ptuFrameInterface = PTU_ARTICULAR_FRAME;
498 "in articular frame");
504 "in the reference frame:"
505 "functionality not implemented");
510 "functionality not implemented");
514 "in the end-effector frame:"
515 "functionality not implemented");
523 double ptuSpeedInterface[2];
530 for (
unsigned int i = 0; i < 2; ++i)
532 if (fabs(v[i]) > max) {
543 for (
unsigned int i = 0; i < 2; ++i)
544 ptuSpeedInterface[i] = v[i] * max;
553 vpCDEBUG(12) <<
"v: " << ptuSpeedInterface[0] <<
" " << ptuSpeedInterface[1] << std::endl;
554 ptu.move(ptuSpeedInterface, ptuFrameInterface);
579 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
584 "functionality not implemented");
587 ptuFrameInterface = PTU_ARTICULAR_FRAME;
592 "functionality not implemented");
596 "functionality not implemented");
600 "functionality not implemented");
605 double ptuSpeedInterface[2];
607 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
609 q_dot[0] = ptuSpeedInterface[0];
610 q_dot[1] = ptuSpeedInterface[1];
653 std::ifstream fd(filename.c_str(), std::ios::in);
660 std::string key(
"R:");
661 std::string id(
"#PTU-EVI - Position");
662 bool pos_found =
false;
667 while (std::getline(fd, line)) {
670 if (!(line.compare(0,
id.size(),
id) == 0)) {
671 std::cout <<
"Error: this position file " << filename <<
" is not for Ptu-46 robot" << std::endl;
675 if ((line.compare(0, 1,
"#") == 0)) {
678 if ((line.compare(0, key.size(), key) == 0)) {
686 std::istringstream ss(line);
702 std::cout <<
"Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
735 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
745 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
753 "functionality not implemented");
757 "functionality not implemented");
761 "functionality not implemented");
766 #elif !defined(VISP_BUILD_SHARED_LIBS)
769 void dummy_vpRobotPtu46(){};
unsigned int getRows() const
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
void get_cMe(vpHomogeneousMatrix &_cMe) const
static const unsigned int ndof
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
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
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)
Class that defines a generic virtual robot.
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
virtual vpRobotStateType getRobotState(void) const
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double maxRotationVelocity
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)