crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frames-deprecated.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_DEPRECATED_HPP_
11 #define CROCODDYL_MULTIBODY_FRAMES_DEPRECATED_HPP_
12 
13 #include <pinocchio/multibody/fwd.hpp>
14 #include <pinocchio/spatial/se3.hpp>
15 #include <pinocchio/spatial/motion.hpp>
16 #include <pinocchio/spatial/force.hpp>
17 
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"
22 #include "crocoddyl/core/utils/deprecate.hpp"
23 
24 namespace crocoddyl {
25 
26 typedef std::size_t FrameIndex;
27 
28 template <typename _Scalar>
29 struct FrameTranslationTpl {
30  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 
32  typedef _Scalar Scalar;
33  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
34 
35  DEPRECATED("Do not use FrameTranslation", explicit FrameTranslationTpl());
36  DEPRECATED("Do not use FrameTranslation", FrameTranslationTpl(const FrameTranslationTpl<Scalar>& other));
37  DEPRECATED("Do not use FrameTranslation", FrameTranslationTpl(const FrameIndex& id, const Vector3s& translation));
38 
39  friend std::ostream& operator<<(std::ostream& os, const FrameTranslationTpl<Scalar>& X) {
40  os << " id: " << X.id << std::endl
41  << "translation: " << std::endl
42  << X.translation.transpose() << std::endl;
43  return os;
44  }
45 
46  FrameTranslationTpl<Scalar>& operator=(const FrameTranslationTpl<Scalar>& other) {
47  if (this != &other) {
48  id = other.id;
49  translation = other.translation;
50  }
51  return *this;
52  }
53 
54  template <typename OtherScalar>
55  bool operator==(const FrameTranslationTpl<OtherScalar>& other) const {
56  return id == other.id && translation == other.translation;
57  }
58 
59  FrameIndex id;
60  Vector3s translation;
61 };
62 
63 template <typename _Scalar>
64 struct FrameRotationTpl {
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  typedef _Scalar Scalar;
68  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
69 
70  DEPRECATED("Do not use FrameRotation", explicit FrameRotationTpl());
71  DEPRECATED("Do not use FrameRotation", FrameRotationTpl(const FrameRotationTpl<Scalar>& other));
72  DEPRECATED("Do not use FrameRotation", FrameRotationTpl(const FrameIndex& id, const Matrix3s& rotation));
73 
74  friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
75  os << " id: " << X.id << std::endl << "rotation: " << std::endl << X.rotation << std::endl;
76  return os;
77  }
78 
79  FrameRotationTpl<Scalar>& operator=(const FrameRotationTpl<Scalar>& other) {
80  if (this != &other) {
81  id = other.id;
82  rotation = other.rotation;
83  }
84  return *this;
85  }
86 
87  template <typename OtherScalar>
88  bool operator==(const FrameRotationTpl<OtherScalar>& other) const {
89  return id == other.id && rotation == other.rotation;
90  }
91 
92  FrameIndex id;
93  Matrix3s rotation;
94 };
95 
96 template <typename _Scalar>
97 struct FramePlacementTpl {
98  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 
100  typedef _Scalar Scalar;
101  typedef pinocchio::SE3Tpl<Scalar> SE3;
102 
103  DEPRECATED("Do not use FramePlacement", explicit FramePlacementTpl());
104  DEPRECATED("Do not use FramePlacement", FramePlacementTpl(const FramePlacementTpl<Scalar>& other));
105  DEPRECATED("Do not use FramePlacement", FramePlacementTpl(const FrameIndex& id, const SE3& placement));
106 
107  FramePlacementTpl<Scalar>& operator=(const FramePlacementTpl<Scalar>& other) {
108  if (this != &other) {
109  id = other.id;
110  placement = other.placement;
111  }
112  return *this;
113  }
114 
115  template <typename OtherScalar>
116  bool operator==(const FramePlacementTpl<OtherScalar>& other) const {
117  return id == other.id && placement == other.placement;
118  }
119 
120  friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
121  os << " id: " << X.id << std::endl << "placement: " << std::endl << X.placement << std::endl;
122  return os;
123  }
124 
125  FrameIndex id;
126  pinocchio::SE3Tpl<Scalar> placement;
127 };
128 
129 template <typename _Scalar>
130 struct FrameMotionTpl {
131  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
132 
133  typedef _Scalar Scalar;
134  typedef pinocchio::MotionTpl<Scalar> Motion;
135 
136  DEPRECATED("Do not use FrameMotion", explicit FrameMotionTpl());
137  DEPRECATED("Do not use FrameMotion", FrameMotionTpl(const FrameMotionTpl<Scalar>& other));
138  DEPRECATED("Do not use FrameMotion", FrameMotionTpl(const FrameIndex& id, const Motion& motion,
139  pinocchio::ReferenceFrame reference = pinocchio::LOCAL));
140 
141  friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
142  os << " id: " << X.id << std::endl;
143  os << " motion: " << std::endl << X.motion;
144  switch (X.reference) {
145  case pinocchio::WORLD:
146  os << "reference: WORLD" << std::endl;
147  break;
148  case pinocchio::LOCAL:
149  os << "reference: LOCAL" << std::endl;
150  break;
151  case pinocchio::LOCAL_WORLD_ALIGNED:
152  os << "reference: LOCAL_WORLD_ALIGNED" << std::endl;
153  break;
154  }
155  return os;
156  }
157 
158  FrameMotionTpl<Scalar>& operator=(const FrameMotionTpl<Scalar>& other) {
159  if (this != &other) {
160  id = other.id;
161  motion = other.motion;
162  reference = other.reference;
163  }
164  return *this;
165  }
166 
167  template <typename OtherScalar>
168  bool operator==(const FrameMotionTpl<OtherScalar>& other) const {
169  return id == other.id && motion == other.motion && reference == other.reference;
170  }
171 
172  FrameIndex id;
173  pinocchio::MotionTpl<Scalar> motion;
174  pinocchio::ReferenceFrame reference;
175 };
176 
177 template <typename _Scalar>
178 struct FrameForceTpl {
179  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  typedef _Scalar Scalar;
182  typedef pinocchio::ForceTpl<Scalar> Force;
183 
184  DEPRECATED("Do not use FrameForce", explicit FrameForceTpl());
185  DEPRECATED("Do not use FrameForce", FrameForceTpl(const FrameForceTpl<Scalar>& other));
186  DEPRECATED("Do not use FrameForce", FrameForceTpl(const FrameIndex& id, const Force& force));
187 
188  friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
189  os << " id: " << X.id << std::endl << "force: " << std::endl << X.force << std::endl;
190  return os;
191  }
192 
193  FrameForceTpl<Scalar>& operator=(const FrameForceTpl<Scalar>& other) {
194  if (this != &other) {
195  id = other.id;
196  force = other.force;
197  }
198  return *this;
199  }
200 
201  template <typename OtherScalar>
202  bool operator==(const FrameForceTpl<OtherScalar>& other) const {
203  return id == other.id && force == other.force;
204  }
205 
206  FrameIndex id;
207  pinocchio::ForceTpl<Scalar> force;
208 };
209 
210 template <typename _Scalar>
211 struct FrameFrictionConeTpl {
212  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
213 
214  typedef _Scalar Scalar;
215  typedef FrictionConeTpl<Scalar> FrictionCone;
216 
217  DEPRECATED("Do not use FrameFrictionCone", explicit FrameFrictionConeTpl());
218  DEPRECATED("Do not use FrameFrictionCone", FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& other));
219  DEPRECATED("Do not use FrameFrictionCone", FrameFrictionConeTpl(const FrameIndex& id, const FrictionCone& cone));
220 
221  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
222  os << " id: " << X.id << std::endl << "cone: " << std::endl << X.cone << std::endl;
223  return os;
224  }
225 
226  FrameFrictionConeTpl<Scalar>& operator=(const FrameFrictionConeTpl<Scalar>& other) {
227  if (this != &other) {
228  id = other.id;
229  cone = other.cone;
230  }
231  return *this;
232  }
233 
234  FrameIndex id;
235  FrictionCone cone;
236 };
237 
238 template <typename _Scalar>
239 struct FrameWrenchConeTpl {
240  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
241 
242  typedef _Scalar Scalar;
243  typedef WrenchConeTpl<Scalar> WrenchCone;
244 
245  DEPRECATED("Do not use FrameWrenchCone", explicit FrameWrenchConeTpl());
246  DEPRECATED("Do not use FrameWrenchCone", FrameWrenchConeTpl(const FrameWrenchConeTpl<Scalar>& other));
247  DEPRECATED("Do not use FrameWrenchCone", FrameWrenchConeTpl(const FrameIndex& id, const WrenchCone& cone));
248 
249  friend std::ostream& operator<<(std::ostream& os, const FrameWrenchConeTpl& X) {
250  os << "frame: " << X.id << std::endl << " cone: " << std::endl << X.cone << std::endl;
251  return os;
252  }
253 
254  FrameWrenchConeTpl<Scalar>& operator=(const FrameWrenchConeTpl<Scalar>& other) {
255  if (this != &other) {
256  id = other.id;
257  cone = other.cone;
258  }
259  return *this;
260  }
261 
262  FrameIndex id;
263  WrenchCone cone;
264 };
265 
266 template <typename _Scalar>
267 struct FrameCoPSupportTpl {
268  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
269 
270  typedef _Scalar Scalar;
271  typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
272  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
273  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
274 
275  public:
276  DEPRECATED("Do not use FrameCoPSupport", explicit FrameCoPSupportTpl());
277  DEPRECATED("Do not use FrameCoPSupport", FrameCoPSupportTpl(const FrameCoPSupportTpl<Scalar>& other));
278  DEPRECATED("Do not use FrameCoPSupport", FrameCoPSupportTpl(const FrameIndex& id, const Vector2s& box));
279 
280  friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
281  os << " id: " << X.get_id() << std::endl << "box: " << std::endl << X.get_box() << std::endl;
282  return os;
283  }
284 
285  FrameCoPSupportTpl<Scalar>& operator=(const FrameCoPSupportTpl<Scalar>& other) {
286  if (this != &other) {
287  id_ = other.get_id();
288  box_ = other.get_box();
289  A_ = other.get_A();
290  }
291  return *this;
292  }
293 
294  // Define the inequality matrix A to implement A * f >= 0. Compare eq.(18-19) in
295  // https://hal.archives-ouvertes.fr/hal-02108449/document
296  void update_A() {
297  A_ << Scalar(0), Scalar(0), box_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
298  box_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(1),
299  Scalar(0), Scalar(0), Scalar(0), Scalar(0), box_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
300  }
301 
302  void set_id(FrameIndex id) { id_ = id; }
303  void set_box(const Vector2s& box) {
304  box_ = box;
305  update_A();
306  }
307 
308  const FrameIndex& get_id() const { return id_; }
309  const Vector2s& get_box() const { return box_; }
310  const Matrix46& get_A() const { return A_; }
311 
312  private:
313  FrameIndex id_;
314  Vector2s box_;
315  Matrix46 A_;
316 };
317 
318 template <typename Scalar>
319 FrameTranslationTpl<Scalar>::FrameTranslationTpl() : id(0), translation(Vector3s::Zero()) {
320  std::cerr << "Deprecated: Do not use FrameTranslation." << std::endl;
321 }
322 template <typename Scalar>
323 FrameTranslationTpl<Scalar>::FrameTranslationTpl(const FrameTranslationTpl<Scalar>& other)
324  : id(other.id), translation(other.translation) {
325  std::cerr << "Deprecated: Do not use FrameTranslation." << std::endl;
326 }
327 template <typename Scalar>
328 FrameTranslationTpl<Scalar>::FrameTranslationTpl(const FrameIndex& id, const Vector3s& translation)
329  : id(id), translation(translation) {
330  std::cerr << "Deprecated: Do not use FrameTranslation." << std::endl;
331 }
332 
333 template <typename Scalar>
334 FrameRotationTpl<Scalar>::FrameRotationTpl() : id(0), rotation(Matrix3s::Identity()) {
335  std::cerr << "Deprecated: Do not use FrameRotation." << std::endl;
336 }
337 template <typename Scalar>
338 FrameRotationTpl<Scalar>::FrameRotationTpl(const FrameRotationTpl<Scalar>& other)
339  : id(other.id), rotation(other.rotation) {
340  std::cerr << "Deprecated: Do not use FrameRotation." << std::endl;
341 }
342 template <typename Scalar>
343 FrameRotationTpl<Scalar>::FrameRotationTpl(const FrameIndex& id, const Matrix3s& rotation)
344  : id(id), rotation(rotation) {
345  std::cerr << "Deprecated: Do not use FrameRotation." << std::endl;
346 }
347 
348 template <typename Scalar>
349 FramePlacementTpl<Scalar>::FramePlacementTpl() : id(0), placement(SE3::Identity()) {
350  std::cerr << "Deprecated: Do not use FramePlacement." << std::endl;
351 }
352 template <typename Scalar>
353 FramePlacementTpl<Scalar>::FramePlacementTpl(const FramePlacementTpl<Scalar>& other)
354  : id(other.id), placement(other.placement) {
355  std::cerr << "Deprecated: Do not use FramePlacement." << std::endl;
356 }
357 template <typename Scalar>
358 FramePlacementTpl<Scalar>::FramePlacementTpl(const FrameIndex& id, const SE3& placement)
359  : id(id), placement(placement) {
360  std::cerr << "Deprecated: Do not use FramePlacement." << std::endl;
361 }
362 
363 template <typename Scalar>
364 FrameMotionTpl<Scalar>::FrameMotionTpl() : id(0), motion(Motion::Zero()), reference(pinocchio::LOCAL) {
365  std::cerr << "Deprecated: Do not use FrameMotion." << std::endl;
366 }
367 template <typename Scalar>
368 FrameMotionTpl<Scalar>::FrameMotionTpl(const FrameMotionTpl<Scalar>& other)
369  : id(other.id), motion(other.motion), reference(other.reference) {
370  std::cerr << "Deprecated: Do not use FrameMotion." << std::endl;
371 }
372 template <typename Scalar>
373 FrameMotionTpl<Scalar>::FrameMotionTpl(const FrameIndex& id, const Motion& motion, pinocchio::ReferenceFrame reference)
374  : id(id), motion(motion), reference(reference) {
375  std::cerr << "Deprecated: Do not use FrameMotion." << std::endl;
376 }
377 
378 template <typename Scalar>
379 FrameForceTpl<Scalar>::FrameForceTpl() : id(0), force(Force::Zero()) {
380  std::cerr << "Deprecated: Do not use FrameForce." << std::endl;
381 }
382 template <typename Scalar>
383 FrameForceTpl<Scalar>::FrameForceTpl(const FrameForceTpl<Scalar>& other) : id(other.id), force(other.force) {
384  std::cerr << "Deprecated: Do not use FrameForce." << std::endl;
385 }
386 template <typename Scalar>
387 FrameForceTpl<Scalar>::FrameForceTpl(const FrameIndex& id, const Force& force) : id(id), force(force) {
388  std::cerr << "Deprecated: Do not use FrameForce." << std::endl;
389 }
390 
391 template <typename Scalar>
392 FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl() : id(0), cone(FrictionCone()) {
393  std::cerr << "Deprecated: Do not use FrameFrictionCone." << std::endl;
394 }
395 template <typename Scalar>
396 FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& other)
397  : id(other.id), cone(other.cone) {
398  std::cerr << "Deprecated: Do not use FrameFrictionCone." << std::endl;
399 }
400 template <typename Scalar>
401 FrameFrictionConeTpl<Scalar>::FrameFrictionConeTpl(const FrameIndex& id, const FrictionCone& cone)
402  : id(id), cone(cone) {
403  std::cerr << "Deprecated: Do not use FrameFrictionCone." << std::endl;
404 }
405 
406 template <typename Scalar>
407 FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl() : id(0), cone(WrenchCone()) {
408  std::cerr << "Deprecated: Do not use FrameWrenchCone." << std::endl;
409 }
410 template <typename Scalar>
411 FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl(const FrameWrenchConeTpl<Scalar>& other)
412  : id(other.id), cone(other.cone) {
413  std::cerr << "Deprecated: Do not use FrameWrenchCone." << std::endl;
414 }
415 template <typename Scalar>
416 FrameWrenchConeTpl<Scalar>::FrameWrenchConeTpl(const FrameIndex& id, const WrenchCone& cone) : id(id), cone(cone) {
417  std::cerr << "Deprecated: Do not use FrameWrenchCone." << std::endl;
418 }
419 
420 template <typename Scalar>
421 FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl() : id_(0), box_(Vector2s::Zero()) {
422  update_A();
423  std::cerr << "Deprecated: Do not use FrameCoPSupport." << std::endl;
424 }
425 template <typename Scalar>
426 FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl(const FrameCoPSupportTpl<Scalar>& other)
427  : id_(other.get_id()), box_(other.get_box()), A_(other.get_A()) {
428  std::cerr << "Deprecated: Do not use FrameCoPSupport." << std::endl;
429 }
430 template <typename Scalar>
431 FrameCoPSupportTpl<Scalar>::FrameCoPSupportTpl(const FrameIndex& id, const Vector2s& box) : id_(id), box_(box) {
432  update_A();
433  std::cerr << "Deprecated: Do not use FrameCoPSupport." << std::endl;
434 }
435 
436 } // namespace crocoddyl
437 
438 #endif // CROCODDYL_MULTIBODY_FRAMES_DEPRECATED_HPP_
crocoddyl::MathBaseTpl
Definition: mathbase.hpp:18
crocoddyl::FrameCoPSupportTpl
Definition: frames-deprecated.hpp:267
crocoddyl::FrameTranslationTpl
Definition: frames-deprecated.hpp:29
crocoddyl::FrameRotationTpl
Definition: frames-deprecated.hpp:64
crocoddyl::FramePlacementTpl
Definition: frames-deprecated.hpp:97
crocoddyl::FrictionConeTpl< Scalar >
crocoddyl::WrenchConeTpl< Scalar >
crocoddyl::FrameFrictionConeTpl
Definition: frames-deprecated.hpp:211
crocoddyl::FrameForceTpl
Definition: frames-deprecated.hpp:178
crocoddyl::FrameMotionTpl
Definition: frames-deprecated.hpp:130
crocoddyl::FrameWrenchConeTpl
Definition: frames-deprecated.hpp:239