![]() |
Visual Servoing Platform
version 3.3.0
|
#include <vpRobotFlirPtu.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 void | emergencyStop (int signo) |
Static Public Member Functions inherited from vpRobot | |
static vpColVector | saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false) |
Protected Member Functions | |
void | init () |
void | getLimits () |
void | getJointPosition (vpColVector &q) |
void | setCartVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &v) |
void | setJointVelocity (const vpColVector &qdot) |
Protected Member Functions Inherited from vpRobot | |
vpControlFrameType | setRobotFrame (vpRobot::vpControlFrameType newFrame) |
vpControlFrameType | getRobotFrame (void) const |
Protected Attributes | |
vpHomogeneousMatrix | m_eMc |
struct cerial * | m_cer |
uint16_t | m_status |
std::vector< int > | m_pos_max_tics |
std::vector< int > | m_pos_min_tics |
std::vector< int > | m_vel_max_tics |
std::vector< double > | m_res |
bool | m_connected |
int | m_njoints |
double | m_positioning_velocity |
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 FLIR pan-tilt units compatible with FLIR PTU-SDK.
config.mk
to add -fPIC
build flag Definition at line 95 of file vpRobotFlirPtu.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.
vpRobotFlirPtu::vpRobotFlirPtu | ( | ) |
Default constructor.
Definition at line 111 of file vpRobotFlirPtu.cpp.
|
virtual |
Destructor.
Definition at line 129 of file vpRobotFlirPtu.cpp.
void vpRobotFlirPtu::connect | ( | const std::string & | portname, |
int | baudrate = 9600 |
||
) |
Connect to FLIR PTU.
[in] | portname | : Connect to serial/socket. |
[in] | baudrate | : Use baud rate (default: 9600). |
Definition at line 530 of file vpRobotFlirPtu.cpp.
void vpRobotFlirPtu::disconnect | ( | ) |
|
static |
Emergency stops the robot if the program is interrupted by a SIGINT (CTRL C), SIGSEGV (segmentation fault), SIGBUS (bus error), SIGKILL or SIGQUIT signal.
Definition at line 64 of file vpRobotFlirPtu.cpp.
vpVelocityTwistMatrix vpRobotFlirPtu::get_cVe | ( | ) | const |
Return the velocity twist transformation matrix from camera frame to end-effector frame. This transformation allows to transform a velocity twist expressed in the end-effector frame into the camera frame thanks to the homogeneous matrix eMc set using set_eMc().
Definition at line 238 of file vpRobotFlirPtu.cpp.
vpMatrix vpRobotFlirPtu::get_eJe | ( | ) |
Get the robot Jacobian expressed in the end-effector frame.
Definition at line 148 of file vpRobotFlirPtu.cpp.
|
virtual |
Get the robot Jacobian expressed in the end-effector frame.
[out] | eJe | : End-effector frame Jacobian. |
Implements vpRobot.
Definition at line 167 of file vpRobotFlirPtu.cpp.
|
inline |
Return constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.
Definition at line 114 of file vpRobotFlirPtu.h.
vpMatrix vpRobotFlirPtu::get_fJe | ( | ) |
Get the robot Jacobian expressed in the robot reference frame.
Definition at line 175 of file vpRobotFlirPtu.cpp.
|
virtual |
Get the robot Jacobian expressed in the robot reference frame.
[out] | fJe | : Base (or reference) frame Jacobian. |
Implements vpRobot.
Definition at line 195 of file vpRobotFlirPtu.cpp.
vpMatrix vpRobotFlirPtu::get_fMe | ( | ) |
Get the robot geometric model corresponding to the homogeneous transformation between base (or reference) frame and end-effector frame.
Definition at line 201 of file vpRobotFlirPtu.cpp.
|
virtual |
Get a displacement.
[in] | frame | : Considered cartesian frame or joint state. |
[out] | q | : Displacement in meter and rad. |
Implements vpRobot.
Definition at line 517 of file vpRobotFlirPtu.cpp.
|
protected |
Get robot joint positions.
[in] | q | : Joint position as a 2-dim vector [pan, tilt] with values in radians. |
Definition at line 411 of file vpRobotFlirPtu.cpp.
|
protected |
Read min/max position and speed.
Definition at line 612 of file vpRobotFlirPtu.cpp.
|
inherited |
Get the maximal rotation velocity that can be sent to the robot during a velocity control.
Definition at line 272 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorAfma6::findHighestPositioningSpeed(), vpSimulatorViper850::findHighestPositioningSpeed(), vpSimulatorAfma6::setPosition(), and vpSimulatorAfma6::setVelocity().
|
inherited |
Get the maximal translation velocity that can be sent to the robot during a velocity control.
Definition at line 250 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::setPosition(), and vpSimulatorAfma6::setVelocity().
std::string vpRobotFlirPtu::getNetworkGateway | ( | ) |
When connected to the PTU by serial, get the PTU network gateway.
Definition at line 885 of file vpRobotFlirPtu.cpp.
std::string vpRobotFlirPtu::getNetworkHostName | ( | ) |
When connected to the PTU, get the PTU network hostname.
Definition at line 905 of file vpRobotFlirPtu.cpp.
std::string vpRobotFlirPtu::getNetworkIP | ( | ) |
When connected to the PTU by serial, get the PTU network IP address.
Definition at line 865 of file vpRobotFlirPtu.cpp.
vpColVector vpRobotFlirPtu::getPanPosLimits | ( | ) |
Return pan axis min and max position limits in radians as a 2-dim vector, with first value the pan min position and second value, the pan max position.
Definition at line 649 of file vpRobotFlirPtu.cpp.
vpColVector vpRobotFlirPtu::getPanTiltVelMax | ( | ) |
Return pan/tilt axis max velocity in rad/s as a 2-dim vector, with first value the pan max velocity and second value, the max tilt velocity.
Definition at line 687 of file vpRobotFlirPtu.cpp.
|
inherited |
Return the current robot position in the specified frame.
Definition at line 215 of file vpRobot.cpp.
|
virtual |
Get robot position.
[in] | frame | : Considered cartesian frame or joint state. |
[out] | q | : Position of the arm. |
Implements vpRobot.
Definition at line 440 of file vpRobotFlirPtu.cpp.
|
inlineprotectedinherited |
Definition at line 171 of file vpRobot.h.
Referenced by vpSimulatorAfma6::computeArticularVelocity().
|
inlinevirtualinherited |
Definition at line 143 of file vpRobot.h.
Referenced by vpSimulatorAfma6::setPosition(), vpSimulatorAfma6::setRobotState(), vpSimulatorAfma6::setVelocity(), and vpSimulatorAfma6::stopMotion().
vpColVector vpRobotFlirPtu::getTiltPosLimits | ( | ) |
Return tilt axis min and max position limits in radians as a 2-dim vector, with first value the tilt min position and second value, the tilt max position.
Definition at line 668 of file vpRobotFlirPtu.cpp.
|
protectedvirtual |
void vpRobotFlirPtu::reset | ( | ) |
Reset PTU axis.
Definition at line 831 of file vpRobotFlirPtu.cpp.
|
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 162 of file vpRobot.cpp.
|
inline |
Set constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.
Definition at line 134 of file vpRobotFlirPtu.h.
|
protected |
Send to the controller a 6-dim velocity skew vector expressed in a Cartesian frame.
[in] | frame | : Cartesian control frame (either tool frame or end-effector) in which the velocity v is expressed. Units are m/s for translation and rad/s for rotation velocities. |
[in] | v | : 6-dim vector that contains the 6 components of the velocity skew to send to the robot. Units are m/s and rad/s. |
Definition at line 258 of file vpRobotFlirPtu.cpp.
|
protected |
Send a joint velocity to the controller.
[in] | qdot | : Joint velocities vector. Units are rad/s for a robot arm. |
Definition at line 320 of file vpRobotFlirPtu.cpp.
|
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 259 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::setPosition().
|
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 238 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::setPosition().
void vpRobotFlirPtu::setPanPosLimits | ( | const vpColVector & | pan_limits | ) |
Modify pan position limit.
pan_limits | : 2-dim vector that contains pan min and max limits in rad. |
Definition at line 706 of file vpRobotFlirPtu.cpp.
|
virtual |
Set a position to reach.
[in] | frame | : Considered cartesian frame or joint state. |
[in] | q | : Position to reach. |
Implements vpRobot.
Definition at line 454 of file vpRobotFlirPtu.cpp.
void vpRobotFlirPtu::setPositioningVelocity | ( | double | velocity | ) |
Set the velocity used for a position control.
velocity | : Velocity in % of the maximum velocity between [0, 100]. Default value is 20. |
Definition at line 766 of file vpRobotFlirPtu.cpp.
|
protectedinherited |
Definition at line 206 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::init(), and vpSimulatorAfma6::setVelocity().
|
virtual |
Change the robot state.
newState | : New requested robot state if the robot is connected. If the robot is not connected, we return the current state. |
Reimplemented from vpRobot.
Definition at line 775 of file vpRobotFlirPtu.cpp.
void vpRobotFlirPtu::setTiltPosLimits | ( | const vpColVector & | tilt_limits | ) |
Modify tilt position limit.
tilt_limits | : 2-dim vector that contains tilt min and max limits in rad. |
Definition at line 736 of file vpRobotFlirPtu.cpp.
|
virtual |
Send to the controller a velocity in a given frame.
[in] | frame | : Control frame in which the velocity vel is expressed. Velocities could be joint velocities, or cartesian velocities. Units are m/s for translation and rad/s for rotation velocities. |
[in] | vel | : Vector that contains the velocity to apply to the robot. |
Implements vpRobot.
Definition at line 351 of file vpRobotFlirPtu.cpp.
|
inlineinherited |
void vpRobotFlirPtu::stopMotion | ( | void | ) |
Stop PTU motion in velocity control mode.
Definition at line 845 of file vpRobotFlirPtu.cpp.
|
protectedinherited |
|
protectedinherited |
|
protectedinherited |
|
protectedinherited |
|
protected |
Definition at line 161 of file vpRobotFlirPtu.h.
|
protected |
Definition at line 167 of file vpRobotFlirPtu.h.
|
protected |
Constant transformation between end-effector and tool (or camera) frame.
Definition at line 159 of file vpRobotFlirPtu.h.
|
protected |
Definition at line 168 of file vpRobotFlirPtu.h.
|
protected |
Pan min/max position in robot tics unit.
Definition at line 163 of file vpRobotFlirPtu.h.
|
protected |
Tilt min/max position in robot tics unit.
Definition at line 164 of file vpRobotFlirPtu.h.
|
protected |
Definition at line 169 of file vpRobotFlirPtu.h.
|
protected |
Pan/tilt tic resolution in deg.
Definition at line 166 of file vpRobotFlirPtu.h.
|
protected |
Definition at line 162 of file vpRobotFlirPtu.h.
|
protected |
Pan/tilt max velocity in robot tics unit.
Definition at line 165 of file vpRobotFlirPtu.h.
|
staticprotectedinherited |
|
staticprotectedinherited |
|
protectedinherited |