Visual Servoing Platform
version 3.6.1 under development (2024-12-07)
|
#include <visp3/robot/vpRobotPtu46.h>
Public Types | |
enum | vpRobotStateType { STATE_STOP , STATE_VELOCITY_CONTROL , STATE_POSITION_CONTROL , STATE_ACCELERATION_CONTROL , STATE_FORCE_TORQUE_CONTROL } |
enum | vpControlFrameType { REFERENCE_FRAME , ARTICULAR_FRAME , JOINT_STATE = ARTICULAR_FRAME , END_EFFECTOR_FRAME , CAMERA_FRAME , TOOL_FRAME = CAMERA_FRAME , MIXT_FRAME } |
Static Public Member Functions | |
Static Public Member Functions inherited from vpRobot | |
static vpColVector | saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false) |
Static Public Attributes | |
static const double | defaultPositioningVelocity = 10.0 |
static const unsigned int | ndof = 2 |
static const float | L = 0.0765f |
static const float | h = 0.068f |
Protected Member Functions | |
Protected Member Functions Inherited from vpRobot | |
vpControlFrameType | setRobotFrame (vpRobot::vpControlFrameType newFrame) |
vpControlFrameType | getRobotFrame (void) const |
Protected Attributes | |
double | maxTranslationVelocity |
double | maxRotationVelocity |
int | nDof |
vpMatrix | eJe |
int | eJeAvailable |
vpMatrix | fJe |
int | fJeAvailable |
int | areJointLimitsAvailable |
double * | qmin |
double * | qmax |
bool | verbose_ |
Static Protected Attributes | |
static const double | maxTranslationVelocityDefault = 0.2 |
static const double | maxRotationVelocityDefault = 0.7 |
Interface for the Directed Perception ptu-46 pan, tilt head .
See http://www.DPerception.com for more details.
This class provide a position and a speed control interface for the ptu-46 head.
Definition at line 77 of file vpRobotPtu46.h.
|
inherited |
Robot control frames.
Enumerator | |
---|---|
REFERENCE_FRAME | Corresponds to a fixed reference frame attached to the robot structure. |
ARTICULAR_FRAME | Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE. |
JOINT_STATE | Corresponds to the joint state. |
END_EFFECTOR_FRAME | Corresponds to robot end-effector frame. |
CAMERA_FRAME | Corresponds to a frame attached to the camera mounted on the robot end-effector. |
TOOL_FRAME | Corresponds to a frame attached to the tool (camera, gripper...) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME. |
MIXT_FRAME | Corresponds to a "virtual" frame where translations are expressed in the reference frame, and rotations in the camera frame. |
|
inherited |
Robot control states.
vpRobotPtu46::vpRobotPtu46 | ( | const std::string & | device = "/dev/ttyS0" | ) |
Default constructor.
Initialize the ptu-46 pan, tilt head by opening the serial port.
Definition at line 70 of file vpRobotPtu46.cpp.
References defaultPositioningVelocity, init(), setRobotState(), and vpRobot::STATE_STOP.
VP_EXPLICIT vpRobotPtu46::vpRobotPtu46 | ( | vpRobotPtu46 * | pub | ) |
|
virtual |
Destructor. Close the serial connection with the head.
Definition at line 104 of file vpRobotPtu46.cpp.
References setRobotState(), and vpRobot::STATE_STOP.
|
inherited |
Return the direct geometric model of the camera: fMc
q | : Articular position for pan and tilt axis. |
Definition at line 118 of file vpPtu46.cpp.
References vpPtu46::computeMGD().
|
inherited |
Compute the direct geometric model of the camera: fMc
q | : Articular position for pan and tilt axis. |
fMc | : Homogeneous matrix corresponding to the direct geometric model of the camera. Describes the transformation between the robot reference frame (called fixed) and the camera frame. |
Definition at line 68 of file vpPtu46.cpp.
References vpException::dimensionError, vpArray2D< Type >::getRows(), vpPtu46::h, and vpPtu46::L.
Referenced by vpPtu46::computeMGD().
|
inherited |
Compute the direct geometric model of the camera in terms of pose vector.
q | : Articular position for pan and tilt axis. |
r | : Pose vector corresponding to the transformation between the robot reference frame (called fixed) and the camera frame. |
Definition at line 136 of file vpPtu46.cpp.
References vpPoseVector::buildFrom(), vpPtu46::computeMGD(), and vpHomogeneousMatrix::inverse().
void vpRobotPtu46::get_cMe | ( | vpHomogeneousMatrix & | cMe | ) | const |
Get the homogeneous matrix corresponding to the transformation between the camera frame and the end effector frame. The end effector frame is located on the tilt axis.
cMe | : Homogeneous matrix between camera and end effector frame. |
Definition at line 222 of file vpRobotPtu46.cpp.
References vpPtu46::get_cMe().
void vpRobotPtu46::get_cVe | ( | vpVelocityTwistMatrix & | cVe | ) | const |
Get the twist matrix corresponding to the transformation between the camera frame and the end effector frame. The end effector frame is located on the tilt axis.
cVe | : Twist transformation between camera and end effector frame to express a velocity skew from end effector frame in camera frame. |
Definition at line 205 of file vpRobotPtu46.cpp.
References vpVelocityTwistMatrix::buildFrom(), and vpPtu46::get_cMe().
|
inherited |
Get the robot jacobian expressed in the end-effector frame.
q | : Articular position for pan and tilt axis. |
eJe | : Jacobian between end effector frame and end effector frame (on tilt axis). |
Definition at line 247 of file vpPtu46.cpp.
References vpException::dimensionError, vpArray2D< Type >::getRows(), and vpArray2D< Type >::resize().
Referenced by get_eJe().
|
virtual |
Get the robot jacobian expressed in the end-effector frame.
eJe | : Jacobian between end effector frame and end effector frame (on tilt axis). |
Implements vpRobot.
Definition at line 234 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::eJe, vpPtu46::get_eJe(), and getPosition().
|
inherited |
Get the robot jacobian expressed in the robot reference frame
q | : Articular position for pan and tilt axis. |
fJe | : Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis). |
Definition at line 276 of file vpPtu46.cpp.
References vpException::dimensionError, vpArray2D< Type >::getRows(), and vpArray2D< Type >::resize().
Referenced by get_fJe().
|
virtual |
Get the robot jacobian expressed in the robot reference frame
fJe | : Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis). |
Implements vpRobot.
Definition at line 255 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::fJe, vpPtu46::get_fJe(), and getPosition().
|
virtual |
Get the robot displacement since the last call of this method.
frame | The frame in which the measured displacement is expressed. |
d | The displacement:
|
vpRobotException::wrongStateError | If a not supported frame type is given. |
Implements vpRobot.
Definition at line 735 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::MIXT_FRAME, vpPtu46::ndof, vpRobot::REFERENCE_FRAME, and vpRobotException::wrongStateError.
|
inherited |
Get the maximal rotation velocity that can be sent to the robot during a velocity control.
Definition at line 274 of file vpRobot.cpp.
References vpRobot::maxRotationVelocity.
Referenced by vpRobotPololuPtu::setPosition(), vpRobotPololuPtu::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inherited |
Get the maximal translation velocity that can be sent to the robot during a velocity control.
Definition at line 252 of file vpRobot.cpp.
References vpRobot::maxTranslationVelocity.
Referenced by vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inlineinherited |
Return robot degrees of freedom number.
|
inherited |
Return the current robot position in the specified frame.
Definition at line 217 of file vpRobot.cpp.
References vpRobot::getPosition().
|
virtual |
Return the position of each axis.
frame | : Control frame. This head can only be controlled in articular. |
q | : The position of the axis. |
vpRobotException::wrongStateError | : If a not supported frame type is given. |
Implements vpRobot.
Definition at line 411 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpPtu46::ndof, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpRobotException::wrongStateError.
double vpRobotPtu46::getPositioningVelocity | ( | void | ) |
Get the velocity in % used for a position control.
Definition at line 282 of file vpRobotPtu46.cpp.
|
inlineprotectedinherited |
|
inlinevirtualinherited |
Definition at line 155 of file vpRobot.h.
Referenced by vpRobotBiclops::getPosition(), vpRobotBiclops::getVelocity(), vpSimulatorCamera::setPosition(), vpRobotAfma4::setPosition(), vpRobotAfma6::setPosition(), vpRobotFranka::setPosition(), vpRobotUniversalRobots::setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotBiclops::setPosition(), vpRobotPololuPtu::setPosition(), setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpRobotAfma4::setRobotState(), vpRobotAfma6::setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotBiclops::setVelocity(), vpRobotPololuPtu::setVelocity(), setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpRobotFlirPtu::stopMotion(), vpRobotFranka::stopMotion(), vpRobotViper650::stopMotion(), and vpRobotViper850::stopMotion().
vpColVector vpRobotPtu46::getVelocity | ( | const vpRobot::vpControlFrameType | frame | ) |
Return the articular velocity.
frame | : Control frame. This head can only be controlled in articular. |
vpRobotException::wrongStateError | : If a not supported frame type is given. |
Definition at line 631 of file vpRobotPtu46.cpp.
References getVelocity().
void vpRobotPtu46::getVelocity | ( | const vpRobot::vpControlFrameType | frame, |
vpColVector & | q_dot | ||
) |
Get the articular velocity.
frame | : Control frame. This head can only be controlled in articular. |
q_dot | : The measured articular velocity in rad/s. |
vpRobotException::wrongStateError | : If a not supported frame type is given. |
Definition at line 583 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::MIXT_FRAME, vpPtu46::ndof, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpRobotException::wrongStateError.
Referenced by getVelocity().
|
virtual |
Open the serial port.
vpRobotException::constructionError | : If the device cannot be opened. |
Implements vpRobot.
Definition at line 134 of file vpRobotPtu46.cpp.
References vpRobotException::constructionError.
Referenced by vpRobotPtu46().
bool vpRobotPtu46::readPositionFile | ( | const std::string & | filename, |
vpColVector & | q | ||
) |
Get an articular position from the position file.
filename | : Position file. |
q | : The articular position read in the file. |
Definition at line 658 of file vpRobotPtu46.cpp.
References vpColVector::deg2rad(), vpPtu46::ndof, vpColVector::resize(), and vpIoTools::splitChain().
Referenced by setPosition().
|
staticinherited |
Saturate velocities.
v_in | : Vector of input velocities to saturate. Translation velocities should be expressed in m/s while rotation velocities in rad/s. |
v_max | : Vector of maximal allowed velocities. Maximal translation velocities should be expressed in m/s while maximal rotation velocities in rad/s. |
verbose | : Print a message indicating which axis causes the saturation. |
vpRobotException::dimensionError | : If the input vectors have different dimensions. |
The code below shows how to use this static method in order to saturate a velocity skew vector.
Definition at line 164 of file vpRobot.cpp.
References vpException::dimensionError, and vpArray2D< Type >::size().
Referenced by vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inherited |
Set the maximal rotation velocity that can be sent to the robot during a velocity control.
w_max | : Maximum rotational velocity expressed in rad/s. |
Definition at line 261 of file vpRobot.cpp.
References vpRobot::maxRotationVelocity.
Referenced by vpRobotViper650::setMaxRotationVelocity(), and vpRobotViper850::setMaxRotationVelocity().
|
inherited |
Set the maximal translation velocity that can be sent to the robot during a velocity control.
v_max | : Maximum translation velocity expressed in m/s. |
Definition at line 240 of file vpRobot.cpp.
References vpRobot::maxTranslationVelocity.
void vpRobotPtu46::setPosition | ( | const char * | filename | ) |
Read the content of the position file and moves to head to articular position.
filename | : Position filename |
vpRobotException::readingParametersError | : If the articular position cannot be read from file. |
Definition at line 388 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobotException::readingParametersError, readPositionFile(), and setPosition().
void vpRobotPtu46::setPosition | ( | const vpRobot::vpControlFrameType | frame, |
const double & | q1, | ||
const double & | q2 | ||
) |
Move the robot in position control.
frame | : Control frame. This head can only be controlled in articular. |
q1 | : The pan position to set. |
q2 | : The tilt position to set. |
vpRobotException::wrongStateError | : If a not supported frame type is given. |
Definition at line 360 of file vpRobotPtu46.cpp.
References setPosition().
|
virtual |
Move the robot in position control.
frame | : Control frame. This head can only be controlled in articular. |
q | : The position to set for each axis. |
vpRobotException::wrongStateError | : If a not supported frame type is given. |
Implements vpRobot.
Definition at line 300 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getRobotState(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, setRobotState(), vpRobot::STATE_POSITION_CONTROL, and vpRobotException::wrongStateError.
Referenced by setPosition().
void vpRobotPtu46::setPositioningVelocity | ( | double | velocity | ) |
Set the velocity used for a position control.
velocity | : Velocity in % of the maximum velocity between [0,100]. |
Definition at line 275 of file vpRobotPtu46.cpp.
|
protectedinherited |
Definition at line 208 of file vpRobot.cpp.
Referenced by vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), and vpSimulatorPioneerPan::setVelocity().
|
virtual |
Change the state of the robot either to stop them, or to set position or speed control.
Reimplemented from vpRobot.
Definition at line 151 of file vpRobotPtu46.cpp.
References vpRobot::getRobotState(), vpRobot::setRobotState(), vpRobot::STATE_POSITION_CONTROL, vpRobot::STATE_STOP, and vpRobot::STATE_VELOCITY_CONTROL.
Referenced by setPosition(), stopMotion(), vpRobotPtu46(), and ~vpRobotPtu46().
|
virtual |
Send a velocity on each axis.
frame | : Control frame. This head can only be controlled in articular and camera frame. Be aware, the reference frame (vpRobot::REFERENCE_FRAME) end-effector frame (vpRobot::END_EFFECTOR_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are not implemented. |
v | : The desired velocity of the axis. The size of this vector is always 2. Velocities are expressed in rad/s. |
vpRobotException::wrongStateError | : If a the robot is not configured to handle a velocity. The robot can handle a velocity only if the velocity control mode is set. For that, call setRobotState( vpRobot::STATE_VELOCITY_CONTROL) before setVelocity(). |
vpRobotException::wrongStateError | : If a non supported frame type (vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::MIXT_FRAME) is given. |
Implements vpRobot.
Definition at line 479 of file vpRobotPtu46.cpp.
References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getRobotState(), vpArray2D< Type >::getRows(), vpRobot::maxRotationVelocity, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::STATE_VELOCITY_CONTROL, and vpRobotException::wrongStateError.
|
inlineinherited |
Definition at line 170 of file vpRobot.h.
Referenced by vpRobotAfma4::vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().
void vpRobotPtu46::stopMotion | ( | void | ) |
Halt all the axis.
Definition at line 189 of file vpRobotPtu46.cpp.
References setRobotState(), and vpRobot::STATE_STOP.
|
protectedinherited |
Definition at line 114 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
static |
Definition at line 94 of file vpRobotPtu46.h.
Referenced by vpRobotPtu46().
|
protectedinherited |
robot Jacobian expressed in the end-effector frame
Definition at line 106 of file vpRobot.h.
Referenced by vpRobotFlirPtu::get_eJe(), vpRobotAfma6::get_eJe(), get_eJe(), vpRobotAfma4::get_eJe(), vpRobotBiclops::get_eJe(), vpRobotKinova::get_eJe(), vpRobotPololuPtu::get_eJe(), vpRobotViper650::get_eJe(), vpRobotViper850::get_eJe(), vpSimulatorCamera::get_eJe(), vpRobot::operator=(), and vpRobotAfma4::setVelocity().
|
protectedinherited |
is the robot Jacobian expressed in the end-effector frame available
Definition at line 108 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
protectedinherited |
robot Jacobian expressed in the robot reference frame available
Definition at line 110 of file vpRobot.h.
Referenced by vpRobotFlirPtu::get_fJe(), vpRobotAfma6::get_fJe(), get_fJe(), vpRobotAfma4::get_fJe(), vpRobotBiclops::get_fJe(), vpRobotKinova::get_fJe(), vpRobotPololuPtu::get_fJe(), vpRobotViper650::get_fJe(), vpRobotViper850::get_fJe(), and vpRobot::operator=().
|
protectedinherited |
is the robot Jacobian expressed in the robot reference frame available
Definition at line 112 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
staticinherited |
Horizontal offset along the last joint, from last joint to camera frame.
Definition at line 80 of file vpPtu46.h.
Referenced by vpPtu46::computeMGD(), and vpPtu46::get_cMe().
|
staticinherited |
Geometric model
Definition at line 79 of file vpPtu46.h.
Referenced by vpPtu46::computeMGD(), and vpPtu46::get_cMe().
|
protectedinherited |
Definition at line 100 of file vpRobot.h.
Referenced by vpRobot::getMaxRotationVelocity(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotTemplate::init(), vpRobot::operator=(), vpRobot::setMaxRotationVelocity(), setVelocity(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().
|
staticprotectedinherited |
Definition at line 101 of file vpRobot.h.
Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().
|
protectedinherited |
Definition at line 98 of file vpRobot.h.
Referenced by vpRobot::getMaxTranslationVelocity(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotTemplate::init(), vpRobot::operator=(), and vpRobot::setMaxTranslationVelocity().
|
staticprotectedinherited |
Definition at line 99 of file vpRobot.h.
Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().
|
staticinherited |
Nombre d'articulations du robot. Number of dof
Definition at line 76 of file vpPtu46.h.
Referenced by getDisplacement(), getPosition(), getVelocity(), and readPositionFile().
|
protectedinherited |
number of degrees of freedom
Definition at line 104 of file vpRobot.h.
Referenced by vpRobotPololuPtu::get_eJe(), vpRobotPololuPtu::get_fJe(), vpRobotKinova::getJointPosition(), vpRobotPololuPtu::getPosition(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotUniversalRobots::init(), vpRobotTemplate::init(), vpRobot::operator=(), vpRobotUniversalRobots::readPosFile(), vpRobotKinova::setDoF(), vpRobotKinova::setJointVelocity(), vpRobotKinova::setPosition(), vpRobotPololuPtu::setPosition(), vpRobotPololuPtu::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotKinova::setVelocity(), vpRobotTemplate::setVelocity(), and vpRobotPololuPtu::vpRobotPololuPtu().
|
protectedinherited |
Definition at line 116 of file vpRobot.h.
Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().
|
protectedinherited |
Definition at line 115 of file vpRobot.h.
Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().
|
protectedinherited |
Definition at line 118 of file vpRobot.h.
Referenced by vpRobotAfma4::init(), vpRobotAfma6::init(), vpRobotViper650::init(), vpRobotViper850::init(), vpRobot::operator=(), vpRobotAfma4::vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().