Skip to content

Stagewise Functions

ssqp::model::stagewise defines the per-stage function interface shared by costs, dynamics and constraints. See Costs, Constraints & Dynamics in Concepts for the conceptual explanation. The reference below is generated directly from the headers' own doc comments.

Function

The common base for StageCost, StageDynamics and StageConstraints:

Class ssqp::model::stagewise::Function

ClassList > ssqp > model > stagewise > Function

Inherited by the following classes: ssqp::model::stagewise::StageConstraints, ssqp::model::stagewise::StageCost, ssqp::model::stagewise::StageDynamics

Public Functions

Type Name
Function (const std::string & name)
virtual VecX evaluate (const VecX & x, const VecX & u) const = 0
virtual const std::vector< std::string > names () const
virtual uint output_size () const = 0

Public Functions Documentation

function Function
ssqp::model::stagewise::Function::Function (
    const std::string & name
)
function evaluate
virtual VecX ssqp::model::stagewise::Function::evaluate (
    const VecX & x,
    const VecX & u
) const = 0
function names
virtual const std::vector< std::string > ssqp::model::stagewise::Function::names () const
function output_size
virtual uint ssqp::model::stagewise::Function::output_size () const = 0

The documentation for this class was generated from the following file /home/runner/work/ssqp/ssqp/include/model/stagewise/Function.hpp

StageCost

Class ssqp::model::stagewise::StageCost

ClassList > ssqp > model > stagewise > StageCost

More...

  • #include <StageCost.hpp>

Inherits the following classes: ssqp::model::stagewise::Function

Inherited by the following classes: ssqp::model::stagewise::factory::FunctionStageCost, ssqp::model::stagewise::predefined::costs::CenterOfMassTrackingCost, ssqp::model::stagewise::predefined::costs::FloatingBaseSquaredPoseErrorCost, ssqp::model::stagewise::predefined::costs::FloatingBaseSquaredRotationErrorCost, ssqp::model::stagewise::predefined::costs::FloatingBaseSquaredTranslationErrorCost, ssqp::model::stagewise::predefined::costs::FloatingBaseSquaredVelocityErrorCost, ssqp::model::stagewise::predefined::costs::FrameSquaredPoseErrorCost, ssqp::model::stagewise::predefined::costs::FrameSquaredRotationErrorCost, ssqp::model::stagewise::predefined::costs::FrameSquaredTerrainHeightErrorCost, ssqp::model::stagewise::predefined::costs::FrameSquaredTranslationErrorCost, ssqp::model::stagewise::predefined::costs::FrameSquaredVelocityErrorCost, ssqp::model::stagewise::predefined::costs::FramesInverseSquaredDistanceCost, ssqp::model::stagewise::predefined::costs::SquaredConfigurationErrorCost, ssqp::model::stagewise::predefined::costs::SquaredControlCost, ssqp::model::stagewise::predefined::costs::SquaredForceCost, ssqp::model::stagewise::predefined::costs::SquaredIDTorqueCost, ssqp::model::stagewise::predefined::costs::SquaredJointAngleCost, ssqp::model::stagewise::predefined::costs::SquaredJointVelocityCost

Public Functions

Type Name
StageCost ()
StageCost (const std::string & name)
virtual bool enabled () const
virtual void gradient (const VecX & x, const VecX & u, VecX & dx, VecX & du) const = 0
virtual void hessian_uu (const VecX & x, const VecX & u, MatX & Huu) const = 0
virtual void hessian_xu (const VecX & x, const VecX & u, MatX & Hxu) const = 0
virtual void hessian_xx (const VecX & x, const VecX & u, MatX & Hxx) const = 0
uint output_size () const
virtual void set_enabled (const bool enabled)

Detailed Description

General abstract constraint function for an optimization stage \(k\): \(\ell_k(\boldsymbol{x}, \boldsymbol{u})\) Defines the required functions for a stage cost, while providing default implementations for some

Public Functions Documentation

function StageCost [1/2]
ssqp::model::stagewise::StageCost::StageCost ()
function StageCost [2/2]
ssqp::model::stagewise::StageCost::StageCost (
    const std::string & name
)
function enabled
virtual bool ssqp::model::stagewise::StageCost::enabled () const
function gradient
virtual void ssqp::model::stagewise::StageCost::gradient (
    const VecX & x,
    const VecX & u,
    VecX & dx,
    VecX & du
) const = 0
function hessian_uu
virtual void ssqp::model::stagewise::StageCost::hessian_uu (
    const VecX & x,
    const VecX & u,
    MatX & Huu
) const = 0
function hessian_xu
virtual void ssqp::model::stagewise::StageCost::hessian_xu (
    const VecX & x,
    const VecX & u,
    MatX & Hxu
) const = 0
function hessian_xx
virtual void ssqp::model::stagewise::StageCost::hessian_xx (
    const VecX & x,
    const VecX & u,
    MatX & Hxx
) const = 0
function output_size
inline uint ssqp::model::stagewise::StageCost::output_size () const
function set_enabled
virtual void ssqp::model::stagewise::StageCost::set_enabled (
    const  bool enabled
)

