Commit 3beb5df2 by Gabriele Buondonno

### [bindings] Expose rpyToJacInv and rpyToJacDerivative

parent e4520147
 ... ... @@ -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, 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, 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" ) ); } } ... ...
 ... ... @@ -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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!