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

[rpy] PR comments

parent b0541daf
Pipeline #12433 passed with stage
in 128 minutes and 15 seconds
......@@ -14,9 +14,9 @@ namespace pinocchio
{
namespace bp = boost::python;
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)
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)
{
......@@ -46,7 +46,7 @@ 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");
......@@ -71,9 +71,9 @@ namespace pinocchio
"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::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)"
......@@ -88,9 +88,9 @@ namespace pinocchio
)
);
bp::def("rpyToJacInv",
&rpyToJacInv<Vector3d>,
rpyToJacInv_overload(
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)"
......@@ -105,9 +105,9 @@ namespace pinocchio
)
);
bp::def("rpyToJacDerivative",
&rpyToJacDerivative<Vector3d, Vector3d>,
rpyToJacDerivative_overload(
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)"
......
......@@ -9,8 +9,6 @@
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include <Eigen/Geometry>
namespace pinocchio
{
namespace rpy
......@@ -23,10 +21,10 @@ namespace pinocchio
/// around axis \f$\alpha\f$.
///
template<typename Scalar>
inline Eigen::Matrix<Scalar,3,3>
rpyToMatrix(const Scalar r,
const Scalar p,
const Scalar y);
Eigen::Matrix<Scalar,3,3>
rpyToMatrix(const Scalar& r,
const Scalar& p,
const Scalar& y);
///
/// \brief Convert from Roll, Pitch, Yaw to rotation Matrix
......@@ -36,7 +34,7 @@ namespace pinocchio
/// around axis \f$\alpha\f$.
///
template<typename Vector3Like>
inline 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);
///
......@@ -53,7 +51,7 @@ namespace pinocchio
/// \warning the method assumes \f$R\f$ is a rotation matrix. If it is not, the result is undefined.
///
template<typename Matrix3Like>
inline 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);
///
......@@ -72,8 +70,8 @@ namespace pinocchio
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJac(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
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
......@@ -91,8 +89,8 @@ namespace pinocchio
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJacInv(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf=LOCAL);
///
/// \brief Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion
......@@ -111,8 +109,8 @@ namespace pinocchio
/// \note for the purpose of this function, WORLD and LOCAL_WORLD_ALIGNED are equivalent
///
template<typename Vector3Like0, typename Vector3Like1>
inline Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
rpyToJacDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf=LOCAL);
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
}
......
......@@ -5,6 +5,8 @@
#ifndef __pinocchio_math_rpy_hxx__
#define __pinocchio_math_rpy_hxx__
#include <Eigen/Geometry>
#include "pinocchio/math/sincos.hpp"
namespace pinocchio
......@@ -12,10 +14,10 @@ namespace pinocchio
namespace rpy
{
template<typename Scalar>
inline Eigen::Matrix<Scalar,3,3>
rpyToMatrix(const Scalar r,
const Scalar p,
const Scalar y)
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;
......@@ -27,7 +29,7 @@ namespace pinocchio
template<typename Vector3Like>
inline 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)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
......@@ -36,7 +38,7 @@ namespace pinocchio
template<typename Matrix3Like>
inline 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)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
......@@ -67,11 +69,12 @@ namespace pinocchio
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJac(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
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;
Eigen::Matrix<Scalar,3,3> J;
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);
......@@ -105,11 +108,12 @@ namespace pinocchio
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
rpyToJacInv(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
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;
Eigen::Matrix<Scalar,3,3> J;
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);
......@@ -143,11 +147,12 @@ namespace pinocchio
}
template<typename Vector3Like0, typename Vector3Like1>
inline Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
rpyToJacDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
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;
Eigen::Matrix<Scalar,3,3> J;
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;
......
......@@ -8,7 +8,7 @@ from random import random
from eigenpy import AngleAxis
import pinocchio as pin
from pinocchio.utils import npToTuple
from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, rpyToJac, rpyToJacInv, rpyToJacDerivative
from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, computeRpyJacobian, computeRpyJacobianInverse, computeRpyJacobianTimeDerivative
from test_case import PinocchioTestCase as TestCase
......@@ -123,16 +123,16 @@ class TestRPY(TestCase):
def test_rpyToJac(self):
def test_computeRpyJacobian(self):
# Check identity at zero
rpy = np.zeros(3)
j0 = rpyToJac(rpy)
j0 = computeRpyJacobian(rpy)
self.assertTrue((j0 == np.eye(3)).all())
jL = rpyToJac(rpy, pin.LOCAL)
jL = computeRpyJacobian(rpy, pin.LOCAL)
self.assertTrue((jL == np.eye(3)).all())
jW = rpyToJac(rpy, pin.WORLD)
jW = computeRpyJacobian(rpy, pin.WORLD)
self.assertTrue((jW == np.eye(3)).all())
jA = rpyToJac(rpy, pin.LOCAL_WORLD_ALIGNED)
jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
self.assertTrue((jA == np.eye(3)).all())
# Check correct identities between different versions
......@@ -141,10 +141,10 @@ class TestRPY(TestCase):
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)
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))
......@@ -165,7 +165,7 @@ class TestRPY(TestCase):
omegaW = jW.dot(rpydot)
self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
def test_rpyToJacInv(self):
def test_computeRpyJacobianInverse(self):
# Check correct identities between different versions
r = random()*2*pi - pi
p = random()*pi - pi/2
......@@ -173,50 +173,50 @@ class TestRPY(TestCase):
y = random()*2*pi - pi
rpy = np.array([r, p, y])
j0 = rpyToJac(rpy)
j0inv = rpyToJacInv(rpy)
j0 = computeRpyJacobian(rpy)
j0inv = computeRpyJacobianInverse(rpy)
self.assertApprox(j0inv, inv(j0))
jL = rpyToJac(rpy, pin.LOCAL)
jLinv = rpyToJacInv(rpy, pin.LOCAL)
jL = computeRpyJacobian(rpy, pin.LOCAL)
jLinv = computeRpyJacobianInverse(rpy, pin.LOCAL)
self.assertApprox(jLinv, inv(jL))
jW = rpyToJac(rpy, pin.WORLD)
jWinv = rpyToJacInv(rpy, pin.WORLD)
jW = computeRpyJacobian(rpy, pin.WORLD)
jWinv = computeRpyJacobianInverse(rpy, pin.WORLD)
self.assertApprox(jWinv, inv(jW))
jA = rpyToJac(rpy, pin.LOCAL_WORLD_ALIGNED)
jAinv = rpyToJacInv(rpy, pin.LOCAL_WORLD_ALIGNED)
jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
jAinv = computeRpyJacobianInverse(rpy, pin.LOCAL_WORLD_ALIGNED)
self.assertApprox(jAinv, inv(jA))
def test_rpyToJacDerivative(self):
def test_computeRpyJacobianTimeDerivative(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)
dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
self.assertTrue((dj0 == np.zeros((3,3))).all())
djL = rpyToJacDerivative(rpy, rpydot, pin.LOCAL)
djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
self.assertTrue((djL == np.zeros((3,3))).all())
djW = rpyToJacDerivative(rpy, rpydot, pin.WORLD)
djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
self.assertTrue((djW == np.zeros((3,3))).all())
djA = rpyToJacDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED)
djA = computeRpyJacobianTimeDerivative(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)
dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
djA = computeRpyJacobianTimeDerivative(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)
jL = computeRpyJacobian(rpy, pin.LOCAL)
jW = computeRpyJacobian(rpy, pin.WORLD)
omegaL = jL.dot(rpydot)
omegaW = jW.dot(rpydot)
self.assertApprox(omegaW, R.dot(omegaL))
......
......@@ -111,17 +111,17 @@ BOOST_AUTO_TEST_CASE(test_matrixToRpy)
}
BOOST_AUTO_TEST_CASE(test_rpyToJac)
BOOST_AUTO_TEST_CASE(test_computeRpyJacobian)
{
// Check identity at zero
Eigen::Vector3d rpy(Eigen::Vector3d::Zero());
Eigen::Matrix3d j0 = pinocchio::rpy::rpyToJac(rpy);
Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
BOOST_CHECK(j0.isIdentity());
Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
BOOST_CHECK(jL.isIdentity());
Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
BOOST_CHECK(jW.isIdentity());
Eigen::Matrix3d jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(jA.isIdentity());
// Check correct identities between different versions
......@@ -130,10 +130,10 @@ BOOST_AUTO_TEST_CASE(test_rpyToJac)
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
rpy = Eigen::Vector3d(r, p, y);
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
j0 = pinocchio::rpy::rpyToJac(rpy);
jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
j0 = pinocchio::rpy::computeRpyJacobian(rpy);
jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(j0 == jL);
BOOST_CHECK(jW == jA);
BOOST_CHECK(jW.isApprox(R*jL));
......@@ -175,7 +175,7 @@ BOOST_AUTO_TEST_CASE(test_rpyToJac)
}
BOOST_AUTO_TEST_CASE(test_rpyToJacInv)
BOOST_AUTO_TEST_CASE(test_computeRpyJacobianInverse)
{
// Check correct identities between different versions
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
......@@ -184,25 +184,25 @@ BOOST_AUTO_TEST_CASE(test_rpyToJacInv)
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Vector3d rpy(r, p, y);
Eigen::Matrix3d j0 = pinocchio::rpy::rpyToJac(rpy);
Eigen::Matrix3d j0inv = pinocchio::rpy::rpyToJacInv(rpy);
Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
Eigen::Matrix3d j0inv = pinocchio::rpy::computeRpyJacobianInverse(rpy);
BOOST_CHECK(j0inv.isApprox(j0.inverse()));
Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jLinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jLinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL);
BOOST_CHECK(jLinv.isApprox(jL.inverse()));
Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
Eigen::Matrix3d jWinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::WORLD);
Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
Eigen::Matrix3d jWinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::WORLD);
BOOST_CHECK(jWinv.isApprox(jW.inverse()));
Eigen::Matrix3d jA = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d jAinv = pinocchio::rpy::rpyToJacInv(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d jAinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(jAinv.isApprox(jA.inverse()));
}
BOOST_AUTO_TEST_CASE(test_rpyToJacDerivative)
BOOST_AUTO_TEST_CASE(test_computeRpyJacobianTimeDerivative)
{
// Check zero at zero velocity
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
......@@ -210,27 +210,27 @@ BOOST_AUTO_TEST_CASE(test_rpyToJacDerivative)
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Vector3d rpy(r, p, y);
Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
Eigen::Matrix3d dj0 = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot);
Eigen::Matrix3d dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
BOOST_CHECK(dj0.isZero());
Eigen::Matrix3d djL = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL);
Eigen::Matrix3d djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
BOOST_CHECK(djL.isZero());
Eigen::Matrix3d djW = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::WORLD);
Eigen::Matrix3d djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
BOOST_CHECK(djW.isZero());
Eigen::Matrix3d djA = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix3d djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(djA.isZero());
// Check correct identities between different versions
rpydot = Eigen::Vector3d::Random();
dj0 = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot);
djL = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL);
djW = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::WORLD);
djA = pinocchio::rpy::rpyToJacDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
BOOST_CHECK(dj0 == djL);
BOOST_CHECK(djW == djA);
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
Eigen::Matrix3d jL = pinocchio::rpy::rpyToJac(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jW = pinocchio::rpy::rpyToJac(rpy, pinocchio::WORLD);
Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
Eigen::Vector3d omegaL = jL * rpydot;
Eigen::Vector3d omegaW = jW * rpydot;
BOOST_CHECK(omegaW.isApprox(R*omegaL));
......@@ -243,25 +243,25 @@ BOOST_AUTO_TEST_CASE(test_rpyToJacDerivative)
Eigen::Vector3d rpyEps = rpy;
rpyEps[0] += eps;
Eigen::Matrix3d djLdr = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
Eigen::Matrix3d djLdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[0] = rpy[0];
rpyEps[1] += eps;
Eigen::Matrix3d djLdp = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
Eigen::Matrix3d djLdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[1] = rpy[1];
rpyEps[2] += eps;
Eigen::Matrix3d djLdy = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::LOCAL) - jL) / eps;
Eigen::Matrix3d djLdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
rpyEps[2] = rpy[2];
Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
BOOST_CHECK(djL.isApprox(djLf, tol));
rpyEps[0] += eps;
Eigen::Matrix3d djWdr = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
Eigen::Matrix3d djWdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[0] = rpy[0];
rpyEps[1] += eps;
Eigen::Matrix3d djWdp = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
Eigen::Matrix3d djWdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[1] = rpy[1];
rpyEps[2] += eps;
Eigen::Matrix3d djWdy = (pinocchio::rpy::rpyToJac(rpyEps, pinocchio::WORLD) - jW) / eps;
Eigen::Matrix3d djWdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
rpyEps[2] = rpy[2];
Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
BOOST_CHECK(djW.isApprox(djWf, tol));
......
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