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"); matrixToRpy(const Eigen::MatrixBase & R); 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; /// /// \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); 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 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__
src/math/rpy.hxx 0 → 100644
 // // Copyright (c) 2016-2020 CNRS INRIA // #ifndef __pinocchio_math_rpy_hxx__ #define __pinocchio_math_rpy_hxx__ #include #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 computeRpyJacobian(const Eigen::MatrixBase & rpy, const ReferenceFrame rf) { typedef typename Vector3Like::Scalar Scalar; typedef Eigen::Matrix ReturnType; ReturnType 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."); } } } template Eigen::Matrix computeRpyJacobianInverse(const Eigen::MatrixBase & rpy, const ReferenceFrame rf) { typedef typename Vector3Like::Scalar Scalar; typedef Eigen::Matrix ReturnType; ReturnType 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."); } } } template Eigen::Matrix computeRpyJacobianTimeDerivative(const Eigen::MatrixBase & rpy, const Eigen::MatrixBase & rpydot, const ReferenceFrame rf) { typedef typename Vector3Like0::Scalar Scalar; typedef Eigen::Matrix ReturnType; ReturnType 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__
 ... ... @@ -2,10 +2,13 @@ import unittest from math import pi import numpy as np import pinocchio as pin from numpy.linalg import inv from random import random from eigenpy import AngleAxis import pinocchio as pin from pinocchio.utils import npToTuple from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, computeRpyJacobian, computeRpyJacobianInverse, computeRpyJacobianTimeDerivative from test_case import PinocchioTestCase as TestCase ... ... @@ -40,5 +43,185 @@ class TestRPY(TestCase): else: self.assertTrue(False) def test_rpyToMatrix(self): r = random()*2*pi - pi p = random()*pi - pi/2 y = random()*2*pi - pi R = rpyToMatrix(r, p, y) Raa = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(AngleAxis(p, np.array([0., 1., 0.])).matrix()) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) self.assertApprox(R, Raa) v = np.array([r, p, y]) Rv = rpyToMatrix(v) self.assertApprox(Rv, Raa) self.assertApprox(Rv, R) def test_matrixToRpy(self): n = 100 for _ in range(n): quat = pin.Quaternion(np.random.rand(4,1)).normalized() R = quat.toRotationMatrix() v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2) self.assertTrue(-pi <= v[2] and v[2] <= pi) n2 = 100 # Test singular case theta = pi/2 Rp = np.array([[ 0.0, 0.0, 1.0], [ 0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) for _ in range(n2): r = random()*2*pi - pi y = random()*2*pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2) self.assertTrue(-pi <= v[2] and v[2] <= pi) # Test singular case theta = -pi/2 Rp = np.array([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]) for _ in range(n2): r = random()*2*pi - pi y = random()*2*pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2) self.assertTrue(-pi <= v[2] and v[2] <= pi) def test_computeRpyJacobian(self): # Check identity at zero rpy = np.zeros(3) j0 = computeRpyJacobian(rpy) self.assertTrue((j0 == np.eye(3)).all()) jL = computeRpyJacobian(rpy, pin.LOCAL) self.assertTrue((jL == np.eye(3)).all()) jW = computeRpyJacobian(rpy, pin.WORLD) self.assertTrue((jW == np.eye(3)).all()) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((jA == np.eye(3)).all()) # Check correct identities between different versions r = random()*2*pi - pi p = random()*pi - pi/2 y = random()*2*pi - pi rpy = np.array([r, p, y]) R = rpyToMatrix(rpy) j0 = computeRpyJacobian(rpy) jL = computeRpyJacobian(rpy, pin.LOCAL) jW = computeRpyJacobian(rpy, pin.WORLD) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((j0 == jL).all()) self.assertTrue((jW == jA).all()) self.assertApprox(jW, R.dot(jL)) # Check against finite differences rpydot = np.random.rand(3) eps = 1e-7 tol = 1e-4 dRdr = (rpyToMatrix(r + eps, p, y) - R) / eps dRdp = (rpyToMatrix(r, p + eps, y) - R) / eps dRdy = (rpyToMatrix(r, p, y + eps) - R) / eps Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2] omegaL = jL.dot(rpydot) self.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol)) omegaW = jW.dot(rpydot) self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol)) def test_computeRpyJacobianInverse(self): # Check correct identities between different versions r = random()*2*pi - pi p = random()*pi - pi/2 p *= 0.999 # ensure we are not too close to a singularity y = random()*2*pi - pi rpy = np.array([r, p, y]) j0 = computeRpyJacobian(rpy) j0inv = computeRpyJacobianInverse(rpy) self.assertApprox(j0inv, inv(j0)) jL = computeRpyJacobian(rpy, pin.LOCAL) jLinv = computeRpyJacobianInverse(rpy, pin.LOCAL) self.assertApprox(jLinv, inv(jL)) jW = computeRpyJacobian(rpy, pin.WORLD) jWinv = computeRpyJacobianInverse(rpy, pin.WORLD) self.assertApprox(jWinv, inv(jW)) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) jAinv = computeRpyJacobianInverse(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertApprox(jAinv, inv(jA)) def test_computeRpyJacobianTimeDerivative(self): # Check zero at zero velocity r = random()*2*pi - pi p = random()*pi - pi/2 y = random()*2*pi - pi