11 #include "crocoddyl/core/solvers/box-ddp.hpp" 12 #include "crocoddyl/core/utils/exception.hpp" 16 SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
17 : SolverDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20 const std::size_t n_alphas = 10;
21 alphas_.resize(n_alphas);
22 for (std::size_t n = 0; n < n_alphas; ++n) {
23 alphas_[n] = 1. / pow(2., static_cast<double>(n));
32 SolverBoxDDP::~SolverBoxDDP() {}
35 START_PROFILER(
"SolverBoxDDP::resizeData");
38 const std::size_t T =
problem_->get_T();
39 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
40 for (std::size_t t = 0; t < T; ++t) {
41 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
42 const std::size_t nu = model->get_nu();
43 Quu_inv_[t].conservativeResize(nu, nu);
44 du_lb_[t].conservativeResize(nu);
45 du_ub_[t].conservativeResize(nu);
47 STOP_PROFILER(
"SolverBoxDDP::resizeData");
53 const std::size_t T =
problem_->get_T();
57 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
58 for (std::size_t t = 0; t < T; ++t) {
59 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
60 const std::size_t nu = model->get_nu();
61 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
62 du_lb_[t] = Eigen::VectorXd::Zero(nu);
63 du_ub_[t] = Eigen::VectorXd::Zero(nu);
68 START_PROFILER(
"SolverBoxDDP::computeGains");
69 const std::size_t nu =
problem_->get_runningModels()[t]->get_nu();
77 du_lb_[t] =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t];
78 du_ub_[t] =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t];
80 START_PROFILER(
"SolverBoxDDP::boxQP");
81 const BoxQPSolution& boxqp_sol = qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
82 START_PROFILER(
"SolverBoxDDP::boxQP");
85 START_PROFILER(
"SolverBoxDDP::Quu_invproj");
86 Quu_inv_[t].setZero();
87 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
88 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
92 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj");
93 START_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
94 K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
95 STOP_PROFILER(
"SolverBoxDDP::Quu_invproj_Qxu");
100 START_PROFILER(
"SolverBoxDDP::Qu_proj");
101 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
104 STOP_PROFILER(
"SolverBoxDDP::Qu_proj");
106 STOP_PROFILER(
"SolverBoxDDP::computeGains");
110 if (steplength > 1. || steplength < 0.) {
111 throw_pretty(
"Invalid argument: " 112 <<
"invalid step length, value is between 0. to 1.");
114 START_PROFILER(
"SolverBoxDDP::forwardPass");
117 const std::size_t T =
problem_->get_T();
118 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
119 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
120 for (std::size_t t = 0; t < T; ++t) {
121 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
122 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
123 const std::size_t nu = m->get_nu();
126 m->get_state()->diff(
xs_[t], xs_try_[t], dx_[t]);
128 us_try_[t].noalias() =
us_[t] - k_[t] * steplength - K_[t] * dx_[t];
129 if (m->get_has_control_limits()) {
130 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
132 m->calc(d, xs_try_[t], us_try_[t]);
134 m->calc(d, xs_try_[t]);
137 cost_try_ += d->cost;
139 if (raiseIfNaN(cost_try_)) {
140 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
141 throw_pretty(
"forward_error");
143 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
144 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
145 throw_pretty(
"forward_error");
149 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
150 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
152 xs_try_.back() = xnext_;
154 m->get_state()->integrate(xnext_,
fs_.back() * (steplength - 1), xs_try_.back());
156 m->calc(d, xs_try_.back());
157 cost_try_ += d->cost;
159 if (raiseIfNaN(cost_try_)) {
160 STOP_PROFILER(
"SolverBoxDDP::forwardPass");
161 throw_pretty(
"forward_error");
165 const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv()
const {
return Quu_inv_; }
virtual void allocateData()
Allocate all the internal data needed for the solver.
bool is_feasible_
Label that indicates is the iteration is feasible.
virtual void forwardPass(const double steplength)
Run the forward pass or rollout.
virtual void allocateData()
Allocate all the internal data needed for the solver.
virtual void resizeData()
Resizing the solver data.
std::vector< Eigen::VectorXd > us_
Control trajectory.
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
Eigen::VectorXd x
Decision vector.
std::vector< Eigen::VectorXd > xs_
State trajectory.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
double th_stop_
Tolerance for stopping the algorithm.
std::vector< size_t > clamped_idx
Clamped space indexes.
virtual void resizeData()
Resizing the solver data.
std::vector< size_t > free_idx
Free space indexes.