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

Merge pull request #1158 from jcarpent/devel

Simplify RPY code
parents 600460ac 457d8f13
//
// Copyright (c) 2016-2019 CNRS, INRIA
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_math_rpy_hpp__
......@@ -7,8 +7,6 @@
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/math/sincos.hpp"
#include <boost/type_traits.hpp>
#include <Eigen/Geometry>
......@@ -24,7 +22,9 @@ 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)
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;
......@@ -45,8 +45,7 @@ namespace pinocchio
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);
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
}
......@@ -58,7 +57,8 @@ namespace pinocchio
/// where \f$R_{\alpha}(\theta)\f$ denotes the rotation of \f$\theta\f$ degrees
/// around axis \f$\alpha\f$.
/// The angles are guaranteed to be in the ranges \f$r\in[-\pi,\pi]\f$
/// \f$p\in[-\frac{\pi}{2},\frac{\pi}{2}]\f$ \f$y\in[-\pi,\pi]\f$.
/// \f$p\in[-\frac{\pi}{2},\frac{\pi}{2}]\f$ \f$y\in[-\pi,\pi]\f$,
/// unlike Eigen's eulerAngles() function
///
/// \warning the method assumes \f$R\f$ is a rotation matrix. If it is not, the result is undefined.
///
......@@ -66,22 +66,29 @@ namespace pinocchio
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);
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> ResultType;
ResultType res;
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();
Scalar m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
Scalar p = atan2(-R(2, 0), m);
Scalar r, y;
if (fabs(fabs(p) - M_PI / 2.) < 0.001) {
r = 0;
y = -atan2(R(0, 1), R(1, 1));
} else {
y = atan2(R(1, 0), R(0, 0));
r = atan2(R(2, 1), R(2, 2));
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;
}
res << r, p, y;
return res;
}
} // namespace rpy
......
......@@ -304,6 +304,8 @@ namespace pinocchio
typename Model::JointIndex idx,chest,ffidx;
static const Scalar pi = PI<Scalar>();
SE3 Marm(SE3::Matrix3::Identity(),SE3::Vector3::UnitZ());
SE3 I4 = SE3::Identity();
Inertia Ijoint(.1,Inertia::Vector3::Zero(),Inertia::Matrix3::Identity()*.01);
......@@ -331,16 +333,16 @@ namespace pinocchio
/* --- Lower limbs --- */
details::addManipulator(model,ffidx,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
SE3(details::rotate(pi,SE3::Vector3::UnitX()),
typename SE3::Vector3(0,-0.2,-.1)),"rleg_");
details::addManipulator(model,ffidx,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
SE3(details::rotate(pi,SE3::Vector3::UnitX()),
typename SE3::Vector3(0, 0.2,-.1)),"lleg_");
model.jointPlacements[7 ].rotation()
= details::rotate(M_PI/2,SE3::Vector3::UnitY()); // rotate right foot
= details::rotate(pi/2,SE3::Vector3::UnitY()); // rotate right foot
model.jointPlacements[13].rotation()
= details::rotate(M_PI/2,SE3::Vector3::UnitY()); // rotate left foot
= details::rotate(pi/2,SE3::Vector3::UnitY()); // rotate left foot
/* --- Chest --- */
idx = model.addJoint(ffidx,typename JC::JointModelRX(),I4 ,"chest1_joint",
......@@ -373,10 +375,10 @@ namespace pinocchio
/* --- Upper Limbs --- */
details::addManipulator(model,chest,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
SE3(details::rotate(pi,SE3::Vector3::UnitX()),
typename SE3::Vector3(0,-0.3, 1.)),"rarm_");
details::addManipulator(model,chest,
SE3(details::rotate(M_PI,SE3::Vector3::UnitX()),
SE3(details::rotate(pi,SE3::Vector3::UnitX()),
typename SE3::Vector3(0, 0.3, 1.)),"larm_");
}
......
//
// Copyright (c) 2019 INRIA
// Copyright (c) 2019-2020 INRIA
//
#include <pinocchio/math/rpy.hpp>
#include <pinocchio/math/quaternion.hpp>
#include <boost/variant.hpp> // to avoid C99 warnings
......@@ -13,41 +14,95 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
Eigen::Matrix3d R_Eig = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
).toRotationMatrix();
BOOST_CHECK(R.isApprox(R_Eig));
Eigen::Vector3d v;
v << r, p, y;
Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rv.isApprox(R_Eig));
const int n = 1e5;
for(int k = 0; k < n ; ++k)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
).toRotationMatrix();
BOOST_CHECK(R.isApprox(Raa));
Eigen::Vector3d v;
v << r, p, y;
Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rv.isApprox(Raa));
BOOST_CHECK(Rv.isApprox(R));
}
}
BOOST_AUTO_TEST_CASE(test_matrixToRpy)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
BOOST_CHECK_CLOSE(r,v[0],1e-12);
BOOST_CHECK_CLOSE(p,v[1],1e-12);
BOOST_CHECK_CLOSE(y,v[2],1e-12);
const int n = 1e6;
for(int k = 0; k < n ; ++k)
{
Eigen::Quaterniond quat;
pinocchio::quaternion::uniformRandom(quat);
const Eigen::Matrix3d R = quat.toRotationMatrix();
const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rprime.isApprox(R));
BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
}
const int n2 = 1e3;
// Test singular case theta = pi/2
for(int k = 0; k < n2 ; ++k)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d Rp;
Rp << 0.0, 0.0, 1.0,
0.0, 1.0, 0.0,
-1.0, 0.0, 0.0;
const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
* Rp
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rprime.isApprox(R));
BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
}
// Test singular case theta = -pi/2
for(int k = 0; k < n2 ; ++k)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d Rp;
Rp << 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0;
const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
* Rp
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rprime.isApprox(R));
BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
}
}
BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
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