Commit 3beb5df2 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[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<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"
)
);
}
}
......
......@@ -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()
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