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
#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]¶
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]¶
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¶
function addRunningCost [1/2]¶
template<typename L, typename... Args>
std::shared_ptr< stagewise::StageCost > ssqp::model::Model::addRunningCost (
Args &&... args
)
function addRunningCost [2/2]¶
function addWrench¶
function centerOfMass¶
function constraints¶
function costs¶
function dDifference¶
void ssqp::model::Model::dDifference (
const VecX & q0,
const VecX & q1,
MatX & J,
const uint arg_pos
) const
function difference¶
function dims¶
function dt¶
function dynamics¶
function finalize¶
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¶
function floatingBase¶
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]¶
function frameId¶
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]¶
function frameJacobian [6/8]¶
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¶
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]¶
function getMatrixActuatedPart [2/2]¶
function hasCustomProperties¶
function hasFrame¶
function horizon¶
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¶
function jointTypes¶
function linear_dims¶
function neutralPose¶
function nforces¶
function njoints¶
function nq¶
function nu¶
function nv¶
function nwrenches¶
function operator=¶
function orderedActuatedJoints¶
function orderedBodyFrames¶
function orderedForceFrames¶
function orderedJoints¶
function orderedWrenchFrames¶
function printFramesAndJoints¶
function randomControls¶
function randomState¶
function setActuatedJoints¶
function setAllDynamicParameters¶
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¶
function setInitControl¶
function setInitState¶
function solverMode¶
function terrain¶
function total_joints¶
function 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¶
The documentation for this class was generated from the following file /home/runner/work/ssqp/ssqp/include/model/Model.hpp