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