Program Listing for File rpy.hxx
↰ Return to documentation for file (include/pinocchio/math/rpy.hxx
)
//
// 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
} // namespace pinocchio
#endif // #ifndef __pinocchio_math_rpy_hxx__