Crocoddyl
Public Member Functions | Protected Attributes | List of all members
SolverKKT Class Reference
Inheritance diagram for SolverKKT:
SolverAbstract

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverKKT (boost::shared_ptr< ShootingProblem > problem)
 
virtual void computeDirection (const bool recalc=true)
 Compute the search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\) for the current guess \((\mathbf{x}^k_s,\mathbf{u}^k_s)\). More...
 
virtual const Eigen::Vector2d & expectedImprovement ()
 Return the expected improvement \(dV_{exp}\) from a given current search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\). More...
 
const std::vector< Eigen::VectorXd > & get_dus () const
 
const std::vector< Eigen::VectorXd > & get_dxs () const
 
const Eigen::MatrixXd & get_kkt () const
 
const Eigen::VectorXd & get_kktref () const
 
const std::vector< Eigen::VectorXd > & get_lambdas () const
 
std::size_t get_ndx () const
 
std::size_t get_nu () const
 
std::size_t get_nx () const
 
const Eigen::VectorXd & get_primaldual () const
 
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 \(\mathbf{x}^*_s,\mathbf{u}^*_s\) as lists of \(T+1\) and \(T\) terms. More...
 
virtual double stoppingCriteria ()
 Return a positive value that quantifies the algorithm termination. More...
 
virtual double tryStep (const double steplength=1)
 Try a predefined step length \(\alpha\) and compute its cost improvement \(dV\). More...
 
- Public Member Functions inherited from SolverAbstract
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract (boost::shared_ptr< ShootingProblem > problem)
 Initialize the solver. More...
 
double computeDynamicFeasibility ()
 Compute the dynamic feasibility \(\|\mathbf{f}_{\mathbf{s}}\|_{\infty,1}\) for the current guess \((\mathbf{x}^k,\mathbf{u}^k)\). More...
 
double computeEqualityFeasibility ()
 Compute the feasibility of the equality constraints for the current guess. More...
 