The documentation for this class was generated from the following file /home/runner/work/ssqp/ssqp/include/model/stagewise/StageCost.hpp

StageDynamics

Class ssqp::model::stagewise::StageDynamics

ClassList > ssqp > model > stagewise > StageDynamics

More...

  • #include <StageDynamics.hpp>

Inherits the following classes: ssqp::model::stagewise::Function

Inherited by the following classes: ssqp::model::stagewise::factory::FunctionStageDynamicsBase

Public Functions

Type Name
StageDynamics ()
StageDynamics (const std::string & name)
virtual uint control_size () const = 0
virtual void gradient (const VecX & x, const VecX & u, MatX & dX, MatX & dU) const = 0
virtual void hessprod_uu (const VecX & x, const VecX & u, const VecX & lambda, MatX & Puu) const = 0
virtual void hessprod_xu (const VecX & x, const VecX & u, const VecX & lambda, MatX & Pxu) const = 0
virtual void hessprod_xx (const VecX & x, const VecX & u, const VecX & lambda, MatX & Pxx) const = 0
virtual VecX init_control () const
virtual VecX init_state () const
virtual MatX parameter_gradient (const VecX & x, const VecX & u) const
virtual uint parameter_size () const
virtual VecX parameters () const
virtual void set_init_control (const VecX & init_control)
virtual void set_init_state (const VecX & init_state)
virtual uint state_diff_size () const
virtual VecX state_right_minus (const VecX & x, const VecX & y) const
virtual VecX state_right_plus (const VecX & x, const VecX & dx) const
virtual uint state_size () const = 0
virtual void update_parameters (const VecX & theta)

Detailed Description

General abstract dynamics function for an optimization stage \(k\): \(\boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) Defines the required functions for stage dynamics, while providing default implementations for some

Public Functions Documentation

function StageDynamics [1/2]
ssqp::model::stagewise::StageDynamics::StageDynamics ()
function StageDynamics [2/2]
ssqp::model::stagewise::StageDynamics::StageDynamics (
    const std::string & name
)
function control_size
virtual uint ssqp::model::stagewise::StageDynamics::control_size () const = 0
function gradient
virtual void ssqp::model::stagewise::StageDynamics::gradient (
    const VecX & x,
    const VecX & u,
    MatX & dX,
    MatX & dU
) const = 0

Get dynamics Jacobians with respect to \(\boldsymbol{x}\) and \(\boldsymbol{u}\), evaluated at given \((\boldsymbol{x}, \boldsymbol{u})\): \(\nabla_x \boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) and \(\nabla_u \boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) Returns: dX: gradient with respect to \(\boldsymbol{x}\), \(n_{dx} \times n_{dx}\) dU: gradient with respect to \(\boldsymbol{u}\), \(n_{dx} \times n_u\)

function hessprod_uu
virtual void ssqp::model::stagewise::StageDynamics::hessprod_uu (
    const VecX & x,
    const VecX & u,
    const VecX & lambda,
    MatX & Puu
) const = 0

Get product \(\nabla_{u} \lambda_k^\top \nabla_{u} \boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) Returns: Puu: Product, \(n_u \times n_u\)

function hessprod_xu
virtual void ssqp::model::stagewise::StageDynamics::hessprod_xu (
    const VecX & x,
    const VecX & u,
    const VecX & lambda,
    MatX & Pxu
) const = 0

Get product \(\nabla_{x} \lambda_k^\top \nabla_{u} \boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) Returns: Pxu: Product, \(n_{dx} \times n_u\)

function hessprod_xx
virtual void ssqp::model::stagewise::StageDynamics::hessprod_xx (
    const VecX & x,
    const VecX & u,
    const VecX & lambda,
    MatX & Pxx
) const = 0

Get product \(\nabla_{x} \lambda_k^\top \nabla_{x} \boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u})\) Returns: Pxx: Product, \(n_{dx} \times n_{dx}\)

function init_control
virtual VecX ssqp::model::stagewise::StageDynamics::init_control () const
function init_state
virtual VecX ssqp::model::stagewise::StageDynamics::init_state () const
function parameter_gradient
virtual MatX ssqp::model::stagewise::StageDynamics::parameter_gradient (
    const VecX & x,
    const VecX & u
) const

