10 #ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_
11 #define CROCODDYL_MULTIBODY_FRAMES_HPP_
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/friction-cone.hpp"
15 #include "crocoddyl/core/mathbase.hpp"
17 #include <pinocchio/spatial/se3.hpp>
18 #include <pinocchio/spatial/motion.hpp>
19 #include <pinocchio/spatial/force.hpp>
23 typedef std::size_t FrameIndex;
25 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 typedef _Scalar Scalar;
34 FrameTranslationTpl(
const FrameIndex& frame,
const Vector3s& oxf) : frame(frame), oxf(oxf) {}
36 os <<
" frame: " << X.frame << std::endl <<
"translation: " << std::endl << X.oxf.transpose() << std::endl;
44 template <
typename _Scalar>
46 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 typedef _Scalar Scalar;
53 FrameRotationTpl(
const FrameIndex& frame,
const Matrix3s& oRf) : frame(frame), oRf(oRf) {}
55 os <<
" frame: " << X.frame << std::endl <<
"rotation: " << std::endl << X.oRf << std::endl;
63 template <
typename _Scalar>
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 typedef _Scalar Scalar;
68 typedef pinocchio::SE3Tpl<Scalar> SE3;
72 FramePlacementTpl(
const FrameIndex& frame,
const SE3& oMf) : frame(frame), oMf(oMf) {}
74 os <<
" frame: " << X.frame << std::endl <<
"placement: " << std::endl << X.oMf << std::endl;
79 pinocchio::SE3Tpl<Scalar> oMf;
82 template <
typename _Scalar>
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86 typedef _Scalar Scalar;
87 typedef pinocchio::MotionTpl<Scalar> Motion;
91 FrameMotionTpl(
const FrameIndex& frame,
const Motion& oMf) : frame(frame), oMf(oMf) {}
93 os <<
" frame: " << X.frame << std::endl <<
"motion: " << std::endl << X.oMf << std::endl;
98 pinocchio::MotionTpl<Scalar> oMf;
101 template <
typename _Scalar>
103 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
105 typedef _Scalar Scalar;
106 typedef pinocchio::ForceTpl<Scalar> Force;
110 FrameForceTpl(
const FrameIndex& frame,
const Force& oFf) : frame(frame), oFf(oFf) {}
112 os <<
"frame: " << X.frame << std::endl <<
"force: " << std::endl << X.oFf << std::endl;
117 pinocchio::ForceTpl<Scalar> oFf;
120 template <
typename _Scalar>
122 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 typedef _Scalar Scalar;
131 os <<
"frame: " << X.frame << std::endl <<
" cone: " << std::endl << X.oRf << std::endl;
141 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_