Template Class SolverIntroTpl

Defined in File intro.hpp

Inheritance Relationships

Base Type

public crocoddyl::SolverFDDPTpl< _Scalar > (Template Class SolverFDDPTpl)

Class Documentation

template<typename _Scalar> class SolverIntroTpl : public crocoddyl::SolverFDDPTpl<_Scalar>

Public Types

typedef SolverAbstractTpl<Scalar> SolverAbstract

typedef SolverFDDPTpl<Scalar> SolverFDDP

typedef ShootingProblemTpl<Scalar> ShootingProblem

typedef ShootingProblem::ActionModelAbstract ActionModelAbstract

typedef ShootingProblem::ActionDataAbstract ActionDataAbstract

typedef MathBaseTpl<Scalar> MathBase

typedef MathBase::VectorXs VectorXs

typedef MathBase::MatrixXs MatrixXs

typedef MathBase::MatrixXsRowMajor MatrixXsRowMajor

Public Functions

explicit SolverIntroTpl(std::shared_ptr<ShootingProblem> problem, const DynamicsSolverType dyn_solver = FeasShoot, const EqualitySolverType eq_solver = LuNull, const EqualitySolverType term_solver = LuNull)

Initialize the INTRO solver.

Parameters:

problem – [in] Shooting problem
eq_solver – [in] Type of equality solver
term_solver – [in] Type of terminal solver

virtual ~SolverIntroTpl() = default

virtual void calcDir() override

Refresh the Intro linearization and constraint factorizations.

The method first delegates to SolverFDDP::calcDir() to update the first- and second-order derivatives of the shooting problem. It then processes the control-equality constraints according to the selected equality solver (null-space or Schur complement), storing the factors that are reused during the backward pass.

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 void computePolicy(const std::size_t t) override: Compute the feedforward and feedback terms (control policy) computed via a Cholesky decomposition.

virtual void computeBatchPolicy(const std::size_t t) override: Compute the feedforward and feedback terms (control policy) associated to the batch’s constraints.

virtual void computeValueFunction(const std::size_t t, const std::shared_ptr<ActionModelAbstract> &model) override: Compute the linear-quadratic approximation of the value function.

virtual void computeBatchValueFunction(const std::size_t t) override: Compute the linear-quadratic approximation of the value function associated to the batch’s constraints.

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

Cast the Intro 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:: SolverIntroTpl<NewScalar> An Intro solver with the new scalar type.

EqualitySolverType get_equality_solver() const: Return the type of solver used for handling the equality constraints.

