[bindings] Expose rpyToJacInv and rpyToJacDerivative

bindings/python/math/expose-rpy.cpp

@@ -25,6 +25,8 @@ namespace pinocchio
     }
 
     BOOST_PYTHON_FUNCTION_OVERLOADS(rpyToJac_overload, rpy::rpyToJac, 1, 2)
+    BOOST_PYTHON_FUNCTION_OVERLOADS(rpyToJacInv_overload, rpy::rpyToJacInv, 1, 2)
+    BOOST_PYTHON_FUNCTION_OVERLOADS(rpyToJacDerivative_overload, rpy::rpyToJacDerivative, 2, 3)
 
     Eigen::Matrix3d rotate(const std::string & axis, const double ang)
     {
@@ -95,6 +97,40 @@ namespace pinocchio
                     " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
                 )
         );
+
+        bp::def("rpyToJacInv",
+                &rpyToJacInv<Vector3d>,
+                rpyToJacInv_overload(
+                    bp::args("rpy","reference_frame"),
+                    "Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion"
+                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
+                    " and reference frame F (either LOCAL or WORLD),"
+                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
+                    " where omega_F is the angular velocity expressed in frame F"
+                    " and J_F is the Jacobian computed with reference frame F"
+                    "\nParameters:\n"
+                    "\trpy Roll-Pitch-Yaw vector"
+                    "\treference_frame  Reference frame in which the angular velocity is expressed."
+                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
+                )
+        );
+
+        bp::def("rpyToJacDerivative",
+                &rpyToJacDerivative<Vector3d, Vector3d>,
+                rpyToJacDerivative_overload(
+                    bp::args("rpy", "rpydot", "reference_frame"),
+                    "Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion"
+                    " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
+                    " and reference frame F (either LOCAL or WORLD),"
+                    " the Jacobian is such that omega_F = J_F(phi)phidot,"
+                    " where omega_F is the angular velocity expressed in frame F"
+                    " and J_F is the Jacobian computed with reference frame F"
+                    "\nParameters:\n"
+                    "\trpy Roll-Pitch-Yaw vector"
+                    "\treference_frame  Reference frame in which the angular velocity is expressed."
+                    " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
+                )
+        );
       }
       
     }

unittest/python/rpy.py

@@ -2,11 +2,12 @@ import unittest
 from math import pi
 
 import numpy as np
+from numpy.linalg import inv
 from random import random
 
 import pinocchio as pin
 from pinocchio.utils import npToTuple
-from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, rpyToJac
+from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, rpyToJac, rpyToJacInv, rpyToJacDerivative
 
 from test_case import PinocchioTestCase as TestCase
 
@@ -83,5 +84,63 @@ class TestRPY(TestCase):
         omegaW = jW.dot(rpydot)
         self.assertApprox(Rdot, pin.skew(omegaW).dot(R), tol)
 
+    def test_rpyToJacInv(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 = rpyToJac(rpy)
+        j0inv = rpyToJacInv(rpy)
+        self.assertApprox(j0inv, inv(j0))
+
+        jL = rpyToJac(rpy, pin.LOCAL)
+        jLinv = rpyToJacInv(rpy, pin.LOCAL)
+        self.assertApprox(jLinv, inv(jL))
+
+        jW = rpyToJac(rpy, pin.WORLD)
+        jWinv = rpyToJacInv(rpy, pin.WORLD)
+        self.assertApprox(jWinv, inv(jW))
+
+        jA = rpyToJac(rpy, pin.LOCAL_WORLD_ALIGNED)
+        jAinv = rpyToJacInv(rpy, pin.LOCAL_WORLD_ALIGNED)
+        self.assertApprox(jAinv, inv(jA))
+
+    def test_rpyToJacDerivative(self):
+        # Check zero at zero velocity
+        r = random()*2*pi - pi
+        p = random()*pi - pi/2
+        y = random()*2*pi - pi
+        rpy = np.array([r, p, y])
+        rpydot = np.zeros(3)
+        dj0 = rpyToJacDerivative(rpy, rpydot)
+        self.assertTrue((dj0 == np.zeros((3,3))).all())
+        djL = rpyToJacDerivative(rpy, rpydot, pin.LOCAL)
+        self.assertTrue((djL == np.zeros((3,3))).all())
+        djW = rpyToJacDerivative(rpy, rpydot, pin.WORLD)
+        self.assertTrue((djW == np.zeros((3,3))).all())
+        djA = rpyToJacDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED)
+        self.assertTrue((djA == np.zeros((3,3))).all())
+
+        # Check correct identities between different versions
+        rpydot = np.random.rand(3)
+        dj0 = rpyToJacDerivative(rpy, rpydot)
+        djL = rpyToJacDerivative(rpy, rpydot, pin.LOCAL)
+        djW = rpyToJacDerivative(rpy, rpydot, pin.WORLD)
+        djA = rpyToJacDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED)
+        self.assertTrue((dj0 == djL).all())
+        self.assertTrue((djW == djA).all())
+
+        R = rpyToMatrix(rpy)
+        jL = rpyToJac(rpy, pin.LOCAL)
+        jW = rpyToJac(rpy, pin.WORLD)
+        omegaL = jL.dot(rpydot)
+        omegaW = jW.dot(rpydot)
+        self.assertApprox(omegaW, R.dot(omegaL))
+        self.assertApprox(djW, pin.skew(omegaW).dot(R).dot(jL) + R.dot(djL))
+        self.assertApprox(djW, R.dot(pin.skew(omegaL)).dot(jL) + R.dot(djL))
+
 if __name__ == '__main__':
     unittest.main()