9 #ifdef CROCODDYL_WITH_MULTITHREADING 11 #endif // CROCODDYL_WITH_MULTITHREADING 13 #include "crocoddyl/core/utils/exception.hpp" 14 #include "crocoddyl/core/solvers/fddp.hpp" 19 :
SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
21 SolverFDDP::~SolverFDDP() {}
23 bool SolverFDDP::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
24 const std::size_t maxiter,
const bool is_feasible,
const double reginit) {
28 if (std::isnan(reginit)) {
37 bool recalcDiff =
true;
42 }
catch (std::exception& e) {
57 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
62 }
catch (std::exception& e) {
98 const std::size_t n_callbacks =
callbacks_.size();
99 for (std::size_t c = 0; c < n_callbacks; ++c) {
113 const std::size_t T = this->
problem_->get_T();
115 problem_->get_terminalModel()->get_state()->diff(
xs_try_.back(),
xs_.back(), dx_.back());
118 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
120 for (std::size_t t = 0; t < T; ++t) {
121 models[t]->get_state()->diff(
xs_try_[t],
xs_[t], dx_[t]);
134 const std::size_t T = this->
problem_->get_T();
140 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
141 for (std::size_t t = 0; t < T; ++t) {
142 const std::size_t nu = models[t]->get_nu();
144 dg_ +=
Qu_[t].head(nu).dot(
k_[t].head(nu));
156 if (steplength > 1. || steplength < 0.) {
157 throw_pretty(
"Invalid argument: " 158 <<
"invalid step length, value is between 0. to 1.");
162 const std::size_t T =
problem_->get_T();
163 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
164 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
166 for (std::size_t t = 0; t < T; ++t) {
167 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
168 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
169 const std::size_t nu = m->get_nu();
172 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
174 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
183 throw_pretty(
"forward_error");
185 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
186 throw_pretty(
"forward_error");
190 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
191 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
197 throw_pretty(
"forward_error");
200 for (std::size_t t = 0; t < T; ++t) {
201 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
202 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
203 const std::size_t nu = m->get_nu();
205 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
207 us_try_[t].head(nu).noalias() =
us_[t].head(nu) -
k_[t].head(nu) * steplength -
K_[t].topRows(nu) * dx_[t];
216 throw_pretty(
"forward_error");
218 if (raiseIfNaN(
xnext_.lpNorm<Eigen::Infinity>())) {
219 throw_pretty(
"forward_error");
223 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
224 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
225 m->get_state()->integrate(
xnext_,
fs_.back() * (steplength - 1),
xs_try_.back());
230 throw_pretty(
"forward_error");
238 if (0. > th_acceptnegstep) {
239 throw_pretty(
"Invalid argument: " 240 <<
"th_acceptnegstep value has to be positive.");
242 th_acceptnegstep_ = th_acceptnegstep;
double reg_max_
Maximum allowed regularization value.
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.
void updateExpectedImprovement()
Update internal values for computing the expected improvement.
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
virtual bool solve(const std::vector< Eigen::VectorXd > &init_xs=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &init_us=DEFAULT_VECTOR, const std::size_t maxiter=100, const bool is_feasible=false, const double regInit=1e-9)
Compute the optimal trajectory as lists of and terms.
std::vector< MatrixXdRowMajor > K_
Feedback gains.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
double th_stepdec_
Step-length threshold used to decrease regularization.
double dq_
Internal data for computing the expected improvement.
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
double th_acceptstep_
Threshold used for accepting step.
double reg_min_
Minimum allowed regularization value.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dV_
Cost reduction obtained by tryStep()
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Eigen::Vector2d d_
LQ approximation of the expected improvement.
Eigen::VectorXd fTVxx_p_
fTVxx_p term
void set_th_acceptnegstep(const double th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
Eigen::VectorXd xnext_
Next state.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverFDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the FDDP solver.
double dVexp_
Expected cost reduction.
double dg_
Internal data for computing the expected improvement.
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool is_feasible=false)
Set the solver candidate warm-point values .
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
std::vector< Eigen::VectorXd > xs_
State trajectory.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
double th_stepinc_
Step-length threshold used to increase regularization.
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double th_grad_
Tolerance of the expected gradient used for testing the step.
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
double th_stop_
Tolerance for stopping the algorithm.
double dv_
Internal data for computing the expected improvement.
Differential Dynamic Programming (DDP) solver.
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Abstract class for solver callbacks.
std::size_t iter_
Number of iteration performed by the solver.
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
virtual const Eigen::Vector2d & expectedImprovement()
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
double cost_try_
Total cost computed by line-search procedure.