![]() |
Visual Servoing Platform
version 3.2.0
|
#include <vpRobotBiclops.h>
Public Types | |
enum | DenavitHartenbergModel { DH1, DH2 } |
enum | vpRobotStateType { STATE_STOP, STATE_VELOCITY_CONTROL, STATE_POSITION_CONTROL, STATE_ACCELERATION_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 unsigned int | ndof = 2 |
static const float | h = 0.048f |
static const float | panJointLimit = (float)(M_PI) |
static const float | tiltJointLimit = (float)(M_PI / 4.5) |
static const float | speedLimit = (float)(M_PI / 3.0) |
Protected Member Functions | |
Protected Member Functions Inherited from vpRobot | |
vpControlFrameType | setRobotFrame (vpRobot::vpControlFrameType newFrame) |
vpControlFrameType | getRobotFrame (void) const |
Protected Attributes | |
DenavitHartenbergModel | dh_model_ |
vpHomogeneousMatrix | cMe_ |
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 biclops, pan, tilt head control.
See http://www.traclabs.com/biclopspt.html for more details.
This class provide a position and a speed control interface for the biclops head. To manage the biclops joint limits in speed control, a control loop is running in a seperate thread (see vpRobotBiclopsSpeedControlLoop()).
The control of the head is done by vpRobotBiclopsController class.
Definition at line 93 of file vpRobotBiclops.h.
|
inherited |
Two different Denavit Hartenberg representations of the robot are implemented. They differ in the orientation of the tilt axis.
The first representation, vpBiclops::DH1 is given by:
The second one, vpBiclops::DH2 is given by:
where are respectively the pan and tilt joint positions.
In those representations, the pan is oriented from left to right, while the tilt is oriented
Enumerator | |
---|---|
DH1 | First Denavit Hartenberg representation. |
DH2 | Second Denavit Hartenberg representation. |
Definition at line 119 of file vpBiclops.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 |
|
inherited |
Return the direct geometric model of the camera: fMc
q | : Articular position for pan and tilt axis. |
Definition at line 118 of file vpBiclops.cpp.
|
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 74 of file vpBiclops.cpp.
|
inherited |
Compute the direct geometric model of the camera in terms of pose vector.
q | : Articular position for pan and tilt axis. |
fvc | : Pose vector corresponding to the transformation between the robot reference frame (called fixed) and the camera frame. |
Definition at line 232 of file vpBiclops.cpp.
|
inlineinherited |
Return the tranformation between the camera frame and the end effector frame.
Definition at line 156 of file vpBiclops.h.
|
inherited |
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 expess a velocity skew from end effector frame in camera frame. |
Definition at line 311 of file vpBiclops.cpp.
|
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 359 of file vpBiclops.cpp.
|
pure virtualinherited |
Get the robot Jacobian expressed in the end-effector frame.
Implemented in vpRobotViper850, vpRobotViper650, vpRobotFranka, vpSimulatorViper850, vpRobotAfma4, vpSimulatorAfma6, vpSimulatorPioneerPan, vpSimulatorPioneer, vpRobotCamera, vpSimulatorCamera, vpRobotPioneer, vpRobotAfma6, vpRobotPtu46, and vpRobotTemplate.
|
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 393 of file vpBiclops.cpp.
|
pure virtualinherited |
Get the robot Jacobian expressed in the robot reference (or world) frame.
Implemented in vpRobotViper850, vpRobotViper650, vpRobotFranka, vpSimulatorViper850, vpRobotAfma4, vpSimulatorAfma6, vpRobotAfma6, vpRobotPtu46, and vpRobotTemplate.
|
inherited |
Return the direct geometric model of the camera: fMc
q | : Articular position for pan and tilt axis. |
Definition at line 137 of file vpBiclops.cpp.
|
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 94 of file vpBiclops.cpp.
|
inherited |
Compute the direct geometric model of the camera in terms of pose vector.
q | : Articular position for pan and tilt axis. |
fvc | : Pose vector corresponding to the transformation between the robot reference frame (called fixed) and the camera frame. |
Definition at line 252 of file vpBiclops.cpp.
|
inherited |
Return the direct geometric model of the end effector: fMe
q | : Articular position for pan and tilt axis. |
Definition at line 156 of file vpBiclops.cpp.
|
inlineinherited |
Return the Denavit Hartenberg representation used to model the head.
Definition at line 171 of file vpBiclops.h.
|
pure virtualinherited |
Get a displacement (frame as to ve specified) between two successive position control.
Implemented in vpRobotPtu46, vpRobotViper850, vpRobotViper650, vpRobotAfma6, vpRobotAfma4, vpRobotTemplate, vpSimulatorViper850, and vpSimulatorAfma6.
|
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().
|
inherited |
Return the current robot position in the specified frame.
Definition at line 215 of file vpRobot.cpp.
|
pure virtualinherited |
Get the robot position (frame has to be specified).
Implemented in vpSimulatorViper850, vpSimulatorAfma6, vpSimulatorPioneerPan, vpSimulatorPioneer, vpRobotCamera, vpSimulatorCamera, vpRobotPtu46, vpRobotTemplate, vpRobotViper850, vpRobotViper650, vpRobotFranka, vpRobotAfma6, and vpRobotAfma4.
|
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().
|
pure virtualinherited |
|
inherited |
Initialization. Set the default transformation.
Definition at line 281 of file vpBiclops.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.
|
inherited |
Set the default 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.
Definition at line 320 of file vpBiclops.cpp.
|
inlineinherited |
Set the transformation between the camera frame and the end effector frame.
Definition at line 178 of file vpBiclops.h.
|
inlineinherited |
Set the Denavit Hartenberg representation used to model the head.
Definition at line 184 of file vpBiclops.h.
|
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().
|
pure virtualinherited |
Set a displacement (frame has to be specified) in position control.
Implemented in vpSimulatorViper850, vpSimulatorAfma6, vpRobotPtu46, vpRobotTemplate, vpRobotViper850, vpRobotViper650, vpRobotFranka, vpRobotAfma6, and vpRobotAfma4.
|
protectedinherited |
Definition at line 206 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::init(), and vpSimulatorAfma6::setVelocity().
|
virtualinherited |
Reimplemented in vpRobotViper850, vpRobotViper650, vpRobotFranka, vpRobotAfma6, vpRobotAfma4, vpRobotPtu46, vpSimulatorViper850, and vpSimulatorAfma6.
Definition at line 200 of file vpRobot.cpp.
Referenced by vpSimulatorAfma6::setRobotState(), and vpSimulatorAfma6::stopMotion().
|
pure virtualinherited |
Set the velocity (frame has to be specified) that will be applied to the velocity controller.
Implemented in vpRobotViper850, vpRobotViper650, vpRobotAfma6, vpRobotAfma4, vpSimulatorViper850, vpSimulatorAfma6, vpRobotFranka, vpSimulatorPioneerPan, vpRobotPioneer, vpSimulatorPioneer, vpSimulatorCamera, vpRobotTemplate, vpRobotCamera, and vpRobotPtu46.
|
inlineinherited |
|
protectedinherited |
Definition at line 136 of file vpBiclops.h.
|
protectedinherited |
Definition at line 135 of file vpBiclops.h.
|
protectedinherited |
|
protectedinherited |
|
protectedinherited |
|
protectedinherited |
|
staticinherited |
Definition at line 128 of file vpBiclops.h.
|
staticprotectedinherited |
|
staticprotectedinherited |
|
protectedinherited |
|
staticinherited |
|
staticinherited |
Pan range (in rad): from -panJointLimit to + panJointLimit
Definition at line 130 of file vpBiclops.h.
|
staticinherited |
Maximum speed (in rad/s) to perform a displacement
Definition at line 132 of file vpBiclops.h.
|
staticinherited |
Tilt range (in rad): from -tiltJointLimit to + tiltJointLimit
Definition at line 131 of file vpBiclops.h.