Commit e4520147 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[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<Vector3d>,
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!
Please register or to comment