Skip to content
Permalink
Browse files

[exotica_core] DynamicTimeIndexedShootingProblem: Expose number of co…

…ntrols as NU to Python, check dimension in Update
  • Loading branch information...
wxmerkt committed Mar 12, 2019
1 parent d4e4ad5 commit 2a1162ed8484f1470476b63edbde9025ebe90391
@@ -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;

@@ -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
@@ -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)

0 comments on commit 2a1162e

Please sign in to comment.
You can’t perform that action at this time.