10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/solvers/box-ddp.hpp" 15 SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
16 : SolverDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
19 const std::size_t& n_alphas = 10;
20 alphas_.resize(n_alphas);
21 for (std::size_t n = 0; n < n_alphas; ++n) {
22 alphas_[n] = 1. / pow(2., static_cast<double>(n));
31 SolverBoxDDP::~SolverBoxDDP() {}
36 const std::size_t& T =
problem_->get_T();
38 const std::size_t& nu =
problem_->get_nu_max();
39 for (std::size_t t = 0; t < T; ++t) {
40 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
47 if (
problem_->get_runningModels()[t]->get_nu() > 0) {
54 du_lb_ =
problem_->get_runningModels()[t]->get_u_lb() -
us_[t];
55 du_ub_ =
problem_->get_runningModels()[t]->get_u_ub() -
us_[t];
57 const BoxQPSolution& boxqp_sol = qp_.solve(Quu_[t], Qu_[t], du_lb_, du_ub_, k_[t]);
60 Quu_inv_[t].setZero();
61 for (std::size_t i = 0; i < boxqp_sol.
free_idx.size(); ++i) {
62 for (std::size_t j = 0; j < boxqp_sol.
free_idx.size(); ++j) {
66 K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
67 k_[t].noalias() = -boxqp_sol.
x;
71 for (std::size_t i = 0; i < boxqp_sol.
clamped_idx.size(); ++i) {
78 if (steplength > 1. || steplength < 0.) {
79 throw_pretty(
"Invalid argument: " 80 <<
"invalid step length, value is between 0. to 1.");
84 const std::size_t& T =
problem_->get_T();
85 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
86 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
87 for (std::size_t t = 0; t < T; ++t) {
88 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
89 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
92 m->get_state()->diff(
xs_[t], xs_try_[t], dx_[t]);
93 if (m->get_nu() != 0) {
94 us_try_[t].noalias() =
us_[t] - k_[t] * steplength - K_[t] * dx_[t];
95 if (m->get_has_control_limits()) {
96 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
98 m->calc(d, xs_try_[t], us_try_[t]);
100 m->calc(d, xs_try_[t]);
103 cost_try_ += d->cost;
105 if (raiseIfNaN(cost_try_)) {
106 throw_pretty(
"forward_error");
108 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
109 throw_pretty(
"forward_error");
113 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
114 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
116 xs_try_.back() = xnext_;
118 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
120 m->calc(d, xs_try_.back());
121 cost_try_ += d->cost;
123 if (raiseIfNaN(cost_try_)) {
124 throw_pretty(
"forward_error");
128 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 allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > us_
Control trajectory.
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.
double th_stop_
Tolerance for stopping the algorithm.
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
std::vector< size_t > clamped_idx
Clamped space indexes.
virtual void forwardPass(const double &steplength)
Run the forward pass or rollout.
std::vector< size_t > free_idx
Free space indexes.
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.