Template Class SolverFDDPTpl

Inheritance Relationships

Base Type

Derived Types

Class Documentation

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

Feasibility-driven Differential Dynamic Programming (FDDP) solver.

The FDDP solver computes an optimal trajectory and control commands by iterates running backwardPass() and forwardPass(). The backward pass accepts infeasible guess as described in the SolverFDDP::backwardPass(). Additionally, the forward pass handles infeasibility simulations that resembles the numerical behaviour of a multiple-shooting formulation, i.e.:

\[\begin{split}\begin{eqnarray} \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0 - (1 - \alpha)\mathbf{\bar{f}}_0,\\ \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\ \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k) - (1 - \alpha)\mathbf{\bar{f}}_{k+1}. \end{eqnarray}\end{split}\]
Note that the forward pass keeps the gaps \(\mathbf{\bar{f}}_s\) open according to the step length \(\alpha\) that has been accepted. This solver has shown empirically greater globalization strategy. Additionally, the expected improvement computation considers the gaps in the dynamics:
\[\begin{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \end{equation}\]
with
\[\begin{split}\begin{eqnarray} \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \end{eqnarray}\end{split}\]

For more details about the feasibility-driven differential dynamic programming algorithm see:

See also

SolverAbstract(), backwardPass(), forwardPass(), and expectedImprovement().

Subclassed by crocoddyl::SolverBoxFDDPTpl< _Scalar >, crocoddyl::SolverIntroTpl< _Scalar >

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

Public Functions

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

Initialize the FDDP solver.

Parameters:

  • problem – [in] Shooting problem

  • dyn_solver – [in] Type of dynamic solver

  • term_solver – [in] Type of terminal solver

virtual ~SolverFDDPTpl() = 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 void computeCandidate(const Scalar step_length = Scalar(1.)) override

Compute new candidate solution using step length.

void forwardPass(const Scalar steplength = Scalar(1.))

Perform a forward pass with a predefined step length.

Our solver supports fourth type of forward passes: feasiblility-driven (FeasShoot), multiple shooting (MultiShoot), hybrid shooting (HybridShoot), and single shooting (SingleShoot). The feasibility-driven shooting decreases monotonocally the dynamics feasibility based on the applied step length. The hybrid shooting combines both feasibility-driven and multiple shooting approaches. Instead, the single shooting is the traditional approach implemented in DDP. The type of dynamics solver can be defined via set_dynamics_solver.

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

virtual void updateDualsAndSlacks(const Scalar stepLength = Scalar(1.))

Update the dual and slack variables with a predefined step length.

Parameters:

  • stepLength – steplength*

  • steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

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

The expected improvement computation considers the gaps in the dynamics:

\[\begin{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \end{equation}\]
with
\[\begin{split}\begin{eqnarray} \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \end{eqnarray}\end{split}\]

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

Run the backward pass (Riccati sweep).

It assumes that the Jacobian and Hessians of the optimal control problem have been compute (i.e., calcDir()). The backward pass handles infeasible guess through a modified Riccati sweep:

\[\begin{split}\begin{eqnarray*} \mathbf{Q}_{\mathbf{x}_k} &=& \mathbf{l}_{\mathbf{x}_k} + \mathbf{f}^\top_{\mathbf{x}_k} (V_{\mathbf{x}_{k+1}} + V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\ \mathbf{Q}_{\mathbf{u}_k} &=& \mathbf{l}_{\mathbf{u}_k} + \mathbf{f}^\top_{\mathbf{u}_k} (V_{\mathbf{x}_{k+1}} + V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\ \mathbf{Q}_{\mathbf{xx}_k} &=& \mathbf{l}_{\mathbf{xx}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}} \mathbf{f}_{\mathbf{x}_k},\\ \mathbf{Q}_{\mathbf{xu}_k} &=& \mathbf{l}_{\mathbf{xu}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}} \mathbf{f}_{\mathbf{u}_k},\\ \mathbf{Q}_{\mathbf{uu}_k} &=& \mathbf{l}_{\mathbf{uu}_k} + \mathbf{f}^\top_{\mathbf{u}_k} V_{\mathbf{xx}_{k+1}} \mathbf{f}_{\mathbf{u}_k}, \end{eqnarray*}\end{split}\]
where \(\mathbf{l}_{\mathbf{x}_k}\), \(\mathbf{l}_{\mathbf{u}_k}\), \(\mathbf{f}_{\mathbf{x}_k}\) and \(\mathbf{f}_{\mathbf{u}_k}\) are the Jacobians of the cost function and dynamics, \(\mathbf{l}_{\mathbf{xx}_k}\), \(\mathbf{l}_{\mathbf{xu}_k}\) and \(\mathbf{l}_{\mathbf{uu}_k}\) are the Hessians of the cost function, \(V_{\mathbf{x}_{k+1}}\) and \(V_{\mathbf{xx}_{k+1}}\) defines the linear-quadratic approximation of the Value function, and \(\mathbf{\bar{f}}_{k+1}\) describes the gaps of the dynamics.

void batchPass()

Run the batch pass to account for its constraints.

Update the direction and feed-forward term to account for the terminal constraint. To do so, we first compute the unscaled search direction accounting for the terminal constraint.

void updateDir()

Update search direction associated with the batch’s constraints.

virtual void computeActionValueFunction(const std::size_t t, const std::shared_ptr<ActionModelAbstract> &model, const std::shared_ptr<ActionDataAbstract> &data)

Compute the linear-quadratic approximation of the control action-value function.

Parameters:

  • t – [in] Time instance

  • model – [in] Action model in the given time instance

  • data – [in] Action data in the given time instance

virtual void computeBatchActionValueFunction(const std::size_t t, const std::shared_ptr<ActionDataAbstract> &data)

Compute the linear-quadratic approximation of the control action-value function associated to the batch’s constraints.

Parameters:

  • t – [in] Time instance

  • data – [in] Action data in the given time instance

virtual void computePolicy(const std::size_t t)

Compute the feedforward and feedback terms (control policy) computed via a Cholesky decomposition.

To compute the feedforward \(\mathbf{k}_k\) and feedback \(\mathbf{K}_k\) terms, we use a Cholesky decomposition to solve \(\mathbf{Q}_{\mathbf{uu}_k}^{-1}\) term:

\[\begin{split}\begin{eqnarray}\mathbf{k}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{u}},\\ \mathbf{K}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{ux}}. \end{eqnarray}\end{split}\]

