Commit b2ac6e40 by Justin Carpentier Committed by GitHub

### Merge pull request #1350 from gabrielebndn/topic/rpy

Improve support for rpy
parents 706ed21e 836d732a
Pipeline #12453 passed with stage
in 130 minutes and 54 seconds
 ... @@ -7,8 +7,7 @@ ... @@ -7,8 +7,7 @@ #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/comparison-operators.hpp" #include "pinocchio/math/comparison-operators.hpp" #include "pinocchio/multibody/fwd.hpp" #include namespace pinocchio namespace pinocchio { { ... @@ -22,17 +21,10 @@ namespace pinocchio ... @@ -22,17 +21,10 @@ namespace pinocchio /// around axis \f$\alpha\f$. /// around axis \f$\alpha\f$. /// /// template template Eigen::Matrix rpyToMatrix(const Scalar r, Eigen::Matrix const Scalar p, rpyToMatrix(const Scalar& r, const Scalar y) 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(); } /// /// /// \brief Convert from Roll, Pitch, Yaw to rotation Matrix /// \brief Convert from Roll, Pitch, Yaw to rotation Matrix ... @@ -43,11 +35,7 @@ namespace pinocchio ... @@ -43,11 +35,7 @@ namespace pinocchio /// /// template template Eigen::Matrix Eigen::Matrix rpyToMatrix(const Eigen::MatrixBase & rpy) rpyToMatrix(const Eigen::MatrixBase & rpy); { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1); return rpyToMatrix(rpy[0], rpy[1], rpy[2]); } /// /// /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw ... @@ -64,33 +52,69 @@ namespace pinocchio ... @@ -64,33 +52,69 @@ namespace pinocchio /// /// template template Eigen::Matrix Eigen::Matrix matrixToRpy(const Eigen::MatrixBase & R) 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) /// { /// \brief Compute the Jacobian of the Roll-Pitch-Yaw conversion res[1] = pi - res[1]; /// if(res[0] < Scalar(0)) /// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD), res[0] += pi; /// the Jacobian is such that \f${}^F\omega = J_F(\phi)\dot{\phi} \f$, else /// where \f${}^F\omega \f$ is the angular velocity expressed in frame F res[0] -= pi; /// and \f$J_F \f$ is the Jacobian computed with reference frame F // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign /// res[2] -= pi; /// \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 computeRpyJacobian(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 computeRpyJacobianInverse(const Eigen::MatrixBase & rpy, const ReferenceFrame rf=LOCAL); return res; /// } /// \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 Eigen::Matrix computeRpyJacobianTimeDerivative(const Eigen::MatrixBase & rpy, const Eigen::MatrixBase & rpydot, const ReferenceFrame rf=LOCAL); } // namespace rpy } // namespace rpy } } /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/math/rpy.hxx" #endif //#ifndef __pinocchio_math_rpy_hpp__ #endif //#ifndef __pinocchio_math_rpy_hpp__