Lodestar
An integrated real-time control package in C++
QuaternionBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/24/21.
3 //
4 
5 #ifndef LODESTAR_QUATERNIONBLOCK_HPP
6 #define LODESTAR_QUATERNIONBLOCK_HPP
7 
8 #include "Lodestar/blocks/Block.hpp"
9 #include "Lodestar/aux/CompileTimeQualifiers.hpp"
10 #include "Eigen/Dense"
11 
12 namespace ls {
13  namespace blocks {
14  namespace std {
15  enum class QuaternionBlockParameter {
16  Parametric,
17  AdditionalInput
18  };
19 
20  template<typename TType,
21  QuaternionBlockParameter TPar = QuaternionBlockParameter::Parametric>
22  class QuaternionBlock {
23  static_assert(::std::is_same<TType, TType>::value,
24  "QuaternionBlock not defined for this type.");
25  };
26 
27  template<typename TScalar>
28  class QuaternionBlock<
29  Eigen::Matrix<TScalar, 3, 1>,
30  QuaternionBlockParameter::Parametric> :
31  public Block<
32  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
33  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
34  ::std::tuple<Eigen::Quaternion<TScalar>>
35  > {
36 
37  static_assert(::std::is_arithmetic<TScalar>::value,
38  "Scalar must be of arithmetic type.");
39 
40  public:
41  using type =
42  QuaternionBlock<Eigen::Matrix<TScalar, 3, 1>, QuaternionBlockParameter::Parametric>;
43 
44  using Base =
45  Block<
46  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
47  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
48  ::std::tuple<Eigen::Quaternion<TScalar>>
49  >;
50 
51  using Quaternion = Eigen::Quaternion<TScalar>;
52 
53  QuaternionBlock()
54  {
55  quaternion(Quaternion::Identity());
56  bindEquation();
57  }
58 
59  QuaternionBlock(const Quaternion &quat)
60  {
61  quaternion(quat);
62  bindEquation();
63  }
64 
65  Quaternion &quaternion()
66  {
67  return this->template p<0>();
68  }
69 
70  Quaternion &quaternion(const Quaternion &quat)
71  {
72  this->template p<0>() = quat;
73  return this->template p<0>();
74  }
75 
76  const Quaternion &quaternion() const
77  {
78  return this->template p<0>();
79  }
80 
81  protected:
82  void bindEquation()
83  {
84  this->equation = ::std::bind(
85  &type::triggerFunction,
86  this,
87  ::std::placeholders::_1
88  );
89  }
90 
91  void triggerFunction(Base &b)
92  {
93  b.template o<0>() = quaternion() * b.template i<0>().object;
94  }
95  };
96 
97  template<typename TScalar>
98  class QuaternionBlock<
99  Eigen::Matrix<TScalar, 3, 1>,
100  QuaternionBlockParameter::AdditionalInput> :
101  public Block<
102  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>, Eigen::Quaternion<TScalar>>,
103  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
104  BlockProto::empty
105  > {
106 
107  static_assert(::std::is_arithmetic<TScalar>::value,
108  "Scalar must be of arithmetic type.");
109 
110  public:
111  using type =
112  QuaternionBlock<Eigen::Matrix<TScalar, 3, 1>, QuaternionBlockParameter::AdditionalInput>;
113 
114  using Base =
115  Block<
116  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>, Eigen::Quaternion<TScalar>>,
117  ::std::tuple<Eigen::Matrix<TScalar, 3, 1>>,
118  BlockProto::empty
119  >;
120 
121  using Quaternion = Eigen::Quaternion<TScalar>;
122 
123  QuaternionBlock()
124  {
125  quaternion(Quaternion::Identity());
126  bindEquation();
127  }
128 
129  QuaternionBlock(const Quaternion &quat)
130  {
131  quaternion(quat);
132  bindEquation();
133  }
134 
135  Signal<Quaternion> &quaternion()
136  {
137  return this->template i<1>();
138  }
139 
140  Signal<Quaternion> &quaternion(const Quaternion &quat)
141  {
142  this->template i<1>() = quat;
143  return this->template i<1>();
144  }
145 
146  Signal<Quaternion> &quaternion(const Signal<Quaternion> &quat)
147  {
148  this->template i<1>() = quat;
149  return this->template i<1>();
150  }
151 
152  const Signal<Quaternion> &quaternion() const
153  {
154  return this->template i<1>();
155  }
156 
157  protected:
158  void bindEquation()
159  {
160  this->equation = ::std::bind(
161  &type::triggerFunction,
162  this,
163  ::std::placeholders::_1
164  );
165  }
166 
167  void triggerFunction(Base &b)
168  {
169  b.template o<0>() =
170  quaternion().object * b.template i<0>().object;
171  }
172  };
173 
174  template<typename TScalar>
175  class QuaternionBlock<
176  Eigen::Matrix<TScalar, 1, 3>,
177  QuaternionBlockParameter::Parametric> :
178  public Block<
179  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
180  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
181  ::std::tuple<Eigen::Quaternion<TScalar>>
182  > {
183 
184  static_assert(::std::is_arithmetic<TScalar>::value,
185  "Scalar must be of arithmetic type.");
186 
187  public:
188  using type =
189  QuaternionBlock<Eigen::Matrix<TScalar, 1, 3>, QuaternionBlockParameter::Parametric>;
190 
191  using Base =
192  Block<
193  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
194  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
195  ::std::tuple<Eigen::Quaternion<TScalar>>
196  >;
197 
198  using Quaternion = Eigen::Quaternion<TScalar>;
199 
200  QuaternionBlock()
201  {
202  quaternion(Quaternion::Identity());
203  bindEquation();
204  }
205 
206  QuaternionBlock(const Quaternion &quat)
207  {
208  quaternion(quat);
209  bindEquation();
210  }
211 
212  Quaternion &quaternion()
213  {
214  return this->template p<0>();
215  }
216 
217  Quaternion &quaternion(const Quaternion &quat)
218  {
219  this->template p<0>() = quat;
220  return this->template p<0>();
221  }
222 
223  const Quaternion &quaternion() const
224  {
225  return this->template p<0>();
226  }
227 
228  protected:
229  void bindEquation()
230  {
231  this->equation = ::std::bind(
232  &type::triggerFunction,
233  this,
234  ::std::placeholders::_1
235  );
236  }
237 
238  void triggerFunction(Base &b)
239  {
240  b.template o<0>() = (quaternion() *
241  b.template i<0>().object.transpose()).transpose();
242  }
243  };
244 
245  template<typename TScalar>
246  class QuaternionBlock<
247  Eigen::Matrix<TScalar, 1, 3>,
248  QuaternionBlockParameter::AdditionalInput> :
249  public Block<
250  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>, Eigen::Quaternion<TScalar>>,
251  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
252  BlockProto::empty
253  > {
254 
255  static_assert(::std::is_arithmetic<TScalar>::value,
256  "Scalar must be of arithmetic type.");
257 
258  public:
259  using type =
260  QuaternionBlock<Eigen::Matrix<TScalar, 1, 3>, QuaternionBlockParameter::AdditionalInput>;
261 
262  using Base =
263  Block<
264  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>, Eigen::Quaternion<TScalar>>,
265  ::std::tuple<Eigen::Matrix<TScalar, 1, 3>>,
266  BlockProto::empty
267  >;
268 
269  using Quaternion = Eigen::Quaternion<TScalar>;
270 
271  QuaternionBlock()
272  {
273  quaternion(Quaternion::Identity());
274  bindEquation();
275  }
276 
277  QuaternionBlock(const Quaternion &quat)
278  {
279  quaternion(quat);
280  bindEquation();
281  }
282 
283  Signal<Quaternion> &quaternion()
284  {
285  return this->template i<1>();
286  }
287 
288  Signal<Quaternion> &quaternion(const Quaternion &quat)
289  {
290  this->template i<1>() = quat;
291  return this->template i<1>();
292  }
293 
294  Signal<Quaternion> &quaternion(const Signal<Quaternion> &quat)
295  {
296  this->template i<1>() = quat;
297  return this->template i<1>();
298  }
299 
300  const Signal<Quaternion> &quaternion() const
301  {
302  return this->template i<1>();
303  }
304 
305  protected:
306  void bindEquation()
307  {
308  this->equation = ::std::bind(
309  &type::triggerFunction,
310  this,
311  ::std::placeholders::_1
312  );
313  }
314 
315  void triggerFunction(Base &b)
316  {
317  b.template o<0>() = (quaternion().object *
318  b.template i<0>().object.transpose()).transpose();
319  }
320  };
321 
322  // Scalar inputs
323 
324  template<typename TScalar>
325  class QuaternionBlock<
326  TScalar,
327  QuaternionBlockParameter::Parametric> :
328  public Block<
329  ::std::tuple<TScalar, TScalar, TScalar>,
330  ::std::tuple<TScalar, TScalar, TScalar>,
331  ::std::tuple<Eigen::Quaternion<TScalar>>
332  > {
333 
334  static_assert(::std::is_arithmetic<TScalar>::value,
335  "Scalar must be of arithmetic type.");
336 
337  public:
338  using type =
339  QuaternionBlock<TScalar, QuaternionBlockParameter::Parametric>;
340 
341  using Base =
342  Block<
343  ::std::tuple<TScalar, TScalar, TScalar>,
344  ::std::tuple<TScalar, TScalar, TScalar>,
345  ::std::tuple<Eigen::Quaternion<TScalar>>
346  >;
347 
348  using Quaternion = Eigen::Quaternion<TScalar>;
349 
350  QuaternionBlock()
351  {
352  quaternion(Quaternion::Identity());
353  bindEquation();
354  }
355 
356  QuaternionBlock(const Quaternion &quat)
357  {
358  quaternion(quat);
359  bindEquation();
360  }
361 
362  Quaternion &quaternion()
363  {
364  return this->template p<0>();
365  }
366 
367  Quaternion &quaternion(const Quaternion &quat)
368  {
369  this->template p<0>() = quat;
370  return this->template p<0>();
371  }
372 
373  const Quaternion &quaternion() const
374  {
375  return this->template p<0>();
376  }
377 
378  protected:
379  Eigen::Vector<TScalar, 3> vec_;
380 
381  void bindEquation()
382  {
383  this->equation = ::std::bind(
384  &type::triggerFunction,
385  this,
386  ::std::placeholders::_1
387  );
388  }
389 
390  void triggerFunction(Base &b)
391  {
392  vec_ << b.template i<0>().object,
393  b.template i<1>().object,
394  b.template i<2>().object;
395  vec_ = quaternion() * vec_;
396  b.template o<0>() = vec_.x();
397  b.template o<1>() = vec_.y();
398  b.template o<2>() = vec_.z();
399  }
400  };
401 
402  template<typename TScalar>
403  class QuaternionBlock<
404  TScalar,
405  QuaternionBlockParameter::AdditionalInput> :
406  public Block<
407  ::std::tuple<TScalar, TScalar, TScalar, Eigen::Quaternion<TScalar>>,
408  ::std::tuple<TScalar, TScalar, TScalar>,
409  BlockProto::empty
410  > {
411 
412  static_assert(::std::is_arithmetic<TScalar>::value,
413  "Scalar must be of arithmetic type.");
414 
415  public:
416  using type =
417  QuaternionBlock<TScalar, QuaternionBlockParameter::AdditionalInput>;
418 
419  using Base =
420  Block<
421  ::std::tuple<TScalar, TScalar, TScalar, Eigen::Quaternion<TScalar>>,
422  ::std::tuple<TScalar, TScalar, TScalar>,
423  BlockProto::empty
424  >;
425 
426  using Quaternion = Eigen::Quaternion<TScalar>;
427 
428  QuaternionBlock()
429  {
430  quaternion(Quaternion::Identity());
431  bindEquation();
432  }
433 
434  QuaternionBlock(const Quaternion &quat)
435  {
436  quaternion(quat);
437  bindEquation();
438  }
439 
440  Signal<Quaternion> &quaternion()
441  {
442  return this->template i<3>();
443  }
444 
445  Signal<Quaternion> &quaternion(const Quaternion &quat)
446  {
447  this->template i<3>() = quat;
448  return this->template i<3>();
449  }
450 
451  Signal<Quaternion> &quaternion(const Signal<Quaternion> &quat)
452  {
453  this->template i<3>() = quat;
454  return this->template i<3>();
455  }
456 
457  const Signal<Quaternion> &quaternion() const
458  {
459  return this->template i<3>();
460  }
461 
462  protected:
463  Eigen::Vector<TScalar, 3> vec_;
464 
465  void bindEquation()
466  {
467  this->equation = ::std::bind(
468  &type::triggerFunction,
469  this,
470  ::std::placeholders::_1
471  );
472  }
473 
474  void triggerFunction(Base &b)
475  {
476  vec_ << b.template i<0>().object,
477  b.template i<1>().object,
478  b.template i<2>().object;
479  vec_ = quaternion().object * vec_;
480  b.template o<0>() = vec_.x();
481  b.template o<1>() = vec_.y();
482  b.template o<2>() = vec_.z();
483  }
484  };
485  }
486  }
487 }
488 
489 
490 #endif //LODESTAR_QUATERNIONBLOCK_HPP
ls::blocks::std::QuaternionBlock< TScalar, QuaternionBlockParameter::AdditionalInput >
Definition: QuaternionBlock.hpp:403
ls::blocks::std::QuaternionBlock< Eigen::Matrix< TScalar, 1, 3 >, QuaternionBlockParameter::Parametric >
Definition: QuaternionBlock.hpp:175
ls::blocks::std::QuaternionBlock< TScalar, QuaternionBlockParameter::Parametric >
Definition: QuaternionBlock.hpp:325
ls::blocks::std::QuaternionBlock< Eigen::Matrix< TScalar, 3, 1 >, QuaternionBlockParameter::AdditionalInput >
Definition: QuaternionBlock.hpp:98
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::blocks::Signal
Definition: Signal.hpp:22
ls::blocks::Block
Generic base template class for all tuple-based Block instances.
Definition: Block.hpp:45
ls::blocks::std::QuaternionBlock
Definition: QuaternionBlock.hpp:22
ls::blocks::std::QuaternionBlock< Eigen::Matrix< TScalar, 1, 3 >, QuaternionBlockParameter::AdditionalInput >
Definition: QuaternionBlock.hpp:246
ls::blocks::std::QuaternionBlock< Eigen::Matrix< TScalar, 3, 1 >, QuaternionBlockParameter::Parametric >
Definition: QuaternionBlock.hpp:28