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 @@ #include "pinocchio/math/fwd.hpp" #include "pinocchio/math/comparison-operators.hpp" #include #include "pinocchio/multibody/fwd.hpp" namespace pinocchio { ... ... @@ -22,17 +21,10 @@ namespace pinocchio /// around axis \f$\alpha\f$. /// 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(); } Eigen::Matrix rpyToMatrix(const Scalar& r, const Scalar& p, const Scalar& y); /// /// \brief Convert from Roll, Pitch, Yaw to rotation Matrix ... ... @@ -43,11 +35,7 @@ namespace pinocchio /// 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]); } rpyToMatrix(const Eigen::MatrixBase & rpy); /// /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw ... ... @@ -64,33 +52,69 @@ namespace pinocchio /// 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; matrixToRpy(const Eigen::MatrixBase & R); 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; } /// /// \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 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 } /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/math/rpy.hxx" #endif //#ifndef __pinocchio_math_rpy_hpp__