Lodestar
An integrated real-time control package in C++
DiscreteStateSpaceBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/30/2021.
3 //
4 
5 #ifndef LODESTAR_DISCRETESTATESPACEBLOCK_HPP
6 #define LODESTAR_DISCRETESTATESPACEBLOCK_HPP
7 
8 
9 #include "Lodestar/blocks/Block.hpp"
10 #include "Lodestar/systems/DiscreteStateSpace.hpp"
11 #include "Eigen/Dense"
12 
13 namespace ls {
14  namespace blocks {
15  namespace std {
16  enum class DiscreteStateSpaceBlockFeedthrough {
17  HasFeedthrough,
18  NoFeedthrough
19  };
20 
21  template<typename TScalar, DiscreteStateSpaceBlockFeedthrough TFeedthroughOps = DiscreteStateSpaceBlockFeedthrough::NoFeedthrough, int NState = 0, int NInput = 0, int NOutput = 0>
22  class DiscreteStateSpaceBlock : public Block<BlockProto::empty, BlockProto::empty, BlockProto::empty> {
23  static_assert(!::std::is_same<TScalar, TScalar>::value,
24  "DiscreteStateSpaceBlock not defined for this type.");
25  };
26 
27  // TODO: Add support for dynamic matrices.
28 
29  template<typename TScalar, int NState, int NInput, int NOutput>
31  TScalar,
32  DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
33  NState,
34  NInput,
35  NOutput
36  > :
37  public Block<
38  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
39  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
40  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
41  > {
42  public:
43  static_assert(::std::is_arithmetic<TScalar>::value, "Scalar type must be arithmetic.");
44 
45  using type =
47  TScalar,
48  DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
49  NState,
50  NInput,
51  NOutput
52  >;
53 
54  using Base =
55  Block<
56  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
57  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
58  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
59  >;
60 
62  using StateVector = Eigen::Vector<TScalar, NState>;
63  using InputVector = Eigen::Vector<TScalar, NInput>;
64  using OutputVector = Eigen::Vector<TScalar, NOutput>;
65 
66  static constexpr const int kNState = NState;
67  static constexpr const int kNInput = NInput;
68  static constexpr const int kNOutput = NOutput;
69 
70 
72  {
73  bindFunction();
74  }
75 
77  {
78  this->template p<0>() = dss;
79  bindFunction();
80  }
81 
82  void setState(const StateVector &x)
83  {
84  x_ = x;
85  }
86 
87  const DiscreteStateSpace &getSystem() const
88  {
89  return this->template p<0>();
90  }
91 
92  protected:
93  void bindFunction()
94  {
95  this->equation = [&](Base &b)
96  {
97  this->triggerFunction(b);
98  };
99  }
100 
101  void triggerFunction(Base &b)
102  {
103  b.template o<0>() = b.template p<0>().getC() * this->x_;
104  this->setState(b.template p<0>().getA() * this->x_ + b.template p<0>().getB() * b.template i<0>().object);
105  }
106 
107  StateVector x_;
108  };
109 
110  template<typename TScalar, int NState, int NInput, int NOutput>
113  DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
114  0,
115  0,
116  0
117  > :
118  public Block<
119  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
120  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
121  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
122  > {
123  public:
124  static_assert(::std::is_arithmetic<TScalar>::value, "Scalar type must be arithmetic.");
125 
126  using type =
128  TScalar,
129  DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
130  NState,
131  NInput,
132  NOutput
133  >;
134 
135  using Base =
136  Block<
137  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
138  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
139  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
140  >;
141 
143  using StateVector = Eigen::Vector<TScalar, NState>;
144  using InputVector = Eigen::Vector<TScalar, NInput>;
145  using OutputVector = Eigen::Vector<TScalar, NOutput>;
146 
147  static constexpr const int kNState = NState;
148  static constexpr const int kNInput = NInput;
149  static constexpr const int kNOutput = NOutput;
150 
151 
153  {
154  bindFunction();
155  }
156 
158  {
159  this->template p<0>() = dss;
160  bindFunction();
161  }
162 
163  void setState(const StateVector &x)
164  {
165  x_ = x;
166  }
167 
168  const DiscreteStateSpace &getSystem() const
169  {
170  return this->template p<0>();
171  }
172 
173  protected:
174  void bindFunction()
175  {
176  this->equation = [&](Base &b)
177  {
178  this->triggerFunction(b);
179  };
180  }
181 
182  void triggerFunction(Base &b)
183  {
184  b.template o<0>() = b.template p<0>().getC() * this->x_;
185  this->setState(b.template p<0>().getA() * this->x_ + b.template p<0>().getB() * b.template i<0>().object);
186  }
187 
188  StateVector x_;
189  };
190 
191  template<typename TScalar, int NState, int NInput, int NOutput>
193  TScalar,
194  DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
195  NState,
196  NInput,
197  NOutput
198  > :
199  public Block<
200  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
201  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
202  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
203  > {
204  public:
205  static_assert(::std::is_arithmetic<TScalar>::value, "Scalar type must be arithmetic.");
206 
207  using type =
209  TScalar,
210  DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
211  NState,
212  NInput,
213  NOutput
214  >;
215 
216  using Base =
217  Block<
218  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
219  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
220  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
221  >;
222 
224  using StateVector = Eigen::Vector<TScalar, NState>;
225  using InputVector = Eigen::Vector<TScalar, NInput>;
226  using OutputVector = Eigen::Vector<TScalar, NOutput>;
227 
228  static constexpr const int kNState = NState;
229  static constexpr const int kNInput = NInput;
230  static constexpr const int kNOutput = NOutput;
231 
232 
234  {
235  bindFunction();
236  }
237 
239  {
240  this->template p<0>() = dss;
241  bindFunction();
242  }
243 
244  void setState(const StateVector &x)
245  {
246  x_ = x;
247  }
248 
249  const DiscreteStateSpace &getSystem() const
250  {
251  return this->template p<0>();
252  }
253 
254  protected:
255  void bindFunction()
256  {
257  this->equation = [&](Base &b)
258  {
259  this->triggerFunction(b);
260  };
261  }
262 
263  void triggerFunction(Base &b)
264  {
265  b.template o<0>() = b.template p<0>().getC() * x_ + b.template p<0>().getD() * b.template i<0>().object;
266  this->setState(b.template p<0>().getA() * x_ + b.template p<0>().getB() * b.template i<0>().object);
267  }
268 
269  StateVector x_;
270  };
271 
272  template<typename TScalar, int NState, int NInput, int NOutput>
275  DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
276  0,
277  0,
278  0
279  > :
280  public Block<
281  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
282  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
283  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
284  > {
285  public:
286  static_assert(::std::is_arithmetic<TScalar>::value, "Scalar type must be arithmetic.");
287 
288  using type =
290  TScalar,
291  DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
292  NState,
293  NInput,
294  NOutput
295  >;
296 
297  using Base =
298  Block<
299  ::std::tuple<Eigen::Vector<TScalar, NInput>>,
300  ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
301  ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
302  >;
303 
305  using StateVector = Eigen::Vector<TScalar, NState>;
306  using InputVector = Eigen::Vector<TScalar, NInput>;
307  using OutputVector = Eigen::Vector<TScalar, NOutput>;
308 
309  static constexpr const int kNState = NState;
310  static constexpr const int kNInput = NInput;
311  static constexpr const int kNOutput = NOutput;
312 
313 
315  {
316  bindFunction();
317  }
318 
320  {
321  this->template p<0>() = dss;
322  bindFunction();
323  }
324 
325  void setState(const StateVector &x)
326  {
327  x_ = x;
328  }
329 
330  const DiscreteStateSpace &getSystem() const
331  {
332  return this->template p<0>();
333  }
334 
335  protected:
336  void bindFunction()
337  {
338  this->equation = [&](Base &b)
339  {
340  this->triggerFunction(b);
341  };
342  }
343 
344  void triggerFunction(Base &b)
345  {
346  b.template o<0>() = b.template p<0>().getC() * x_ + b.template p<0>().getD() * b.template i<0>().object;
347  this->setState(b.template p<0>().getA() * x_ + b.template p<0>().getB() * b.template i<0>().object);
348  }
349 
350  StateVector x_;
351  };
352  }
353 
354  // TODO: Check if partial specialization are needed for both feedthrough cases.
355  template<typename TScalar, int NState, int NInput, int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
357  public:
358  static constexpr const BlockType blockType = BlockType::DiscreteStateSpaceBlock;
359  enum {
360  directFeedthrough = (TFeedthroughOps == std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough)
361  };
362 
364  using Base = typename type::Base;
365 
366  enum {
367  kIns = Base::kIns,
368  kOuts = Base::kOuts,
369  kPars = Base::kPars
370  };
371 
372  enum {
373  kNState = type::kNState,
374  kNInput = type::kNInput,
375  kNOutput = type::kNOutput
376  };
377 
378  static const ::std::array<::std::string, kIns> inTypes;
379  static const ::std::array<::std::string, kOuts> outTypes;
380  static const ::std::array<::std::string, kPars> parTypes;
381 
382  static const ::std::array<::std::string, 5> templateTypes;
383  };
384 
385  template<typename TScalar, int NState, int NInput, int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
386  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::kIns> BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::inTypes =
387  { demangle(typeid(Eigen::Vector<TScalar, NInput>).name()) };
388 
389  template<typename TScalar, int NState, int NInput, int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
390  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::kOuts> BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::outTypes =
391  { demangle(typeid(Eigen::Vector<TScalar, NOutput>).name()) };
392 
393  template<typename TScalar, int NState, int NInput, int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
394  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::kPars> BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::parTypes =
396 
397  template<typename TScalar, int NState, int NInput, int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
398  const ::std::array<::std::string, 5> BlockTraits<std::DiscreteStateSpaceBlock<TScalar, TFeedthroughOps, NState, NInput, NOutput>>::templateTypes =
399  { demangle(typeid(TScalar).name()), demangle(typeid(NState).name()), demangle(typeid(NInput).name()), demangle(typeid(NOutput).name()), demangle(typeid(TFeedthroughOps).name()) };
400 
401  template<typename TScalar, int NState, int NInput, int NOutput>
403  ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>,
404  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
405  0,
406  0,
407  0
408  >> {
409  public:
410  static constexpr const BlockType blockType = BlockType::DiscreteStateSpaceBlock;
411  enum {
412  directFeedthrough = false
413  };
414 
417  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
418  0,
419  0,
420  0
421  >;
422  using Base = typename type::Base;
423 
424  enum {
425  kIns = Base::kIns,
426  kOuts = Base::kOuts,
427  kPars = Base::kPars
428  };
429 
430  enum {
431  kNState = type::kNState,
432  kNInput = type::kNInput,
433  kNOutput = type::kNOutput
434  };
435 
436  static const ::std::array<::std::string, kIns> inTypes;
437  static const ::std::array<::std::string, kOuts> outTypes;
438  static const ::std::array<::std::string, kPars> parTypes;
439 
440  static const ::std::array<::std::string, 2> templateTypes;
441  };
442 
443  template<typename TScalar, int NState, int NInput, int NOutput>
444  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
446  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
447  0,
448  0,
449  0
452  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
453  0,
454  0,
455  0
456  >>::inTypes =
457  { demangle(typeid(Eigen::Vector<TScalar, NInput>).name()) };
458 
459  template<typename TScalar, int NState, int NInput, int NOutput>
460  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
462  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
463  0,
464  0,
465  0
466  >>::kOuts> BlockTraits<std::DiscreteStateSpaceBlock<
468  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
469  0,
470  0,
471  0
472  >>::outTypes =
473  { demangle(typeid(Eigen::Vector<TScalar, NOutput>).name()) };
474 
475  template<typename TScalar, int NState, int NInput, int NOutput>
476  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
478  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
479  0,
480  0,
481  0
482  >>::kPars> BlockTraits<std::DiscreteStateSpaceBlock<
484  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
485  0,
486  0,
487  0
488  >>::parTypes =
490 
491  template<typename TScalar, int NState, int NInput, int NOutput>
492  const ::std::array<::std::string, 2> BlockTraits<std::DiscreteStateSpaceBlock<
494  std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
495  0,
496  0,
497  0
498  >>::templateTypes =
499  { demangle(typeid(ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>).name()), demangle(typeid(std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough).name()) };
500 
501  // Has feedthrough
502 
503  template<typename TScalar, int NState, int NInput, int NOutput>
505  ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>,
506  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
507  0,
508  0,
509  0
510  >> {
511  public:
512  static constexpr const BlockType blockType = BlockType::DiscreteStateSpaceBlock;
513  enum {
514  directFeedthrough = true
515  };
516 
519  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
520  0,
521  0,
522  0
523  >;
524  using Base = typename type::Base;
525 
526  enum {
527  kIns = Base::kIns,
528  kOuts = Base::kOuts,
529  kPars = Base::kPars
530  };
531 
532  enum {
533  kNState = type::kNState,
534  kNInput = type::kNInput,
535  kNOutput = type::kNOutput
536  };
537 
538  static const ::std::array<::std::string, kIns> inTypes;
539  static const ::std::array<::std::string, kOuts> outTypes;
540  static const ::std::array<::std::string, kPars> parTypes;
541 
542  static const ::std::array<::std::string, 2> templateTypes;
543  };
544 
545  template<typename TScalar, int NState, int NInput, int NOutput>
546  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
548  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
549  0,
550  0,
551  0
554  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
555  0,
556  0,
557  0
558  >>::inTypes =
559  { demangle(typeid(Eigen::Vector<TScalar, NInput>).name()) };
560 
561  template<typename TScalar, int NState, int NInput, int NOutput>
562  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
564  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
565  0,
566  0,
567  0
568  >>::kOuts> BlockTraits<std::DiscreteStateSpaceBlock<
570  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
571  0,
572  0,
573  0
574  >>::outTypes =
575  { demangle(typeid(Eigen::Vector<TScalar, NOutput>).name()) };
576 
577  template<typename TScalar, int NState, int NInput, int NOutput>
578  const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
580  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
581  0,
582  0,
583  0
584  >>::kPars> BlockTraits<std::DiscreteStateSpaceBlock<
586  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
587  0,
588  0,
589  0
590  >>::parTypes =
592 
593  template<typename TScalar, int NState, int NInput, int NOutput>
594  const ::std::array<::std::string, 2> BlockTraits<std::DiscreteStateSpaceBlock<
596  std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
597  0,
598  0,
599  0
600  >>::templateTypes =
601  { demangle(typeid(ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>).name()), demangle(typeid(std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough).name()) };
602  }
603 }
604 
605 
606 #endif //LODESTAR_DISCRETESTATESPACEBLOCK_HPP
ls::blocks::std::DiscreteStateSpaceBlock< TScalar, DiscreteStateSpaceBlockFeedthrough::HasFeedthrough, NState, NInput, NOutput >
Definition: DiscreteStateSpaceBlock.hpp:192
ls::systems::DiscreteStateSpace
Definition: DiscreteStateSpace.hpp:15
ls::blocks::BlockTraits::directFeedthrough
static constexpr const bool directFeedthrough
Whether or not the block has direct feedthrough.
Definition: BlockTraits.hpp:44
ls::blocks::BlockTraits
A traits object that exposes information about TBlock.
Definition: BlockTraits.hpp:37
ls::blocks::BlockTraits::inTypes
static const ::std::array<::std::string, kIns > inTypes
Input types (as strings).
Definition: BlockTraits.hpp:59
ls::blocks::BlockType
BlockType
Block type information.
Definition: BlockType.hpp:25
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::blocks::std::DiscreteStateSpaceBlock
Definition: DiscreteStateSpaceBlock.hpp:22
ls::blocks::BlockTraits::outTypes
static const ::std::array<::std::string, kOuts > outTypes
Output types (as strings).
Definition: BlockTraits.hpp:61
ls::blocks::std::DiscreteStateSpaceBlock< TScalar, DiscreteStateSpaceBlockFeedthrough::NoFeedthrough, NState, NInput, NOutput >
Definition: DiscreteStateSpaceBlock.hpp:30
ls::blocks::BlockTraits::blockType
static constexpr const BlockType blockType
Block type.
Definition: BlockTraits.hpp:42
ls::blocks::BlockTraits::templateTypes
static const ::std::array<::std::string, 1 > templateTypes
Template parameter types (as strings).
Definition: BlockTraits.hpp:66
ls::blocks::Block
Generic base template class for all tuple-based Block instances.
Definition: Block.hpp:45
ls::blocks::BlockTraits::kIns
static const constexpr int kIns
Number of input slots.
Definition: BlockTraits.hpp:52
ls::blocks::BlockTraits::parTypes
static const ::std::array<::std::string, kPars > parTypes
Parameter types (as strings).
Definition: BlockTraits.hpp:63
ls::blocks::BlockTraits::kPars
static const constexpr int kPars
Number of parameters.
Definition: BlockTraits.hpp:56
ls::blocks::BlockType::DiscreteStateSpaceBlock
@ DiscreteStateSpaceBlock
Discrete state-space block.
ls::blocks::BlockTraits::kOuts
static const constexpr int kOuts
Number of output slots.
Definition: BlockTraits.hpp:54