Skip to content

Model

ssqp::model::Model is a prerequisite for solving an OCP with SSQP. See Model in Concepts for the conceptual explanation (horizon/state/control layout, solver modes, integration schemes, contacts). The reference below is generated directly from include/model/Model.hpp's own doc comments, including the nested Structure, Config and CustomProperties types (each also has its own full page, linked from the "Classes" table below).

Class ssqp::model::Model

ClassList > ssqp > model > Model

More...

  • #include <Model.hpp>

Classes

Type Name
struct Config
struct CustomProperties
struct Structure

Public Functions

Type Name
Model (uint horizon, double dt, const Structure & structure=Structure::Default())
Model (uint horizon, double dt, const Config & config)
Model (uint horizon, double dt, const Structure & structure, const Config & config)
Model (const Model &) = delete
std::shared_ptr< stagewise::StageConstraints > addConstraints (const uint k, Args &&... args)
void addConstraints (const uint k, const std::shared_ptr< stagewise::StageConstraints > & constraints)
std::shared_ptr< stagewise::StageCost > addCost (const uint k, Args &&... args)
void addCost (const uint k, const std::shared_ptr< stagewise::StageCost > & cost)
std::shared_ptr< Force > addForce (const uint k, const std::string & frame)
std::shared_ptr< stagewise::StageCost > addRunningCost (Args &&... args)
void addRunningCost (const std::shared_ptr< stagewise::StageCost > & cost)
std::shared_ptr< Force > addWrench (const uint k, const std::string & frame)
const Vec3 centerOfMass (const VecX & q) const
const ConstraintsContainer & constraints () const
const CostsContainer & costs () const
void dDifference (const VecX & q0, const VecX & q1, MatX & J, const uint arg_pos) const
VecX difference (const VecX & q0, const VecX & q1) const
const Dimensions & dims () noexcept const
double dt () const
const DynamicsContainer & dynamics () const
void finalize (const Config & config=Config::Default())
bool finalized () const
bool floatingBase () const
const VecX forwardDynamics (const VecX & q, const VecX & v, const VecX & tau) const
const VecX forwardDynamics (const VecX & q, const VecX & v, const VecX & tau, const std::unordered_map< std::string, std::vector< Vec3 > > & forces) const
const VecX forwardDynamics (const VecX & q, const VecX & v, const VecX & tau, const std::vector< Vec6 > & joint_forces) const
const std::pair< Vec3, Mat3 > forwardKinematics (const VecX & q, const std::string & frame) const
const std::pair< Vec3, Mat3 > forwardKinematics (const VecX & q, const uint frame_id) const
const Mat4 forwardKinematicsSE3 (const VecX & q, const std::string & frame) const
const Mat4 forwardKinematicsSE3 (const VecX & q, const uint frame_id) const
uint frameId (const std::string & frame) const
const MatX frameJacobian (const VecX & q, const std::string & frame, bool local=false) const
const MatX frameJacobian (const VecX & q, const uint frame_id, bool local=false) const
void frameJacobian (const VecX & q, const std::string & frame, bool local, Eigen::Ref< MatX > J) const
void frameJacobian (const VecX & q, const uint frame_id, bool local, Eigen::Ref< MatX > J) const
const MatX frameJacobian (const std::string & frame, bool local=false) const
const MatX frameJacobian (const uint frame_id, bool local=false) const
void frameJacobian (const std::string & frame, bool local, Eigen::Ref< MatX > J) const
void frameJacobian (const uint frame_id, bool local, Eigen::Ref< MatX > J) const
const Vec6 frameVelocity (const VecX & q, const VecX & v, const std::string & frame, bool local=false) const
const Vec6 frameVelocity (const VecX & q, const VecX & v, const uint frame_id, bool local=false) const
std::pair< MatX, MatX > frameVelocityDerivatives (const VecX & q, const VecX & v, const std::string & frame, bool local=false) const
std::pair< MatX, MatX > frameVelocityDerivatives (const VecX & q, const VecX & v, const uint frame_id, bool local=false) const
void frameVelocityDerivatives (const VecX & q, const VecX & v, const std::string & frame, bool local, Eigen::Ref< MatX > dVdq, Eigen::Ref< MatX > dVdv) const
void frameVelocityDerivatives (const VecX & q, const VecX & v, const uint frame_id, bool local, Eigen::Ref< MatX > dVdq, Eigen::Ref< MatX > dVdv) const
const std::vector< std::shared_ptr< stagewise::Function > > & functions () const
VecX getActuatedTorqueVector (const VecX & x, const VecX & u, const uint k=0) const
void getActuatedTorqueVectorDerivatives (const VecX & x, const VecX & u, MatX & dtau_dx, MatX & dtau_du, const uint k=0) const
const VecX getAllDynamicParameters () const
VecX getFullAccelerationVector (const VecX & x, const VecX & u, const uint k=0) const
const MatX getJointTorqueRegressor (const VecX & q, const VecX & v, const VecX & a) const
MatX getMatrixActuatedPart (const MatX & M) const
void getMatrixActuatedPart (const MatX & M, Eigen::Ref< MatX > out) const
bool hasCustomProperties () const
bool hasFrame (const std::string & frame) const
const uint horizon () const
const VecX inverseDynamics (const VecX & q, const VecX & v, const VecX & a) const
const VecX inverseDynamics (const VecX & q, const VecX & v, const VecX & a, const std::unordered_map< std::string, std::vector< Vec3 > > & forces) const
const VecX inverseDynamics (const VecX & q, const VecX & v, const VecX & a, const std::vector< Vec6 > & joint_forces) const
const void inverseDynamicsDerivatives (const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & a, Eigen::MatrixXd & rnea_partial_dq, Eigen::MatrixXd & rnea_partial_dv, Eigen::MatrixXd & rnea_partial_da) const
const void inverseDynamicsDerivatives (const Eigen::VectorXd & q, const Eigen::VectorXd & v, const Eigen::VectorXd & a, const std::vector< Eigen::Vector< double, 6 > > & fext, Eigen::MatrixXd & rnea_partial_dq, Eigen::MatrixXd & rnea_partial_dv, Eigen::MatrixXd & rnea_partial_da) const
const VecX inverseKinematics (const VecX & desired_translation, const MatX & desired_rotation, const std::string & joint)
const MatX jacobianCenterOfMass (const VecX & q) const
std::vector< std::string > jointTypes () const
const Dimensions & linear_dims () noexcept const
const VecX neutralPose () const
uint nforces (const uint k) const
uint njoints () const
uint nq () const
uint nu () const
uint nv () const
uint nwrenches (const uint k) const
Model & operator= (const Model &) = delete
std::vector< std::string > orderedActuatedJoints () const
std::vector< std::string > orderedBodyFrames () const
std::vector< std::string > orderedForceFrames (const uint k) const
std::vector< std::string > orderedJoints () const
std::vector< std::string > orderedWrenchFrames (const uint k) const
void printFramesAndJoints () const
VecX randomControls (const uint stage) const
VecX randomState (const uint stage) const
void setActuatedJoints (const std::vector< uint > & actuated_joints)
void setAllDynamicParameters (const VecX & parameters)
void setCustomProperties (const CustomProperties & custom_properties)
void setInitControl (const uint k, const VecX & init_control)
void setInitState (const uint k, const VecX & init_state)
SolverMode solverMode () const
const std::shared_ptr< terrain::Terrain > terrain () const
uint total_joints () const
void updateDimensions ()
bool xml () const

