Template Class SolverOdynSQPTpl

Inheritance Relationships

Base Type

Class Documentation

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

Odyn-based Sequential Quadratic Programming (SQP) solver.

This solver wraps Odyn’s sparse QP engine to solve Crocoddyl shooting problems with equality and inequality constraints. At each iteration it builds a sparse QP in computeQuadraticModel(), solves it with Odyn, and then maps the QP step back to state/control updates in computeSearchDirection() / tryStep(). The try step supports infeasible iterates (open dynamics gaps) and line-searches them, which improves globalization on hard-constrained problems. The expected improvement calculation also accounts for the dynamics gaps.

See SolverAbstract(), computeSearchDirection(), tryStep(), and expectedImprovement() for the main iteration steps.

Public Types

typedef SolverAbstractTpl<Scalar> SolverAbstract
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
typedef MathBase::MatrixXs MatrixXs
typedef MathBase::MatrixXsRowMajor MatrixXsRowMajor
using Solver = typename odyn::SparseQPTpl<Scalar>
using Model = typename odyn::SparseModelTpl<Scalar>
using Data = typename odyn::SparseDataTpl<Scalar>
using Params = typename odyn::ParamsTpl<Scalar>

Public Functions

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

Initialize the OdynSQP solver.

Parameters:

  • problem – [in] Shooting problem

  • dyn_solver – [in] Type of dynamic solver

  • term_solver – [in] Type of terminal solver

virtual ~SolverOdynSQPTpl() = default
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)\).

virtual Scalar stoppingCriteria() override

Return a positive value that quantifies the algorithm termination.

virtual Vector3s expectedImprovement() override

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

virtual void computeMeritFunctionImprovement() override

Compute the merit function improvement for the current step.

virtual void computeExpectedMeritFunctionImprovement() override

Compute the expected merit function improvement for the current step.

virtual void updateMeritFunction() override

Update the merit function value for the current guess.

virtual bool checkAcceptance() override

Check if we should accept or not the step.

Returns:

True if we should accept the step. False otherwise

virtual void computeCandidate(const Scalar step_length = Scalar(1.)) override

Compute new candidate solution using step length.

void computeQuadraticModel()

Build the local Quadratic Program (QP) for the current iterate.

The QP has the form

\[ \min_{x}\ \tfrac{1}{2}\, x^\top Q\, x \;+\; c^\top x \quad\text{s.t.}\quad A\,x = b,\;\; G\,x \le h . \]
Here, x is the full decision vector over the trajectory and controls, and Q, c, A, b, G, h are the corresponding (sparse, structured) cost and constraint matrices/vectors induced by the problem’s temporal layout.

Decision vector ordering:

\[ x = [\, \Delta x_0,\ \Delta u_0,\ \Delta x_1,\ \Delta u_1,\ \ldots,\ \Delta x_T \,]. \]

The sparsity pattern reflects time-coupling (dynamics and local costs/constraints).

Template Parameters:

Scalar – Scalar type (e.g., float, double).

virtual void updateCandidate() override

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

virtual bool decreaseRegularizationCriteria() override

Criteria used to decrease regularization.

virtual bool increaseRegularizationCriteria() override

Criteria used to increase regularization.

virtual void increaseRegularization() override

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

virtual void decreaseRegularization() override

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

void extractQpDirection(const VectorXs &x)

Extract the QP direction into the solver data structures.

Parameters:

x – [in] decision vector from the QP solver

template<typename NewScalar>
SolverOdynSQPTpl<NewScalar> cast() const

Cast the OdynSQP 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:

SolverOdynSQPTpl<NewScalar> A OdynSQP solver with the new scalar type.

Model &qp_model() noexcept

Return the QP model used in the OdynSQP solver.

Data &qp_data() noexcept

Return the QP data used in the OdynSQP solver.

std::size_t get_n() const noexcept

Return the number of decision variables in the SQP.

std::size_t get_m() const noexcept

Return the number of equality constraints in the SQP.

std::size_t get_p() const noexcept

Return the number of inequality constraints in the SQP.

Params &qp_params() noexcept

Return the QP params used in the OdynSQP solver.

odyn::VerboseLevel get_verbose_level() const noexcept

Return the verbose level used in the OdynSQP solver.

Scalar get_reg_incfactor() const

Return the regularization factor used to increase the damping value.

Scalar get_reg_decfactor() const

Return the regularization factor used to decrease the damping value.

Scalar get_th_grad() const

Return the tolerance of the expected gradient used for testing the step.

Scalar get_th_stepdec() const

Return the step-length threshold used to decrease regularization.

Scalar get_th_stepinc() const

Return the step-length threshold used to increase regularization.

Scalar get_th_minimprove() const

Return the minimum improvement threshold used to increase regularization.

Scalar get_th_acceptnegstep() const

Return the threshold used for accepting step along ascent direction.

Scalar get_th_acceptminstep() const

Return the threshold used for accepting minimum steps.

Scalar get_rho() const

Return the rho parameter used in the merit function.

Scalar get_th_minfeas() const

Return the threshold for switching to feasibility.

Scalar get_upsilon() const

Return the estimated penalty parameter that balances relative contribution of the cost function and equality constraints.

Scalar get_upsilon_decfactor() const

Return the upsilon decresing factor used to estimate to balance optimality and feasibility.

bool get_zero_upsilon() const

Return the zero-upsilon label.

True if we set the estimated penalty parameter (upsilon) to zero when solve is called.

