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;
108 if (0 != ptu.close()) {
109 vpERROR_TRACE(
"Error while closing communications with the robot ptu-46.");
112 vpRobotPtu46::robotAlreadyCreated =
false;
136 if (0 != ptu.init(device.c_str())) {
304 "Modification of the robot state");
335 if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
437 if (0 != ptu.getCurrentPosition(artpos)) {
480 TPtuFrame ptuFrameInterface;
484 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
486 "Cannot send a velocity to the robot "
487 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
492 ptuFrameInterface = PTU_CAMERA_FRAME;
494 vpERROR_TRACE(
"Bad dimension fo speed vector in camera frame");
501 ptuFrameInterface = PTU_ARTICULAR_FRAME;
504 "in articular frame");
510 "in the reference frame:"
511 "functionality not implemented");
516 "functionality not implemented");
520 "in the end-effector frame:"
521 "functionality not implemented");
529 double ptuSpeedInterface[2];
536 for (
unsigned int i = 0; i < 2; ++i)
538 if (fabs(v[i]) > max) {
549 for (
unsigned int i = 0; i < 2; ++i)
550 ptuSpeedInterface[i] = v[i] * max;
559 vpCDEBUG(12) <<
"v: " << ptuSpeedInterface[0] <<
" " << ptuSpeedInterface[1] << std::endl;
560 ptu.move(ptuSpeedInterface, ptuFrameInterface);
585 TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
590 "functionality not implemented");
593 ptuFrameInterface = PTU_ARTICULAR_FRAME;
598 "functionality not implemented");
602 "functionality not implemented");
606 "functionality not implemented");
611 double ptuSpeedInterface[2];
613 ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
615 q_dot[0] = ptuSpeedInterface[0];
616 q_dot[1] = ptuSpeedInterface[1];
659 std::ifstream fd(filename.c_str(), std::ios::in);
666 std::string key(
"R:");
667 std::string id(
"#PTU-EVI - Position");
668 bool pos_found =
false;
673 while (std::getline(fd, line)) {
676 if (!(line.compare(0,
id.size(),
id) == 0)) {
677 std::cout <<
"Error: this position file " << filename <<
" is not for Ptu-46 robot" << std::endl;
681 if ((line.compare(0, 1,
"#") == 0)) {
684 if ((line.compare(0, key.size(), key) == 0)) {
692 std::istringstream ss(line);
708 std::cout <<
"Error: unable to find a position for Ptu-46 robot in " << filename << std::endl;
741 ptu.measureDpl(d_, PTU_CAMERA_FRAME);
751 ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
759 "functionality not implemented");
763 "functionality not implemented");
767 "functionality not implemented");
772 #elif !defined(VISP_BUILD_SHARED_LIBS)
775 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 emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ readingParametersError
Cannot parse parameters.
@ lowLevelError
Error thrown by the low level sdk.
void setPositioningVelocity(double velocity)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override
double getPositioningVelocity(void)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void get_fJe(vpMatrix &_fJe) override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override
static const double defaultPositioningVelocity
virtual ~vpRobotPtu46(void)
void get_eJe(vpMatrix &_eJe) override
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
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)