5 #ifndef __pinocchio_math_rpy_hpp__ 6 #define __pinocchio_math_rpy_hpp__ 8 #include "pinocchio/math/fwd.hpp" 9 #include "pinocchio/math/comparison-operators.hpp" 10 #include "pinocchio/multibody/fwd.hpp" 23 template<
typename Scalar>
24 Eigen::Matrix<Scalar,3,3>
36 template<
typename Vector3Like>
37 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
53 template<
typename Matrix3Like>
54 Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
55 matrixToRpy(
const Eigen::MatrixBase<Matrix3Like> & R);
72 template<
typename Vector3Like>
73 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
91 template<
typename Vector3Like>
92 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
111 template<
typename Vector3Like0,
typename Vector3Like1>
112 Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
118 #include "pinocchio/math/rpy.hxx" 120 #endif //#ifndef __pinocchio_math_rpy_hpp__ Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobian(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the Jacobian of the Roll-Pitch-Yaw conversion.
ReferenceFrame
List of Reference Frames supported by Pinocchio.
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > matrixToRpy(const Eigen::MatrixBase< Matrix3Like > &R)
Convert from Transformation Matrix to Roll, Pitch, Yaw.
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Main pinocchio namespace.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobianInverse(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion.
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar &r, const Scalar &p, const Scalar &y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
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)
Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion.