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
}
#endif //#ifndef __pinocchio_math_rpy_hxx__