Note that if the Cholesky decomposition fails, then we re-start the backward pass and increase the state and control regularization values.

virtual void computeBatchPolicy(const std::size_t t)

Compute the feedforward and feedback terms (control policy) associated to the batch’s constraints.

Note that if the Cholesky decomposition fails, then we re-start the backward pass and increase the state and control regularization values.

virtual void computeValueFunction(const std::size_t t, const std::shared_ptr<ActionModelAbstract> &model)

Compute the linear-quadratic approximation of the value function.

This function is called in the backward pass after updating the local action-value and policy functions.

Parameters:

  • t – [in] Time instance

  • model – [in] Action model in the given time instance

virtual void computeBatchValueFunction(const std::size_t t)

Compute the linear-quadratic approximation of the value function associated to the batch’s constraints.

This function is called in the backward pass after updating the local action-value and policy functions.

Parameters:

t – [in] Time instance

void linearRollout()

Perform a linear rollout give the current control policy.

The results of this linear rollout are stored in dxs and dus.

virtual void feasShootForwardPass(const Scalar steplength)

Run the feasibility-driven nonlinear rollout.

It rollouts the action model given the feasibility-driven approach described in “Crocoddyl: An Efficient and Versatile Framework for

Multi-Contact Optimal Control”

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

virtual void multiShootForwardPass(const Scalar steplength)

Run the multiple-shooting rollout.

It rollouts the action model given the multiple-shooting approach described in (TODO: add paper title)

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

virtual void hybridShootForwardPass(const Scalar steplength)

Run the multiple-shooting rollout with intervals of feasibility-driven search.

It rollouts the action model given the hybrid-shooting approach described in (TODO: add paper title)

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

virtual void singleShootForwardPass(const Scalar steplength)

Run the classical nonlinear rollout.

It rollouts the action model given the classical approach in DDP. You can find details in “A second-order gradient method for determining optimal

trajectories of non-linear discrete-time systems”

Parameters:

steplength – [in] applied step length ( \(0\leq\alpha\leq1\))

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.

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

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

SolverFDDPTpl<NewScalar> A FDDP solver with the new scalar type.

DynamicsSolverType get_dynamics_solver() const

Return the type of solver used for handling the dynamics constraints.

EqualitySolverType get_terminal_solver() const

Return the type of solver used for handling the terminal constraints.

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.

const std::vector<std::size_t> &get_Ts() const

Return the hybrid shooting intervals.

