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() {}
37 const std::size_t T =
problem_->get_T();
39 const std::size_t nu =
problem_->get_nu_max();
40 for (std::size_t t = 0; t < T; ++t) {
41 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
48 const std::size_t nu =
problem_->get_runningModels()[t]->get_nu();
56 du_lb_.head(nu) =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t].head(nu);
57 du_ub_.head(nu) =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t].head(nu);
60 qp_.solve(Quu_[t].topLeftCorner(nu, nu), Qu_[t].head(nu), du_lb_.head(nu), du_ub_.head(nu), k_[t].head(nu));
63 Quu_inv_[t].topLeftCorner(nu, nu).setZero();
64 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
65 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
69 K_[t].topRows(nu).noalias() = Quu_inv_[t].topLeftCorner(nu, nu) * Qxu_[t].leftCols(nu).transpose();
70 k_[t].topRows(nu) = -boxqp_sol.
x;
74 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
81 if (steplength > 1. || steplength < 0.) {
82 throw_pretty(
"Invalid argument: " 83 <<
"invalid step length, value is between 0. to 1.");
87 const std::size_t T =
problem_->get_T();
88 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
89 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
90 for (std::size_t t = 0; t < T; ++t) {
91 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
92 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
93 const std::size_t nu = m->get_nu();
96 m->get_state()->diff(
xs_[t], xs_try_[t], dx_[t]);
98 us_try_[t].head(nu).noalias() =
us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
99 if (m->get_has_control_limits()) {
100 us_try_[t].head(nu) = us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
102 m->calc(d, xs_try_[t], us_try_[t].head(nu));
104 m->calc(d, xs_try_[t]);
107 cost_try_ += d->cost;
109 if (raiseIfNaN(cost_try_)) {
110 throw_pretty(
"forward_error");
112 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
113 throw_pretty(
"forward_error");
117 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
118 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
120 xs_try_.back() = xnext_;
122 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
124 m->calc(d, xs_try_.back());
125 cost_try_ += d->cost;
127 if (raiseIfNaN(cost_try_)) {
128 throw_pretty(
"forward_error");
132 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.
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.
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.
std::vector< size_t > free_idx
Free space indexes.