Get dynamics Jacobian with respect to any model parameters, evaluated at given \((\boldsymbol{x}, \boldsymbol{u})\): \(\nabla_{\theta} \boldsymbol{f}_{\theta, k}(\boldsymbol{x}, \boldsymbol{u})\) Returns: gradient with respect to \(\theta\), \(n_{dx} \times n_{\theta}\)

Note: Implementing this method is optional; this method allows for online adaptation of any model parameters, \(\theta\)

function parameter_size
virtual uint ssqp::model::stagewise::StageDynamics::parameter_size () const
function parameters
virtual VecX ssqp::model::stagewise::StageDynamics::parameters () const

Get parameter vector \(\theta\)

Note: Implementing this method is optional; this method allows for online adaptation of any model parameters, \(\theta\)

function set_init_control
virtual void ssqp::model::stagewise::StageDynamics::set_init_control (
    const VecX & init_control
)
function set_init_state
virtual void ssqp::model::stagewise::StageDynamics::set_init_state (
    const VecX & init_state
)
function state_diff_size
virtual uint ssqp::model::stagewise::StageDynamics::state_diff_size () const
function state_right_minus
virtual VecX ssqp::model::stagewise::StageDynamics::state_right_minus (
    const VecX & x,
    const VecX & y
) const

Defines right minus operator between states Inputs: Composite states \(\boldsymbol{x}\), \(\boldsymbol{y}\) (vectors/lie group elements, size \(n_x\)) Output: State difference vector \(\boldsymbol{dx}\) (size \(n_{dx}\)) Default behavior: \(\boldsymbol{x} - \boldsymbol{y}\)

function state_right_plus
virtual VecX ssqp::model::stagewise::StageDynamics::state_right_plus (
    const VecX & x,
    const VecX & dx
) const

Operator definitions for states Defines operators between states and state differences If state is a euclidian vector, these operators reduce to simple addition/subtraction If state is a lie group element, these operators perform conversions between elements of the group (manifold) and tangent space elements The state can also be composite, i.e made up of one or more manifolds and one or more euclidian vectors, in which case operators act segment-wise

Defines right plus operator between state vector and tangent space vector Inputs: State vector \(\boldsymbol{x}\) (size \(n_x\)), state difference vector \(\boldsymbol{dx}\) (size \(n_{dx}\)) Output: State \(\boldsymbol{y}\) (size \(n_x\)) Default behavior: \(\boldsymbol{x} + \boldsymbol{dx}\)

function state_size
virtual uint ssqp::model::stagewise::StageDynamics::state_size () const = 0
function update_parameters
virtual void ssqp::model::stagewise::StageDynamics::update_parameters (
    const VecX & theta
)

Update parameter vector \(\theta\)

Note: Implementing this method is optional; this method allows for online adaptation of any model parameters, \(\theta\)

The documentation for this class was generated from the following file /home/runner/work/ssqp/ssqp/include/model/stagewise/StageDynamics.hpp

StageConstraints

Class ssqp::model::stagewise::StageConstraints

ClassList > ssqp > model > stagewise > StageConstraints

More...

  • #include <StageConstraints.hpp>

Inherits the following classes: ssqp::model::stagewise::Function

Inherited by the following classes: ssqp::model::stagewise::factory::FunctionStageConstraints, ssqp::model::stagewise::predefined::constraints::CenterOfMassBound, ssqp::model::stagewise::predefined::constraints::ContactConstraint, ssqp::model::stagewise::predefined::constraints::FloatingBaseAngularVelocityBound, ssqp::model::stagewise::predefined::constraints::FloatingBaseLinearVelocityBound, ssqp::model::stagewise::predefined::constraints::FloatingBasePoseBound, ssqp::model::stagewise::predefined::constraints::FloatingBaseRotationBound, ssqp::model::stagewise::predefined::constraints::FloatingBaseTranslationBound, ssqp::model::stagewise::predefined::constraints::FrameAngularVelocityBound, ssqp::model::stagewise::predefined::constraints::FrameLinearVelocityBound, ssqp::model::stagewise::predefined::constraints::FramePoseBound, ssqp::model::stagewise::predefined::constraints::FrameRotationBound, ssqp::model::stagewise::predefined::constraints::FrameTerrainHeightBound, ssqp::model::stagewise::predefined::constraints::FrameTranslationBound, ssqp::model::stagewise::predefined::constraints::JointAngleConstraint, ssqp::model::stagewise::predefined::constraints::JointControlConstraint, ssqp::model::stagewise::predefined::constraints::JointVelocityConstraint

Public Functions

