[rpy] Implement rpyToJac

src/math/rpy.hpp

@@ -7,6 +7,7 @@
 
 #include "pinocchio/math/fwd.hpp"
 #include "pinocchio/math/comparison-operators.hpp"
+#include "pinocchio/multibody/fwd.hpp"
 
 #include <Eigen/Geometry>
 
@@ -53,6 +54,25 @@ namespace pinocchio
     template<typename Matrix3Like>
     Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
     matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R);
+
+    ///
+    /// \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<typename Vector3Like>
+    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
+    rpyToJac(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
   } // namespace rpy
 }
 

src/math/rpy.hxx

+//
+// Copyright (c) 2016-2020 CNRS INRIA
+//
+
+#ifndef __pinocchio_math_rpy_hxx__
+#define __pinocchio_math_rpy_hxx__
+
+#include "pinocchio/math/sincos.hpp"
+
+namespace pinocchio
+{
+  namespace rpy
+  {
+    template<typename Scalar>
+    Eigen::Matrix<Scalar,3,3> rpyToMatrix(const Scalar r,
+                                          const Scalar p,
+                                          const Scalar y)
+    {
+      typedef Eigen::AngleAxis<Scalar> AngleAxis;
+      typedef Eigen::Matrix<Scalar,3,1> Vector3s;
+      return (AngleAxis(y, Vector3s::UnitZ())
+              * AngleAxis(p, Vector3s::UnitY())
+              * AngleAxis(r, Vector3s::UnitX())
+             ).toRotationMatrix();
+    }
+
+
+    template<typename Vector3Like>
+    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
+    rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
+    {
+      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
+      return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
+    }
+
+
+    template<typename Matrix3Like>
+    Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
+    matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & 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<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
+      static const Scalar pi = PI<Scalar>();
+
+      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<typename Vector3Like>
+    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
+    rpyToJac(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
+    {
+      typedef typename Vector3Like::Scalar Scalar;
+      Eigen::Matrix<Scalar,3,3> 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.");
+        }
+      }
+    }
+  } // namespace rpy
+}
+#endif //#ifndef __pinocchio_math_rpy_hxx__

unittest/rpy.cpp

@@ -117,4 +117,68 @@ BOOST_AUTO_TEST_CASE(test_matrixToRpy)
   }
 }
 
+
+BOOST_AUTO_TEST_CASE(test_rpyToJac)
+{
+  // Check identity at zero
+  Eigen::Vector3d rpy(Eigen::Vector3d::Zero());
+  Eigen::Matrix3d j0 = pinocchio::rpy::rpyToJac(rpy);
+  BOOST_CHECK(j0.isIdentity());
+  Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
+  BOOST_CHECK(jL.isIdentity());
+  Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
+  BOOST_CHECK(jW.isIdentity());
+  Eigen::Matrix3d jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
+  BOOST_CHECK(jA.isIdentity());
+
+  // Check correct identities between different versions
+  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
+  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
+  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
+  rpy = Eigen::Vector3d(r, p, y);
+  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
+  j0 = pinocchio::rpy::rpyToJac(rpy);
+  jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
+  jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
+  jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
+  BOOST_CHECK(j0 == jL);
+  BOOST_CHECK(jW == jA);
+  BOOST_CHECK(jW.isApprox(R*jL));
+
+  // Check against analytical formulas 
+  Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX();
+  Eigen::Vector3d jL1Expected = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1);
+  Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
+                               * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
+                                ).toRotationMatrix().transpose().col(2);
+  BOOST_CHECK(jL.col(0).isApprox(jL0Expected));
+  BOOST_CHECK(jL.col(1).isApprox(jL1Expected));
+  BOOST_CHECK(jL.col(2).isApprox(jL2Expected));
+
+  Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
+                               * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
+                                ).toRotationMatrix().col(0);
+  Eigen::Vector3d jW1Expected = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1);
+  Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ();
+  BOOST_CHECK(jW.col(0).isApprox(jW0Expected));
+  BOOST_CHECK(jW.col(1).isApprox(jW1Expected));
+  BOOST_CHECK(jW.col(2).isApprox(jW2Expected));
+
+  // Check against finite differences
+  Eigen::Vector3d rpydot = Eigen::Vector3d::Random();
+  double const eps = 1e-7;
+  double const tol = 1e-5;
+
+  Eigen::Matrix3d dRdr = (pinocchio::rpy::rpyToMatrix(r + eps, p, y) - R) / eps;
+  Eigen::Matrix3d dRdp = (pinocchio::rpy::rpyToMatrix(r, p + eps, y) - R) / eps;
+  Eigen::Matrix3d dRdy = (pinocchio::rpy::rpyToMatrix(r, p, y + eps) - R) / eps;
+  Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2];
+
+  Eigen::Vector3d omegaL = jL * rpydot;
+  BOOST_CHECK(Rdot.isApprox(R * pinocchio::skew(omegaL), tol));
+
+  Eigen::Vector3d omegaW = jW * rpydot;
+  BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol));
+}
+
 BOOST_AUTO_TEST_SUITE_END()