Template Class SolverKKTTpl

virtual bool solve(const std::vector<VectorXs> &init_xs = DefaultVector<Scalar>::value, const std::vector<VectorXs> &init_us = DefaultVector<Scalar>::value, const std::size_t maxiter = 100, const bool is_feasible = false, const Scalar regInit = 1e-9) override

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:

init_xs – [in] initial guess for state trajectory with \(T+1\) elements (default [])
init_us – [in] initial guess for control trajectory with \(T\) elements (default [])
maxiter – [in] maximum allowed number of iterations (default 100)
is_feasible – [in] true if the init_xs are obtained from integrating the init_us (rollout) (default false)
init_reg – [in] initial 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.

virtual void computeDirection(const bool recalc = true) override

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. When recalc is true (the default in the provided algorithms), the method refreshes the linearization by calling calcDir() before running the backward pass. Setting recalc to false reuses the most recent derivatives, which is useful when calcDir() has already been triggered explicitly. The resulting state and control variations, together with the dynamics multipliers, are stored in the solver data structures (e.g., dxs_, dus_, and lambdas_).

Parameters:: recalc – [in] true to refresh the derivatives before computing the search direction

virtual Scalar tryStep(const Scalar steplength = Scalar(1.)) override

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

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\). Internally, it updates the cost improvement \(dV\) and merit improvement \(d\Phi\).

Parameters:: steplength – [in] applied step length ( \(0\leq\alpha\leq1\), with 1 as default)
Returns:: the cost improvement

virtual Scalar stoppingCriteria() override

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().

virtual Vector3s expectedImprovement() override

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(). The quadratic improvement model is described as dV = DV_0 + DV_1 + 0.5 * DV_2, where DV_0, DV_1, and DV_2 are the constant, linear, and quadratic terms, respectively.

template<typename NewScalar> SolverKKTTpl<NewScalar> cast() const

Cast the KKT solver to a different scalar type.

It is useful for operations requiring different precision or scalar types.

Template Parameters:: NewScalar – The new scalar type to cast to.
Returns:: SolverKKTTpl<NewScalar> A KKT solver with the new scalar type.

const MatrixXs &get_kkt() const

const VectorXs &get_kktref() const

const VectorXs &get_primaldual() const

const std::vector<VectorXs> &get_dxs() const

const std::vector<VectorXs> &get_dus() const

const std::vector<VectorXs> &get_lambdas() const

std::size_t get_nx() const

std::size_t get_ndx() const

std::size_t get_nu() const

void set_alphas(const std::vector<Scalar> &alphas): Modify the set of step lengths using by the line-search procedure.

void set_reg_incfactor(const Scalar reg_factor): Modify the regularization factor used to increase the damping value.

void set_reg_decfactor(const Scalar reg_factor): Modify the regularization factor used to decrease the damping value.

void set_reg_min(const Scalar regmin): Modify the minimum regularization value.

void set_reg_max(const Scalar regmax): Modify the maximum regularization value.

void set_th_grad(const Scalar th_grad): Modify the tolerance of the expected gradient used for testing the step.

virtual void setCandidate(const std::vector<VectorXs> &xs_warm = DefaultVector<Scalar>::value, const std::vector<VectorXs> &us_warm = DefaultVector<Scalar>::value, const bool is_feasible = false)

Set the solver candidate trajectories \((\mathbf{x}_s,\mathbf{u}_s)\).

The solver candidates are defined as a state and control trajectories \((\mathbf{x}_s,\mathbf{u}_s)\) of \(T+1\) and \(T\) elements, respectively. Additionally, we need to define the dynamic feasibility of the \((\mathbf{x}_s,\mathbf{u}_s)\) pair. Note that the trajectories are feasible if \(\mathbf{x}_s\) is the resulting trajectory from the system rollout with \(\mathbf{u}_s\) inputs. Updating the candidate invalidates any previously computed linearization; the next call to computeDirection() will therefore refresh the derivatives on demand.

Parameters:

xs – [in] state trajectory of \(T+1\) elements (default [])
us – [in] control trajectory of \(T\) elements (default [])
isFeasible – [in] true if the xs are obtained from integrating the us (rollout)

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Functions

void allocateData()

Protected Attributes

Scalar reg_incfactor_

Scalar reg_decfactor_

Scalar reg_min_

Scalar reg_max_

std::vector<Scalar> alphas_

Scalar th_grad_

bool was_feasible_

std::vector<std::shared_ptr<CallbackAbstract>> callbacks_: Callback functions.

Scalar cost_: Cost for the current guess.

Scalar cost_try_: Total cost computed by line-search procedure.

Scalar dreg_: Current dual-variable regularization value.

Vector3s DV_: LQ approximation of the expected improvement.

Scalar dV_: Reduction in the cost function computed by tryStep().

Scalar dVexp_: Expected reduction in the cost function for the selected step length

bool is_feasible_: Label that indicates is the iteration is feasible.

std::size_t iter_: Number of iteration performed by the solver.

Scalar preg_: Current primal-variable regularization value.

std::shared_ptr<ShootingProblem> problem_: optimal control problem

Scalar steplength_: Current applied step length.

Scalar stop_: Value computed by stoppingCriteria().

Scalar th_acceptstep_: Threshold used for accepting step.

Scalar th_stop_: Tolerance for stopping the algorithm.

std::vector<VectorXs> us_: Control trajectory.

std::vector<VectorXs> us_try_: Control trajectory computed by line-search procedure.

std::vector<VectorXs> xs_: State trajectory.

std::vector<VectorXs> xs_try_: State trajectory computed by line-search procedure.