[exotica_core] DynamicTimeIndexedShootingProblem: Expose number of co…

…ntrols as NU to Python, check dimension in Update
ipab-slmc · Mar 12, 2019 · 2a1162e · 2a1162e
1 parent d4e4ad5
commit 2a1162e
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 0 deletions.
diff --git a/exotica_core/include/exotica_core/problems/dynamic_time_indexed_shooting_problem.h b/exotica_core/include/exotica_core/problems/dynamic_time_indexed_shooting_problem.h
@@ -65,6 +65,8 @@ class DynamicTimeIndexedShootingProblem : public PlanningProblem, public Instant
     Eigen::MatrixXd get_Q(int t) const;               ///< Returns the precision matrix at time step t
     void set_Q(Eigen::MatrixXdRefConst Q_in, int t);  ///< Sets the precision matrix for time step t
 
+    int get_num_controls() const; ///< Returns size of control vector
+
     double GetStateCost(int t) const;
     double GetControlCost(int t) const;
 

diff --git a/exotica_core/src/problems/dynamic_time_indexed_shooting_problem.cpp b/exotica_core/src/problems/dynamic_time_indexed_shooting_problem.cpp
@@ -197,6 +197,11 @@ void DynamicTimeIndexedShootingProblem::set_Q(Eigen::MatrixXdRefConst Q_in, int
     Q_[t] = Q_in;
 }
 
+int DynamicTimeIndexedShootingProblem::get_num_controls() const
+{
+    return dynamics_solver_->get_num_controls();
+}
+
 void DynamicTimeIndexedShootingProblem::Update(Eigen::VectorXdRefConst u_in, int t)
 {
     // We can only update t=0, ..., T-1 - the last state will be created from integrating u_{T-1} to get x_T
@@ -209,6 +214,11 @@ void DynamicTimeIndexedShootingProblem::Update(Eigen::VectorXdRefConst u_in, int
         t = T_ - 2;
     }
 
+    if (u_in.rows() != dynamics_solver_->get_num_controls())
+    {
+        ThrowPretty("Mismatching in size of control vector: " << u_in.rows() << " given, expected: " << dynamics_solver_->get_num_controls());
+    }
+
     U_.col(t) = u_in;
 
     // Set the corresponding KinematicResponse for KinematicTree in order to

diff --git a/exotica_python/src/pyexotica.cpp b/exotica_python/src/pyexotica.cpp
@@ -934,6 +934,7 @@ PYBIND11_MODULE(_pyexotica, module)
         .def_property_readonly("tau", &DynamicTimeIndexedShootingProblem::get_tau)
         .def_property("T", &DynamicTimeIndexedShootingProblem::get_T, &DynamicTimeIndexedShootingProblem::set_T)
         .def_readonly("N", &DynamicTimeIndexedShootingProblem::N)
+        .def_property_readonly("NU", &DynamicTimeIndexedShootingProblem::get_num_controls)
         .def("dynamics", &DynamicTimeIndexedShootingProblem::Dynamics)
         .def("simulate", &DynamicTimeIndexedShootingProblem::Simulate)
         .def("get_Q", &DynamicTimeIndexedShootingProblem::get_Q)