Commit 0ca25deb by Gabriele Buondonno

### [rpy] Implement rpyToJacInv

parent ac5520f9
 ... ... @@ -73,6 +73,25 @@ namespace pinocchio template Eigen::Matrix rpyToJac(const Eigen::MatrixBase & 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 Eigen::Matrix rpyToJacInv(const Eigen::MatrixBase & rpy, const ReferenceFrame rf=LOCAL); } // namespace rpy } ... ...
 ... ... @@ -101,6 +101,45 @@ namespace pinocchio } } } template Eigen::Matrix rpyToJacInv(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); 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 (rand()) / (static_cast (RAND_MAX/(2*M_PI))) - M_PI; double p = static_cast (rand()) / (static_cast (RAND_MAX/M_PI)) - (M_PI/2); p *= 0.999; // ensure we are not too close to a singularity double y = static_cast (rand()) / (static_cast (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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!