/*!
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).
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
around the vertical axis, while q[1] corresponds to the vertical
translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
correspond to the pan and tilt of the camera (respectively joint 4
and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
q[3] are expressed in radians. The translation q[1] is expressed in
meters.
\return The homogeneous matrix corresponding to the direct geometric
model which expresses the transformation between the fix frame and the
camera frame (\f${^f}M_c\f$) with:
\f[
{^f}M_c = {^f}M_e * {^e}M_c
\f]
\sa getForwardKinematics(const vpColVector & q)
*/
vpHomogeneousMatrix
vpAfma4::get_fMc (const vpColVector & q) const
{
vpHomogeneousMatrix fMc;
get_fMc(q, fMc);
return fMc;
}
/*!
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).
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
around the vertical axis, while q[1] corresponds to the vertical
translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
correspond to the pan and tilt of the camera (respectively joint 4
and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
q[3] are expressed in radians. The translation q[1] is expressed in
meters.
\return The homogeneous matrix corresponding to the direct geometric
model which expresses the transformation between the fix frame and the
camera frame (\f${^f}M_c\f$) with:
\f[
{^f}M_c = {^f}M_e * {^e}M_c
\f]
\sa get_fMc(const vpColVector & q)
\sa getInverseKinematics()
*/
vpHomogeneousMatrix
vpAfma4::getForwardKinematics(const vpColVector & q) const
{
vpHomogeneousMatrix fMc;
fMc = get_fMc(q);
return fMc;
}
/*!
Return the direct geometric model of the camera: fMc
\param q : Articular position for pan and tilt axis.
\return fMc, the homogeneous matrix corresponding to the direct geometric
model of the camera. Discribes the transformation between the robot reference
frame (called fixed) and the camera frame.
*/
vpHomogeneousMatrix
vpBiclops::get_fMc (const vpColVector & q)
{
vpHomogeneousMatrix fMc;
get_fMc (q, fMc);
return fMc;
}
void
vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc)
{
vpHomogeneousMatrix fMc;
get_fMc (q, fMc);
fvc.buildFrom(fMc.inverse());
return ;
}
/*!
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.
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
around the vertical axis, while q[1] corresponds to the vertical
translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
correspond to the pan and tilt of the camera (respectively joint 4
and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
q[3] are expressed in radians. The translation q[1] is expressed in
meters.
\param cVf : Twist transformation.
*/
void
vpAfma4::get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const
{
vpHomogeneousMatrix fMc, cMf ;
get_fMc(q, fMc) ;
cMf = fMc.inverse();
cVf.buildFrom(cMf) ;
return;
}