Detailed Description

Defines an SSQP model An SSQP model object is a prerequisite to performing optimization using SSQP The model contains the following:

  • An optimization horizon, \(H\)

  • A cost function for each stage of the horizon, \(\ell_k(\boldsymbol{x}, \boldsymbol{u}) \in \mathcal{R}\), \(0 \leq k \leq H\)

  • Dynamics functions for each stage of the horizon, \(\boldsymbol{f}_k(\boldsymbol{x}, \boldsymbol{u}) \in \mathcal{R}^n\), \(0 \leq k \leq H\)

  • Constraint functions for each stage of the horizon, \(\boldsymbol{c}_k(\boldsymbol{x}, \boldsymbol{u}) \in \mathcal{R}^c\), \(0 \leq k \leq H\)

Of the above, all stage cost and dynamics functions must be defined, but constraints may be left empty.

Note that models optimize for \(H\) total steps while considering the initial state to be constant, whereas the initial controls can be altered. This means that there are \(H\) total controls (\(\boldsymbol{u}_0\) to \(\boldsymbol{u}_{H-1}\)) and \(H + 1\) total states (\(\boldsymbol{x}_0\) to \(\boldsymbol{x}_H\)), with \(\boldsymbol{x}_0\) being constant. At the terminal stage (stage \(H\)), there are no controls and, as such, stagewise functions must be appropriately defined to ignore them. Predefined functions all have constructors to account for this.

