Commit ac5520f9 by Gabriele Buondonno

### [rpy] Implement rpyToJac

parent c520069a
 ... ... @@ -7,6 +7,7 @@ #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/comparison-operators.hpp" #include "pinocchio/multibody/fwd.hpp" #include ... ... @@ -53,6 +54,25 @@ namespace pinocchio template Eigen::Matrix matrixToRpy(const Eigen::MatrixBase & R); /// /// \brief Compute the 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 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 Eigen::Matrix rpyToJac(const Eigen::MatrixBase & rpy, const ReferenceFrame rf=LOCAL); } // namespace rpy } ... ...
src/math/rpy.hxx 0 → 100644
 // // Copyright (c) 2016-2020 CNRS INRIA // #ifndef __pinocchio_math_rpy_hxx__ #define __pinocchio_math_rpy_hxx__ #include "pinocchio/math/sincos.hpp" namespace pinocchio { namespace rpy { template Eigen::Matrix rpyToMatrix(const Scalar r, const Scalar p, const Scalar y) { typedef Eigen::AngleAxis AngleAxis; typedef Eigen::Matrix Vector3s; return (AngleAxis(y, Vector3s::UnitZ()) * AngleAxis(p, Vector3s::UnitY()) * AngleAxis(r, Vector3s::UnitX()) ).toRotationMatrix(); } template Eigen::Matrix rpyToMatrix(const Eigen::MatrixBase & rpy) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1); return rpyToMatrix(rpy[0], rpy[1], rpy[2]); } template Eigen::Matrix matrixToRpy(const Eigen::MatrixBase & R) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3); assert(R.isUnitary() && "R is not a unitary matrix"); typedef typename Matrix3Like::Scalar Scalar; typedef Eigen::Matrix ReturnType; static const Scalar pi = PI(); ReturnType res = R.eulerAngles(2,1,0).reverse(); if(res[1] < -pi/2) res[1] += 2*pi; if(res[1] > pi/2) { res[1] = pi - res[1]; if(res[0] < Scalar(0)) res[0] += pi; else res[0] -= pi; // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign res[2] -= pi; } return res; } template Eigen::Matrix rpyToJac(const Eigen::MatrixBase & rpy, const ReferenceFrame rf) { typedef typename Vector3Like::Scalar Scalar; Eigen::Matrix J; const Scalar p = rpy[1]; Scalar sp, cp; SINCOS(p, &sp, &cp); switch (rf) { case LOCAL: { const Scalar r = rpy[0]; Scalar sr, cr; SINCOS(r, &sr, &cr); J << Scalar(1.0), Scalar(0.0), -sp, Scalar(0.0), cr, sr*cp, Scalar(0.0), -sr, cr*cp; return J; } case WORLD: case LOCAL_WORLD_ALIGNED: { const Scalar y = rpy[2]; Scalar sy, cy; SINCOS(y, &sy, &cy); J << cp*cy, -sy, Scalar(0.0), cp*sy, cy, Scalar(0.0), -sp, Scalar(0.0), Scalar(1.0); return J; } default: { throw std::invalid_argument("Bad reference frame."); } } } } // namespace rpy } #endif //#ifndef __pinocchio_math_rpy_hxx__
 ... ... @@ -117,4 +117,68 @@ BOOST_AUTO_TEST_CASE(test_matrixToRpy) } } BOOST_AUTO_TEST_CASE(test_rpyToJac) { // Check identity at zero Eigen::Vector3d rpy(Eigen::Vector3d::Zero()); Eigen::Matrix3d j0 = pinocchio::rpy::rpyToJac(rpy); BOOST_CHECK(j0.isIdentity()); Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL); BOOST_CHECK(jL.isIdentity()); Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD); BOOST_CHECK(jW.isIdentity()); Eigen::Matrix3d jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED); BOOST_CHECK(jA.isIdentity()); // Check correct identities between different versions double r = static_cast (rand()) / (static_cast (RAND_MAX/(2*M_PI))) - M_PI; double p = static_cast (rand()) / (static_cast (RAND_MAX/M_PI)) - (M_PI/2); double y = static_cast (rand()) / (static_cast (RAND_MAX/(2*M_PI))) - M_PI; rpy = Eigen::Vector3d(r, p, y); Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy); j0 = pinocchio::rpy::rpyToJac(rpy); jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL); jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD); jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED); BOOST_CHECK(j0 == jL); BOOST_CHECK(jW == jA); BOOST_CHECK(jW.isApprox(R*jL)); // Check against analytical formulas Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX(); Eigen::Vector3d jL1Expected = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1); Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()) ).toRotationMatrix().transpose().col(2); BOOST_CHECK(jL.col(0).isApprox(jL0Expected)); BOOST_CHECK(jL.col(1).isApprox(jL1Expected)); BOOST_CHECK(jL.col(2).isApprox(jL2Expected)); Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY()) ).toRotationMatrix().col(0); Eigen::Vector3d jW1Expected = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1); Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ(); BOOST_CHECK(jW.col(0).isApprox(jW0Expected)); BOOST_CHECK(jW.col(1).isApprox(jW1Expected)); BOOST_CHECK(jW.col(2).isApprox(jW2Expected)); // Check against finite differences Eigen::Vector3d rpydot = Eigen::Vector3d::Random(); double const eps = 1e-7; double const tol = 1e-5; Eigen::Matrix3d dRdr = (pinocchio::rpy::rpyToMatrix(r + eps, p, y) - R) / eps; Eigen::Matrix3d dRdp = (pinocchio::rpy::rpyToMatrix(r, p + eps, y) - R) / eps; Eigen::Matrix3d dRdy = (pinocchio::rpy::rpyToMatrix(r, p, y + eps) - R) / eps; Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2]; Eigen::Vector3d omegaL = jL * rpydot; BOOST_CHECK(Rdot.isApprox(R * pinocchio::skew(omegaL), tol)); Eigen::Vector3d omegaW = jW * rpydot; BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol)); } BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!