Skip to content

GitLab

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
Hide whitespace changes
Inline Side-by-side
bindings/python/math/expose-rpy.cpp
View file @ b2ac6e40
...@@ -14,15 +14,9 @@ namespace pinocchio ...@@ -14,15 +14,9 @@ namespace pinocchio
{ {
namespace bp = boost::python; namespace bp = boost::python;
Eigen::Matrix3d rpyToMatrix_proxy(const Eigen::Vector3d & rpy) BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2)
{ BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2)
return pinocchio::rpy::rpyToMatrix(rpy); BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3)
}
Eigen::Vector3d matrixToRpy_proxy(const Eigen::Matrix3d & R)
{
return pinocchio::rpy::matrixToRpy(R);
}
Eigen::Matrix3d rotate(const std::string & axis, const double ang) Eigen::Matrix3d rotate(const std::string & axis, const double ang)
{ {
...@@ -52,19 +46,19 @@ namespace pinocchio ...@@ -52,19 +46,19 @@ namespace pinocchio
bp::scope current_scope = getOrCreatePythonNamespace("rpy"); bp::scope current_scope = getOrCreatePythonNamespace("rpy");
bp::def("rpyToMatrix", 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"), bp::args("roll", "pitch", "yaw"),
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," "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"); " where R_a(theta) denotes the rotation of theta radians axis a");
bp::def("rpyToMatrix", bp::def("rpyToMatrix",
&rpyToMatrix_proxy, static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix),
bp::arg("rpy"), bp::arg("rpy"),
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," "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"); " where R_a(theta) denotes the rotation of theta radians axis a");
bp::def("matrixToRpy", bp::def("matrixToRpy",
&matrixToRpy_proxy, &matrixToRpy<Matrix3d>,
bp::arg("R"), 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)," "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." " where R_a(theta) denotes the rotation of theta radians axis a."
...@@ -76,6 +70,57 @@ namespace pinocchio ...@@ -76,6 +70,57 @@ namespace pinocchio
bp::args("axis", "ang"), bp::args("axis", "ang"),
"Rotation matrix corresponding to a rotation about x, y or z" "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"); " 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"
)
);
} }
} }
......
src/math/rpy.hpp
View file @ b2ac6e40
...@@ -7,8 +7,7 @@ ...@@ -7,8 +7,7 @@
#include "pinocchio/math/fwd.hpp" #include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp" #include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include <Eigen/Geometry>
namespace pinocchio namespace pinocchio
{ {
...@@ -22,17 +21,10 @@ namespace pinocchio ...@@ -22,17 +21,10 @@ namespace pinocchio
/// around axis \f$\alpha\f$. /// around axis \f$\alpha\f$.
/// ///
template<typename Scalar> template<typename Scalar>
Eigen::Matrix<Scalar,3,3> rpyToMatrix(const Scalar r, Eigen::Matrix<Scalar,3,3>
const Scalar p, rpyToMatrix(const Scalar& r,
const Scalar y) 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();
}
/// ///
/// \brief Convert from Roll, Pitch, Yaw to rotation Matrix /// \brief Convert from Roll, Pitch, Yaw to rotation Matrix
...@@ -43,11 +35,7 @@ namespace pinocchio ...@@ -43,11 +35,7 @@ namespace pinocchio
/// ///
template<typename Vector3Like> template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy) rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy);
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
}
/// ///
/// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw
...@@ -64,33 +52,69 @@ namespace pinocchio ...@@ -64,33 +52,69 @@ namespace pinocchio
/// ///
template<typename Matrix3Like> template<typename Matrix3Like>
Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R) 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) ///
{ /// \brief Compute the Jacobian of the Roll-Pitch-Yaw conversion
res[1] = pi - res[1]; ///
if(res[0] < Scalar(0)) /// Given \f$\phi = (r, p, y)\f$ and reference frame F (either LOCAL or WORLD),
res[0] += pi; /// the Jacobian is such that \f$ {}^F\omega = J_F(\phi)\dot{\phi} \f$,
else /// where \f$ {}^F\omega \f$ is the angular velocity expressed in frame F
res[0] -= pi; /// and \f$ J_F \f$ is the Jacobian computed with reference frame F
// res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign ///
res[2] -= pi; /// \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);
///
/// \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 } // namespace rpy
} }
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/math/rpy.hxx"
#endif //#ifndef __pinocchio_math_rpy_hpp__ #endif //#ifndef __pinocchio_math_rpy_hpp__
src/math/rpy.hxx 0 → 100644
View file @ b2ac6e40
//
// 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__
unittest/python/rpy.py
View file @ b2ac6e40
...@@ -2,10 +2,13 @@ import unittest ...@@ -2,10 +2,13 @@ import unittest
from math import pi from math import pi
import numpy as np 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.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 from test_case import PinocchioTestCase as TestCase
...@@ -40,5 +43,185 @@ class TestRPY(TestCase): ...@@ -40,5 +43,185 @@ class TestRPY(TestCase):
else: else:
self.assertTrue(False) 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