Commit 04e7c245 by Gabriele Buondonno

### [rpy] Implement rpyToJacDerivative

parent dc3814b7
 ... ... @@ -93,6 +93,26 @@ namespace pinocchio template inline Eigen::Matrix rpyToJacInv(const Eigen::MatrixBase & 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 inline Eigen::Matrix rpyToJacDerivative(const Eigen::MatrixBase & rpy, const Eigen::MatrixBase & rpydot, const ReferenceFrame rf=LOCAL); } // namespace rpy } ... ...
 ... ... @@ -141,6 +141,47 @@ namespace pinocchio } } } template inline Eigen::Matrix rpyToJacDerivative(const Eigen::MatrixBase & rpy, const Eigen::MatrixBase & rpydot, const ReferenceFrame rf) { typedef typename Vector3Like0::Scalar Scalar; Eigen::Matrix 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 #include #include #include // 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 (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; 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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!