Template Class SolverAbstractTpl

Inheritance Relationships

Base Type

Derived Types

Class Documentation

template<typename _Scalar>
class SolverAbstractTpl : public crocoddyl::SolverBase

Abstract class for optimal control solvers.

A solver resolves an optimal control solver of the form

\[\begin{split}\begin{eqnarray*}\begin{Bmatrix} \mathbf{x}^*_0,\cdots,\mathbf{x}^*_{T} \\ \mathbf{u}^*_0,\cdots,\mathbf{u}^*_{T-1} \end{Bmatrix} = \arg\min_{\mathbf{x}_s,\mathbf{u}_s} && l_T (\mathbf{x}_T) + \sum_{k=0}^{T-1} l_k(\mathbf{x}_t,\mathbf{u}_t) \\ \operatorname{subject}\,\operatorname{to} && \mathbf{x}_0 = \mathbf{\tilde{x}}_0\\ && \mathbf{x}_{k+1} = \mathbf{f}_k(\mathbf{x}_k,\mathbf{u}_k)\\ && \mathbf{x}_k\in\mathcal{X}, \mathbf{u}_k\in\mathcal{U} \end{eqnarray*}\end{split}\]
where \(l_T(\mathbf{x}_T)\), \(l_k(\mathbf{x}_t,\mathbf{u}_t)\) are the terminal and running cost functions, respectively, \(\mathbf{f}_k(\mathbf{x}_k,\mathbf{u}_k)\) describes evolution of the system, and state and control admissible sets are defined by \(\mathbf{x}_k\in\mathcal{X}\), \(\mathbf{u}_k\in\mathcal{U}\). An action model, defined in the shooting problem, describes each node \(k\). Inside the action model, we specialize the cost functions, the system evolution and the admissible sets.

The main routines are computeDirection() and tryStep(). The former finds a search direction and typically computes the derivatives of each action model. The latter rollout the dynamics and cost (i.e., the action) to try the search direction found by computeDirection. Both functions used the current guess defined by setCandidate(). Finally, solve() function is used to define when the search direction and length are computed in each iterate. It also describes the globalization strategy (i.e., regularization) of the numerical optimization.

See also

solve(), computeDirection(), tryStep(), stoppingCriteria()

Subclassed by crocoddyl::SolverFDDPTpl< _Scalar >, crocoddyl::SolverKKTTpl< _Scalar >, crocoddyl::SolverOdynSQPTpl< _Scalar >

Public Types

typedef ShootingProblemTpl<Scalar> ShootingProblem
typedef ShootingProblem::ActionModelAbstract ActionModelAbstract
typedef ShootingProblem::ActionDataAbstract ActionDataAbstract
typedef CallbackAbstractTpl<Scalar> CallbackAbstract
typedef MathBaseTpl<Scalar> MathBase
typedef MathBase::VectorXs VectorXs
typedef MathBase::Vector3s Vector3s

Public Functions

explicit SolverAbstractTpl(std::shared_ptr<ShootingProblem> problem)

Initialize the solver.

Parameters:

problem – [in] shooting problem

virtual ~SolverAbstractTpl() = default
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 reg_init = std::numeric_limits<Scalar>::quiet_NaN())

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) = 0

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

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 void computeCandidate(const Scalar steplength = Scalar(1.))

Compute new candidate solution using step length.

This method updates the candidate primal and dual variables by taking a step from the current solution along the computed search direction, scaled by the step length alpha. It prepares a candidate point that is evaluated before being accepted by the solver.

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\), with 1 as default)

virtual Scalar stoppingCriteria() = 0

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() = 0

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.

virtual void computeMeritFunctionImprovement()

Compute the merit function improvement for the current step.

This function computes the current merit function improvement given the current cost improvement and feasibility improvement. The feasibility improvement are stored in dfeas_.

virtual void computeExpectedMeritFunctionImprovement()

Compute the expected merit function improvement for the current step.

This function computes the expected merit function improvement given the expected cost improvement and feasibility improvement. The feasibility improvement are stored in dfeas_.

virtual void updateMeritFunction()

Update the merit function value for the current guess.

virtual bool checkAcceptance()

Check if we should accept or not the step.

Returns:

True if we should accept the step. False otherwise

virtual void calcDir()

Refresh the linearization of the optimal control problem around the current candidate.

This routine evaluates the shooting problem along the candidate trajectories (xs_, us_) to update the cost, dynamics, and constraint derivatives together with the feasibility metrics used by the solver. The main solve() loop invokes calcDir() automatically whenever a new linearization is required, but the method remains available when driving the solver manually (e.g., before calling computeDirection(false)).

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)

virtual void updateCandidate()

Update the candidate solution: cost, feasibilities, and merit value.

virtual bool decreaseRegularizationCriteria()

Criteria used to decrease regularization.

virtual bool increaseRegularizationCriteria()

Criteria used to increase regularization.

virtual void increaseRegularization()

Increase the state and control regularization values by a regfactor_ factor.

virtual void decreaseRegularization()

Decrease the state and control regularization values by a regfactor_ factor.

virtual void resizeData()

Resizing the solver data.

If the shooting problem has changed after construction, then this function resizes all the data before starting resolve the problem.

Scalar computeFeasibility(const std::vector<VectorXs> &fs)

Compute the feasibility from a given residual vector.

As in the computeDynamicFeasibility, computeInequalityFeasibility or computeEqualityFeasibility, we can compute the feasibility using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm.

Parameters:

fs – [in] Vector of residual vectors which we wish to compute the feasibility

Returns:

the residuals’ feasibility

Scalar computeDynamicFeasibility()

Compute the dynamic feasibility \(\|\mathbf{f}_{\mathbf{s}}\|_{\infty,1}\) for the current guess \((\mathbf{x}^k,\mathbf{u}^k)\).

