Visual Servoing Platform  version 3.4.0

#include <vpAfma4.h>

+ Inheritance diagram for vpAfma4:

Public Member Functions

 vpAfma4 ()
 
virtual ~vpAfma4 ()
 

Static Public Attributes

static const unsigned int njoint = 4
 

Protected Attributes

double _a1
 
double _d3
 
double _d4
 
double _joint_max [4]
 
double _joint_min [4]
 
vpTranslationVector _etc
 
vpRxyzVector _erc
 
vpHomogeneousMatrix _eMc
 

Inherited functionalities from vpAfma4

void init (void)
 
vpHomogeneousMatrix getForwardKinematics (const vpColVector &q) const
 
vpHomogeneousMatrix get_fMc (const vpColVector &q) const
 
void get_fMe (const vpColVector &q, vpHomogeneousMatrix &fMe) const
 
void get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
 
void get_cMe (vpHomogeneousMatrix &cMe) const
 
void get_cVe (vpVelocityTwistMatrix &cVe) const
 
void get_cVf (const vpColVector &q, vpVelocityTwistMatrix &cVf) const
 
void get_eJe (const vpColVector &q, vpMatrix &eJe) const
 
void get_fJe (const vpColVector &q, vpMatrix &fJe) const
 
void get_fJe_inverse (const vpColVector &q, vpMatrix &fJe_inverse) const
 
vpColVector getJointMin () const
 
vpColVector getJointMax () const
 
VISP_EXPORT std::ostream & operator<< (std::ostream &os, const vpAfma4 &afma4)
 

Detailed Description

Modelisation of Irisa's cylindrical robot named Afma4.

This robot has five degrees of freedom, but only four motorized joints (joint 3 is not motorized). Joint 2 and 3 are prismatic. The other ones are revolute joints.

The non modified Denavit-Hartenberg representation of the robot is given in the table below, where $q_1^*, q_2^*,q_4^*, q_5^*$ are the variable joint positions.

\[ \begin{tabular}{|c|c|c|c|c|} \hline Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ \hline 1 & 0 & 0 & 0 & $q_1^*$ \\ 2 & $a_1$ & $q_2^*$ & $-\pi/2$ & 0 \\ 3 & 0 & $d_3$ & $\pi/2$ & 0 \\ 4 & 0 & $d_4$ & $-\pi/2$ & $q_4^*-\pi/2$ \\ 5 & 0 & 0 & 0 & $q_5^*$ \\ \hline \end{tabular} \]

The forward kinematics of the robot is given by the homogeneous matrix ${^f}M_e$ which is implemented in get_fMe().

\[ {^f}M_e = \left[\begin{array}{cccc} c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\ -s_5 & -c_5 & d_4+q_2 \\ 0 & 0 & 0 & 1 \\ \end{array} \right] \]

The robot forward jacobian used to compute the cartesian velocities from joint ones is given and implemented in get_fJe() and get_eJe().

The robot inverse jacobian used to compute the joint velocities from cartesian ones are given and implemented in get_fJe_inverse().

Examples:
testAfma4.cpp.

Definition at line 110 of file vpAfma4.h.

Constructor & Destructor Documentation

vpAfma4::vpAfma4 ( )

Default constructor.

Definition at line 66 of file vpAfma4.cpp.

References _a1, _d3, _d4, _eMc, _erc, _etc, _joint_max, _joint_min, vpHomogeneousMatrix::buildFrom(), and init().

virtual vpAfma4::~vpAfma4 ( )
inlinevirtual

Destructor that does nothing.

Definition at line 115 of file vpAfma4.h.

Member Function Documentation

void vpAfma4::get_cMe ( vpHomogeneousMatrix cMe) const

Get the geometric transformation between the camera frame and the end-effector frame. This transformation is constant and correspond to the extrinsic camera parameters estimated by hand or by calibration.

Parameters
cMe: Transformation between the camera frame and the end-effector frame.

