Commit e4520147 by Gabriele Buondonno

### [bindings] Expose rpyToJac

parent 04e7c245
 ... ... @@ -24,6 +24,8 @@ namespace pinocchio return pinocchio::rpy::matrixToRpy(R); } BOOST_PYTHON_FUNCTION_OVERLOADS(rpyToJac_overload, rpy::rpyToJac, 1, 2) Eigen::Matrix3d rotate(const std::string & axis, const double ang) { if(axis.length() != 1U) ... ... @@ -76,6 +78,23 @@ namespace pinocchio bp::args("axis", "ang"), "Rotation matrix corresponding to a rotation about x, y or z" " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis"); bp::def("rpyToJac", &rpyToJac, rpyToJac_overload( bp::args("rpy","reference_frame"), "Compute 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,10 +2,11 @@ import unittest from math import pi import numpy as np import pinocchio as pin from random import random import pinocchio as pin from pinocchio.utils import npToTuple from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, rpyToJac from test_case import PinocchioTestCase as TestCase ... ... @@ -40,5 +41,47 @@ class TestRPY(TestCase): else: self.assertTrue(False) def test_rpyToJac(self): # Check identity at zero rpy = np.zeros(3) j0 = rpyToJac(rpy) self.assertTrue((j0 == np.eye(3)).all()) jL = rpyToJac(rpy, pin.LOCAL) self.assertTrue((jL == np.eye(3)).all()) jW = rpyToJac(rpy, pin.WORLD) self.assertTrue((jW == np.eye(3)).all()) jA = rpyToJac(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 = rpyToJac(rpy) jL = rpyToJac(rpy, pin.LOCAL) jW = rpyToJac(rpy, pin.WORLD) jA = rpyToJac(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-5 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.assertApprox(Rdot, R.dot(pin.skew(omegaL)), tol) omegaW = jW.dot(rpydot) self.assertApprox(Rdot, pin.skew(omegaW).dot(R), tol) if __name__ == '__main__': unittest.main()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!