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/multibody/wrench-cone.hpp" 16 #include "crocoddyl/core/mathbase.hpp" 18 #include <pinocchio/multibody/fwd.hpp> 19 #include <pinocchio/spatial/se3.hpp> 20 #include <pinocchio/spatial/motion.hpp> 21 #include <pinocchio/spatial/force.hpp> 25 typedef std::size_t FrameIndex;
27 template <
typename _Scalar>
29 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 typedef _Scalar Scalar;
36 FrameTranslationTpl(
const FrameIndex&
id,
const Vector3s& translation) : id(
id), translation(translation) {}
37 friend std::ostream& operator<<(std::ostream& os, const FrameTranslationTpl<Scalar>& X) {
38 os <<
" id: " << X.id << std::endl
39 <<
"translation: " << std::endl
40 << X.translation.transpose() << std::endl;
48 template <
typename _Scalar>
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 typedef _Scalar Scalar;
57 FrameRotationTpl(
const FrameIndex&
id,
const Matrix3s& rotation) : id(
id), rotation(rotation) {}
58 friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
59 os <<
" id: " << X.id << std::endl <<
"rotation: " << std::endl << X.rotation << std::endl;
67 template <
typename _Scalar>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
71 typedef _Scalar Scalar;
72 typedef pinocchio::SE3Tpl<Scalar> SE3;
76 FramePlacementTpl(
const FrameIndex&
id,
const SE3& placement) : id(
id), placement(placement) {}
77 friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
78 os <<
" id: " << X.id << std::endl <<
"placement: " << std::endl << X.placement << std::endl;
83 pinocchio::SE3Tpl<Scalar> placement;
86 template <
typename _Scalar>
88 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
90 typedef _Scalar Scalar;
91 typedef pinocchio::MotionTpl<Scalar> Motion;
93 explicit FrameMotionTpl() : id(0), motion(Motion::Zero()), reference(pinocchio::LOCAL) {}
95 : id(value.id), motion(value.motion), reference(value.reference) {}
96 FrameMotionTpl(
const FrameIndex&
id,
const Motion& motion, pinocchio::ReferenceFrame reference = pinocchio::LOCAL)
97 : id(
id), motion(motion), reference(reference) {}
98 friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
99 os <<
" id: " << X.id << std::endl;
100 os <<
" motion: " << std::endl << X.motion;
101 switch (X.reference) {
102 case pinocchio::WORLD:
103 os <<
"reference: WORLD" << std::endl;
105 case pinocchio::LOCAL:
106 os <<
"reference: LOCAL" << std::endl;
108 case pinocchio::LOCAL_WORLD_ALIGNED:
109 os <<
"reference: LOCAL_WORLD_ALIGNED" << std::endl;
116 pinocchio::MotionTpl<Scalar> motion;
117 pinocchio::ReferenceFrame reference;
120 template <
typename _Scalar>
122 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 typedef _Scalar Scalar;
125 typedef pinocchio::ForceTpl<Scalar> Force;
129 FrameForceTpl(
const FrameIndex&
id,
const Force& force) : id(
id), force(force) {}
130 friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
131 os <<
" id: " << X.id << std::endl <<
"force: " << std::endl << X.force << std::endl;
136 pinocchio::ForceTpl<Scalar> force;
139 template <
typename _Scalar>
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 typedef _Scalar Scalar;
150 os <<
" id: " << X.id << std::endl <<
"cone: " << std::endl << X.cone << std::endl;
158 template <
typename _Scalar>
160 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
162 typedef _Scalar Scalar;
167 FrameWrenchConeTpl(
const FrameIndex&
id,
const WrenchCone& cone) : id(
id), cone(cone) {}
169 os <<
"frame: " << X.id << std::endl <<
" cone: " << std::endl << X.cone << std::endl;
177 template <
typename _Scalar>
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 typedef _Scalar Scalar;
184 typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
189 : id_(value.get_id()), box_(value.get_box()), A_(value.get_A()) {}
190 FrameCoPSupportTpl(
const FrameIndex&
id,
const Vector2s& box) : id_(
id), box_(box) { update_A(); }
191 friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
192 os <<
" id: " << X.get_id() << std::endl <<
"box: " << std::endl << X.get_box() << std::endl;
199 A_ << Scalar(0), Scalar(0), box_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
200 box_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(1),
201 Scalar(0), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
204 void set_id(FrameIndex
id) { id_ = id; }
205 void set_box(
const Vector2s& box) {
210 const FrameIndex& get_id()
const {
return id_; }
211 const Vector2s& get_box()
const {
return box_; }
212 const Matrix46& get_A()
const {
return A_; }
222 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_