Definition at line 309 of file vpAfma4.cpp.

References _eMc, and vpHomogeneousMatrix::inverse().

Referenced by vpRobotAfma4::get_cMe(), get_cVe(), and vpRobotAfma4::get_cVe().

void vpAfma4::get_cVe ( vpVelocityTwistMatrix cVe) const

Get the twist transformation from camera frame to end-effector frame. This transformation allows to compute a velocity expressed in the end-effector frame into the camera frame.

Parameters
cVe: Twist transformation.

Definition at line 320 of file vpAfma4.cpp.

References vpVelocityTwistMatrix::buildFrom(), and get_cMe().

void vpAfma4::get_cVf ( const vpColVector q,
vpVelocityTwistMatrix cVf 
) const

Get the twist transformation from camera frame to the reference frame. This transformation allows to compute a velocity expressed in the reference frame into the camera frame.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
cVf: Twist transformation.

Definition at line 348 of file vpAfma4.cpp.

References vpVelocityTwistMatrix::buildFrom(), get_fMc(), and vpHomogeneousMatrix::inverse().

Referenced by vpRobotAfma4::get_cVf().

void vpAfma4::get_eJe ( const vpColVector q,
vpMatrix eJe 
) const

Get the robot jacobian expressed in the end-effector frame:

\[ {^e}J_e = \left[\begin{array}{cccc} -c_5(a_1c_4+d_3s_4) & -s_5 & 0 & 0 \\ s_5(a_1c_4+d_3s_4) & -c_5 & 0 & 0 \\ a_1s_4-d_3c_4 & 0 & 0 & 0 \\ -s_5 & 0 & -s_5 & 0 \\ -c_5 & 0 & -c_5 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
eJe: Robot jacobian expressed in the end-effector frame, with:

\[ {^e}J_e = \left[\begin{array}{cc} {^f}R_e^T & 0_{3 \times 3} \\ 0_{3 \times 3} & {^f}R_e^T \\ \end{array} \right] {^f}J_e \]

See also
get_fJe()

Definition at line 395 of file vpAfma4.cpp.

References _a1, _d3, and vpArray2D< Type >::resize().

Referenced by vpRobotAfma4::get_eJe().

void vpAfma4::get_fJe ( const vpColVector q,
vpMatrix fJe 
) const

Get the robot jacobian expressed in the robot reference frame also called fix frame:

\[ {^f}J_e = \left[\begin{array}{cccc} -a_1s_1-d_3c_1 & 0 & 0 & 0 \\ a_1c_1-d_3s_1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & c_{14} \\ 0 & 0 & 0 & s_{14} \\ 1 & 0 & 1 & 0 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fJe: Robot jacobian expressed in the robot reference frame.
See also
get_eJe() and get_fJe_inverse()

Definition at line 450 of file vpAfma4.cpp.

References _a1, _d3, and vpArray2D< Type >::resize().

Referenced by vpRobotAfma4::get_fJe().

void vpAfma4::get_fJe_inverse ( const vpColVector q,
vpMatrix fJe_inverse 
) const

Get the inverse jacobian.

\[ {^f}J_e^+ = \left[\begin{array}{cccccc} -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\ 0 & 0 & 0 & c_{14} & s_{14} & 0 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fJe_inverse: Inverse robot jacobian expressed in the robot reference frame.
See also
get_eJe() and get_fJe()

Definition at line 506 of file vpAfma4.cpp.

References _a1, _d3, and vpArray2D< Type >::resize().

Referenced by vpRobotAfma4::setVelocity().

vpHomogeneousMatrix vpAfma4::get_fMc ( const vpColVector q) const

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

This method is the same than getForwardKinematics(const vpColVector & q).

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
Returns
The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

See also
getForwardKinematics(const vpColVector & q)

Definition at line 176 of file vpAfma4.cpp.

Referenced by get_cVf(), getForwardKinematics(), vpRobotAfma4::getPosition(), and vpRobotAfma4::getVelocity().

void vpAfma4::get_fMc ( const vpColVector q,
vpHomogeneousMatrix fMc 
) const

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fMc: The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

Definition at line 210 of file vpAfma4.cpp.

References _eMc, and get_fMe().

void vpAfma4::get_fMe ( const vpColVector q,
vpHomogeneousMatrix fMe 
) const

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the end effector with respect to the base frame given the articular positions of all the four variable joints.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fMeThe homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the end effector frame ( ${^f}M_e$) with

\[ {^f}M_e = \left[\begin{array}{cccc} c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\ -s_5 & -c_5 & d_4+q_2 \\ 0 & 0 & 0 & 1 \\ \end{array} \right] \]

Definition at line 259 of file vpAfma4.cpp.

References _a1, _d3, and _d4.

Referenced by get_fMc(), and vpRobotAfma4::setVelocity().

vpHomogeneousMatrix vpAfma4::getForwardKinematics ( const vpColVector q) const

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

This method is the same than get_fMc(const vpColVector & q).

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
Returns
The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

See also
get_fMc(const vpColVector & q)
getInverseKinematics()

Definition at line 139 of file vpAfma4.cpp.

References get_fMc().

vpColVector vpAfma4::getJointMax ( ) const

Get max joint values.

Returns
Maximal joint values for the 4 dof X, Y, A, B. Translation Y is expressed in meters. Rotations X, A and B in radians.

Definition at line 557 of file vpAfma4.cpp.

References _joint_max.

vpColVector vpAfma4::getJointMin ( ) const

Get min joint values.

Returns
Minimal joint values for the 4 dof X, Y, A, B. Translation Y is expressed in meters. Rotations X,A and B in radians.

Definition at line 541 of file vpAfma4.cpp.

References _joint_min.

void vpAfma4::init ( void  )

Does nothing for the moment.

Definition at line 106 of file vpAfma4.cpp.

Referenced by vpAfma4().

Friends And Related Function Documentation

VISP_EXPORT std::ostream& operator<< ( std::ostream &  os,
const vpAfma4 afma4 
)
friend

Print on the output stream os the robot parameters (joint min/max, distance between axis 5 and 6, coupling factor between axis 5 and 6, hand-to-eye homogeneous matrix.

Parameters
os: Output stream.
afma4: Robot parameters.

Definition at line 574 of file vpAfma4.cpp.

Member Data Documentation

double vpAfma4::_a1
protected

Definition at line 146 of file vpAfma4.h.

Referenced by get_eJe(), get_fJe(), get_fJe_inverse(), get_fMe(), and vpAfma4().

double vpAfma4::_d3
protected

Definition at line 147 of file vpAfma4.h.

Referenced by get_eJe(), get_fJe(), get_fJe_inverse(), get_fMe(), and vpAfma4().

double vpAfma4::_d4
protected

Definition at line 148 of file vpAfma4.h.

Referenced by get_fMe(), and vpAfma4().

vpHomogeneousMatrix vpAfma4::_eMc
protected

Definition at line 156 of file vpAfma4.h.

Referenced by get_cMe(), get_fMc(), vpRobotAfma4::setVelocity(), and vpAfma4().

vpRxyzVector vpAfma4::_erc
protected

Definition at line 154 of file vpAfma4.h.

Referenced by vpAfma4().

vpTranslationVector vpAfma4::_etc
protected

Definition at line 153 of file vpAfma4.h.

Referenced by vpAfma4().

double vpAfma4::_joint_max[4]
protected

Definition at line 149 of file vpAfma4.h.

Referenced by getJointMax(), vpRobotAfma4::init(), and vpAfma4().

double vpAfma4::_joint_min[4]
protected

Definition at line 150 of file vpAfma4.h.

Referenced by getJointMin(), vpRobotAfma4::init(), and vpAfma4().