crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frames.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2019-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  template <typename OtherScalar>
45  bool operator==(const FrameTranslationTpl<OtherScalar>& other) const {
46  return id == other.id && translation == other.translation;
47  }
48 
49  FrameIndex id;
50  Vector3s translation;
51 };
52 
53 template <typename _Scalar>
55  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56 
57  typedef _Scalar Scalar;
58  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
59 
60  explicit FrameRotationTpl() : id(0), rotation(Matrix3s::Identity()) {}
61  FrameRotationTpl(const FrameRotationTpl<Scalar>& value) : id(value.id), rotation(value.rotation) {}
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;
65  return os;
66  }
67 
68  template <typename OtherScalar>
69  bool operator==(const FrameRotationTpl<OtherScalar>& other) const {
70  return id == other.id && rotation == other.rotation;
71  }
72 
73  FrameIndex id;
74  Matrix3s rotation;
75 };
76 
77 template <typename _Scalar>
79  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
80 
81  typedef _Scalar Scalar;
82  typedef pinocchio::SE3Tpl<Scalar> SE3;
83 
84  explicit FramePlacementTpl() : id(0), placement(SE3::Identity()) {}
85  FramePlacementTpl(const FramePlacementTpl<Scalar>& value) : id(value.id), placement(value.placement) {}
86  FramePlacementTpl(const FrameIndex& id, const SE3& placement) : id(id), placement(placement) {}
87 
88  template <typename OtherScalar>
89  bool operator==(const FramePlacementTpl<OtherScalar>& other) const {
90  return id == other.id && placement == other.placement;
91  }
92 
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;
95  return os;
96  }
97 
98  FrameIndex id;
99  pinocchio::SE3Tpl<Scalar> placement;
100 };
101 
102 template <typename _Scalar>
104  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
105 
106  typedef _Scalar Scalar;
107  typedef pinocchio::MotionTpl<Scalar> Motion;
108 
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;
120  break;
121  case pinocchio::LOCAL:
122  os << "reference: LOCAL" << std::endl;
123  break;
124  case pinocchio::LOCAL_WORLD_ALIGNED:
125  os << "reference: LOCAL_WORLD_ALIGNED" << std::endl;
126  break;
127  }
128  return os;
129  }
130 
131  template <typename OtherScalar>
132  bool operator==(const FrameMotionTpl<OtherScalar>& other) const {
133  return id == other.id && motion == other.motion && reference == other.reference;
134  }
135 
136  FrameIndex id;
137  pinocchio::MotionTpl<Scalar> motion;
138  pinocchio::ReferenceFrame reference;
139 };
140 
141 template <typename _Scalar>
143  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
144 
145  typedef _Scalar Scalar;
146  typedef pinocchio::ForceTpl<Scalar> Force;
147 
148  explicit FrameForceTpl() : id(0), force(Force::Zero()) {}
149  FrameForceTpl(const FrameForceTpl<Scalar>& value) : id(value.id), force(value.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;
153  return os;
154  }
155 
156  template <typename OtherScalar>
157  bool operator==(const FrameForceTpl<OtherScalar>& other) const {
158  return id == other.id && force == other.force;
159  }
160 
161  FrameIndex id;
162  pinocchio::ForceTpl<Scalar> force;
163 };
164 
165 template <typename _Scalar>
167  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
168 
169  typedef _Scalar Scalar;
171 
172  explicit FrameFrictionConeTpl() : id(0), cone(FrictionCone()) {}
173  FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& value) : id(value.id), cone(value.cone) {}
174  FrameFrictionConeTpl(const FrameIndex& id, const FrictionCone& cone) : id(id), cone(cone) {}
175  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
176  os << " id: " << X.id << std::endl << "cone: " << std::endl << X.cone << std::endl;
177  return os;
178  }
179 
180  FrameIndex id;
181  FrictionCone cone;
182 };
183 
184 template <typename _Scalar>
186  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
187 
188  typedef _Scalar Scalar;
190 
191  explicit FrameWrenchConeTpl() : id(0), cone(WrenchCone()) {}
192  FrameWrenchConeTpl(const FrameWrenchConeTpl<Scalar>& value) : id(value.id), cone(value.cone) {}
193  FrameWrenchConeTpl(const FrameIndex& id, const WrenchCone& cone) : id(id), cone(cone) {}
194  friend std::ostream& operator<<(std::ostream& os, const FrameWrenchConeTpl& X) {
195  os << "frame: " << X.id << std::endl << " cone: " << std::endl << X.cone << std::endl;
196  return os;
197  }
198 
199  FrameIndex id;
200  WrenchCone cone;
201 };
202 
203 template <typename _Scalar>
205  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 
207  typedef _Scalar Scalar;
208  typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
209  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
210  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
211 
212  public:
213  explicit FrameCoPSupportTpl() : id_(0), box_(Vector2s::Zero()) { update_A(); }
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;
219  return os;
220  }
221 
222  // Define the inequality matrix A to implement A * f >= 0. Compare eq.(18-19) in
223  // https://hal.archives-ouvertes.fr/hal-02108449/document
224  void update_A() {
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);
228  }
229 
230  void set_id(FrameIndex id) { id_ = id; }
231  void set_box(const Vector2s& box) {
232  box_ = box;
233  update_A();
234  }
235 
236  const FrameIndex& get_id() const { return id_; }
237  const Vector2s& get_box() const { return box_; }
238  const Matrix46& get_A() const { return A_; }
239 
240  private:
241  FrameIndex id_;
242  Vector2s box_;
243  Matrix46 A_;
244 };
245 
246 } // namespace crocoddyl
247 
248 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_