spatial-dyn
spatial_inertia.h
1 
10 #ifndef SPATIAL_DYN_EIGEN_SPATIAL_INERTIA_H_
11 #define SPATIAL_DYN_EIGEN_SPATIAL_INERTIA_H_
12 
13 #include "spatial_motion_base.h"
14 #include "spatial_force_base.h"
15 
16 #include <utility> // std::move
17 
18 namespace spatial_dyn {
19 
20 template<typename Scalar>
21 class SpatialInertia {
22 
23  public:
24 
25  SpatialInertia() {}
26 
27  SpatialInertia(const SpatialInertia<Scalar>& other)
28  : mass(other.mass), com(other.com), I_com(other.I_com) {}
29 
30  SpatialInertia(SpatialInertia<Scalar>&& other)
31  : mass(other.mass), com(std::move(other.com)), I_com(std::move(other.I_com)) {}
32 
33  template<typename ComDerived, typename IComDerived>
34  SpatialInertia(Scalar mass,
35  const Eigen::MatrixBase<ComDerived>& com,
36  const Eigen::MatrixBase<IComDerived>& I_com_flat);
37 
38  SpatialInertia<Scalar>& operator=(const SpatialInertia<Scalar>& other);
39 
40  SpatialInertia<Scalar>& operator=(SpatialInertia<Scalar>&& other);
41 
42  template<typename OtherDerived>
43  struct MotionProductTraits {
44  typedef typename Eigen::ScalarBinaryOpTraits<Scalar,
45  typename OtherDerived::Scalar>::ReturnType ScalarT;
46  typedef SpatialForce<ScalarT, OtherDerived::ColsAtCompileTime> ReturnType;
47  };
48 
49  template<typename OtherDerived>
50  typename MotionProductTraits<OtherDerived>::ReturnType
51  operator*(const SpatialMotionBase<OtherDerived>& other) const;
52 
53  SpatialInertia<Scalar>& operator+=(const SpatialInertia<Scalar>& other);
54 
55  SpatialInertia<Scalar> operator+(const SpatialInertia<Scalar>& other) const;
56 
57  bool operator==(const SpatialInertia<Scalar>& other) const;
58 
59  bool operator!=(const SpatialInertia<Scalar>& other) const;
60 
61  SpatialInertiaMatrix<Scalar> matrix() const;
62 
63  Eigen::Matrix<Scalar,6,1> I_com_flat() const;
64 
65  double mass = 0;
66  Eigen::Matrix<Scalar,3,1> com = Eigen::Matrix<Scalar,3,1>::Zero();
67  Eigen::Matrix<Scalar,3,3> I_com = Eigen::Matrix<Scalar,3,3>::Zero();
68 
69 };
70 
71 template<typename Scalar>
72 class SpatialInertiaMatrix : public Eigen::Matrix<Scalar,6,6> {
73 
74  public:
75  SpatialInertiaMatrix() { this->setZero(); }
76 
77  SpatialInertiaMatrix(const SpatialInertia<Scalar>& other);
78 
79  template<typename OtherDerived>
80  SpatialInertiaMatrix(const Eigen::MatrixBase<OtherDerived>& other)
81  : Eigen::Matrix<Scalar,6,6>(other) {};
82 
83  template<typename OtherDerived>
84  SpatialInertiaMatrix(Eigen::MatrixBase<OtherDerived>&& other)
85  : Eigen::Matrix<Scalar,6,6>(std::move(other)) {};
86 
87  template<typename OtherDerived>
88  struct MotionProductTraits {
89  typedef typename Eigen::ScalarBinaryOpTraits<Scalar,
90  typename OtherDerived::Scalar>::ReturnType _Scalar;
91  typedef SpatialForce<_Scalar, OtherDerived::ColsAtCompileTime> ReturnType;
92  };
93 
94  template<typename OtherDerived>
95  typename MotionProductTraits<OtherDerived>::ReturnType
96  operator*(const SpatialMotionBase<OtherDerived>& other) const;
97 
98  SpatialInertiaMatrix<Scalar>& operator+=(const SpatialInertia<Scalar>& other);
99 
100  template<typename OtherDerived>
101  SpatialInertiaMatrix<Scalar>& operator+=(const Eigen::MatrixBase<OtherDerived>& other);
102 
103 };
104 
105 // Transform operations
106 template<typename TransformScalar, int Dim, typename Scalar>
107 inline SpatialInertia<Scalar>
108 operator*(const Eigen::Translation<TransformScalar, Dim>& T, const SpatialInertia<Scalar>& I) {
109  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_INERTIAS");
110 
111  SpatialInertia<Scalar> result;
112  result.mass = I.mass;
113  result.com = T.translation() + I.com;
114  result.I_com = I.I_com;
115  return result;
116 }
117 
118 template<typename TransformScalar, int Dim, int Mode, int Options, typename Scalar>
119 inline SpatialInertia<Scalar>
120 operator*(const Eigen::Transform<TransformScalar, Dim, Mode, Options>& T, const SpatialInertia<Scalar>& I) {
121  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_INERTIAS");
122  static_assert(Mode == int(Eigen::Isometry),
123  "YOU_CAN_ONLY_APPLY_ISOMETRY_TRANSFORMS_TO_SPATIAL_INERTIAS");
124 
125  SpatialInertia<Scalar> result;
126  result.mass = I.mass;
127  result.com = T * I.com;
128  result.I_com = T.linear() * I.I_com * T.linear().transpose();
129  return result;
130 }
131 
132 template<typename TransformScalar, int Dim, typename Scalar>
133 SpatialInertiaMatrix<Scalar>
134 operator*(const Eigen::Translation<TransformScalar, Dim>& T, const SpatialInertiaMatrix<Scalar>& I) {
135  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_INERTIAS");
136 
137  SpatialInertiaMatrix<Scalar> result = I;
138  result.template rightCols<3>() -= result.template leftCols<3>().rowwise().cross(T.translation());
139  result.template bottomRows<3>() -= result.template topRows<3>().colwise().cross(T.translation());
140  return result;
141 }
142 
143 template<typename TransformScalar, int Dim, int Mode, int Options, typename Scalar>
144 SpatialInertiaMatrix<Scalar>
145 operator*(const Eigen::Transform<TransformScalar, Dim, Mode, Options>& T,
146  const SpatialInertiaMatrix<Scalar>& I) {
147  static_assert(Dim == 3, "YOU_CAN_ONLY_APPLY_3D_TRANSFORMS_TO_SPATIAL_INERTIAS");
148  static_assert(Mode == int(Eigen::Isometry),
149  "YOU_CAN_ONLY_APPLY_ISOMETRY_TRANSFORMS_TO_SPATIAL_INERTIAS");
150 
151  SpatialInertiaMatrix<Scalar> result = I;
152  result.template leftCols<3>() = result.template leftCols<3>() * T.linear().transpose();
153  result.template rightCols<3>() = result.template rightCols<3>() * T.linear().transpose() -
154  result.template leftCols<3>().rowwise().cross(T.translation());
155  result.template topRows<3>() = T.linear() * result.template topRows<3>();
156  result.template bottomRows<3>() = T.linear() * result.template bottomRows<3>() -
157  result.template topRows<3>().colwise().cross(T.translation());
158  return result;
159 }
160 
161 template<typename Scalar>
162 template<typename ComDerived, typename IComDerived>
163 inline SpatialInertia<Scalar>::SpatialInertia(Scalar m,
164  const Eigen::MatrixBase<ComDerived>& c,
165  const Eigen::MatrixBase<IComDerived>& I) : mass(m), com(c) {
166  static_assert(ComDerived::RowsAtCompileTime == 3 && ComDerived::ColsAtCompileTime == 1,
167  "COM_MUST_BE_A_VECTOR_OF_SIZE_3");
168  static_assert(IComDerived::RowsAtCompileTime == 6 && ComDerived::ColsAtCompileTime == 1,
169  "I_COM_MUST_BE_A_VECTOR_OF_SIZE_6");
170 
171  I_com << I(0), I(3), I(4),
172  I(3), I(1), I(5),
173  I(4), I(5), I(2);
174 }
175 
176 template<typename Scalar>
177 inline SpatialInertia<Scalar>&
178 SpatialInertia<Scalar>::operator=(const SpatialInertia<Scalar>& other) {
179  mass = other.mass;
180  com = other.com;
181  I_com = other.I_com;
182  return *this;
183 }
184 
185 template<typename Scalar>
186 inline SpatialInertia<Scalar>&
187 SpatialInertia<Scalar>::operator=(SpatialInertia<Scalar>&& other) {
188  mass = other.mass;
189  com = std::move(other.com);
190  I_com = std::move(other.I_com);
191  return *this;
192 }
193 
194 template<typename Scalar>
195 template<typename OtherDerived>
196 typename SpatialInertia<Scalar>::template MotionProductTraits<OtherDerived>::ReturnType
197 SpatialInertia<Scalar>::operator*(const SpatialMotionBase<OtherDerived>& other) const {
198  typename MotionProductTraits<OtherDerived>::ReturnType result;
199  Eigen::Matrix<Scalar,3,1> mc = mass * com;
200  result.template topRows<3>() = mass * other.matrix().template topRows<3>() +
201  other.matrix().template bottomRows<3>().cross(mc);
202  result.template bottomRows<3>() = I_com * other.matrix().template bottomRows<3>() -
203  other.matrix().template topRows<3>().cross(mc) -
204  other.matrix().template bottomRows<3>().cross(com).cross(mc);
205  return result;
206 }
207 
208 template<typename Scalar>
209 SpatialInertia<Scalar>&
210 SpatialInertia<Scalar>::operator+=(const SpatialInertia<Scalar>& other) {
211  Eigen::Vector3d com_new = (mass * com + other.mass * other.com) / (mass + other.mass);
212  I_com += other.I_com -
213  mass * ctrl_utils::DoubleCrossMatrix(com_new - com) -
214  other.mass * ctrl_utils::DoubleCrossMatrix(com_new - other.com);
215  mass += other.mass;
216  com = std::move(com_new);
217  return *this;
218 }
219 
220 template<typename Scalar>
221 SpatialInertia<Scalar>
222 SpatialInertia<Scalar>::operator+(const SpatialInertia<Scalar>& other) const {
223  SpatialInertia<Scalar> I_total;
224  I_total.com = (mass * com + other.mass * other.com) / (mass + other.mass);
225  I_total.I_com = I_com + other.I_com -
226  mass * ctrl_utils::DoubleCrossMatrix(I_total.com - com) -
227  other.mass * ctrl_utils::DoubleCrossMatrix(I_total.com - other.com);
228  I_total.mass = mass + other.mass;
229  return I_total;
230 }
231 
232 template<typename Scalar>
233 inline bool SpatialInertia<Scalar>::operator==(const SpatialInertia<Scalar>& other) const {
234  return mass == other.mass && com == other.com && I_com == other.I_com;
235 }
236 
237 template<typename Scalar>
238 inline bool SpatialInertia<Scalar>::operator!=(const SpatialInertia<Scalar>& other) const {
239  return !(*this == other);
240 }
241 
242 template<typename Scalar>
243 SpatialInertiaMatrix<Scalar> SpatialInertia<Scalar>::matrix() const {
244  const double mcx = mass * com(0);
245  const double mcy = mass * com(1);
246  const double mcz = mass * com(2);
247  const double Imcxx = I_com(0,0) + mass * (com(1) * com(1) + com(2) * com(2));
248  const double Imcyy = I_com(1,1) + mass * (com(0) * com(0) + com(2) * com(2));
249  const double Imczz = I_com(2,2) + mass * (com(0) * com(0) + com(1) * com(1));
250  const double Imcxy = I_com(0,1) - mass * com(0) * com(1);
251  const double Imcxz = I_com(0,2) - mass * com(0) * com(2);
252  const double Imcyz = I_com(1,2) - mass * com(1) * com(2);
253  SpatialInertiaMatrix<Scalar> result;
254  result << mass, 0, 0, 0, mcz, -mcy,
255  0, mass, 0, -mcz, 0, mcx,
256  0, 0, mass, mcy, -mcx, 0,
257  0, -mcz, mcy, Imcxx, Imcxy, Imcxz,
258  mcz, 0, -mcx, Imcxy, Imcyy, Imcyz,
259  -mcy, mcx, 0, Imcxz, Imcyz, Imczz;
260  return result;
261 }
262 
263 template<typename Scalar>
264 inline Eigen::Matrix<Scalar,6,1> SpatialInertia<Scalar>::I_com_flat() const {
265  Eigen::Matrix<Scalar,6,1> result;
266  result << I_com(0,0), I_com(1,1), I_com(2,2), I_com(0,1), I_com(0,2), I_com(1,2);
267  return result;
268 }
269 
270 template<typename Scalar>
271 inline SpatialInertiaMatrix<Scalar>::SpatialInertiaMatrix(const SpatialInertia<Scalar>& other) {
272  const double& mass = other.mass;
273  const Eigen::Matrix<Scalar,3,1>& com = other.com;
274  const Eigen::Matrix<Scalar,3,3>& I_com = other.I_com;
275  const double mcx = mass * com(0);
276  const double mcy = mass * com(1);
277  const double mcz = mass * com(2);
278  const double Imcxx = I_com(0,0) + mass * (com(1) * com(1) + com(2) * com(2));
279  const double Imcyy = I_com(1,1) + mass * (com(0) * com(0) + com(2) * com(2));
280  const double Imczz = I_com(2,2) + mass * (com(0) * com(0) + com(1) * com(1));
281  const double Imcxy = I_com(0,1) - mass * com(0) * com(1);
282  const double Imcxz = I_com(0,2) - mass * com(0) * com(2);
283  const double Imcyz = I_com(1,2) - mass * com(1) * com(2);
284  *this << mass, 0, 0, 0, mcz, -mcy,
285  0, mass, 0, -mcz, 0, mcx,
286  0, 0, mass, mcy, -mcx, 0,
287  0, -mcz, mcy, Imcxx, Imcxy, Imcxz,
288  mcz, 0, -mcx, Imcxy, Imcyy, Imcyz,
289  -mcy, mcx, 0, Imcxz, Imcyz, Imczz;
290 }
291 
292 template<typename Scalar>
293 template<typename OtherDerived>
294 inline typename SpatialInertiaMatrix<Scalar>::template MotionProductTraits<OtherDerived>::ReturnType
295 SpatialInertiaMatrix<Scalar>::operator*(const SpatialMotionBase<OtherDerived>& other) const {
296  return Eigen::Matrix<Scalar,6,6>::operator*(other.matrix());
297 };
298 
299 template<typename Scalar>
300 inline SpatialInertiaMatrix<Scalar>&
301 SpatialInertiaMatrix<Scalar>::operator+=(const SpatialInertia<Scalar>& other) {
302  *this += other.matrix();
303  return *this;
304 }
305 
306 template<typename Scalar>
307 template<typename OtherDerived>
308 inline SpatialInertiaMatrix<Scalar>&
309 SpatialInertiaMatrix<Scalar>::operator+=(const Eigen::MatrixBase<OtherDerived>& other) {
310  Eigen::Matrix<Scalar,6,6>::operator+=(other);
311  return *this;
312 }
313 
314 } // namespace spatial_dyn
315 
316 #endif // SPATIAL_DYN_EIGEN_SPATIAL_INERTIA_H_
spatial_dyn::SpatialForce
Definition: spatial_force.h:18
spatial_dyn::SpatialInertiaMatrix
Definition: spatial_inertia.h:72
spatial_dyn::SpatialInertia
Definition: spatial_inertia.h:21
spatial_dyn::SpatialMotionBase
Definition: spatial_motion_base.h:18
spatial_dyn
Definition: discrete_dynamics.cc:21
spatial_dyn::SpatialInertiaMatrix::MotionProductTraits
Definition: spatial_inertia.h:88
spatial_dyn::SpatialInertia::MotionProductTraits
Definition: spatial_inertia.h:43
Generated by doxygen 1.9.1