crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frames.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
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>& other) : id(other.id), translation(other.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 
45  if (this != &other) {
46  id = other.id;
47  translation = other.translation;
48  }
49  return *this;
50  }
51 
52  template <typename OtherScalar>
53  bool operator==(const FrameTranslationTpl<OtherScalar>& other) const {
54  return id == other.id && translation == other.translation;
55  }
56 
57  FrameIndex id;
58  Vector3s translation;
59 };
60 
61 template <typename _Scalar>
63  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 
65  typedef _Scalar Scalar;
66  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
67 
68  explicit FrameRotationTpl() : id(0), rotation(Matrix3s::Identity()) {}
69  FrameRotationTpl(const FrameRotationTpl<Scalar>& other) : id(other.id), rotation(other.rotation) {}
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;
73  return os;
74  }
75 
76  FrameRotationTpl<Scalar>& operator=(const FrameRotationTpl<Scalar>& other) {
77  if (this != &other) {
78  id = other.id;
79  rotation = other.rotation;
80  }
81  return *this;
82  }
83 
84  template <typename OtherScalar>
85  bool operator==(const FrameRotationTpl<OtherScalar>& other) const {
86  return id == other.id && rotation == other.rotation;
87  }
88 
89  FrameIndex id;
90  Matrix3s rotation;
91 };
92 
93 template <typename _Scalar>
95  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
96 
97  typedef _Scalar Scalar;
98  typedef pinocchio::SE3Tpl<Scalar> SE3;
99 
100  explicit FramePlacementTpl() : id(0), placement(SE3::Identity()) {}
101  FramePlacementTpl(const FramePlacementTpl<Scalar>& other) : id(other.id), placement(other.placement) {}
102  FramePlacementTpl(const FrameIndex& id, const SE3& placement) : id(id), placement(placement) {}
103 
104  FramePlacementTpl<Scalar>& operator=(const FramePlacementTpl<Scalar>& other) {
105  if (this != &other) {
106  id = other.id;
107  placement = other.placement;
108  }
109  return *this;
110  }
111 
112  template <typename OtherScalar>
113  bool operator==(const FramePlacementTpl<OtherScalar>& other) const {
114  return id == other.id && placement == other.placement;
115  }
116 
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;
119  return os;
120  }
121 
122  FrameIndex id;
123  pinocchio::SE3Tpl<Scalar> placement;
124 };
125 
126 template <typename _Scalar>
128  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
129 
130  typedef _Scalar Scalar;
131  typedef pinocchio::MotionTpl<Scalar> Motion;
132 
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;
144  break;
145  case pinocchio::LOCAL:
146  os << "reference: LOCAL" << std::endl;
147  break;
148  case pinocchio::LOCAL_WORLD_ALIGNED:
149  os << "reference: LOCAL_WORLD_ALIGNED" << std::endl;
150  break;
151  }
152  return os;
153  }
154 
155  FrameMotionTpl<Scalar>& operator=(const FrameMotionTpl<Scalar>& other) {
156  if (this != &other) {
157  id = other.id;
158  motion = other.motion;
159  reference = other.reference;
160  }
161  return *this;
162  }
163 
164  template <typename OtherScalar>
165  bool operator==(const FrameMotionTpl<OtherScalar>& other) const {
166  return id == other.id && motion == other.motion && reference == other.reference;
167  }
168 
169  FrameIndex id;
170  pinocchio::MotionTpl<Scalar> motion;
171  pinocchio::ReferenceFrame reference;
172 };
173 
174 template <typename _Scalar>
176  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
177 
178  typedef _Scalar Scalar;
179  typedef pinocchio::ForceTpl<Scalar> Force;
180 
181  explicit FrameForceTpl() : id(0), force(Force::Zero()) {}
182  FrameForceTpl(const FrameForceTpl<Scalar>& other) : id(other.id), force(other.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;
186  return os;
187  }
188 
189  FrameForceTpl<Scalar>& operator=(const FrameForceTpl<Scalar>& other) {
190  if (this != &other) {
191  id = other.id;
192  force = other.force;
193  }
194  return *this;
195  }
196 
197  template <typename OtherScalar>
198  bool operator==(const FrameForceTpl<OtherScalar>& other) const {
199  return id == other.id && force == other.force;
200  }
201 
202  FrameIndex id;
203  pinocchio::ForceTpl<Scalar> force;
204 };
205 
206 template <typename _Scalar>
208  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
209 
210  typedef _Scalar Scalar;
212 
213  explicit FrameFrictionConeTpl() : id(0), cone(FrictionCone()) {}
214  FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& other) : id(other.id), cone(other.cone) {}
215  FrameFrictionConeTpl(const FrameIndex& id, const FrictionCone& cone) : id(id), cone(cone) {}
216  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
217  os << " id: " << X.id << std::endl << "cone: " << std::endl << X.cone << std::endl;
218  return os;
219  }
220 
222  if (this != &other) {
223  id = other.id;
224  cone = other.cone;
225  }
226  return *this;
227  }
228 
229  FrameIndex id;
230  FrictionCone cone;
231 };
232 
233 template <typename _Scalar>
235  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
236 
237  typedef _Scalar Scalar;
239 
240  explicit FrameWrenchConeTpl() : id(0), cone(WrenchCone()) {}
241  FrameWrenchConeTpl(const FrameWrenchConeTpl<Scalar>& other) : id(other.id), cone(other.cone) {}
242  FrameWrenchConeTpl(const FrameIndex& id, const WrenchCone& cone) : id(id), cone(cone) {}
243  friend std::ostream& operator<<(std::ostream& os, const FrameWrenchConeTpl& X) {
244  os << "frame: " << X.id << std::endl << " cone: " << std::endl << X.cone << std::endl;
245  return os;
246  }
247 
248  FrameWrenchConeTpl<Scalar>& operator=(const FrameWrenchConeTpl<Scalar>& other) {
249  if (this != &other) {
250  id = other.id;
251  cone = other.cone;
252  }
253  return *this;
254  }
255 
256  FrameIndex id;
257  WrenchCone cone;
258 };
259 
260 template <typename _Scalar>
262  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
263 
264  typedef _Scalar Scalar;
265  typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
266  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
267  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
268 
269  public:
270  explicit FrameCoPSupportTpl() : id_(0), box_(Vector2s::Zero()) { update_A(); }
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;
276  return os;
277  }
278 
279  FrameCoPSupportTpl<Scalar>& operator=(const FrameCoPSupportTpl<Scalar>& other) {
280  if (this != &other) {
281  id_ = other.get_id();
282  box_ = other.get_box();
283  A_ = other.get_A();
284  }
285  return *this;
286  }
287 
288  // Define the inequality matrix A to implement A * f >= 0. Compare eq.(18-19) in
289  // https://hal.archives-ouvertes.fr/hal-02108449/document
290  void update_A() {
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);
294  }
295 
296  void set_id(FrameIndex id) { id_ = id; }
297  void set_box(const Vector2s& box) {
298  box_ = box;
299  update_A();
300  }
301 
302  const FrameIndex& get_id() const { return id_; }
303  const Vector2s& get_box() const { return box_; }
304  const Matrix46& get_A() const { return A_; }
305 
306  private:
307  FrameIndex id_;
308  Vector2s box_;
309  Matrix46 A_;
310 };
311 
312 } // namespace crocoddyl
313 
314 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_