Add assert and ConstVectorRef to getStateDerivative

Author: edantec

include/simple-mpc/mpc.hpp

@@ -176,10 +176,11 @@ namespace simple_mpc
       }
     }
 
-    const Eigen::VectorXd getStateDerivative(const std::size_t t)
+    const ConstVectorRef getStateDerivative(const std::size_t t)
     {
       ExplicitIntegratorData * int_data =
         dynamic_cast<ExplicitIntegratorData *>(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data);
+      assert(int_data != nullptr);
       return int_data->continuous_data->xdot_;
     }
 

src/centroidal-dynamics.cpp

@@ -283,7 +283,7 @@ namespace simple_mpc
     std::vector<bool> contact_state;
     CentroidalFwdDynamics * ode =
       problem_->stages_[t]->getDynamics<IntegratorEuler>()->getDynamics<CentroidalFwdDynamics>();
-
+    assert(ode != nullptr);
     for (auto name : model_handler_.getFeetNames())
       contact_state.push_back(ode->contact_map_.getContactState(name));
 

src/fulldynamics.cpp

@@ -378,7 +378,7 @@ namespace simple_mpc
     std::vector<bool> contact_state;
     MultibodyConstraintFwdDynamics * ode =
       problem_->stages_[t]->getDynamics<IntegratorSemiImplEuler>()->getDynamics<MultibodyConstraintFwdDynamics>();
-
+    assert(ode != nullptr);
     for (auto name : model_handler_.getFeetNames())
     {
       std::size_t i;

src/kinodynamics.cpp

@@ -326,7 +326,7 @@ namespace simple_mpc
   {
     KinodynamicsFwdDynamics * ode =
       problem_->stages_[t]->getDynamics<IntegratorSemiImplEuler>()->getDynamics<KinodynamicsFwdDynamics>();
-
+    assert(ode != nullptr);
     return ode->contact_states_;
   }