crocoddyl
1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
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 th_grad_
Tolerance of the expected gradient used for testing the step.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Eigen::VectorXd fTVxx_p_
fTVxx_p term
double th_acceptstep_
Threshold used for accepting step.
double th_stepinc_
Step-length threshold used to increase regularization.
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.
Abstract class for solver callbacks.
void updateExpectedImprovement()
Update internal values for computing the expected improvement.
double dq_
Internal data for computing the expected improvement.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
bool is_feasible_
Label that indicates is the iteration is feasible.
Eigen::Vector2d d_
LQ approximation of the expected improvement.
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
std::size_t iter_
Number of iteration performed by the solver.
double ureg_
Current control regularization values.
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
double steplength_
Current applied step-length.
double cost_try_
Total cost computed by line-search procedure.
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 .
double th_stop_
Tolerance for stopping the algorithm.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverFDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the FDDP solver.
double reg_max_
Maximum allowed regularization value.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
void set_th_acceptnegstep(const double th_acceptnegstep)
Modify the threshold used for accepting step along ascent direction.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
double get_th_acceptnegstep() const
Return the threshold used for accepting step along ascent direction.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
std::vector< MatrixXdRowMajor > K_
Feedback gains.
Differential Dynamic Programming (DDP) solver.
double xreg_
Current state regularization value.
double dg_
Internal data for computing the expected improvement.
double dVexp_
Expected cost reduction.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dv_
Internal data for computing the expected improvement.
std::vector< Eigen::VectorXd > xs_
State trajectory.
double reg_min_
Minimum allowed regularization value.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
double dV_
Cost reduction obtained by tryStep()
Eigen::VectorXd xnext_
Next state.
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
double th_stepdec_
Step-length threshold used to decrease regularization.
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
std::vector< Eigen::VectorXd > k_
Feed-forward terms.