10 #ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_ 11 #define CROCODDYL_MULTIBODY_FRAMES_HPP_ 13 #include <pinocchio/multibody/fwd.hpp> 14 #include <pinocchio/spatial/se3.hpp> 15 #include <pinocchio/spatial/motion.hpp> 16 #include <pinocchio/spatial/force.hpp> 18 #include "crocoddyl/multibody/fwd.hpp" 19 #include "crocoddyl/core/mathbase.hpp" 20 #include "crocoddyl/multibody/friction-cone.hpp" 21 #include "crocoddyl/multibody/wrench-cone.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;
47 translation = other.translation;
52 template <
typename OtherScalar>
54 return id == other.id && translation == other.translation;
61 template <
typename _Scalar>
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 typedef _Scalar Scalar;
70 FrameRotationTpl(
const FrameIndex&
id,
const Matrix3s& rotation) : id(
id), rotation(rotation) {}
71 friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
72 os <<
" id: " << X.id << std::endl <<
"rotation: " << std::endl << X.rotation << std::endl;
79 rotation = other.rotation;
84 template <
typename OtherScalar>
86 return id == other.id && rotation == other.rotation;
93 template <
typename _Scalar>
95 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
97 typedef _Scalar Scalar;
98 typedef pinocchio::SE3Tpl<Scalar> SE3;
102 FramePlacementTpl(
const FrameIndex&
id,
const SE3& placement) : id(
id), placement(placement) {}
105 if (
this != &other) {
107 placement = other.placement;
112 template <
typename OtherScalar>
114 return id == other.id && placement == other.placement;
117 friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
118 os <<
" id: " << X.id << std::endl <<
"placement: " << std::endl << X.placement << std::endl;
123 pinocchio::SE3Tpl<Scalar> placement;
126 template <
typename _Scalar>
128 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
130 typedef _Scalar Scalar;
131 typedef pinocchio::MotionTpl<Scalar> Motion;
133 explicit FrameMotionTpl() : id(0), motion(Motion::Zero()), reference(pinocchio::LOCAL) {}
135 : id(other.id), motion(other.motion), reference(other.reference) {}
136 FrameMotionTpl(
const FrameIndex&
id,
const Motion& motion, pinocchio::ReferenceFrame reference = pinocchio::LOCAL)
137 : id(
id), motion(motion), reference(reference) {}
138 friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
139 os <<
" id: " << X.id << std::endl;
140 os <<
" motion: " << std::endl << X.motion;
141 switch (X.reference) {
142 case pinocchio::WORLD:
143 os <<
"reference: WORLD" << std::endl;
145 case pinocchio::LOCAL:
146 os <<
"reference: LOCAL" << std::endl;
148 case pinocchio::LOCAL_WORLD_ALIGNED:
149 os <<
"reference: LOCAL_WORLD_ALIGNED" << std::endl;
156 if (
this != &other) {
158 motion = other.motion;
159 reference = other.reference;
164 template <
typename OtherScalar>
166 return id == other.id && motion == other.motion && reference == other.reference;
170 pinocchio::MotionTpl<Scalar> motion;
171 pinocchio::ReferenceFrame reference;
174 template <
typename _Scalar>
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
178 typedef _Scalar Scalar;
179 typedef pinocchio::ForceTpl<Scalar> Force;
183 FrameForceTpl(
const FrameIndex&
id,
const Force& force) : id(
id), force(force) {}
184 friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
185 os <<
" id: " << X.id << std::endl <<
"force: " << std::endl << X.force << std::endl;
190 if (
this != &other) {
197 template <
typename OtherScalar>
199 return id == other.id && force == other.force;
203 pinocchio::ForceTpl<Scalar> force;
206 template <
typename _Scalar>
208 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
210 typedef _Scalar Scalar;
217 os <<
" id: " << X.id << std::endl <<
"cone: " << std::endl << X.cone << std::endl;
222 if (
this != &other) {
233 template <
typename _Scalar>
235 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
237 typedef _Scalar Scalar;
242 FrameWrenchConeTpl(
const FrameIndex&
id,
const WrenchCone& cone) : id(
id), cone(cone) {}
244 os <<
"frame: " << X.id << std::endl <<
" cone: " << std::endl << X.cone << std::endl;
249 if (
this != &other) {
260 template <
typename _Scalar>
262 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
264 typedef _Scalar Scalar;
267 typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
272 : id_(other.get_id()), box_(other.get_box()), A_(other.get_A()) {}
273 FrameCoPSupportTpl(
const FrameIndex&
id,
const Vector2s& box) : id_(
id), box_(box) { update_A(); }
274 friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
275 os <<
" id: " << X.get_id() << std::endl <<
"box: " << std::endl << X.get_box() << std::endl;
280 if (
this != &other) {
281 id_ = other.get_id();
282 box_ = other.get_box();
291 A_ << Scalar(0), Scalar(0), box_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
292 box_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(1),
293 Scalar(0), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
296 void set_id(FrameIndex
id) { id_ = id; }
297 void set_box(
const Vector2s& box) {
302 const FrameIndex& get_id()
const {
return id_; }
303 const Vector2s& get_box()
const {
return box_; }
304 const Matrix46& get_A()
const {
return A_; }
314 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_