[core] Add templated getters for cost and dynamics

Author: ManifoldFR

CHANGELOG.md

@@ -7,6 +7,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
+- Templated getters `getCost<U>()` and `getDynamics<U>()` in the StageModel class, to get the concrete type.
+
 ### Changed
 
 - All map types are now `boost::unordered_map` ([#203](https://github.com/Simple-Robotics/aligator/pull/203))

include/aligator/core/stage-model.hpp

@@ -10,6 +10,10 @@
 
 namespace aligator {
 
+#define ALIGATOR_CHECK_DERIVED_CLASS(Base, Derived)                            \
+  static_assert((std::is_base_of_v<Base, Derived>),                            \
+                "Failed check for derived class.")
+
 /** @brief    A stage in the control problem.
  *
  *  @details  Each stage containts cost functions, dynamical
@@ -46,6 +50,30 @@ template <typename _Scalar> struct StageModelTpl {
   /// Dynamics model
   PolyDynamics dynamics_;
 
+  /// @brief Get a pointer to an expected concrete type for the cost function.
+  template <typename U> U *getCost() {
+    ALIGATOR_CHECK_DERIVED_CLASS(Cost, U);
+    return dynamic_cast<U *>(&*cost_);
+  }
+
+  /// @copybrief castCost()
+  template <typename U> const U *getCost() const {
+    ALIGATOR_CHECK_DERIVED_CLASS(Cost, U);
+    return dynamic_cast<const U *>(&*cost_);
+  }
+
+  /// @brief Get a pointer to an expected concrete type for the dynamics class.
+  template <typename U> U *getDynamics() {
+    ALIGATOR_CHECK_DERIVED_CLASS(Dynamics, U);
+    return dynamic_cast<U *>(&*dynamics_);
+  }
+
+  /// @copybrief castDynamics()
+  template <typename U> const U *getDynamics() const {
+    ALIGATOR_CHECK_DERIVED_CLASS(Dynamics, U);
+    return dynamic_cast<const U *>(&*dynamics_);
+  }
+
   /// Constructor assumes the control space is a Euclidean space of
   /// dimension @p nu.
   StageModelTpl(const PolyCost &cost, const PolyDynamics &dynamics);

tests/problem.cpp

@@ -86,10 +86,15 @@ BOOST_AUTO_TEST_CASE(test_problem) {
 
   auto nu = f.nu;
   auto &space = f.space;
-  auto &stage = *f.problem.stages_[0];
+  const auto &stage = *f.problem.stages_[0];
   BOOST_CHECK_EQUAL(stage.numPrimal(), space.ndx() + nu);
   BOOST_CHECK_EQUAL(stage.numDual(), space.ndx());
 
+  auto *p_dyn = stage.getDynamics<MyModel>();
+  BOOST_CHECK(p_dyn);
+  auto *p_cost = stage.getCost<MyCost>();
+  BOOST_CHECK(p_cost);
+
   Eigen::VectorXd u0(nu);
   u0.setZero();
   auto x0 = stage.xspace_->rand();