The feasibility can be computed using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm. Note that \(\mathbf{f}_{\mathbf{s}}\) are the gaps on the dynamics, which are computed at each node as \(\mathbf{x}^{'}-\mathbf{f}(\mathbf{x},\mathbf{u})\).

Scalar computeStateFeasibility(const std::vector<VectorXs> &xs)

Compute the state box-constraint feasibility from a given state trajectory.

As in the computeDynamicFeasibility, computeInequalityFeasibility or computeEqualityFeasibility, we can compute the feasibility using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm.

Parameters:

xs – [in] Vector state trajectory feasibility

Returns:

the state trajectory’s feasibility

Scalar computeControlFeasibility(const std::vector<VectorXs> &us)

Compute the control box-constraint feasibility from a given control trajectory.

As in the computeDynamicFeasibility, computeInequalityFeasibility or computeEqualityFeasibility, we can compute the feasibility using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm.

Parameters:

us – [in] Vector control trajectory feasibility

Returns:

the control trajectory’s feasibility

Scalar computeInequalityFeasibility()

Compute the feasibility of the inequality constraints for the current guess.

The feasibility can be computed using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm.

Scalar computeEqualityFeasibility()

Compute the feasibility of the equality constraints for the current guess.

The feasibility can be computed using different norms (e.g, \(\ell_\infty\) or \(\ell_1\) norms). By default we use the \(\ell_\infty\) norm, however, we can change the type of norm using set_feasnorm.

void setCallbacks(const std::vector<std::shared_ptr<CallbackAbstract>> &callbacks)

Set a list of callback functions using for the solver diagnostic.

Each iteration, the solver calls these set of functions in order to allowed user the diagnostic of its performance.

Parameters:

callbacks – set of callback functions

const std::vector<std::shared_ptr<CallbackAbstract>> &getCallbacks() const

Return the list of callback functions using for diagnostic.

const std::shared_ptr<ShootingProblem> &get_problem() const

Return the shooting problem.

const std::vector<Scalar> &get_alphas() const

Return the set of step lengths using by the line-search procedure.

const std::vector<VectorXs> &get_xs() const

Return the state trajectory \(\mathbf{x}_s\).

const std::vector<VectorXs> &get_us() const

Return the control trajectory \(\mathbf{u}_s\).

const std::vector<VectorXs> &get_fs() const

Return the dynamic infeasibility \(\mathbf{f}_{s}\).

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

Return the linear update in \(\delta\mathbf{x}_s\).

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

Return the feedforward gains \(\delta\mathbf{u}_s\).

const std::vector<VectorXs> &get_xs_try() const

Return the trial state trajectory \(\mathbf{x}_s\).

const std::vector<VectorXs> &get_us_try() const

Return the trial control trajectory \(\mathbf{u}_s\).

const std::vector<VectorXs> &get_fs_try() const

Return the trail dynamic infeasibility \(\mathbf{f}_{s}\).

bool get_is_feasible() const

Return the feasibility status of the \((\mathbf{x}_s,\mathbf{u}_s)\) trajectory.

Scalar get_cost() const

Return the cost for the current guess.

Scalar get_merit() const

Return the merit for the current guess.

Scalar get_stop() const

Return the stopping-criteria value computed by stoppingCriteria().

const Vector3s &get_DV() const

Return the constant, linear, and quadratic terms of the expected improvement.

Scalar get_dV() const

Return the reduction in the cost function \(\Delta V\).

Scalar get_dPhi() const

Return the reduction in the merit function \(\Delta\Phi\).

Scalar get_dVexp() const

Return the expected reduction in the cost function \(\Delta V_{exp}\).

Scalar get_dPhiexp() const

Return the expected reduction in the merit function \(\Delta\Phi_{exp}\).

Scalar get_dfeas() const

Return the reduction in the feasibility.

Scalar get_feas() const

Return the total feasibility for the current guess.

Scalar get_ffeas() const

Return the dynamic feasibility for the current guess.

Scalar get_gfeas() const

Return the inequality feasibility for the current guess.

Scalar get_hfeas() const

Return the equality feasibility for the current guess.

Scalar get_ffeas_try() const

Return the dynamic feasibility for the current step length.

Scalar get_gfeas_try() const

Return the inequality feasibility for the current step length.

Scalar get_hfeas_try() const

Return the equality feasibility for the current step length.

Scalar get_preg() const

Return the primal-variable regularization.

Scalar get_dreg() const

Return the dual-variable regularization.

DEPRECATED ("Use get_preg for primal-variable regularization", Scalar get_xreg() const { return preg_;}) DEPRECATED("Use get_preg for primal-variable regularization"
inline Scalar get_ureg() const
Scalar get_reg_min() const

Return the minimum regularization value.

Scalar get_reg_max() const

Return the maximum regularization value.

Scalar get_steplength() const

Return the step length \(\alpha\).

Scalar get_th_acceptstep() const

Return the threshold used for accepting a step.

Scalar get_th_stop() const

Return the tolerance for stopping the algorithm.

DEPRECATED ("Do not use this threshold. It is not needed by our solvers", Scalar get_th_gaptol() const { return th_gaptol_;}) FeasibilityNorm get_feasnorm() const

Return the threshold for accepting a gap as non-zero.

Return the type of norm used to evaluate the dynamic and constraints feasibility

std::size_t get_iter() const

Return the number of iterations performed by the solver.

void set_alphas(const std::vector<Scalar> &alphas)

Modify the set of step lengths using by the line-search procedure.

void set_xs(const std::vector<VectorXs> &xs)

Modify the state trajectory \(\mathbf{x}_s\).

void set_us(const std::vector<VectorXs> &us)

Modify the control trajectory \(\mathbf{u}_s\).

void set_preg(const Scalar preg)

Modify the primal-variable regularization value.

void set_dreg(const Scalar dreg)

Modify the dual-variable regularization 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.

DEPRECATED ("Use set_preg for primal-variable regularization", void set_xreg(const Scalar xreg) { if(xreg< Scalar(0.)) { throw_pretty("Invalid argument: "<< "xreg value has to be positive.");} xreg_=xreg;preg_=xreg;}) DEPRECATED("Use set_preg for primal-variable regularization"
inline void set_ureg(const Scalar ureg)
void set_th_acceptstep(const Scalar th_acceptstep)

Modify the threshold used for accepting step.

void set_th_stop(const Scalar th_stop)

Modify the tolerance for stopping the algorithm.

DEPRECATED ("Do not use this threshold. It is not needed by our solvers", void set_th_gaptol(const Scalar th_gaptol);) void set_feasnorm(const FeasibilityNorm feas_norm)

Modify the threshold for accepting a gap as non-zero.

Modify the current norm used for computed the dynamic and constraint feasibility

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Functions

void allocateData()

Allocate all the basic data needed in solvers.

virtual void resizeRunningData()

Resize data associated with the running models of the shooting problem.

This function is invoked by resizeData() whenever the horizon or the running action models change. Derived classes can override it to adjust their own storage before the solver resumes.

virtual void resizeTerminalData()

Resize data associated with the terminal model of the shooting problem.

This function is invoked by resizeData() whenever the terminal model changes. Derived classes can override it to resize custom storage that depends on the terminal constraints or cost.

DEPRECATED ("Do not use this threshold. It is not needed by our solvers", Scalar th_gaptol_;) enum FeasibilityNorm feasnorm_

Type of norm used to evaluate the dynamics and constraints feasibility

DEPRECATED ("Use preg_ for primal-variable regularization", Scalar xreg_;) DEPRECATED("Use dreg_ for primal-variable regularization"

Protected Attributes

std::shared_ptr<ShootingProblem> problem_

optimal control problem

std::vector<std::shared_ptr<CallbackAbstract>> callbacks_

Callback functions.

std::vector<Scalar> alphas_

Set of step lengths using by the line-search procedure.

Scalar th_acceptstep_

Threshold used for accepting step.

Scalar th_stop_

Tolerance for stopping the algorithm.

Scalar dImpr_

Reduction in the iteration improvement (i.e., maximum between cost and merit values)

std::vector<VectorXs> xs_

State trajectory.

std::vector<VectorXs> us_

Control trajectory.

std::vector<VectorXs> fs_

Dynamics gaps in each node.

std::vector<VectorXs> xs_try_

State trajectory computed by line-search procedure.

std::vector<VectorXs> us_try_

Control trajectory computed by line-search procedure.

std::vector<VectorXs> fs_try_

Dynamics gaps in each node computed by the line-search procedure

std::vector<VectorXs> dxs_

Linear state direction (size T + 1).

std::vector<VectorXs> dus_

Linear control direction (size T).

bool is_feasible_

Label that indicates is the iteration is feasible.

bool was_feasible_

Label that indicates in the previous iterate was feasible

Scalar cost_

Cost for the current guess.

Scalar cost_try_

Total cost computed by line-search procedure.

Scalar merit_

Merit for the current guess.

Scalar stop_

Value computed by stoppingCriteria().

std::size_t iter_

Number of iteration performed by the solver.

Vector3s DV_

LQ approximation of the expected improvement.

Scalar dV_

Reduction in the cost function computed by tryStep().

Scalar dPhi_

Reduction in the merit function computed by tryStep().

Scalar dVexp_full_

Expected reduction in the cost function for a full step length

Scalar dVexp_

Expected reduction in the cost function for the selected step length

Scalar dPhiexp_

Expected reduction in the merit function for the selected step length

Scalar dfeas_

Reduction in the feasibility.

Scalar feas_

Total feasibility for the current guess.

Scalar ffeas_

Feasibility of the dynamic constraints for the current guess.

Scalar gfeas_

Feasibility of the inequality constraints for the current guess

Scalar hfeas_

Feasibility of the equality constraints for the current guess

Scalar ffeas_try_

Feasibility of the dynamic constraints evaluated for the current step length

Scalar gfeas_try_

Feasibility of the inequality constraints evaluated for the current step length

Scalar hfeas_try_

Feasibility of the equality constraints evaluated for the current step length

Scalar tmp_feas_

Temporal variables used for computed the feasibility.

Scalar preg_

Current primal-variable regularization value.

Scalar dreg_

Current dual-variable regularization value.

Scalar ureg_
Scalar reg_min_

< Current control regularization values

Minimum allowed regularization value

Scalar reg_max_

Maximum allowed regularization value.

Scalar steplength_

Current applied step length.

std::vector<VectorXs> g_adj_

Adjusted inequality bound.

std::vector<VectorXs> x_adj_

Adjusted state bound.

std::vector<VectorXs> u_adj_

Adjusted control bound.

std::size_t nh_T_

Dimension of terminal equality constraints.

std::size_t ng_T_

Dimension of termianl inequality constraints.

bool acceptstep_
bool recalcdir_