Lodestar
An integrated real-time control package in C++
SaturationBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/23/21.
3 //
4 
5 #ifndef LODESTAR_SATURATIONBLOCK_HPP
6 #define LODESTAR_SATURATIONBLOCK_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 SaturationBlockOperator {
16  Scalar,
17  Elementwise
18  };
19 
20  enum class SaturationBlockParameter {
21  Parametric,
22  AdditionalInput
23  };
24 
25  template<typename TType,
26  SaturationBlockOperator TOps = SaturationBlockOperator::Scalar,
27  SaturationBlockParameter TPar = SaturationBlockParameter::Parametric>
28  class SaturationBlock {
29  static_assert(!::std::is_same<TType, TType>::value,
30  "SaturationBlock not defined for this type.");
31  };
32 
33  template<typename TType, SaturationBlockOperator TOps>
34  class SaturationBlock<
35  TType,
36  TOps,
37  SaturationBlockParameter::Parametric
38  > :
39  public Block<
40  ::std::tuple<TType>,
41  ::std::tuple<TType>,
42  ::std::tuple<TType, TType>
43  > {
44  public:
45  using type =
46  SaturationBlock<
47  TType,
48  TOps,
49  SaturationBlockParameter::Parametric
50  >;
51 
52  using Base =
53  Block<
54  ::std::tuple<TType>,
55  ::std::tuple<TType>,
56  ::std::tuple<TType, TType>
57  >;
58 
59  SaturationBlock()
60  {
61  bindEquation();
62  }
63 
64  TType &lower()
65  {
66  return this->template p<0>();
67  }
68 
69  TType &lower(const TType lo)
70  {
71  this->template p<0>() = lo;
72  return this->template p<0>();
73  }
74 
75  TType lower() const
76  {
77  return this->template p<0>();
78  }
79 
80  TType &upper()
81  {
82  return this->template p<1>();
83  }
84 
85  TType &upper(const TType up)
86  {
87  this->template p<1>() = up;
88  return this->template p<1>();
89  }
90 
91  TType upper() const
92  {
93  return this->template p<1>();
94  }
95 
96  protected:
97  void bindEquation()
98  {
99  this->equation = ::std::bind(
100  &type::triggerFunction,
101  this,
102  ::std::placeholders::_1
103  );
104  }
105 
106  TType saturate(const TType x) const
107  {
108  return (x < lower() ? lower() :
109  (x > upper() ? upper() : x));
110  }
111 
112  void triggerFunction(Base &b)
113  {
114  b.template o<0>() = saturate(b.template i<0>().object);
115  }
116  };
117 
118  template<typename TType, SaturationBlockOperator TOps>
119  class SaturationBlock<
120  TType,
121  TOps,
122  SaturationBlockParameter::AdditionalInput
123  > :
124  public Block<
125  ::std::tuple<TType, TType, TType>,
126  ::std::tuple<TType>,
127  ::std::tuple<TType>
128  > {
129  public:
130  using type =
131  SaturationBlock<
132  TType,
133  TOps,
134  SaturationBlockParameter::AdditionalInput
135  >;
136 
137  using Base =
138  Block<
139  ::std::tuple<TType, TType, TType>,
140  ::std::tuple<TType>,
141  ::std::tuple<TType>
142  >;
143 
144  SaturationBlock()
145  {
146  bindEquation();
147  }
148 
149  Signal<TType> &lower()
150  {
151  return this->template i<1>();
152  }
153 
154  Signal<TType> &lower(const TType &lo)
155  {
156  this->template i<1>() = lo;
157  return this->template i<1>();
158  }
159 
160  Signal<TType> &lower(const Signal<TType> &lo)
161  {
162  this->template i<1>() = lo;
163  return this->template i<1>();
164  }
165 
166  const Signal<TType> &lower() const
167  {
168  return this->template i<1>();
169  }
170 
171  Signal<TType> &upper()
172  {
173  return this->template i<2>();
174  }
175 
176  Signal<TType> &upper(const TType &up)
177  {
178  this->template i<2>() = up;
179  return this->template i<2>();
180  }
181 
182  Signal<TType> &upper(const Signal<TType> up)
183  {
184  this->template i<2>() = up;
185  return this->template i<2>();
186  }
187 
188  const Signal<TType> &upper() const
189  {
190  return this->template i<2>();
191  }
192 
193  protected:
194  void bindEquation()
195  {
196  this->equation = ::std::bind(
197  &type::triggerFunction,
198  this,
199  ::std::placeholders::_1
200  );
201  }
202 
203  TType saturate(const TType x) const
204  {
205  return (x < lower().object ? lower().object :
206  (x > upper().object ? upper().object : x));
207  }
208 
209  void triggerFunction(Base &b)
210  {
211  b.template o<0>() = saturate(b.template i<0>().object);
212  }
213  };
214 
215  template<typename TScalar, int TRows, int TCols>
216  class SaturationBlock<
217  Eigen::Matrix<TScalar, TRows, TCols>,
218  SaturationBlockOperator::Scalar,
219  SaturationBlockParameter::Parametric
220  > :
221  public Block<
222  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
223  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
224  ::std::tuple<TScalar, TScalar>
225  > {
226  public:
227  using type =
228  SaturationBlock<
229  Eigen::Matrix<TScalar, TRows, TCols>,
230  SaturationBlockOperator::Scalar,
231  SaturationBlockParameter::Parametric
232  >;
233 
234  using Base =
235  Block<
236  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
237  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
238  ::std::tuple<TScalar, TScalar>
239  >;
240 
241  SaturationBlock()
242  {
243  bindEquation();
244  }
245 
246  TScalar &lower()
247  {
248  return this->template p<0>();
249  }
250 
251  TScalar &lower(const TScalar lo)
252  {
253  this->template p<0>() = lo;
254  return this->template p<0>();
255  }
256 
257  TScalar lower() const
258  {
259  return this->template p<0>();
260  }
261 
262  TScalar &upper()
263  {
264  return this->template p<1>();
265  }
266 
267  TScalar &upper(const TScalar up)
268  {
269  this->template p<1>() = up;
270  return this->template p<1>();
271  }
272 
273  TScalar upper() const
274  {
275  return this->template p<1>();
276  }
277 
278  protected:
279  void bindEquation()
280  {
281  this->equation = ::std::bind(
282  &type::triggerFunction,
283  this,
284  ::std::placeholders::_1
285  );
286  }
287 
288  TScalar saturate(const TScalar x) const
289  {
290  return (x < lower() ? lower() :
291  (x > upper() ? upper() : x));
292  }
293 
294  void triggerFunction(Base &b)
295  {
296  b.template o<0>() = b.template i<0>().object.unaryExpr(
297  ::std::bind(
298  &type::saturate,
299  this,
300  ::std::placeholders::_1
301  )
302  );
303  }
304  };
305 
306  template<typename TScalar, int TRows, int TCols>
307  class SaturationBlock<
308  Eigen::Matrix<TScalar, TRows, TCols>,
309  SaturationBlockOperator::Scalar,
310  SaturationBlockParameter::AdditionalInput
311  > :
312  public Block<
313  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, TScalar, TScalar>,
314  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
315  BlockProto::empty
316  > {
317  public:
318  using type =
319  SaturationBlock<
320  Eigen::Matrix<TScalar, TRows, TCols>,
321  SaturationBlockOperator::Scalar,
322  SaturationBlockParameter::AdditionalInput
323  >;
324 
325  using Base =
326  Block<
327  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, TScalar, TScalar>,
328  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
329  BlockProto::empty
330  >;
331 
332  SaturationBlock()
333  {
334  bindEquation();
335  }
336 
337  Signal<TScalar> &lower()
338  {
339  return this->template i<1>();
340  }
341 
342  Signal<TScalar> &lower(const TScalar lo)
343  {
344  this->template i<1>() = lo;
345  return this->template i<1>();
346  }
347 
348  Signal<TScalar> &lower(const Signal<TScalar> lo)
349  {
350  this->template i<1>() = lo;
351  return this->template i<1>();
352  }
353 
354  const Signal<TScalar> &lower() const
355  {
356  return this->template i<1>();
357  }
358 
359  Signal<TScalar> &upper()
360  {
361  return this->template i<2>();
362  }
363 
364  Signal<TScalar> &upper(const TScalar up)
365  {
366  this->template i<2>() = up;
367  return this->template i<2>();
368  }
369 
370  Signal<TScalar> &upper(const Signal<TScalar> up)
371  {
372  this->template i<2>() = up;
373  return this->template i<2>();
374  }
375 
376  const Signal<TScalar> &upper() const
377  {
378  return this->template i<2>();
379  }
380 
381  protected:
382  void bindEquation()
383  {
384  this->equation = ::std::bind(
385  &type::triggerFunction,
386  this,
387  ::std::placeholders::_1
388  );
389  }
390 
391  TScalar saturate(const TScalar x) const
392  {
393  return (x < lower().object ? lower().object :
394  (x > upper().object ? upper().object : x));
395  }
396 
397  void triggerFunction(Base &b)
398  {
399  b.template o<0>() = b.template i<0>().object.unaryExpr(
400  ::std::bind(
401  &type::saturate,
402  this,
403  ::std::placeholders::_1
404  )
405  );
406  }
407  };
408 
409  template<typename TScalar, int TRows, int TCols>
410  class SaturationBlock<
411  Eigen::Matrix<TScalar, TRows, TCols>,
412  SaturationBlockOperator::Elementwise,
413  SaturationBlockParameter::Parametric
414  > :
415  public Block<
416  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
417  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
418  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>>
419  > {
420  public:
421  using type =
422  SaturationBlock<
423  Eigen::Matrix<TScalar, TRows, TCols>,
424  SaturationBlockOperator::Elementwise,
425  SaturationBlockParameter::Parametric
426  >;
427 
428  using Base =
429  Block<
430  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
431  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
432  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>>
433  >;
434 
435  using BoundMatrix = Eigen::Matrix<TScalar, TRows, TCols>;
436 
437  SaturationBlock()
438  {
439  bindEquation();
440  }
441 
442  BoundMatrix &lower()
443  {
444  return this->template p<0>();
445  }
446 
447  BoundMatrix &lower(const BoundMatrix &lo)
448  {
449  this->template p<0>() = lo;
450  return this->template p<0>();
451  }
452 
453  BoundMatrix lower() const
454  {
455  return this->template p<0>();
456  }
457 
458  BoundMatrix &upper()
459  {
460  return this->template p<1>();
461  }
462 
463  BoundMatrix &upper(const BoundMatrix &up)
464  {
465  this->template p<1>() = up;
466  return this->template p<1>();
467  }
468 
469  BoundMatrix upper() const
470  {
471  return this->template p<1>();
472  }
473 
474  protected:
475  void bindEquation()
476  {
477  this->equation = ::std::bind(
478  &type::triggerFunction,
479  this,
480  ::std::placeholders::_1
481  );
482  }
483 
484  TScalar saturate(const TScalar x, int i = 0, int j = 0) const
485  {
486  return (x < lower()(i, j) ? lower()(i, j) :
487  (x > upper()(i, j) ? upper()(i, j) : x));
488  }
489 
490  int row(int idx) const
491  {
492  return floor<int, double>(idx / TCols);
493  }
494 
495  int col(int idx) const
496  {
497  return idx - floor<int, double>(idx / TCols) * TCols;
498  }
499 
500  void triggerFunction(Base &b)
501  {
502  int idx = 0;
503  int i = 0;
504  int j = 0;
505 
506  for (auto x: b.template i<0>().object.reshaped()) {
507  i = row(idx);
508  j = col(idx);
509 
510  b.template o<0>().object(i, j) = saturate(x, i, j);
511  idx++;
512  }
513 
514  b.template o<0>().propagate();
515  }
516  };
517 
518  template<typename TScalar, int TRows, int TCols>
519  class SaturationBlock<
520  Eigen::Matrix<TScalar, TRows, TCols>,
521  SaturationBlockOperator::Elementwise,
522  SaturationBlockParameter::AdditionalInput
523  > :
524  public Block<
525  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>>,
526  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
527  BlockProto::empty
528  > {
529  public:
530  using type =
531  SaturationBlock<
532  Eigen::Matrix<TScalar, TRows, TCols>,
533  SaturationBlockOperator::Elementwise,
534  SaturationBlockParameter::AdditionalInput
535  >;
536 
537  using Base =
538  Block<
539  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>, Eigen::Matrix<TScalar, TRows, TCols>>,
540  ::std::tuple<Eigen::Matrix<TScalar, TRows, TCols>>,
541  BlockProto::empty
542  >;
543 
544  using BoundMatrix = Eigen::Matrix<TScalar, TRows, TCols>;
545 
546  SaturationBlock()
547  {
548  bindEquation();
549  }
550 
551  Signal<BoundMatrix> &lower()
552  {
553  return this->template i<1>();
554  }
555 
556  Signal<BoundMatrix> &lower(const BoundMatrix &lo)
557  {
558  this->template i<1>() = lo;
559  return this->template i<1>();
560  }
561 
562  Signal<BoundMatrix> &lower(const Signal<BoundMatrix> &lo)
563  {
564  this->template i<1>() = lo;
565  return this->template i<1>();
566  }
567 
568  const Signal<BoundMatrix> &lower() const
569  {
570  return this->template i<1>();
571  }
572 
573  Signal<BoundMatrix> &upper()
574  {
575  return this->template i<2>();
576  }
577 
578  Signal<BoundMatrix> &upper(const BoundMatrix &up)
579  {
580  this->template i<2>() = up;
581  return this->template i<2>();
582  }
583 
584  Signal<BoundMatrix> &upper(const Signal<BoundMatrix> up)
585  {
586  this->template i<2>() = up;
587  return this->template i<2>();
588  }
589 
590  const Signal<BoundMatrix> &upper() const
591  {
592  return this->template i<2>();
593  }
594 
595  protected:
596  void bindEquation()
597  {
598  this->equation = ::std::bind(
599  &type::triggerFunction,
600  this,
601  ::std::placeholders::_1
602  );
603  }
604 
605  TScalar saturate(const TScalar x, int i = 0, int j = 0) const
606  {
607  return (x < lower().object(i, j) ? lower().object(i, j) :
608  (x > upper().object(i, j) ? upper().object(i, j)
609  : x));
610  }
611 
612  int row(int idx) const
613  {
614  return floor<int, double>(idx / TCols);
615  }
616 
617  int col(int idx) const
618  {
619  return idx - floor<int, double>(idx / TCols) * TCols;
620  }
621 
622  void triggerFunction(Base &b)
623  {
624  int idx = 0;
625  int i = 0;
626  int j = 0;
627 
628  for (auto x: b.template i<0>().object.reshaped()) {
629  i = row(idx);
630  j = col(idx);
631 
632  b.template o<0>().object(i, j) = saturate(x, i, j);
633  idx++;
634  }
635 
636  b.template o<0>().propagate();
637  }
638  };
639 
640  template<typename TType, SaturationBlockOperator TOps>
641  using SaturationBlockDynamic = SaturationBlock<TType, TOps, SaturationBlockParameter::AdditionalInput>;
642  }
643 
644  template<typename TType, std::SaturationBlockOperator TOps>
645  class BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>> {
646  public:
647  static constexpr const BlockType blockType = BlockType::SaturationBlock;
648  enum {
649  directFeedthrough = true
650  };
651 
652  using type = std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>;
653  using Base = typename type::Base;
654 
655  enum {
656  kIns = Base::kIns,
657  kOuts = Base::kOuts,
658  kPars = Base::kPars
659  };
660 
661  static const ::std::array<::std::string, kIns> inTypes;
662  static const ::std::array<::std::string, kOuts> outTypes;
663  static const ::std::array<::std::string, kPars> parTypes;
664 
665  static const ::std::array<::std::string, 3> templateTypes;
666  };
667 
668  template<typename TType, std::SaturationBlockOperator TOps>
669  const ::std::array<::std::string, BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::kIns>
670  BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::inTypes =
671  {demangle(typeid(TType).name())};
672 
673  template<typename TType, std::SaturationBlockOperator TOps>
674  const ::std::array<::std::string, BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::kOuts>
675  BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::outTypes =
676  {demangle(typeid(TType).name())};
677 
678  template<typename TType, std::SaturationBlockOperator TOps>
679  const ::std::array<::std::string, BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::kPars>
680  BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::parTypes =
681  {demangle(typeid(TType).name()), demangle(typeid(TType).name())};
682 
683  template<typename TType, std::SaturationBlockOperator TOps>
684  const ::std::array<::std::string, 3>
685  BlockTraits<std::SaturationBlock<TType, TOps, std::SaturationBlockParameter::Parametric>>::templateTypes =
686  {demangle(typeid(TType).name()), demangle(typeid(std::SaturationBlockOperator).name()), demangle(typeid(std::SaturationBlockParameter).name())};
687  }
688 }
689 
690 
691 #endif //LODESTAR_SATURATIONBLOCK_HPP
ls::blocks::std::SaturationBlock
Definition: SaturationBlock.hpp:28
ls::blocks::BlockType::SaturationBlock
@ SaturationBlock
Saturation block.
ls::blocks::BlockTraits
A traits object that exposes information about TBlock.
Definition: BlockTraits.hpp:37
ls::blocks::BlockType
BlockType
Block type information.
Definition: BlockType.hpp:25
ls::blocks::std::SaturationBlock< Eigen::Matrix< TScalar, TRows, TCols >, SaturationBlockOperator::Scalar, SaturationBlockParameter::AdditionalInput >
Definition: SaturationBlock.hpp:307
ls::blocks::std::SaturationBlock< TType, TOps, SaturationBlockParameter::Parametric >
Definition: SaturationBlock.hpp:34
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::blocks::std::SaturationBlock< Eigen::Matrix< TScalar, TRows, TCols >, SaturationBlockOperator::Elementwise, SaturationBlockParameter::Parametric >
Definition: SaturationBlock.hpp:410
ls::blocks::std::SaturationBlock< TType, TOps, SaturationBlockParameter::AdditionalInput >
Definition: SaturationBlock.hpp:119
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::SaturationBlock< Eigen::Matrix< TScalar, TRows, TCols >, SaturationBlockOperator::Elementwise, SaturationBlockParameter::AdditionalInput >
Definition: SaturationBlock.hpp:519
ls::blocks::std::SaturationBlock< Eigen::Matrix< TScalar, TRows, TCols >, SaturationBlockOperator::Scalar, SaturationBlockParameter::Parametric >
Definition: SaturationBlock.hpp:216