![]() |
Visual Servoing Platform
version 3.2.0
|
#include <vpBiclops.h>
Public Types | |
enum | DenavitHartenbergModel { DH1, DH2 } |
Public Member Functions | |
vpBiclops (void) | |
virtual | ~vpBiclops () |
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 Attributes | |
DenavitHartenbergModel | dh_model_ |
vpHomogeneousMatrix | cMe_ |
Inherited functionalities from vpBiclops | |
void | init (void) |
void | computeMGD (const vpColVector &q, vpHomogeneousMatrix &fMc) const |
vpHomogeneousMatrix | computeMGD (const vpColVector &q) const |
void | computeMGD (const vpColVector &q, vpPoseVector &fvc) const |
vpHomogeneousMatrix | get_cMe () const |
void | get_cVe (vpVelocityTwistMatrix &_cVe) const |
void | get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const |
void | get_fMc (const vpColVector &q, vpPoseVector &fvc) const |
vpHomogeneousMatrix | get_fMc (const vpColVector &q) const |
vpHomogeneousMatrix | get_fMe (const vpColVector &q) const |
void | get_eJe (const vpColVector &q, vpMatrix &eJe) const |
void | get_fJe (const vpColVector &q, vpMatrix &fJe) const |
vpBiclops::DenavitHartenbergModel | getDenavitHartenbergModel () const |
void | set_cMe () |
void | set_cMe (const vpHomogeneousMatrix &cMe) |
void | setDenavitHartenbergModel (vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1) |
VISP_EXPORT std::ostream & | operator<< (std::ostream &os, const vpBiclops &constant) |
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Two different Denavit Hartenberg representations of the robot are implemented. As mentionned in vpBiclops::DenavitHartenbergModel they differ in the orientation of the tilt axis. Use setDenavitHartenbergModel() to select the representation.
See http://www.traclabs.com/tracbiclops.htm for more details concerning the hardware.
Definition at line 76 of file vpBiclops.h.
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.
vpBiclops::vpBiclops | ( | void | ) |
Default construtor. Call init().
Definition at line 271 of file vpBiclops.cpp.
|
inlinevirtual |
Destructor that does nothing.
Definition at line 141 of file vpBiclops.h.
vpHomogeneousMatrix vpBiclops::computeMGD | ( | const vpColVector & | q | ) | const |
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.
void vpBiclops::computeMGD | ( | const vpColVector & | q, |
vpHomogeneousMatrix & | fMc | ||
) | const |
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.
void vpBiclops::computeMGD | ( | const vpColVector & | q, |
vpPoseVector & | fvc | ||
) | const |
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.
|
inline |
Return the tranformation between the camera frame and the end effector frame.
Definition at line 156 of file vpBiclops.h.
void vpBiclops::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 expess a velocity skew from end effector frame in camera frame. |
Definition at line 311 of file vpBiclops.cpp.
void vpBiclops::get_eJe | ( | const vpColVector & | q, |
vpMatrix & | eJe | ||
) | const |
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.
void vpBiclops::get_fJe | ( | const vpColVector & | q, |
vpMatrix & | fJe | ||
) | const |
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.
vpHomogeneousMatrix vpBiclops::get_fMc | ( | const vpColVector & | q | ) | const |
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.
void vpBiclops::get_fMc | ( | const vpColVector & | q, |
vpHomogeneousMatrix & | fMc | ||
) | const |
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.
void vpBiclops::get_fMc | ( | const vpColVector & | q, |
vpPoseVector & | fvc | ||
) | const |
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.
vpHomogeneousMatrix vpBiclops::get_fMe | ( | const vpColVector & | q | ) | const |
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.
|
inline |
Return the Denavit Hartenberg representation used to model the head.
Definition at line 171 of file vpBiclops.h.
void vpBiclops::init | ( | void | ) |
Initialization. Set the default transformation.
Definition at line 281 of file vpBiclops.cpp.
void vpBiclops::set_cMe | ( | ) |
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.
|
inline |
Set the transformation between the camera frame and the end effector frame.
Definition at line 178 of file vpBiclops.h.
|
inline |
Set the Denavit Hartenberg representation used to model the head.
Definition at line 184 of file vpBiclops.h.
|
friend |
Definition at line 292 of file vpBiclops.cpp.
|
protected |
Definition at line 136 of file vpBiclops.h.
|
protected |
Definition at line 135 of file vpBiclops.h.
|
static |
Definition at line 128 of file vpBiclops.h.
|
static |
|
static |
Pan range (in rad): from -panJointLimit to + panJointLimit
Definition at line 130 of file vpBiclops.h.
|
static |
Maximum speed (in rad/s) to perform a displacement
Definition at line 132 of file vpBiclops.h.
|
static |
Tilt range (in rad): from -tiltJointLimit to + tiltJointLimit
Definition at line 131 of file vpBiclops.h.