Models may be defined either by passing custom function implementations, or by using XML description files. Some constructors allow for defining a model using an XML file, but without passing the required cost functions. This is done to allow access to robot-related algorithms such as forward dynamics, which may be used to define model costs. Note that, if such constructors are used, the finalize method must be called before attempting to use the model for optimization. The user is responsible for either manually assigning all required costs, or passing them to the finalize method.

Public Functions Documentation

function Model [1/4]

ssqp::model::Model::Model (
    uint horizon,
    double dt,
    const  Structure & structure=Structure::Default()
) 

Model Structure Constructor

Initializes a model with the given structure

Input:

  • horizon: The time horizon to optimize over

  • dt: Time step per solver stage

  • structure: The structure of the problem

Note:

This constructor does not finalize the model. The finalize method must be explicitly called before passing the model to a solver. If an unfinalized model is passed to a solver, the solver will attempt to finalize it with the default configuration.


function Model [2/4]

ssqp::model::Model::Model (
    uint horizon,
    double dt,
    const  Config & config
) 

Model Config Constructor

Initializes model with default structure and the given configuration

Input:

  • horizon: The time horizon to optimize over

  • dt: Time step per solver stage

  • config: The system configuration

Note:

This constructor finalizes the model. When using this constructor to finalize the model, the finalize method must not be called afterwards.


function Model [3/4]

ssqp::model::Model::Model (
    uint horizon,
    double dt,
    const  Structure & structure,
    const  Config & config
) 

Full Model Constructor

Initializes a model with the given structure and finalizes the model with the given configuration

Input:

  • horizon: The time horizon to optimize over

  • dt: Time step per solver stage

  • structure: The structure of the problem

  • config: The system configuration

Note:

This constructor finalizes the model. When using this constructor to finalize the model, the finalize method must not be called afterwards.


function Model [4/4]

ssqp::model::Model::Model (
    const  Model &
) = delete

function addConstraints [1/2]

template<typename  C, typename... Args>
std::shared_ptr< stagewise::StageConstraints > ssqp::model::Model::addConstraints (
    const  uint k,
    Args &&... args
) 

function addConstraints [2/2]

void ssqp::model::Model::addConstraints (
    const  uint k,
    const std::shared_ptr< stagewise::StageConstraints > & constraints
) 

function addCost [1/2]

template<typename  L, typename... Args>
std::shared_ptr< stagewise::StageCost > ssqp::model::Model::addCost (
    const  uint k,
    Args &&... args
) 

function addCost [2/2]

void ssqp::model::Model::addCost (
    const  uint k,
    const std::shared_ptr< stagewise::StageCost > & cost
) 

function addForce

std::shared_ptr< Force > ssqp::model::Model::addForce (
    const  uint k,
    const std::string & frame
) 

function addRunningCost [1/2]

template<typename  L, typename... Args>
std::shared_ptr< stagewise::StageCost > ssqp::model::Model::addRunningCost (
    Args &&... args
) 

function addRunningCost [2/2]

void ssqp::model::Model::addRunningCost (
    const std::shared_ptr< stagewise::StageCost > & cost
) 

function addWrench

std::shared_ptr< Force > ssqp::model::Model::addWrench (
    const  uint k,
    const std::string & frame
) 

function centerOfMass

const Vec3 ssqp::model::Model::centerOfMass (
    const VecX & q
) const

function constraints

const  ConstraintsContainer & ssqp::model::Model::constraints () const

function costs

const  CostsContainer & ssqp::model::Model::costs () const

function dDifference

void ssqp::model::Model::dDifference (
    const VecX & q0,
    const VecX & q1,
    MatX & J,
    const  uint arg_pos
) const

