crocoddyl  1.7.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
kkt.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft,
5 // University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_SOLVERS_KKT_HPP_
11 #define CROCODDYL_CORE_SOLVERS_KKT_HPP_
12 
13 #include <Eigen/Dense>
14 #include <Eigen/Cholesky>
15 
16 #include "crocoddyl/core/solver-base.hpp"
17 
18 namespace crocoddyl {
19 
20 class SolverKKT : public SolverAbstract {
21  public:
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  explicit SolverKKT(boost::shared_ptr<ShootingProblem> problem);
25  virtual ~SolverKKT();
26 
27  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
28  const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t maxiter = 100,
29  const bool is_feasible = false, const double regInit = 1e-9);
30  virtual void computeDirection(const bool recalc = true);
31  virtual double tryStep(const double steplength = 1);
32  virtual double stoppingCriteria();
33  virtual const Eigen::Vector2d& expectedImprovement();
34 
35  const Eigen::MatrixXd& get_kkt() const;
36  const Eigen::VectorXd& get_kktref() const;
37  const Eigen::VectorXd& get_primaldual() const;
38  const std::vector<Eigen::VectorXd>& get_dxs() const;
39  const std::vector<Eigen::VectorXd>& get_dus() const;
40  const std::vector<Eigen::VectorXd>& get_lambdas() const;
41  std::size_t get_nx() const;
42  std::size_t get_ndx() const;
43  std::size_t get_nu() const;
44 
45  protected:
46  double reg_incfactor_;
47  double reg_decfactor_;
48  double reg_min_;
49  double reg_max_;
50  double cost_try_;
51  std::vector<Eigen::VectorXd> xs_try_;
52  std::vector<Eigen::VectorXd> us_try_;
53 
54  private:
55  double calcDiff();
56  void computePrimalDual();
57  void increaseRegularization();
58  void decreaseRegularization();
59  void allocateData();
60 
61  std::size_t nx_;
62  std::size_t ndx_;
63  std::size_t nu_;
64  std::vector<Eigen::VectorXd> dxs_;
65  std::vector<Eigen::VectorXd> dus_;
66  std::vector<Eigen::VectorXd> lambdas_;
67 
68  // allocate data
69  Eigen::MatrixXd kkt_;
70  Eigen::VectorXd kktref_;
71  Eigen::VectorXd primaldual_;
72  Eigen::VectorXd primal_;
73  Eigen::VectorXd dual_;
74  std::vector<double> alphas_;
75  double th_grad_;
76  bool was_feasible_;
77  Eigen::VectorXd kkt_primal_;
78  Eigen::VectorXd dF;
79 };
80 
81 } // namespace crocoddyl
82 
83 #endif // CROCODDYL_CORE_SOLVERS_KKT_HPP_
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: kkt.cpp:129
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: kkt.cpp:111
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
Definition: kkt.cpp:152
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: kkt.cpp:85
Abstract class for optimal control solvers.
Definition: solver-base.hpp:50
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.
Definition: kkt.cpp:35