crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frames.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_FRAMES_HPP_
11 #define CROCODDYL_MULTIBODY_FRAMES_HPP_
12 
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"
17 
18 #include <pinocchio/multibody/fwd.hpp>
19 #include <pinocchio/spatial/se3.hpp>
20 #include <pinocchio/spatial/motion.hpp>
21 #include <pinocchio/spatial/force.hpp>
22 
23 namespace crocoddyl {
24 
25 typedef std::size_t FrameIndex;
26 
27 template <typename _Scalar>
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
31  typedef _Scalar Scalar;
32  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
33 
34  explicit FrameTranslationTpl() : id(0), translation(Vector3s::Zero()) {}
35  FrameTranslationTpl(const FrameTranslationTpl<Scalar>& value) : id(value.id), translation(value.translation) {}
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;
41  return os;
42  }
43 
44  FrameIndex id;
45  Vector3s translation;
46 };
47 
48 template <typename _Scalar>
50  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 
52  typedef _Scalar Scalar;
53  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
54 
55  explicit FrameRotationTpl() : id(0), rotation(Matrix3s::Identity()) {}
56  FrameRotationTpl(const FrameRotationTpl<Scalar>& value) : id(value.id), rotation(value.rotation) {}
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;
60  return os;
61  }
62 
63  FrameIndex id;
64  Matrix3s rotation;
65 };
66 
67 template <typename _Scalar>
69  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 
71  typedef _Scalar Scalar;
72  typedef pinocchio::SE3Tpl<Scalar> SE3;
73 
74  explicit FramePlacementTpl() : id(0), placement(SE3::Identity()) {}
75  FramePlacementTpl(const FramePlacementTpl<Scalar>& value) : id(value.id), placement(value.placement) {}
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;
79  return os;
80  }
81 
82  FrameIndex id;
83  pinocchio::SE3Tpl<Scalar> placement;
84 };
85 
86 template <typename _Scalar>
88  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 
90  typedef _Scalar Scalar;
91  typedef pinocchio::MotionTpl<Scalar> Motion;
92 
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;
104  break;
105  case pinocchio::LOCAL:
106  os << "reference: LOCAL" << std::endl;
107  break;
108  case pinocchio::LOCAL_WORLD_ALIGNED:
109  os << "reference: LOCAL_WORLD_ALIGNED" << std::endl;
110  break;
111  }
112  return os;
113  }
114 
115  FrameIndex id;
116  pinocchio::MotionTpl<Scalar> motion;
117  pinocchio::ReferenceFrame reference;
118 };
119 
120 template <typename _Scalar>
122  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
123 
124  typedef _Scalar Scalar;
125  typedef pinocchio::ForceTpl<Scalar> Force;
126 
127  explicit FrameForceTpl() : id(0), force(Force::Zero()) {}
128  FrameForceTpl(const FrameForceTpl<Scalar>& value) : id(value.id), force(value.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;
132  return os;
133  }
134 
135  FrameIndex id;
136  pinocchio::ForceTpl<Scalar> force;
137 };
138 
139 template <typename _Scalar>
141  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142 
143  typedef _Scalar Scalar;
145 
146  explicit FrameFrictionConeTpl() : id(0), cone(FrictionCone()) {}
147  FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& value) : id(value.id), cone(value.cone) {}
148  FrameFrictionConeTpl(const FrameIndex& id, const FrictionCone& cone) : id(id), cone(cone) {}
149  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
150  os << " id: " << X.id << std::endl << "cone: " << std::endl << X.cone << std::endl;
151  return os;
152  }
153 
154  FrameIndex id;
155  FrictionCone cone;
156 };
157 
158 template <typename _Scalar>
160  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
161 
162  typedef _Scalar Scalar;
164 
165  explicit FrameWrenchConeTpl() : id(0), cone(WrenchCone()) {}
166  FrameWrenchConeTpl(const FrameWrenchConeTpl<Scalar>& value) : id(value.id), cone(value.cone) {}
167  FrameWrenchConeTpl(const FrameIndex& id, const WrenchCone& cone) : id(id), cone(cone) {}
168  friend std::ostream& operator<<(std::ostream& os, const FrameWrenchConeTpl& X) {
169  os << "frame: " << X.id << std::endl << " cone: " << std::endl << X.cone << std::endl;
170  return os;
171  }
172 
173  FrameIndex id;
174  WrenchCone cone;
175 };
176 
177 template <typename _Scalar>
179  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  typedef _Scalar Scalar;
182  typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
183  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
184  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
185 
186  public:
187  explicit FrameCoPSupportTpl() : id_(0), box_(Vector2s::Zero()) { update_A(); }
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;
193  return os;
194  }
195 
196  // Define the inequality matrix A to implement A * f >= 0. Compare eq.(18-19) in
197  // https://hal.archives-ouvertes.fr/hal-02108449/document
198  void update_A() {
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);
202  }
203 
204  void set_id(FrameIndex id) { id_ = id; }
205  void set_box(const Vector2s& box) {
206  box_ = box;
207  update_A();
208  }
209 
210  const FrameIndex& get_id() const { return id_; }
211  const Vector2s& get_box() const { return box_; }
212  const Matrix46& get_A() const { return A_; }
213 
214  private:
215  FrameIndex id_;
216  Vector2s box_;
217  Matrix46 A_;
218 };
219 
220 } // namespace crocoddyl
221 
222 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_