function difference

VecX ssqp::model::Model::difference (
    const VecX & q0,
    const VecX & q1
) const

function dims

const Dimensions & ssqp::model::Model::dims () noexcept const

function dt

double ssqp::model::Model::dt () const

function dynamics

const  DynamicsContainer & ssqp::model::Model::dynamics () const

function finalize

void ssqp::model::Model::finalize (
    const  Config & config=Config::Default()
) 

finalize method

Finalizes the model with the given configuration

Input:

  • config: The system configuration

Note:

This method must only be called once, in the case that the model is not finalized, before passing the model to a solver. If it is not called, the default configuration will be used.


function finalized

bool ssqp::model::Model::finalized () const

function floatingBase

bool ssqp::model::Model::floatingBase () const

function forwardDynamics [1/3]

const VecX ssqp::model::Model::forwardDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & tau
) const

function forwardDynamics [2/3]

const VecX ssqp::model::Model::forwardDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & tau,
    const std::unordered_map< std::string, std::vector< Vec3 > > & forces
) const

function forwardDynamics [3/3]

const VecX ssqp::model::Model::forwardDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & tau,
    const std::vector< Vec6 > & joint_forces
) const

function forwardKinematics [1/2]

const std::pair< Vec3, Mat3 > ssqp::model::Model::forwardKinematics (
    const VecX & q,
    const std::string & frame
) const

function forwardKinematics [2/2]

const std::pair< Vec3, Mat3 > ssqp::model::Model::forwardKinematics (
    const VecX & q,
    const  uint frame_id
) const

function forwardKinematicsSE3 [1/2]

const Mat4 ssqp::model::Model::forwardKinematicsSE3 (
    const VecX & q,
    const std::string & frame
) const

function forwardKinematicsSE3 [2/2]

const Mat4 ssqp::model::Model::forwardKinematicsSE3 (
    const VecX & q,
    const  uint frame_id
) const

function frameId

uint ssqp::model::Model::frameId (
    const std::string & frame
) const

function frameJacobian [1/8]

const MatX ssqp::model::Model::frameJacobian (
    const VecX & q,
    const std::string & frame,
    bool local=false
) const

function frameJacobian [2/8]

const MatX ssqp::model::Model::frameJacobian (
    const VecX & q,
    const  uint frame_id,
    bool local=false
) const

function frameJacobian [3/8]

void ssqp::model::Model::frameJacobian (
    const VecX & q,
    const std::string & frame,
    bool local,
    Eigen::Ref< MatX > J
) const

function frameJacobian [4/8]

void ssqp::model::Model::frameJacobian (
    const VecX & q,
    const  uint frame_id,
    bool local,
    Eigen::Ref< MatX > J
) const

function frameJacobian [5/8]

const MatX ssqp::model::Model::frameJacobian (
    const std::string & frame,
    bool local=false
) const

function frameJacobian [6/8]

const MatX ssqp::model::Model::frameJacobian (
    const  uint frame_id,
    bool local=false
) const

function frameJacobian [7/8]

void ssqp::model::Model::frameJacobian (
    const std::string & frame,
    bool local,
    Eigen::Ref< MatX > J
) const

function frameJacobian [8/8]

void ssqp::model::Model::frameJacobian (
    const  uint frame_id,
    bool local,
    Eigen::Ref< MatX > J
) const

function frameVelocity [1/2]

const Vec6 ssqp::model::Model::frameVelocity (
    const VecX & q,
    const VecX & v,
    const std::string & frame,
    bool local=false
) const

function frameVelocity [2/2]

const Vec6 ssqp::model::Model::frameVelocity (
    const VecX & q,
    const VecX & v,
    const  uint frame_id,
    bool local=false
) const

function frameVelocityDerivatives [1/4]

std::pair< MatX, MatX > ssqp::model::Model::frameVelocityDerivatives (
    const VecX & q,
    const VecX & v,
    const std::string & frame,
    bool local=false
) const

function frameVelocityDerivatives [2/4]

std::pair< MatX, MatX > ssqp::model::Model::frameVelocityDerivatives (
    const VecX & q,
    const VecX & v,
    const  uint frame_id,
    bool local=false
) const

