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) {}
35 friend std::ostream& operator<<(std::ostream& os, const FrameTranslationTpl<Scalar>& X) {
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) {}
54 friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
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) {}
73 friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
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) {}
92 friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
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) {}
111 friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
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;
129 FrameFrictionConeTpl(
const FrameIndex& frame,
const FrictionCone& oRf) : frame(frame), oRf(oRf) {}
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) {
156 friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
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_