#include <visp3/robot/vpAfma4.h>

Inheritance diagram for vpAfma4:

Public Member Functions
	vpAfma4 ()

virtual	~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

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

Friends
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 108 of file vpAfma4.h.

Constructor & Destructor Documentation

vpAfma4::vpAfma4 ( )

Default constructor.

Definition at line 67 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 113 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 320 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 335 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 364 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 412 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 466 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 522 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 186 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 221 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.

fMe The 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 269 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 ) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value ), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values and ). 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 148 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 575 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 558 of file vpAfma4.cpp.

References _joint_min.

void vpAfma4::init ( void )

Does nothing for the moment.

Definition at line 111 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