const std::vector<MatrixXs> &get_YZ() const: Return the rank of control-equality constraints \(\mathbf{H_u}\f */ const std::vector<std::size_t>& get_Hu_rank() const; /** @brief Return the span and kernel of control-equality constraints \)\mathbf{H_u}\f.

const std::vector<MatrixXs> &get_Qzz() const: Return Hessian of the reduced Hamiltonian \(\mathbf{Q_{zz}}\).

const std::vector<MatrixXs> &get_Qxz() const: Return Hessian of the reduced Hamiltonian \(\mathbf{Q_{xz}}\).

const std::vector<MatrixXs> &get_Quz() const: Return Hessian of the reduced Hamiltonian \(\mathbf{Q_{uz}}\).

const std::vector<VectorXs> &get_Qz() const: Return Jacobian of the reduced Hamiltonian \(\mathbf{Q_{z}}\).

const std::vector<MatrixXs> &get_Hy() const: Return span-projected Jacobian of the equality-constraint with respect to the control.

const std::vector<VectorXs> &get_kz() const: Return feedforward term related to the nullspace of \(\mathbf{H_u}\).

const std::vector<MatrixXs> &get_Kz() const: Return feedback gain related to the nullspace of \(\mathbf{H_u}\).

const std::vector<VectorXs> &get_ks() const: Return feedforward term related to the equality constraints.

const std::vector<MatrixXs> &get_Ks() const: Return feedback gain related to the equality constraints.

const std::vector<MatrixXs> &get_Qzc() const: Return Hessian of the reduced Hamiltonian \(\mathbf{Q_{zc}}\).

void set_equality_solver(const EqualitySolverType type)

Modify the type of solver used for handling the equality constraints.

Note that the default solver is nullspace LU. When we enable parallelization, this strategy is generally faster than others for medium to large systems.

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Functions

void allocateData()

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 calcLuNullDir()

void calcQrNullDir()

void computeNullPolicy(const std::size_t t)

void computeNullBatchPolicy(const std::size_t t)

void computeSchurPolicy(const std::size_t t)

void computeSchurBatchPolicy(const std::size_t t)

Protected Attributes

enum EqualitySolverType eq_solver_: Strategy used for handling the equality constraints.

std::vector<std::size_t> Hu_rank_: Rank of the control Jacobian of the equality constraints.

std::vector<MatrixXsRowMajor> KQuu_2Qxu_

std::vector<MatrixXs> YZ_: Span \(\mathbf{Y}\in\mathbb{R}^{rank}\) and kernel \(\mathbf{Z}\in\mathbb{R}^{nullity}\) of the control-equality constraints \(\mathbf{H_u}\)

std::vector<MatrixXs> Hy_: Span-projected Jacobian of the equality-constraint with respect to the control

std::vector<VectorXs> Qz_: Jacobian of the reduced Hamiltonian \(\mathbf{Q_{z}}\).

std::vector<MatrixXs> Qzz_: Hessian of the reduced Hamiltonian \(\mathbf{Q_{zz}}\).

std::vector<MatrixXs> Qxz_: Hessian of the reduced Hamiltonian \(\mathbf{Q_{xz}}\).

std::vector<MatrixXs> Quz_: Hessian of the reduced Hamiltonian \(\mathbf{Q_{uz}}\).

std::vector<VectorXs> kz_: Feedforward term in the nullspace of \(\mathbf{H_u}\).

std::vector<MatrixXs> Kz_: Feedback gain in the nullspace of \(\mathbf{H_u}\).

std::vector<VectorXs> ks_: Feedforward term related to the equality constraints.

std::vector<MatrixXs> Ks_: Feedback gain related to the equality constraints.

std::vector<MatrixXs> QuuinvHuT_

std::vector<Eigen::LLT<MatrixXs>> Qzz_llt_: Cholesky LLT solver.

std::vector<Eigen::FullPivLU<MatrixXs>> Hu_lu_: Full-pivot LU solvers used for computing the span and nullspace matrices

std::vector<Eigen::ColPivHouseholderQR<MatrixXs>> Hu_qr_: Column-pivot QR solvers used for computing the span and nullspace matrices

std::vector<Eigen::PartialPivLU<MatrixXs>> Hy_lu_: Partial-pivot LU solvers used for computing the feedforward and feedback gain related to the equality constraint

std::vector<MatrixXs> Kcs_

std::vector<MatrixXs> QuuKc_Quc_

std::vector<MatrixXs> Qzc_: Hessian of the reduced Hamiltonian \(\mathbf{Q_{zc}}\).

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

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

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

DynamicsSolverType dyn_solver_: Type of dynamics solver.

std::vector<MatrixXsRowMajor> K_: Feedback gains \(\mathbf{K}\).

std::vector<VectorXs> k_: Feed-forward terms \(\mathbf{l}\).

std::vector<VectorXs> Qu_: Gradient of the Hamiltonian \(\mathbf{Q_u}\).

std::vector<MatrixXs> Quu_: Hessian of the Hamiltonian \(\mathbf{Q_{uu}}\).

std::vector<Eigen::LLT<MatrixXs>> Quu_llt_: Cholesky LLT solver.

std::vector<VectorXs> Qx_: Gradient of the Hamiltonian \(\mathbf{Q_x}\).

std::vector<MatrixXs> Qxu_: Hessian of the Hamiltonian \(\mathbf{Q_{xu}}\).

std::vector<MatrixXs> Qxx_: Hessian of the Hamiltonian \(\mathbf{Q_{xx}}\).

Scalar reg_decfactor_: Regularization factor used to decrease the damping value

Scalar reg_incfactor_: Regularization factor used to increase the damping value

Scalar rho_: Parameter used in the merit function to predict the expected reduction

EqualitySolverType term_solver_: Type of terminal solver.

Scalar th_acceptminstep_: Threshold used for accepting step along with a minimum length

Scalar th_acceptnegstep_: Threshold used for accepting step along ascent direction

Scalar th_grad_: Tolerance of the expected gradient used for testing the step

Scalar th_minfeas_: Threshold for switching to feasibility.

Scalar th_minimprove_: Minimum improvement threshold used in the regularization scheme

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.

std::vector<std::size_t> Ts_: Index that describes the hybrid shoots.

Scalar upsilon_decfactor_: Estimated penalty parameter factor used to decrease its value

std::vector<VectorXs> Vx_: Gradient of the Value function \(\mathbf{V_x}\).

std::vector<MatrixXs> Vxx_: Hessian of the Value function \(\mathbf{V_{xx}}\).

std::vector<VectorXs> Vxx_f_: Hessian of the Value function times the gap \(\mathbf{V_{xx} \bar{f}}\)

MatrixXs Vxx_tmp_: Temporary variable for ensuring symmetry of Vxx.

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