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;
44 template <
typename OtherScalar>
46 return id == other.id && translation == other.translation;
53 template <
typename _Scalar>
55 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 typedef _Scalar Scalar;
62 FrameRotationTpl(
const FrameIndex&
id,
const Matrix3s& rotation) : id(
id), rotation(rotation) {}
63 friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
64 os <<
" id: " << X.id << std::endl <<
"rotation: " << std::endl << X.rotation << std::endl;
68 template <
typename OtherScalar>
70 return id == other.id && rotation == other.rotation;
77 template <
typename _Scalar>
79 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81 typedef _Scalar Scalar;
82 typedef pinocchio::SE3Tpl<Scalar> SE3;
86 FramePlacementTpl(
const FrameIndex&
id,
const SE3& placement) : id(
id), placement(placement) {}
88 template <
typename OtherScalar>
90 return id == other.id && placement == other.placement;
93 friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
94 os <<
" id: " << X.id << std::endl <<
"placement: " << std::endl << X.placement << std::endl;
99 pinocchio::SE3Tpl<Scalar> placement;
102 template <
typename _Scalar>
104 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
106 typedef _Scalar Scalar;
107 typedef pinocchio::MotionTpl<Scalar> Motion;
109 explicit FrameMotionTpl() : id(0), motion(Motion::Zero()), reference(pinocchio::LOCAL) {}
111 : id(value.id), motion(value.motion), reference(value.reference) {}
112 FrameMotionTpl(
const FrameIndex&
id,
const Motion& motion, pinocchio::ReferenceFrame reference = pinocchio::LOCAL)
113 : id(
id), motion(motion), reference(reference) {}
114 friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
115 os <<
" id: " << X.id << std::endl;
116 os <<
" motion: " << std::endl << X.motion;
117 switch (X.reference) {
118 case pinocchio::WORLD:
119 os <<
"reference: WORLD" << std::endl;
121 case pinocchio::LOCAL:
122 os <<
"reference: LOCAL" << std::endl;
124 case pinocchio::LOCAL_WORLD_ALIGNED:
125 os <<
"reference: LOCAL_WORLD_ALIGNED" << std::endl;
131 template <
typename OtherScalar>
133 return id == other.id && motion == other.motion && reference == other.reference;
137 pinocchio::MotionTpl<Scalar> motion;
138 pinocchio::ReferenceFrame reference;
141 template <
typename _Scalar>
143 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 typedef _Scalar Scalar;
146 typedef pinocchio::ForceTpl<Scalar> Force;
150 FrameForceTpl(
const FrameIndex&
id,
const Force& force) : id(
id), force(force) {}
151 friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
152 os <<
" id: " << X.id << std::endl <<
"force: " << std::endl << X.force << std::endl;
156 template <
typename OtherScalar>
158 return id == other.id && force == other.force;
162 pinocchio::ForceTpl<Scalar> force;
165 template <
typename _Scalar>
167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169 typedef _Scalar Scalar;
176 os <<
" id: " << X.id << std::endl <<
"cone: " << std::endl << X.cone << std::endl;
184 template <
typename _Scalar>
186 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
188 typedef _Scalar Scalar;
193 FrameWrenchConeTpl(
const FrameIndex&
id,
const WrenchCone& cone) : id(
id), cone(cone) {}
195 os <<
"frame: " << X.id << std::endl <<
" cone: " << std::endl << X.cone << std::endl;
203 template <
typename _Scalar>
205 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
207 typedef _Scalar Scalar;
210 typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
215 : id_(value.get_id()), box_(value.get_box()), A_(value.get_A()) {}
216 FrameCoPSupportTpl(
const FrameIndex&
id,
const Vector2s& box) : id_(
id), box_(box) { update_A(); }
217 friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
218 os <<
" id: " << X.get_id() << std::endl <<
"box: " << std::endl << X.get_box() << std::endl;
225 A_ << Scalar(0), Scalar(0), box_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
226 box_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(1),
227 Scalar(0), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
230 void set_id(FrameIndex
id) { id_ = id; }
231 void set_box(
const Vector2s& box) {
236 const FrameIndex& get_id()
const {
return id_; }
237 const Vector2s& get_box()
const {
return box_; }
238 const Matrix46& get_A()
const {
return A_; }
248 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_