double computeInequalityFeasibility ()
 Compute the feasibility of the inequality constraints for the current guess. More...
 
 DEPRECATED ("Use get_preg for primal-variable regularization", double get_xreg() const ;) DEPRECATED("Use get_preg for primal-variable regularization"
 
 DEPRECATED ("Use set_preg for primal-variable regularization", void set_xreg(const double xreg);) DEPRECATED("Use set_preg for primal-variable regularization"
 
double get_cost () const
 Return the cost for the current guess.
 
const Eigen::Vector2d & get_d () const
 Return the linear and quadratic terms of the expected improvement.
 
double get_dfeas () const
 Return the reduction in the feasibility.
 
double get_dPhi () const
 Return the reduction in the merit function \(\Delta\Phi\).
 
double get_dPhiexp () const
 Return the expected reduction in the merit function \(\Delta\Phi_{exp}\).
 
double get_dreg () const
 Return the dual-variable regularization.
 
double get_dV () const
 Return the reduction in the cost function \(\Delta V\).
 
double get_dVexp () const
 Return the expected reduction in the cost function \(\Delta V_{exp}\).
 
double get_feas () const
 Return the total feasibility for the current guess.
 
FeasibilityNorm get_feasnorm () const
 Return the type of norm used to evaluate the dynamic and constraints feasibility.
 
double get_ffeas () const
 Return the dynamic feasibility for the current guess.
 
double get_ffeas_try () const
 Return the dynamic feasibility for the current step length.
 
const std::vector< Eigen::VectorXd > & get_fs () const
 Return the dynamic infeasibility \(\mathbf{f}_{s}\).
 
double get_gfeas () const
 Return the inequality feasibility for the current guess.
 
double get_gfeas_try () const
 Return the inequality feasibility for the current step length.
 
double get_hfeas () const
 Return the equality feasibility for the current guess.
 
double get_hfeas_try () const
 Return the equality feasibility for the current step length.
 
bool get_is_feasible () const
 Return the feasibility status of the \((\mathbf{x}_s,\mathbf{u}_s)\) trajectory.
 
std::size_t get_iter () const
 Return the number of iterations performed by the solver.
 
double get_merit () const
 Return the merit for the current guess.
 
double get_preg () const
 Return the primal-variable regularization.
 
const boost::shared_ptr< ShootingProblem > & get_problem () const
 Return the shooting problem.
 
double get_steplength () const
 Return the step length \(\alpha\).
 
double get_stop () const
 Return the stopping-criteria value computed by stoppingCriteria()
 
double get_th_acceptstep () const
 Return the threshold used for accepting a step.
 
double get_th_gaptol () const
 Return the threshold for accepting a gap as non-zero.
 
double get_th_stop () const
 Return the tolerance for stopping the algorithm.
 
double get_ureg () const
 
const std::vector< Eigen::VectorXd > & get_us () const
 Return the control trajectory \(\mathbf{u}_s\).
 
const std::vector< Eigen::VectorXd > & get_xs () const
 Return the state trajectory \(\mathbf{x}_s\).
 
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks () const
 Return the list of callback functions using for diagnostic.
 
virtual void resizeData ()
 Resizing the solver data. More...
 
void set_dreg (const double dreg)
 Modify the dual-variable regularization value.
 
void set_feasnorm (const FeasibilityNorm feas_norm)
 Modify the current norm used for computed the dynamic and constraint feasibility.
 
void set_preg (const double preg)
 Modify the primal-variable regularization value.
 
void set_th_acceptstep (const double th_acceptstep)
 Modify the threshold used for accepting step.
 
void set_th_gaptol (const double th_gaptol)
 Modify the threshold for accepting a gap as non-zero.
 
void set_th_stop (const double th_stop)
 Modify the tolerance for stopping the algorithm.
 
void set_ureg (const double ureg)
 
void set_us (const std::vector< Eigen::VectorXd > &us)
 Modify the control trajectory \(\mathbf{u}_s\).
 
void set_xs (const std::vector< Eigen::VectorXd > &xs)
 Modify the state trajectory \(\mathbf{x}_s\).
 
void setCallbacks (const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
 Set a list of callback functions using for the solver diagnostic. More...
 
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 trajectories \((\mathbf{x}_s,\mathbf{u}_s)\). More...
 

Protected Attributes

double cost_try_
 
double reg_decfactor_
 
double reg_incfactor_
 
double reg_max_
 
double reg_min_
 
std::vector< Eigen::VectorXd > us_try_
 
std::vector< Eigen::VectorXd > xs_try_
 
- Protected Attributes inherited from SolverAbstract
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
 Callback functions.
 
double cost_
 Cost for the current guess.
 
Eigen::Vector2d d_
 LQ approximation of the expected improvement.
 
double dfeas_
 Reduction in the feasibility.
 
double dPhi_
 Reduction in the merit function computed by tryStep()
 
double dPhiexp_
 Expected reduction in the merit function.
 
double dreg_
 Current dual-variable regularization value.
 
double dV_
 Reduction in the cost function computed by tryStep()
 
double dVexp_
 Expected reduction in the cost function.
 
double feas_
 Total feasibility for the current guess.
 
enum FeasibilityNorm feasnorm_
 
double ffeas_
 Feasibility of the dynamic constraints for the current guess.
 
double ffeas_try_
 
std::vector< Eigen::VectorXd > fs_
 Gaps/defects between shooting nodes.
 
double gfeas_
 
double gfeas_try_
 
double hfeas_
 
double hfeas_try_
 
bool is_feasible_
 Label that indicates is the iteration is feasible.
 
std::size_t iter_
 Number of iteration performed by the solver.
 
double merit_
 Merit for the current guess.
 
double preg_
 Current primal-variable regularization value.
 
boost::shared_ptr< ShootingProblem > problem_
 optimal control problem
 
double steplength_
 < Current control regularization values More...
 
double stop_
 Value computed by stoppingCriteria()
 
double th_acceptstep_
 Threshold used for accepting step.
 
double th_gaptol_
 Threshold limit to check non-zero gaps.
 
double th_stop_
 Tolerance for stopping the algorithm.
 
double tmp_feas_
 Temporal variables used for computed the feasibility.
 
double ureg_
 
std::vector< Eigen::VectorXd > us_
 Control trajectory.
 
bool was_feasible_
 
std::vector< Eigen::VectorXd > xs_
 State trajectory.
 

Additional Inherited Members

- Protected Member Functions inherited from SolverAbstract
 DEPRECATED ("Use preg_ for primal-variable regularization", double xreg_;) DEPRECATED("Use dreg_ for primal-variable regularization"
 

Detailed Description

Definition at line 21 of file kkt.hpp.

Member Function Documentation

◆ solve()

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  reg_init = 1e-9 
)
virtual

Compute the optimal trajectory \(\mathbf{x}^*_s,\mathbf{u}^*_s\) as lists of \(T+1\) and \(T\) terms.

From an initial guess init_xs, init_us (feasible or not), iterate over computeDirection() and tryStep() until stoppingCriteria() is below threshold. It also describes the globalization strategy used during the numerical optimization.

Parameters
[in]init_xsinitial guess for state trajectory with \(T+1\) elements (default [])
[in]init_usinitial guess for control trajectory with \(T\) elements (default [])
[in]maxitermaximum allowed number of iterations (default 100)
[in]is_feasibletrue if the init_xs are obtained from integrating the init_us (rollout) (default false)
[in]init_reginitial guess for the regularization value. Very low values are typical used with very good guess points (default 1e-9).
Returns
A boolean that describes if convergence was reached.

Implements SolverAbstract.

Definition at line 36 of file kkt.cpp.

◆ computeDirection()

void computeDirection ( const bool  recalc = true)
virtual

Compute the search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\) for the current guess \((\mathbf{x}^k_s,\mathbf{u}^k_s)\).

You must call setCandidate() first in order to define the current guess. A current guess defines a state and control trajectory \((\mathbf{x}^k_s,\mathbf{u}^k_s)\) of \(T+1\) and \(T\) elements, respectively.

Parameters
[in]recalctrue for recalculating the derivatives at current state and control
Returns
The search direction \((\delta\mathbf{x},\delta\mathbf{u})\) and the dual lambdas as lists of \(T+1\), \(T\) and \(T+1\) lengths, respectively

Implements SolverAbstract.

Definition at line 89 of file kkt.cpp.

◆ tryStep()

double tryStep ( const double  steplength = 1)
virtual

Try a predefined step length \(\alpha\) and compute its cost improvement \(dV\).

It uses the search direction found by computeDirection() to try a determined step length \(\alpha\). Therefore, it assumes that we have run computeDirection() first. Additionally, it returns the cost improvement \(dV\) along the predefined step length \(\alpha\).

Parameters
[in]steplengthapplied step length ( \(0\leq\alpha\leq1\))
Returns
the cost improvement

Implements SolverAbstract.

Definition at line 119 of file kkt.cpp.

◆ stoppingCriteria()

double stoppingCriteria ( )
virtual

Return a positive value that quantifies the algorithm termination.

These values typically represents the gradient norm which tell us that it's been reached the local minima. The stopping criteria strictly speaking depends on the search direction (calculated by computeDirection()) but it could also depend on the chosen step length, tested by tryStep().

Implements SolverAbstract.

Definition at line 139 of file kkt.cpp.

◆ expectedImprovement()

const Eigen::Vector2d & expectedImprovement ( )
virtual

Return the expected improvement \(dV_{exp}\) from a given current search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\).

For computing the expected improvement, you need to compute the search direction first via computeDirection().

Implements SolverAbstract.

Definition at line 166 of file kkt.cpp.

The documentation for this class was generated from the following files: