Commit 04e7c245 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[rpy] Implement rpyToJacDerivative

parent dc3814b7
......@@ -93,6 +93,26 @@ namespace pinocchio
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJacInv(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
///
/// \brief Compute the time derivative 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] rpydot Time derivative of the Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The time derivative 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 Vector3Like0, typename Vector3Like1>
inline Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
rpyToJacDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf=LOCAL);
} // namespace rpy
}
......
......@@ -141,6 +141,47 @@ namespace pinocchio
}
}
}
template<typename Vector3Like0, typename Vector3Like1>
inline Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
rpyToJacDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
{
typedef typename Vector3Like0::Scalar Scalar;
Eigen::Matrix<Scalar,3,3> J;
const Scalar p = rpy[1];
const Scalar dp = rpydot[1];
Scalar sp, cp;
SINCOS(p, &sp, &cp);
switch (rf)
{
case LOCAL:
{
const Scalar r = rpy[0];
const Scalar dr = rpydot[0];
Scalar sr, cr; SINCOS(r, &sr, &cr);
J << Scalar(0.0), Scalar(0.0), -cp*dp,
Scalar(0.0), -sr*dr, cr*cp*dr - sr*sp*dp,
Scalar(0.0), -cr*dr, -sr*cp*dr - cr*sp*dp;
return J;
}
case WORLD:
case LOCAL_WORLD_ALIGNED:
{
const Scalar y = rpy[2];
const Scalar dy = rpydot[2];
Scalar sy, cy; SINCOS(y, &sy, &cy);
J << -sp*cy*dp - cp*sy*dy, -cy*dy, Scalar(0.0),
cp*cy*dy - sp*sy*dp, -sy*dy, Scalar(0.0),
-cp*dp, Scalar(0.0), Scalar(0.0);
return J;
}
default:
{
throw std::invalid_argument("Bad reference frame.");
}
}
}
} // namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__
......@@ -4,6 +4,7 @@
#include <pinocchio/math/rpy.hpp>
#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/skew.hpp>
#include <boost/variant.hpp> // to avoid C99 warnings
......@@ -208,4 +209,70 @@ BOOST_AUTO_TEST_CASE(test_rpyToJacInv)
BOOST_CHECK(jAinv.isApprox(jA.inverse()));
}
BOOST_AUTO_TEST_CASE(test_rpyToJacDerivative)
{
// Check zero at zero velocity
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);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Vector3d rpy(r, p, y);
Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
Eigen::Matrix3d dj0 = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot);
BOOST_CHECK(dj0.isZero());
Eigen::Matrix3d djL = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL);
BOOST_CHECK(djL.isZero());
Eigen::Matrix3d djW = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::WORLD);
BOOST_CHECK(djW.isZero());
Eigen::Matrix3d djA = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(djA.isZero());
// Check correct identities between different versions
rpydot = Eigen::Vector3d::Random();
dj0 = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot);
djL = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL);
djW = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::WORLD);
djA = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(dj0 == djL);
BOOST_CHECK(djW == djA);
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
Eigen::Vector3d omegaL = jL * rpydot;
Eigen::Vector3d omegaW = jW * rpydot;
BOOST_CHECK(omegaW.isApprox(R*omegaL));
BOOST_CHECK(djW.isApprox(pinocchio::skew(omegaW)*R*jL + R*djL));
BOOST_CHECK(djW.isApprox(R*pinocchio::skew(omegaL)*jL + R*djL));
// Check against finite differences
double const eps = 1e-7;
double const tol = 1e-5;
Eigen::Vector3d rpyEps = rpy;
rpyEps[0] += eps;
Eigen::Matrix3d djLdr = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[0] = rpy[0];
rpyEps[1] += eps;
Eigen::Matrix3d djLdp = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[1] = rpy[1];
rpyEps[2] += eps;
Eigen::Matrix3d djLdy = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[2] = rpy[2];
Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
BOOST_CHECK(djL.isApprox(djLf, tol));
rpyEps[0] += eps;
Eigen::Matrix3d djWdr = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[0] = rpy[0];
rpyEps[1] += eps;
Eigen::Matrix3d djWdp = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[1] = rpy[1];
rpyEps[2] += eps;
Eigen::Matrix3d djWdy = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[2] = rpy[2];
Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
BOOST_CHECK(djW.isApprox(djWf, tol));
}
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