Unverified Commit b2ac6e40 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1350 from gabrielebndn/topic/rpy

Improve support for rpy
parents 706ed21e 836d732a
Pipeline #12453 passed with stage
in 130 minutes and 54 seconds
......@@ -14,15 +14,9 @@ namespace pinocchio
{
namespace bp = boost::python;
Eigen::Matrix3d rpyToMatrix_proxy(const Eigen::Vector3d & rpy)
{
return pinocchio::rpy::rpyToMatrix(rpy);
}
Eigen::Vector3d matrixToRpy_proxy(const Eigen::Matrix3d & R)
{
return pinocchio::rpy::matrixToRpy(R);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2)
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2)
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3)
Eigen::Matrix3d rotate(const std::string & axis, const double ang)
{
......@@ -52,19 +46,19 @@ namespace pinocchio
bp::scope current_scope = getOrCreatePythonNamespace("rpy");
bp::def("rpyToMatrix",
static_cast<Matrix3d (*)(const double, const double, const double)>(&rpyToMatrix),
static_cast<Matrix3d (*)(const double&, const double&, const double&)>(&rpyToMatrix),
bp::args("roll", "pitch", "yaw"),
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
" where R_a(theta) denotes the rotation of theta radians axis a");
bp::def("rpyToMatrix",
&rpyToMatrix_proxy,
static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix),
bp::arg("rpy"),
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
" where R_a(theta) denotes the rotation of theta radians axis a");
bp::def("matrixToRpy",
&matrixToRpy_proxy,
&matrixToRpy<Matrix3d>,
bp::arg("R"),
"Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
" where R_a(theta) denotes the rotation of theta radians axis a."
......@@ -76,6 +70,57 @@ 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("computeRpyJacobian",
&computeRpyJacobian<Vector3d>,
computeRpyJacobian_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"
)
);
bp::def("computeRpyJacobianInverse",
&computeRpyJacobianInverse<Vector3d>,
computeRpyJacobianInverse_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("computeRpyJacobianTimeDerivative",
&computeRpyJacobianTimeDerivative<Vector3d, Vector3d>,
computeRpyJacobianTimeDerivative_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"
)
);
}
}
......
......@@ -7,8 +7,7 @@
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include <Eigen/Geometry>
#include "pinocchio/multibody/fwd.hpp"
namespace pinocchio
{
......@@ -22,17 +21,10 @@ namespace pinocchio
/// around axis \f$\alpha\f$.
///
template<typename Scalar>
Eigen::Matrix<Scalar,3,3> rpyToMatrix(const Scalar r,
const Scalar p,
const Scalar y)
{
typedef Eigen::AngleAxis<Scalar> AngleAxis;
typedef Eigen::Matrix<Scalar,3,1> Vector3s;
return (AngleAxis(y, Vector3s::UnitZ())
* AngleAxis(p, Vector3s::UnitY())
* AngleAxis(r, Vector3s::UnitX())
).toRotationMatrix();
}
Eigen::Matrix<Scalar,3,3>
rpyToMatrix(const Scalar& r,
const Scalar& p,
const Scalar& y);
///
/// \brief Convert from Roll, Pitch, Yaw to rotation Matrix
......@@ -43,11 +35,7 @@ namespace pinocchio
///
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
}
rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy);
///
/// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw
......@@ -64,33 +52,69 @@ namespace pinocchio
///
template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
assert(R.isUnitary() && "R is not a unitary matrix");
matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R);
typedef typename Matrix3Like::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
static const Scalar pi = PI<Scalar>();
ReturnType res = R.eulerAngles(2,1,0).reverse();
if(res[1] < -pi/2)
res[1] += 2*pi;
///
/// \brief Compute the Jacobian of the Roll-Pitch-Yaw conversion
///
/// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
/// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
/// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
/// and \f$ J_F \f$ is the Jacobian computed with reference frame F
///
/// \param[in] rpy Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The Jacobian of the Roll-Pitch-Yaw conversion in the appropriate frame
///
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
if(res[1] > pi/2)
{
res[1] = pi - res[1];
if(res[0] < Scalar(0))
res[0] += pi;
else
res[0] -= pi;
// res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
res[2] -= pi;
}
///
/// \brief Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion
///
/// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
/// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
/// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
/// and \f$ J_F \f$ is the Jacobian computed with reference frame F
///
/// \param[in] rpy Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The inverse of the Jacobian of the Roll-Pitch-Yaw conversion in the appropriate frame
///
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
return res;
}
///
/// \brief Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion
///
/// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
/// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
/// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
/// and \f$ J_F \f$ is the Jacobian computed with reference frame F
///
/// \param[in] rpy Roll-Pitch-Yaw vector
/// \param[in] rpydot Time derivative of the Roll-Pitch-Yaw vector
/// \param[in] rf Reference frame in which the angular velocity is expressed
///
/// \return The time derivative of the Jacobian of the Roll-Pitch-Yaw conversion in the appropriate frame
///
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like0, typename Vector3Like1>
Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
computeRpyJacobianTimeDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf=LOCAL);
} // namespace rpy
}
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/math/rpy.hxx"
#endif //#ifndef __pinocchio_math_rpy_hpp__
//
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_math_rpy_hxx__
#define __pinocchio_math_rpy_hxx__
#include <Eigen/Geometry>
#include "pinocchio/math/sincos.hpp"
namespace pinocchio
{
namespace rpy
{
template<typename Scalar>
Eigen::Matrix<Scalar,3,3>
rpyToMatrix(const Scalar& r,
const Scalar& p,
const Scalar& y)
{
typedef Eigen::AngleAxis<Scalar> AngleAxis;
typedef Eigen::Matrix<Scalar,3,1> Vector3s;
return (AngleAxis(y, Vector3s::UnitZ())
* AngleAxis(p, Vector3s::UnitY())
* AngleAxis(r, Vector3s::UnitX())
).toRotationMatrix();
}
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
}
template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
assert(R.isUnitary() && "R is not a unitary matrix");
typedef typename Matrix3Like::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
static const Scalar pi = PI<Scalar>();
ReturnType res = R.eulerAngles(2,1,0).reverse();
if(res[1] < -pi/2)
res[1] += 2*pi;
if(res[1] > pi/2)
{
res[1] = pi - res[1];
if(res[0] < Scalar(0))
res[0] += pi;
else
res[0] -= pi;
// res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
res[2] -= pi;
}
return res;
}
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
{
typedef typename Vector3Like::Scalar Scalar;
typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
ReturnType J;
const Scalar p = rpy[1];
Scalar sp, cp;
SINCOS(p, &sp, &cp);
switch (rf)
{
case LOCAL:
{
const Scalar r = rpy[0];
Scalar sr, cr; SINCOS(r, &sr, &cr);
J << Scalar(1.0), Scalar(0.0), -sp,
Scalar(0.0), cr, sr*cp,
Scalar(0.0), -sr, cr*cp;
return J;
}
case WORLD:
case LOCAL_WORLD_ALIGNED:
{
const Scalar y = rpy[2];
Scalar sy, cy; SINCOS(y, &sy, &cy);
J << cp*cy, -sy, Scalar(0.0),
cp*sy, cy, Scalar(0.0),
-sp, Scalar(0.0), Scalar(1.0);
return J;
}
default:
{
throw std::invalid_argument("Bad reference frame.");
}
}
}
template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
{
typedef typename Vector3Like::Scalar Scalar;
typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
ReturnType J;
const Scalar p = rpy[1];
Scalar sp, cp;
SINCOS(p, &sp, &cp);
Scalar tp = sp/cp;
switch (rf)
{
case LOCAL:
{
const Scalar r = rpy[0];
Scalar sr, cr; SINCOS(r, &sr, &cr);
J << Scalar(1.0), sr*tp, cr*tp,
Scalar(0.0), cr, -sr,
Scalar(0.0), sr/cp, cr/cp;
return J;
}
case WORLD:
case LOCAL_WORLD_ALIGNED:
{
const Scalar y = rpy[2];
Scalar sy, cy; SINCOS(y, &sy, &cy);
J << cy/cp, sy/cp, Scalar(0.0),
-sy, cy, Scalar(0.0),
cy*tp, sy*tp, Scalar(1.0);
return J;
}
default:
{
throw std::invalid_argument("Bad reference frame.");
}
}
}
template<typename Vector3Like0, typename Vector3Like1>
Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
computeRpyJacobianTimeDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
{
typedef typename Vector3Like0::Scalar Scalar;
typedef Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options> ReturnType;
ReturnType J;
const Scalar p = rpy[1];
const Scalar dp = rpydot[1];
Scalar sp, cp;
SINCOS(p, &sp, &cp);
switch (rf)
{
case LOCAL:
{
const Scalar r = rpy[0];
const Scalar dr = rpydot[0];
Scalar sr, cr; SINCOS(r, &sr, &cr);
J << Scalar(0.0), Scalar(0.0), -cp*dp,
Scalar(0.0), -sr*dr, cr*cp*dr - sr*sp*dp,
Scalar(0.0), -cr*dr, -sr*cp*dr - cr*sp*dp;
return J;
}
case WORLD:
case LOCAL_WORLD_ALIGNED:
{
const Scalar y = rpy[2];
const Scalar dy = rpydot[2];
Scalar sy, cy; SINCOS(y, &sy, &cy);
J << -sp*cy*dp - cp*sy*dy, -cy*dy, Scalar(0.0),
cp*cy*dy - sp*sy*dp, -sy*dy, Scalar(0.0),
-cp*dp, Scalar(0.0), Scalar(0.0);
return J;
}
default:
{
throw std::invalid_argument("Bad reference frame.");
}
}
}
} // namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__
......@@ -2,10 +2,13 @@ import unittest
from math import pi
import numpy as np
import pinocchio as pin
from numpy.linalg import inv
from random import random
from eigenpy import AngleAxis
import pinocchio as pin
from pinocchio.utils import npToTuple
from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate
from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, computeRpyJacobian, computeRpyJacobianInverse, computeRpyJacobianTimeDerivative
from test_case import PinocchioTestCase as TestCase
......@@ -40,5 +43,185 @@ class TestRPY(TestCase):
else:
self.assertTrue(False)
def test_rpyToMatrix(self):
r = random()*2*pi - pi
p = random()*pi - pi/2
y = random()*2*pi - pi
R = rpyToMatrix(r, p, y)
Raa = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(AngleAxis(p, np.array([0., 1., 0.])).matrix()) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
self.assertApprox(R, Raa)
v = np.array([r, p, y])
Rv = rpyToMatrix(v)
self.assertApprox(Rv, Raa)
self.assertApprox(Rv, R)
def test_matrixToRpy(self):
n = 100
for _ in range(n):
quat = pin.Quaternion(np.random.rand(4,1)).normalized()
R = quat.toRotationMatrix()
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
n2 = 100
# Test singular case theta = pi/2
Rp = np.array([[ 0.0, 0.0, 1.0],
[ 0.0, 1.0, 0.0],
[-1.0, 0.0, 0.0]])
for _ in range(n2):
r = random()*2*pi - pi
y = random()*2*pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
# Test singular case theta = -pi/2
Rp = np.array([[0.0, 0.0, -1.0],
[0.0, 1.0, 0.0],
[1.0, 0.0, 0.0]])
for _ in range(n2):
r = random()*2*pi - pi
y = random()*2*pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
def test_computeRpyJacobian(self):
# Check identity at zero
rpy = np.zeros(3)
j0 = computeRpyJacobian(rpy)
self.assertTrue((j0 == np.eye(3)).all())
jL = computeRpyJacobian(rpy, pin.LOCAL)
self.assertTrue((jL == np.eye(3)).all())
jW = computeRpyJacobian(rpy, pin.WORLD)
self.assertTrue((jW == np.eye(3)).all())
jA = computeRpyJacobian(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 = computeRpyJacobian(rpy)
jL = computeRpyJacobian(rpy, pin.LOCAL)
jW = computeRpyJacobian(rpy, pin.WORLD)
jA = computeRpyJacobian(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-4
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.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol))
omegaW = jW.dot(rpydot)
self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
def test_computeRpyJacobianInverse(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 = computeRpyJacobian(rpy)
j0inv = computeRpyJacobianInverse(rpy)
self.assertApprox(j0inv, inv(j0))
jL = computeRpyJacobian(rpy, pin.LOCAL)
jLinv = computeRpyJacobianInverse(rpy, pin.LOCAL)
self.assertApprox(jLinv, inv(jL))
jW = computeRpyJacobian(rpy, pin.WORLD)
jWinv = computeRpyJacobianInverse(rpy, pin.WORLD)
self.assertApprox(jWinv, inv(jW))
jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
jAinv = computeRpyJacobianInverse(rpy, pin.LOCAL_WORLD_ALIGNED)
self.assertApprox(jAinv, inv(jA))
def test_computeRpyJacobianTimeDerivative(self):
# Check zero at zero velocity
r = random()*2*pi - pi
p = random()*pi - pi/2
y = random()*2*pi - pi