11 #include "crocoddyl/core/solvers/box-fddp.hpp" 12 #include "crocoddyl/core/utils/exception.hpp" 16 SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
17 : SolverFDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20 const std::size_t n_alphas = 10;
22 for (std::size_t n = 0; n < n_alphas; ++n) {
23 alphas_[n] = 1. / pow(2., static_cast<double>(n));
32 SolverBoxFDDP::~SolverBoxFDDP() {}
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();
91 for (std::size_t t = 0; t < T; ++t) {
92 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
93 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
94 const std::size_t nu = m->get_nu();
97 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
99 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
100 if (m->get_has_control_limits()) {
101 us_try_[t].head(nu) =
us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
111 throw_pretty(
"forward_error");
113 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
114 throw_pretty(
"forward_error");
118 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
119 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
125 throw_pretty(
"forward_error");
128 for (std::size_t t = 0; t < T; ++t) {
129 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
130 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
131 const std::size_t nu = m->get_nu();
133 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
135 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
136 if (m->get_has_control_limits()) {
137 us_try_[t].head(nu) =
us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
147 throw_pretty(
"forward_error");
149 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
150 throw_pretty(
"forward_error");
154 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
155 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
156 m->get_state()->integrate(
xnext_,
fs_.back() * (steplength - 1),
xs_try_.back());
161 throw_pretty(
"forward_error");
166 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv()
const {
return Quu_inv_; }
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
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 computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
std::vector< MatrixXdRowMajor > K_
Feedback gains.
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
const BoxQPSolution & solve(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::VectorXd &xinit)
Compute the solution of bound-constrained QP based on Newton projection.
std::vector< Eigen::VectorXd > us_
Control trajectory.
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Eigen::VectorXd xnext_
Next state.
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.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
double th_stop_
Tolerance for stopping the algorithm.
std::vector< size_t > clamped_idx
Clamped space indexes.
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
std::vector< size_t > free_idx
Free space indexes.
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
double cost_try_
Total cost computed by line-search procedure.