Commit 0ca25deb authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[rpy] Implement rpyToJacInv

parent ac5520f9
......@@ -73,6 +73,25 @@ namespace pinocchio
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJac(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
///
/// \brief Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion
///
/// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
/// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
/// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
/// and \f$ J_F \f$ is the Jacobian computed with reference frame F
///
/// \param[in] rpy Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The inverse of the Jacobian of the Roll-Pitch-Yaw conversion in the appropriate frame
///
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJacInv(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
} // namespace rpy
}
......
......@@ -101,6 +101,45 @@ namespace pinocchio
}
}
}
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJacInv(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
{
typedef typename Vector3Like::Scalar Scalar;
Eigen::Matrix<Scalar,3,3> J;
const Scalar p = rpy[1];
Scalar sp, cp;
SINCOS(p, &sp, &cp);
Scalar tp = sp/cp;
switch (rf)
{
case LOCAL:
{
const Scalar r = rpy[0];
Scalar sr, cr; SINCOS(r, &sr, &cr);
J << Scalar(1.0), sr*tp, cr*tp,
Scalar(0.0), cr, -sr,
Scalar(0.0), sr/cp, cr/cp;
return J;
}
case WORLD:
case LOCAL_WORLD_ALIGNED:
{
const Scalar y = rpy[2];
Scalar sy, cy; SINCOS(y, &sy, &cy);
J << cy/cp, sy/cp, Scalar(0.0),
-sy, cy, Scalar(0.0),
cy*tp, sy*tp, Scalar(1.0);
return J;
}
default:
{
throw std::invalid_argument("Bad reference frame.");
}
}
}
} // namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__
......@@ -181,4 +181,31 @@ BOOST_AUTO_TEST_CASE(test_rpyToJac)
BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol));
}
BOOST_AUTO_TEST_CASE(test_rpyToJacInv)
{
// Check correct identities between different versions
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
p *= 0.999; // ensure we are not too close to a singularity
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Vector3d rpy(r, p, y);
Eigen::Matrix3d j0 = pinocchio::rpy::rpyToJac(rpy);
Eigen::Matrix3d j0inv = pinocchio::rpy::rpyToJacInv(rpy);
BOOST_CHECK(j0inv.isApprox(j0.inverse()));
Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jLinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::LOCAL);
BOOST_CHECK(jLinv.isApprox(jL.inverse()));
Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
Eigen::Matrix3d jWinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::WORLD);
BOOST_CHECK(jWinv.isApprox(jW.inverse()));
Eigen::Matrix3d jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d jAinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(jAinv.isApprox(jA.inverse()));
}
BOOST_AUTO_TEST_SUITE_END()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment