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;
139 template <
typename _Scalar>
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 typedef _Scalar Scalar;
146 typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
149 explicit FrameCoPSupportTpl() : frame_(0), support_region_(Vector2s::Zero()) { update_A(); }
151 : frame_(value.get_frame()), support_region_(value.get_support_region()), A_(value.get_A()) {}
153 : frame_(frame), support_region_(support_region) {
157 os <<
" frame: " << X.get_frame() << std::endl
158 <<
"foot dimensions: " << std::endl
159 << X.get_support_region() << std::endl;
166 A_ << Scalar(0), Scalar(0), support_region_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
167 support_region_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0),
168 support_region_[1] / Scalar(2), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
169 support_region_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
172 void set_frame(FrameIndex frame) { frame_ = frame; }
173 void set_support_region(
const Vector2s& support_region) {
174 support_region_ = support_region;
178 const FrameIndex& get_frame()
const {
return frame_; }
179 const Vector2s& get_support_region()
const {
return support_region_; }
180 const Matrix46& get_A()
const {
return A_; }
184 Vector2s support_region_;
190 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_