Commit 6f9ba1a7 authored by JaehyunShim's avatar JaehyunShim
Browse files

[bugfix] Resizing data in box-fddp

parent 1596ed0d
......@@ -447,7 +447,7 @@ void ShootingProblemTpl<Scalar>::set_runningModels(
<< "ndx in " << i << " node is not consistent with the other nodes")
}
}
is_updated_ = false;
is_updated_ = true;
T_ = models.size();
running_models_.clear();
running_datas_.clear();
......@@ -467,7 +467,7 @@ void ShootingProblemTpl<Scalar>::set_terminalModel(boost::shared_ptr<ActionModel
throw_pretty("Invalid argument: "
<< "ndx is not consistent with the other nodes")
}
is_updated_ = false;
is_updated_ = true;
terminal_model_ = model;
terminal_data_ = terminal_model_->createData();
}
......
......@@ -27,6 +27,7 @@ class SolverBoxFDDP : public SolverFDDP {
virtual void allocateData();
virtual void computeGains(const std::size_t t);
virtual void forwardPass(const double steplength);
virtual void resizeData();
const std::vector<Eigen::MatrixXd>& get_Quu_inv() const;
......
......@@ -31,8 +31,24 @@ SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
SolverBoxFDDP::~SolverBoxFDDP() {}
void SolverBoxFDDP::resizeData() {
START_PROFILER("SolverBoxFDDP::resizeData");
SolverFDDP::resizeData();
const std::size_t T = problem_->get_T();
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
for (std::size_t t = 0; t < T; ++t) {
const boost::shared_ptr<ActionModelAbstract>& model = models[t];
const std::size_t nu = model->get_nu();
Quu_inv_[t].conservativeResize(nu, nu);
du_lb_[t].conservativeResize(nu);
du_ub_[t].conservativeResize(nu);
}
STOP_PROFILER("SolverBoxFDDP::resizeData");
}
void SolverBoxFDDP::allocateData() {
SolverDDP::allocateData();
SolverFDDP::allocateData();
const std::size_t T = problem_->get_T();
Quu_inv_.resize(T);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment