crocoddyl  1.4.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/core/mathbase.hpp"
16 
17 #include <pinocchio/spatial/se3.hpp>
18 #include <pinocchio/spatial/motion.hpp>
19 #include <pinocchio/spatial/force.hpp>
20 
21 namespace crocoddyl {
22 
23 typedef std::size_t FrameIndex;
24 
25 template <typename _Scalar>
27  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
28 
29  typedef _Scalar Scalar;
30  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
31 
32  explicit FrameTranslationTpl() : frame(0), oxf(Vector3s::Zero()) {}
33  FrameTranslationTpl(const FrameTranslationTpl<Scalar>& value) : frame(value.frame), oxf(value.oxf) {}
34  FrameTranslationTpl(const FrameIndex& frame, const Vector3s& oxf) : frame(frame), oxf(oxf) {}
35  friend std::ostream& operator<<(std::ostream& os, const FrameTranslationTpl<Scalar>& X) {
36  os << " frame: " << X.frame << std::endl << "translation: " << std::endl << X.oxf.transpose() << std::endl;
37  return os;
38  }
39 
40  FrameIndex frame;
41  Vector3s oxf;
42 };
43 
44 template <typename _Scalar>
46  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 
48  typedef _Scalar Scalar;
49  typedef typename MathBaseTpl<Scalar>::Matrix3s Matrix3s;
50 
51  explicit FrameRotationTpl() : frame(0), oRf(Matrix3s::Identity()) {}
52  FrameRotationTpl(const FrameRotationTpl<Scalar>& value) : frame(value.frame), oRf(value.oRf) {}
53  FrameRotationTpl(const FrameIndex& frame, const Matrix3s& oRf) : frame(frame), oRf(oRf) {}
54  friend std::ostream& operator<<(std::ostream& os, const FrameRotationTpl<Scalar>& X) {
55  os << " frame: " << X.frame << std::endl << "rotation: " << std::endl << X.oRf << std::endl;
56  return os;
57  }
58 
59  FrameIndex frame;
60  Matrix3s oRf;
61 };
62 
63 template <typename _Scalar>
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
67  typedef _Scalar Scalar;
68  typedef pinocchio::SE3Tpl<Scalar> SE3;
69 
70  explicit FramePlacementTpl() : frame(0), oMf(SE3::Identity()) {}
71  FramePlacementTpl(const FramePlacementTpl<Scalar>& value) : frame(value.frame), oMf(value.oMf) {}
72  FramePlacementTpl(const FrameIndex& frame, const SE3& oMf) : frame(frame), oMf(oMf) {}
73  friend std::ostream& operator<<(std::ostream& os, const FramePlacementTpl<Scalar>& X) {
74  os << " frame: " << X.frame << std::endl << "placement: " << std::endl << X.oMf << std::endl;
75  return os;
76  }
77 
78  FrameIndex frame;
79  pinocchio::SE3Tpl<Scalar> oMf;
80 };
81 
82 template <typename _Scalar>
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 
86  typedef _Scalar Scalar;
87  typedef pinocchio::MotionTpl<Scalar> Motion;
88 
89  explicit FrameMotionTpl() : frame(0), oMf(Motion::Zero()) {}
90  FrameMotionTpl(const FrameMotionTpl<Scalar>& value) : frame(value.frame), oMf(value.oMf) {}
91  FrameMotionTpl(const FrameIndex& frame, const Motion& oMf) : frame(frame), oMf(oMf) {}
92  friend std::ostream& operator<<(std::ostream& os, const FrameMotionTpl<Scalar>& X) {
93  os << " frame: " << X.frame << std::endl << "motion: " << std::endl << X.oMf << std::endl;
94  return os;
95  }
96 
97  FrameIndex frame;
98  pinocchio::MotionTpl<Scalar> oMf;
99 };
100 
101 template <typename _Scalar>
103  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 
105  typedef _Scalar Scalar;
106  typedef pinocchio::ForceTpl<Scalar> Force;
107 
108  explicit FrameForceTpl() : frame(0), oFf(Force::Zero()) {}
109  FrameForceTpl(const FrameForceTpl<Scalar>& value) : frame(value.frame), oFf(value.oFf) {}
110  FrameForceTpl(const FrameIndex& frame, const Force& oFf) : frame(frame), oFf(oFf) {}
111  friend std::ostream& operator<<(std::ostream& os, const FrameForceTpl<Scalar>& X) {
112  os << "frame: " << X.frame << std::endl << "force: " << std::endl << X.oFf << std::endl;
113  return os;
114  }
115 
116  FrameIndex frame;
117  pinocchio::ForceTpl<Scalar> oFf;
118 };
119 
120 template <typename _Scalar>
122  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
123 
124  typedef _Scalar Scalar;
126 
127  explicit FrameFrictionConeTpl() : frame(0), oRf(FrictionCone()) {}
128  FrameFrictionConeTpl(const FrameFrictionConeTpl<Scalar>& value) : frame(value.frame), oRf(value.oRf) {}
129  FrameFrictionConeTpl(const FrameIndex& frame, const FrictionCone& oRf) : frame(frame), oRf(oRf) {}
130  friend std::ostream& operator<<(std::ostream& os, const FrameFrictionConeTpl& X) {
131  os << "frame: " << X.frame << std::endl << " cone: " << std::endl << X.oRf << std::endl;
132  return os;
133  }
134 
135  FrameIndex frame;
136  FrictionCone oRf;
137 };
138 
139 template <typename _Scalar>
141  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
142 
143  typedef _Scalar Scalar;
144  typedef typename MathBaseTpl<Scalar>::Vector2s Vector2s;
145  typedef typename MathBaseTpl<Scalar>::Vector3s Vector3s;
146  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46;
147 
148  public:
149  explicit FrameCoPSupportTpl() : frame_(0), support_region_(Vector2s::Zero()) { update_A(); }
151  : frame_(value.get_frame()), support_region_(value.get_support_region()), A_(value.get_A()) {}
152  FrameCoPSupportTpl(const FrameIndex& frame, const Vector2s& support_region)
153  : frame_(frame), support_region_(support_region) {
154  update_A();
155  }
156  friend std::ostream& operator<<(std::ostream& os, const FrameCoPSupportTpl<Scalar>& X) {
157  os << " frame: " << X.get_frame() << std::endl
158  << "foot dimensions: " << std::endl
159  << X.get_support_region() << std::endl;
160  return os;
161  }
162 
163  // Define the inequality matrix A to implement A * f >= 0. Compare eq.(18-19) in
164  // https://hal.archives-ouvertes.fr/hal-02108449/document
165  void update_A() {
166  A_ << Scalar(0), Scalar(0), support_region_[0] / Scalar(2), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
167  support_region_[0] / Scalar(2), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(0),
168  support_region_[1] / Scalar(2), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
169  support_region_[1] / Scalar(2), Scalar(-1), Scalar(0), Scalar(0);
170  }
171 
172  void set_frame(FrameIndex frame) { frame_ = frame; }
173  void set_support_region(const Vector2s& support_region) {
174  support_region_ = support_region;
175  update_A();
176  }
177 
178  const FrameIndex& get_frame() const { return frame_; }
179  const Vector2s& get_support_region() const { return support_region_; }
180  const Matrix46& get_A() const { return A_; }
181 
182  private:
183  FrameIndex frame_;
184  Vector2s support_region_;
185  Matrix46 A_;
186 };
187 
188 } // namespace crocoddyl
189 
190 #endif // CROCODDYL_MULTIBODY_FRAMES_HPP_