Commit c520069a by Gabriele Buondonno

### [rpy] Split hpp from hxx

parent 4dcea71b
 ... ... @@ -24,15 +24,7 @@ namespace pinocchio template Eigen::Matrix rpyToMatrix(const Scalar r, const Scalar p, const Scalar y) { typedef Eigen::AngleAxis AngleAxis; typedef Eigen::Matrix Vector3s; return (AngleAxis(y, Vector3s::UnitZ()) * AngleAxis(p, Vector3s::UnitY()) * AngleAxis(r, Vector3s::UnitX()) ).toRotationMatrix(); } const Scalar y); /// /// \brief Convert from Roll, Pitch, Yaw to rotation Matrix ... ... @@ -43,11 +35,7 @@ namespace pinocchio /// template Eigen::Matrix rpyToMatrix(const Eigen::MatrixBase & rpy) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1); return rpyToMatrix(rpy[0], rpy[1], rpy[2]); } rpyToMatrix(const Eigen::MatrixBase & rpy); /// /// \brief Convert from Transformation Matrix to Roll, Pitch, Yaw ... ... @@ -64,33 +52,11 @@ namespace pinocchio /// template Eigen::Matrix matrixToRpy(const Eigen::MatrixBase & 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 ReturnType; static const Scalar pi = PI(); 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; } matrixToRpy(const Eigen::MatrixBase & R); } // namespace rpy } /* --- Details -------------------------------------------------------------------- */ #include "pinocchio/math/rpy.hxx" #endif //#ifndef __pinocchio_math_rpy_hpp__
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!