function frameVelocityDerivatives [3/4]

void ssqp::model::Model::frameVelocityDerivatives (
    const VecX & q,
    const VecX & v,
    const std::string & frame,
    bool local,
    Eigen::Ref< MatX > dVdq,
    Eigen::Ref< MatX > dVdv
) const

function frameVelocityDerivatives [4/4]

void ssqp::model::Model::frameVelocityDerivatives (
    const VecX & q,
    const VecX & v,
    const  uint frame_id,
    bool local,
    Eigen::Ref< MatX > dVdq,
    Eigen::Ref< MatX > dVdv
) const

function functions

const std::vector< std::shared_ptr< stagewise::Function > > & ssqp::model::Model::functions () const

function getActuatedTorqueVector

VecX ssqp::model::Model::getActuatedTorqueVector (
    const VecX & x,
    const VecX & u,
    const  uint k=0
) const

Returns the actuated-joint torque vector implied by the inverse dynamics computation for the given state and controls, accounting for any forces present at stage k

For InverseDynamics mode, this is the RNEA torque computed directly from the (full) acceleration control. For ReducedInverseDynamics mode, the full acceleration vector is reconstructed first (see getFullAccelerationVector), and only the actuated joints' torque is returned, since by construction the unactuated joints' torque is always zero

Input:

  • x: The model state

  • u: The model controls

  • k: The model stage (only required if there are forces present at that stage)

Note: XML-based function Note: Assumes the model is in InverseDynamics or ReducedInverseDynamics mode


function getActuatedTorqueVectorDerivatives

void ssqp::model::Model::getActuatedTorqueVectorDerivatives (
    const VecX & x,
    const VecX & u,
    MatX & dtau_dx,
    MatX & dtau_du,
    const  uint k=0
) const

Derivatives of getActuatedTorqueVector with respect to the state (x) and controls (u)

Output:

  • dtau_dx: derivative with respect to the state (nu x 2*nv)

  • dtau_du: derivative with respect to the controls (nu x u.size())


function getAllDynamicParameters

const VecX ssqp::model::Model::getAllDynamicParameters () const

Returns a stacked vector of the current values of the loaded model's dynamic parameters for every link This consist of n vectors of size 10, containing the following: \(\boldsymbol{v} = [m, m_{c,x}, m_{c,y}, m_{c_z}, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^\top\) for each link


function getFullAccelerationVector

VecX ssqp::model::Model::getFullAccelerationVector (
    const VecX & x,
    const VecX & u,
    const  uint k=0
) const

Returns the full acceleration vector for a given stage Input:

  • x: The model state

  • u: The model controls

  • k: The model stage (only required if model is in ReducedInverseDynamics mode and there are present forces)

Note: For InverseDynamics mode, this function is trivial, since it only returns the non-force part of the control vector Note: XML-based function


function getJointTorqueRegressor

const MatX ssqp::model::Model::getJointTorqueRegressor (
    const VecX & q,
    const VecX & v,
    const VecX & a
) const

Computes and returns the joint torque regressor matrix linking the joint torque to the dynamic parameters of each link for the given configuration, velocity and acceleration Input:

  • q: The model configuration

  • v: The model velocity

  • a: The model acceleration

Returns: The joint torque regressor matrix


function getMatrixActuatedPart [1/2]

MatX ssqp::model::Model::getMatrixActuatedPart (
    const MatX & M
) const

function getMatrixActuatedPart [2/2]

void ssqp::model::Model::getMatrixActuatedPart (
    const MatX & M,
    Eigen::Ref< MatX > out
) const

function hasCustomProperties

bool ssqp::model::Model::hasCustomProperties () const

function hasFrame

bool ssqp::model::Model::hasFrame (
    const std::string & frame
) const

function horizon

const  uint ssqp::model::Model::horizon () const

function inverseDynamics [1/3]

const VecX ssqp::model::Model::inverseDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & a
) const

function inverseDynamics [2/3]

const VecX ssqp::model::Model::inverseDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & a,
    const std::unordered_map< std::string, std::vector< Vec3 > > & forces
) const

function inverseDynamics [3/3]

