5 #ifndef LODESTAR_DISCRETESTATESPACEBLOCK_HPP
6 #define LODESTAR_DISCRETESTATESPACEBLOCK_HPP
9 #include "Lodestar/blocks/Block.hpp"
10 #include "Lodestar/systems/DiscreteStateSpace.hpp"
11 #include "Eigen/Dense"
16 enum class DiscreteStateSpaceBlockFeedthrough {
21 template<
typename TScalar, DiscreteStateSpaceBlockFeedthrough TFeedthroughOps = DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
int NState = 0,
int NInput = 0,
int NOutput = 0>
23 static_assert(!::std::is_same<TScalar, TScalar>::value,
24 "DiscreteStateSpaceBlock not defined for this type.");
29 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
32 DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
38 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
39 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
40 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
43 static_assert(::std::is_arithmetic<TScalar>::value,
"Scalar type must be arithmetic.");
48 DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
56 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
57 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
58 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
62 using StateVector = Eigen::Vector<TScalar, NState>;
63 using InputVector = Eigen::Vector<TScalar, NInput>;
64 using OutputVector = Eigen::Vector<TScalar, NOutput>;
66 static constexpr
const int kNState = NState;
67 static constexpr
const int kNInput = NInput;
68 static constexpr
const int kNOutput = NOutput;
78 this->
template p<0>() = dss;
82 void setState(
const StateVector &x)
89 return this->
template p<0>();
95 this->equation = [&](
Base &b)
97 this->triggerFunction(b);
101 void triggerFunction(
Base &b)
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);
110 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
113 DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
119 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
120 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
121 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
124 static_assert(::std::is_arithmetic<TScalar>::value,
"Scalar type must be arithmetic.");
129 DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
137 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
138 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
139 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
143 using StateVector = Eigen::Vector<TScalar, NState>;
144 using InputVector = Eigen::Vector<TScalar, NInput>;
145 using OutputVector = Eigen::Vector<TScalar, NOutput>;
147 static constexpr
const int kNState = NState;
148 static constexpr
const int kNInput = NInput;
149 static constexpr
const int kNOutput = NOutput;
159 this->
template p<0>() = dss;
163 void setState(
const StateVector &x)
170 return this->
template p<0>();
176 this->equation = [&](
Base &b)
178 this->triggerFunction(b);
182 void triggerFunction(
Base &b)
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);
191 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
194 DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
200 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
201 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
202 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
205 static_assert(::std::is_arithmetic<TScalar>::value,
"Scalar type must be arithmetic.");
210 DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
218 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
219 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
220 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
224 using StateVector = Eigen::Vector<TScalar, NState>;
225 using InputVector = Eigen::Vector<TScalar, NInput>;
226 using OutputVector = Eigen::Vector<TScalar, NOutput>;
228 static constexpr
const int kNState = NState;
229 static constexpr
const int kNInput = NInput;
230 static constexpr
const int kNOutput = NOutput;
240 this->
template p<0>() = dss;
244 void setState(
const StateVector &x)
251 return this->
template p<0>();
257 this->equation = [&](
Base &b)
259 this->triggerFunction(b);
263 void triggerFunction(
Base &b)
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);
272 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
275 DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
281 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
282 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
283 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
286 static_assert(::std::is_arithmetic<TScalar>::value,
"Scalar type must be arithmetic.");
291 DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
299 ::std::tuple<Eigen::Vector<TScalar, NInput>>,
300 ::std::tuple<Eigen::Vector<TScalar, NOutput>>,
301 ::std::tuple<ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>>
305 using StateVector = Eigen::Vector<TScalar, NState>;
306 using InputVector = Eigen::Vector<TScalar, NInput>;
307 using OutputVector = Eigen::Vector<TScalar, NOutput>;
309 static constexpr
const int kNState = NState;
310 static constexpr
const int kNInput = NInput;
311 static constexpr
const int kNOutput = NOutput;
321 this->
template p<0>() = dss;
325 void setState(
const StateVector &x)
332 return this->
template p<0>();
338 this->equation = [&](
Base &b)
340 this->triggerFunction(b);
344 void triggerFunction(
Base &b)
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);
355 template<
typename TScalar,
int NState,
int NInput,
int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
360 directFeedthrough = (TFeedthroughOps == std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough)
364 using Base =
typename type::Base;
373 kNState = type::kNState,
374 kNInput = type::kNInput,
375 kNOutput = type::kNOutput
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;
382 static const ::std::array<::std::string, 5> templateTypes;
385 template<
typename TScalar,
int NState,
int NInput,
int NOutput, std::DiscreteStateSpaceBlockFeedthrough TFeedthroughOps>
387 { demangle(
typeid(Eigen::Vector<TScalar, NInput>).name()) };
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()) };
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 =
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()) };
401 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
403 ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>,
404 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
417 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
422 using Base =
typename type::Base;
431 kNState = type::kNState,
432 kNInput = type::kNInput,
433 kNOutput = type::kNOutput
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;
443 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
446 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
452 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
457 { demangle(
typeid(Eigen::Vector<TScalar, NInput>).name()) };
459 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
460 const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
462 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
466 >>::kOuts> BlockTraits<std::DiscreteStateSpaceBlock<
468 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
473 { demangle(
typeid(Eigen::Vector<TScalar, NOutput>).name()) };
475 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
476 const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
478 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
482 >>::kPars> BlockTraits<std::DiscreteStateSpaceBlock<
484 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
491 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
492 const ::std::array<::std::string, 2> BlockTraits<std::DiscreteStateSpaceBlock<
494 std::DiscreteStateSpaceBlockFeedthrough::NoFeedthrough,
503 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
505 ls::systems::DiscreteStateSpace<TScalar, NState, NInput, NOutput>,
506 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
519 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
524 using Base =
typename type::Base;
533 kNState = type::kNState,
534 kNInput = type::kNInput,
535 kNOutput = type::kNOutput
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;
545 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
548 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
554 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
559 { demangle(
typeid(Eigen::Vector<TScalar, NInput>).name()) };
561 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
562 const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
564 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
568 >>::kOuts> BlockTraits<std::DiscreteStateSpaceBlock<
570 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
575 { demangle(
typeid(Eigen::Vector<TScalar, NOutput>).name()) };
577 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
578 const ::std::array<::std::string, BlockTraits<std::DiscreteStateSpaceBlock<
580 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
584 >>::kPars> BlockTraits<std::DiscreteStateSpaceBlock<
586 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
593 template<
typename TScalar,
int NState,
int NInput,
int NOutput>
594 const ::std::array<::std::string, 2> BlockTraits<std::DiscreteStateSpaceBlock<
596 std::DiscreteStateSpaceBlockFeedthrough::HasFeedthrough,
606 #endif //LODESTAR_DISCRETESTATESPACEBLOCK_HPP