void set_qp_params(const Params &params)

Modify the QP params used in the OdynSQP solver.

void set_verbose_level(const odyn::VerboseLevel verbose_level)

Modify the verbose level used in the OdynSQP solver.

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_th_grad(const Scalar th_grad)

Modify the tolerance of the expected gradient used for testing the step.

void set_th_noimprovement(const Scalar th_noimprovement)

Modify the threshold used to accept steps that cannot be be improved due to numerical errors the th noimprovement object.

void set_th_stepdec(const Scalar th_step)

Modify the step-length threshold used to decrease regularization.

void set_th_stepinc(const Scalar th_step)

Modify the step-length threshold used to increase regularization.

void set_th_minimprove(const Scalar th_step)

Modify the minimum improvement threshold used to increase regularization.

void set_th_acceptnegstep(const Scalar th_acceptnegstep)

Modify the threshold used for accepting step along ascent direction.

void set_th_acceptminstep(const Scalar th_acceptminstep)

Modify the threshold used for accepting minimum steps.

void set_rho(const Scalar rho)

Modify the rho parameter used in the merit function.

void set_th_minfeas(const Scalar th_minfeas)

Modify the threshold for switching to feasibility.

void set_upsilon_decfactor(const Scalar th_step)

Modify the upsilon decresing factor used to estimate to balance optimality and feasibility.

void set_zero_upsilon(const bool zero_upsilon)

Modify the zero-upsilon label.

Parameters:

zero_upsilon – True if we set estimated penalty parameter (upsilon) to zero when solve is called.

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

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

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.

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Functions

void allocateData()

Allocate all the internal data needed for the solver.

virtual void resizeRunningData() override

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

virtual void resizeTerminalData() override

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

void updateStateAndControlIndex()

Protected Attributes

Scalar reg_incfactor_

Regularization factor used to increase the damping value

Scalar reg_decfactor_

Regularization factor used to decrease the damping value

Scalar th_grad_

Tolerance of the expected gradient used for testing the step

Scalar th_noimprovement_

Threshold used to accept steps that cannot be be improved due to numerical errors

Scalar th_stepdec_

Step-length threshold used to decrease regularization.

Scalar th_stepinc_

Step-length threshold used to increase regularization.

Scalar th_minimprove_

Minimum improvement threshold used in the regularization scheme

Scalar th_acceptnegstep_

Threshold used for accepting step along ascent direction

Scalar th_acceptminstep_

Threshold used for accepting step along with a minimum length

Scalar rho_

Parameter used in the merit function to predict the expected reduction

Scalar th_minfeas_

Threshold for switching to feasibility.

Scalar upsilon_

Estimated penalty parameter that balances relative contribution of the cost function and equality constraints

Scalar upsilon_decfactor_

Estimated penalty parameter factor used to decrease its value

bool zero_upsilon_

True if we wish to set estimated penalty parameter (upsilon) to zero when solve is called.

std::size_t n_
std::size_t m_
std::size_t p_
VectorXs x_
Solver qp_solver_
Model qp_model_
Data qp_data_
Params qp_params_
odyn::VerboseLevel verbose_level_
std::vector<std::size_t> xs_idx_
std::vector<std::size_t> us_idx_
std::vector<VectorXs> Lxx_dx_

Second-order change of the cost function \(\boldsymbol{\ell}_{\mathbf{{xx}}}\delta\mathbf{x}\)

std::vector<VectorXs> Luu_du_

Second-order change of the cost function \(\boldsymbol{\ell}_{\mathbf{{uu}}}\delta\mathbf{u}\)

std::vector<VectorXs> Lxu_du_

Second-order change of the cost function \(\boldsymbol{\ell}_{\mathbf{{xu}}}\delta\mathbf{u}\)

bool acceptstep_
std::vector<Scalar> alphas_

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

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 dfeas_

Reduction in the feasibility.

Scalar dImpr_

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

Scalar dPhi_

Reduction in the merit function computed by tryStep().

Scalar dPhiexp_

Expected reduction in the merit function for the selected step length

Scalar dreg_

Current dual-variable regularization value.

std::vector<VectorXs> dus_

Linear control direction (size T).

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

Scalar dVexp_full_

Expected reduction in the cost function for a full step length

std::vector<VectorXs> dxs_

Linear state direction (size T + 1).

Scalar feas_

Total feasibility for the current guess.

Scalar ffeas_

Feasibility of the dynamic constraints for the current guess.

Scalar ffeas_try_

Feasibility of the dynamic constraints evaluated for the current step length

std::vector<VectorXs> fs_

Dynamics gaps in each node.

std::vector<VectorXs> fs_try_

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

Scalar gfeas_

Feasibility of the inequality constraints for the current guess

Scalar gfeas_try_

Feasibility of the inequality constraints evaluated for the current step length

Scalar hfeas_

Feasibility of the equality constraints for the current guess

Scalar hfeas_try_

Feasibility of the equality constraints evaluated for the current 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 merit_

Merit for the current guess.

std::size_t ng_T_

Dimension of termianl inequality constraints.

std::size_t nh_T_

Dimension of terminal equality constraints.

Scalar preg_

Current primal-variable regularization value.

std::shared_ptr<ShootingProblem> problem_

optimal control problem

Scalar reg_max_

Maximum allowed regularization value.

Scalar reg_min_

< Current control regularization values

Minimum allowed regularization value

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.