/*!
Compute the direct geometric model of the camera: fMc
\param q : Articular position for pan and tilt axis.
\param 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.
*/
void
vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc)
{
vpHomogeneousMatrix fMe = get_fMe(q);
fMc = fMe * cMe_.inverse();
vpCDEBUG (6) << "camera position: " << std::endl << fMc;
return ;
}
/*!
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.
\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 fMc : 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]
*/
void
vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
{
// Compute the direct geometric model: fMe = transformation between
// fix and end effector frame.
vpHomogeneousMatrix fMe;
get_fMe(q, fMe);
fMc = fMe * this->_eMc;
return;
}