const VecX ssqp::model::Model::inverseDynamics (
    const VecX & q,
    const VecX & v,
    const VecX & a,
    const std::vector< Vec6 > & joint_forces
) const

function inverseDynamicsDerivatives [1/2]

const  void ssqp::model::Model::inverseDynamicsDerivatives (
    const Eigen::VectorXd & q,
    const Eigen::VectorXd & v,
    const Eigen::VectorXd & a,
    Eigen::MatrixXd & rnea_partial_dq,
    Eigen::MatrixXd & rnea_partial_dv,
    Eigen::MatrixXd & rnea_partial_da
) const

function inverseDynamicsDerivatives [2/2]

const  void ssqp::model::Model::inverseDynamicsDerivatives (
    const Eigen::VectorXd & q,
    const Eigen::VectorXd & v,
    const Eigen::VectorXd & a,
    const std::vector< Eigen::Vector< double , 6 > > & fext,
    Eigen::MatrixXd & rnea_partial_dq,
    Eigen::MatrixXd & rnea_partial_dv,
    Eigen::MatrixXd & rnea_partial_da
) const

function inverseKinematics

const VecX ssqp::model::Model::inverseKinematics (
    const VecX & desired_translation,
    const MatX & desired_rotation,
    const std::string & joint
) 

function jacobianCenterOfMass

const MatX ssqp::model::Model::jacobianCenterOfMass (
    const VecX & q
) const

function jointTypes

std::vector< std::string > ssqp::model::Model::jointTypes () const

function linear_dims

const Dimensions & ssqp::model::Model::linear_dims () noexcept const

function neutralPose

const VecX ssqp::model::Model::neutralPose () const

function nforces

uint ssqp::model::Model::nforces (
    const  uint k
) const

function njoints

uint ssqp::model::Model::njoints () const

function nq

uint ssqp::model::Model::nq () const

function nu

uint ssqp::model::Model::nu () const

function nv

uint ssqp::model::Model::nv () const

function nwrenches

uint ssqp::model::Model::nwrenches (
    const  uint k
) const

function operator=

Model & ssqp::model::Model::operator= (
    const  Model &
) = delete

function orderedActuatedJoints

std::vector< std::string > ssqp::model::Model::orderedActuatedJoints () const

function orderedBodyFrames

std::vector< std::string > ssqp::model::Model::orderedBodyFrames () const

function orderedForceFrames

std::vector< std::string > ssqp::model::Model::orderedForceFrames (
    const  uint k
) const

function orderedJoints

std::vector< std::string > ssqp::model::Model::orderedJoints () const

function orderedWrenchFrames

std::vector< std::string > ssqp::model::Model::orderedWrenchFrames (
    const  uint k
) const

function printFramesAndJoints

void ssqp::model::Model::printFramesAndJoints () const

function randomControls

VecX ssqp::model::Model::randomControls (
    const  uint stage
) const

function randomState

VecX ssqp::model::Model::randomState (
    const  uint stage
) const

function setActuatedJoints

void ssqp::model::Model::setActuatedJoints (
    const std::vector< uint > & actuated_joints
) 

function setAllDynamicParameters

void ssqp::model::Model::setAllDynamicParameters (
    const VecX & parameters
) 

Sets the dynamic parameters for every link of the loaded model to the provided stacked vector Input:

  • parameters: A stacked vector of the dynamic parameters for each link of the model

function setCustomProperties

void ssqp::model::Model::setCustomProperties (
    const  CustomProperties & custom_properties
) 

function setInitControl

void ssqp::model::Model::setInitControl (
    const  uint k,
    const VecX & init_control
) 

function setInitState

void ssqp::model::Model::setInitState (
    const  uint k,
    const VecX & init_state
) 

function solverMode

SolverMode ssqp::model::Model::solverMode () const

function terrain

const std::shared_ptr< terrain::Terrain > ssqp::model::Model::terrain () const

function total_joints

uint ssqp::model::Model::total_joints () const

function updateDimensions

void ssqp::model::Model::updateDimensions () 

Updates the model's dimensions and linear dimensions This is required to update constraint sizes, when constraints' enabled state changes

TODO: this currently assumes only constraint sizes can change


function xml

bool ssqp::model::Model::xml () const


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