[modelling/dynamics] Add templated casting-getters for underlying DAE/ODE for IntegratorAbstract and ExplicitIntegratorAbstract

Author: ManifoldFR

CHANGELOG.md

@@ -9,7 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
-- Templated getters `getCost<U>()` and `getDynamics<U>()` in the StageModel class, to get the concrete type.
+- Templated getters `getCost<U>()` and `getDynamics<U>()` in the StageModel class, and another `getDynamics<U>()` for integrator classes, to get the concrete types ([##205](https://github.com/Simple-Robotics/aligator/pull/205))
 
 ### Changed
 

include/aligator/modelling/dynamics/integrator-abstract.hpp

@@ -31,6 +31,13 @@ struct IntegratorAbstractTpl : DynamicsModelTpl<_Scalar> {
   /// The underlying continuous dynamics.
   xyz::polymorphic<ContinuousDynamics> continuous_dynamics_;
 
+  template <typename U> U *getDynamics() {
+    return dynamic_cast<U *>(&*continuous_dynamics_);
+  }
+  template <typename U> const U *getDynamics() const {
+    return dynamic_cast<const U *>(&*continuous_dynamics_);
+  }
+
   /// Constructor from instances of DynamicsType.
   explicit IntegratorAbstractTpl(
       const xyz::polymorphic<ContinuousDynamics> &cont_dynamics);

include/aligator/modelling/dynamics/integrator-explicit.hpp

@@ -32,6 +32,11 @@ struct ExplicitIntegratorAbstractTpl : ExplicitDynamicsModelTpl<_Scalar> {
 
   xyz::polymorphic<ODEType> ode_;
 
+  template <typename U> U *getDynamics() { return dynamic_cast<U *>(&*ode_); }
+  template <typename U> const U *getDynamics() const {
+    return dynamic_cast<const U *>(&*ode_);
+  }
+
   explicit ExplicitIntegratorAbstractTpl(
       const xyz::polymorphic<ODEType> &cont_dynamics);
   virtual ~ExplicitIntegratorAbstractTpl() = default;