Type Name
StageConstraints ()
StageConstraints (const std::string & name)
virtual uint constraint_diff_size () const
virtual bool enabled () const
virtual std::string force_frame (const uint i) const
virtual void gradient (const VecX & x, const VecX & u, MatX & dX, MatX & dU) const = 0
virtual void hessprod_uu (const VecX & x, const VecX & u, const VecX & mu, MatX & Puu) const = 0
virtual void hessprod_xu (const VecX & x, const VecX & u, const VecX & mu, MatX & Pxu) const = 0
virtual void hessprod_xx (const VecX & x, const VecX & u, const VecX & mu, MatX & Pxx) const = 0
virtual const VecX & lower_bound () const
virtual uint required_forces () const
virtual VecX right_minus (const VecX & x, const VecX & y) const
virtual void set_enabled (const bool enabled)
virtual void set_forces (const std::vector< std::shared_ptr< Force > > & forces)
virtual const VecX & upper_bound () const

Detailed Description

General abstract constraint function for an optimization stage \(k\): \(\boldsymbol{c}_k(\boldsymbol{x}, \boldsymbol{u})\) Defines the required functions for stage constraints, while providing default implementations for some

Public Functions Documentation

function StageConstraints [1/2]
ssqp::model::stagewise::StageConstraints::StageConstraints ()
function StageConstraints [2/2]
ssqp::model::stagewise::StageConstraints::StageConstraints (
    const std::string & name
)
function constraint_diff_size
virtual uint ssqp::model::stagewise::StageConstraints::constraint_diff_size () const
function enabled
virtual bool ssqp::model::stagewise::StageConstraints::enabled () const
function force_frame
virtual std::string ssqp::model::stagewise::StageConstraints::force_frame (
    const  uint i
) const
function gradient
virtual void ssqp::model::stagewise::StageConstraints::gradient (
    const VecX & x,
    const VecX & u,
    MatX & dX,
    MatX & dU
) const = 0
function hessprod_uu
virtual void ssqp::model::stagewise::StageConstraints::hessprod_uu (
    const VecX & x,
    const VecX & u,
    const VecX & mu,
    MatX & Puu
) const = 0
function hessprod_xu
virtual void ssqp::model::stagewise::StageConstraints::hessprod_xu (
    const VecX & x,
    const VecX & u,
    const VecX & mu,
    MatX & Pxu
) const = 0
function hessprod_xx
virtual void ssqp::model::stagewise::StageConstraints::hessprod_xx (
    const VecX & x,
    const VecX & u,
    const VecX & mu,
    MatX & Pxx
) const = 0
function lower_bound
virtual const VecX & ssqp::model::stagewise::StageConstraints::lower_bound () const
function required_forces
virtual uint ssqp::model::stagewise::StageConstraints::required_forces () const
function right_minus
virtual VecX ssqp::model::stagewise::StageConstraints::right_minus (
    const VecX & x,
    const VecX & y
) const
function set_enabled
virtual void ssqp::model::stagewise::StageConstraints::set_enabled (
    const  bool enabled
)
function set_forces
virtual void ssqp::model::stagewise::StageConstraints::set_forces (
    const std::vector< std::shared_ptr< Force > > & forces
)
function upper_bound
virtual const VecX & ssqp::model::stagewise::StageConstraints::upper_bound () const

The documentation for this class was generated from the following file /home/runner/work/ssqp/ssqp/include/model/stagewise/StageConstraints.hpp

Predefined implementations

predefined/Costs.hpp and predefined/Constraints.hpp provide ready-made subclasses built on Pinocchio kinematics, matching the classes exposed in ssqpy.model.costs and ssqpy.model.constraints:

Costs: SquaredJointAngleCost, SquaredJointVelocityCost, SquaredControlCost, SquaredIDTorqueCost, SquaredForceCost, SquaredConfigurationErrorCost, FloatingBaseSquaredTranslationErrorCost, FloatingBaseSquaredRotationErrorCost, FloatingBaseSquaredPoseErrorCost, FloatingBaseSquaredVelocityErrorCost, FrameSquaredTranslationErrorCost, FrameSquaredTerrainHeightErrorCost, FrameSquaredRotationErrorCost, FrameSquaredPoseErrorCost, FrameSquaredVelocityErrorCost, CenterOfMassTrackingCost, FramesInverseSquaredDistanceCost.

Constraints: JointAngleConstraint, JointVelocityConstraint, JointControlConstraint, FloatingBaseTranslationBound, FloatingBaseRotationBound, FloatingBasePoseBound, FloatingBaseLinearVelocityBound, FloatingBaseAngularVelocityBound, FrameTranslationBound, FrameTerrainHeightBound, FrameRotationBound, FramePoseBound, FrameLinearVelocityBound, FrameAngularVelocityBound, CenterOfMassBound, and ContactConstraint (friction cone + non-penetration + optional no-slip; C++-only, not directly exposed to Python, since Model.addContactConstraint constructs one for you).

Their constructor arguments mirror the Python bindings; see the corresponding entry in the Python API reference for the parameter list of each.