const std::vector<MatrixXs> &get_Vxx() const

Return the Hessian of the Value function \(V_{\mathbf{xx}_s}\).

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

Return the Hessian of the Value function \(V_{\mathbf{x}_s}\).

const std::vector<MatrixXs> &get_Qxx() const

Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{xx}_s}\).

const std::vector<MatrixXs> &get_Qxu() const

Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{xu}_s}\).

const std::vector<MatrixXs> &get_Quu() const

Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{uu}_s}\).

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

Return the Jacobian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{x}_s}\).

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

Return the Jacobian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{u}_s}\).

const std::vector<MatrixXsRowMajor> &get_K() const

Return the feedback gains \(\mathbf{K}_{s}\).

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

Return the feedforward gains \(\mathbf{k}_{s}\).

const std::vector<MatrixXs> &get_Vxc() const

Return the Hessian of the Value function \(V_{\mathbf{xc}_s}\).

const std::vector<MatrixXs> &get_Qxc() const

Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{xc}_s}\).

const std::vector<MatrixXs> &get_Quc() const

Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{uc}_s}\).

const std::vector<MatrixXs> &get_dXc() const

Return the linear update in \(\delta\mathbf{X}_{c_s}\).

const std::vector<MatrixXs> &get_dUc() const

Return the linear update in \(\delta\mathbf{U}_{c_s}\).

const std::vector<MatrixXs> &get_Kc() const

Return the feedforward gains \(\mathbf{K}_{c_s}\).

const MatrixXs &get_dHc() const

Return the Jacobian of the terminal constraint gains \(\delta\mathbf{H}_{c}\).

const VectorXs &get_hc() const

Return the bias term of the terminal constraint gains \(\mathbf{h}_{c}\).

const VectorXs &get_beta_plus() const

Return the next terminal-constraint multiplier \(\boldsymbol{\beta}^+\).

void set_dynamics_solver(const DynamicsSolverType type, const std::size_t Tshoot = 0)

Set the dynamic solver used for handling the dynamics constraints.

It is worth noting that the default solver is the Feasibility-Driven DDP. When we enable parallelization, this strategy is not necessarily the faster one for medium to large systems.

Parameters:

  • type – [in] Type of dynamics solver

  • Tshoot – [in] Number of nodes per each shooting interval

void set_terminal_solver(const EqualitySolverType type)

Set the type of solver used for handling the terminal constraints.

Parameters:

type – [in] Type of terminal 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 computeNullTerminalMultiplier()

compute the multiplier associated to the terminal constraint

Protected Attributes

DynamicsSolverType dyn_solver_

Type of dynamics solver.

EqualitySolverType term_solver_

Type of terminal solver.

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::vector<std::size_t> Ts_

Index that describes the hybrid shoots.

MatrixXs Vxx_tmp_

Temporary variable for ensuring symmetry of Vxx.

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}}\)

std::vector<VectorXs> Vx_

Gradient of the Value function \(\mathbf{V_x}\).

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}\)

std::vector<MatrixXs> Qxx_

Hessian of the Hamiltonian \(\mathbf{Q_{xx}}\).

std::vector<MatrixXs> Qxu_

Hessian of the Hamiltonian \(\mathbf{Q_{xu}}\).

std::vector<MatrixXs> Quu_

Hessian of the Hamiltonian \(\mathbf{Q_{uu}}\).

std::vector<VectorXs> Qx_

Gradient of the Hamiltonian \(\mathbf{Q_x}\).

std::vector<VectorXs> Qu_

Gradient of the Hamiltonian \(\mathbf{Q_u}\).

std::vector<MatrixXsRowMajor> K_

Feedback gains \(\mathbf{K}\).

std::vector<VectorXs> k_

Feed-forward terms \(\mathbf{l}\).

std::vector<VectorXs> dx_

State error during the roll-out/forward-pass (size T).

std::vector<MatrixXsRowMajor> FxTVxx_p_

Store the value of \(\mathbf{f_x}^T\mathbf{V_{xx}}^{'}\)

std::vector<MatrixXsRowMajor> FuTVxx_p_

Store the values of \(\mathbf{f_u}^T\mathbf{V_{xx}}^{'}\) per each running node

VectorXs fTVxx_p_

Store the value of \(\mathbf{\bar{f}}^T\mathbf{V_{xx}}^{'}\)

std::vector<Eigen::LLT<MatrixXs>> Quu_llt_

Cholesky LLT solver.