ssqpy.model

model

Submodule containing classes related to the SSQP model, a respresentation of the
system used for optimization.

SolverMode

Bases: Enum

Solver Mode
The mode the solver uses for the system formulation, corresponding to the
equality constraint of the NLP
This can either be full dynamics, or a kinematics formulation
Options:

• Kinematics: The solver optimizes in a kinematic mode. This means there are no system dynamics. Instead, the generated controls correspond to velocities, while the state corresponds to the system configuration.
• ForwardDynamics: The solver optimizes with a forward dynamics approach. The state corresponds to a stacked vector of the system configuration and velocity. The generated controls correspond to torques which, when applied over time to the system, generate the output trajectory.
• InverseDynamics: The solver optimizes with an inverse dynamics approach. The state corresponds to a stacked vector of the system configuration and velocity. The generated controls correspond to accelerations. This simplifies the LQR formulation and can sometimes lead to more robust solutions. Inverse dynamics is used as a constraint, ensuring the torques producing the generated acceleration controls are within the required effort limits.
• ReducedInverseDynamics: This mode is similar to the Inverse dynamics mode. In this mode, the floating base (if present) and/or some subset of the model's joints are unactuated. While regular Inverse mode enforces this via equality constraints, this mode instead enforces this implicitly through state propagation.

Integration

Bases: Enum

The type of integration used for the system dynamics
Options:

• ExplicitEuler: \(\boldsymbol{x}_{n+1}=\boldsymbol{x}_n+\boldsymbol{v}_ndt\) \(\boldsymbol{v}_{n+1}=\boldsymbol{v}_n+\boldsymbol{a}_ndt\)
• SemiImplicitEuler: \(\boldsymbol{x}_{n+1}=\boldsymbol{x}_n+\boldsymbol{v}_{n+1}dt\) \(\boldsymbol{v}_{n+1}=\boldsymbol{v}_n+\boldsymbol{a}_ndt\)

Preset

Bases: Enum

Preset
Use a preset urdf instead of supplying a path
Mutually exclusive withurdf_path, mjcf_path and free_flyer, since
each preset fixes those
Options:

• NONE: No preset selected (default)
• Go2: Unitree Go2 quadruped
• A1: Unitree A1 quadruped
• Panda: Franka Emika Panda arm
• G1: Unitree G1 humanoid
• Talos: Talos humanoid
• DoublePendulum: Double pendulum

ForceDim

Bases: Enum

The number of control-vector components a Force occupies
Options:

• Linear: A pure point force (e.g. ground contact), no moment
• Wrench: A full spatial force (linear + angular), e.g. rotor thrust plus reaction/drag torque

Force

A external force added to a model stage via Model.addForce/addWrench.

frame property

frame

The frame this force is applied at.

force_index property

force_index

Index of this force among those of the same type (force/wrench) added at the same stage.

dim property

dim

The number of components this force has.

CustomProperties

CustomProperties()

Optional custom properties for non-XML models.

nq property writable

nq

Joint configuration size.

nv property writable

nv

Joint velocity size

nu property writable

nu

Control size

njoints property writable

njoints

Number of actuated joints.

floating_base property writable

floating_base

Whether the model has a floating base.

ordered_joints property writable

ordered_joints

Ordered joint names.

joint_types property writable

joint_types

The types of joints corresponding to each of the ordered joints.

Model

Model(horizon, dt, **kwargs)

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.

Initialize an SSQP Model.

Positional arguments (required):
horizon (int): Optimization horizon (excluding initial stage)
dt (float): Optimization time step

Keyword arguments (optional):
urdf_path (str): Path to URDF file to use for the system dynamics
mjcf_path (str): Path to MJCF file to use for the system dynamics
free_flyer (bool): Whether the model has a floating base
from_preset (ssqpy.model.Preset): Use a preset urdf instead of
supplying a path; mutually exclusive withurdf_path,
mjcf_path and free_flyer
solver_mode (ssqpy.model.SolverMode): The solver mode to solve in
integration (ssqpy.model.Integration): The integration method to use
ignore_innate_constraints (bool): Whether to ignore XML constraints
actuated_joints (list of int): List of joints to apply controls to;
if non-empty, all joints whose indices are not included will not be
actuated

finalized property

finalized

Whether the model has been finalized.

xml property

xml

Whether the model has been loaded from an XML file.

horizon property

horizon

Get the optimization horizon.

dt property

dt

Get the optimization time step.

terrain property

terrain

Get the model's terrain.

finalize

finalize(**kwargs)

Finalize the model with the provided configuration.

Keyword arguments (optional):
costs (list of StageCost): Costs for each optimization stage
stage_cost (StageCost): Cost to add to each non-terminal stage
terminal_cost (StageCost): Cost for the terminal stage
constraints (list of StageConstraints): Constraints for each optimization stage
dynamics (list of StageDynamics): Dynamics for each optimization stage
custom_lower_position_bounds (np.array of float): Lower position bounds to use in place
of the values read from the XML; default XML values are used if left
empty; has no effect if innate constraints are ignored
custom_upper_position_bounds (np.array of float): Upper position bounds to use in place
of the values read from the XML; default XML values are used if left
empty; has no effect if innate constraints are ignored
custom_velocity_bounds (np.array of float): Velocity bounds to use in place of the
values read from the XML; default XML values are used if left empty;
has no effect if innate constraints are ignored
custom_torque_bounds (np.array of float): Torque bounds to use in place of the values
read from the XML; default XML values are used if left empty; has no
effect if innate constraints are ignored
init_states (np.array or list of np.array): Initial states for each optimization stage;
if np.array: initial state to use for every stage
init_controls (np.array or list of np.array): Initial controls for each optimization stage;
if np.array: initial controls to use for every stage

updateDimensions

updateDimensions()

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

addCost

addCost(k, cost)

Add a cost to a stage.

addRunningCost

addRunningCost(cost)

Add a cost to every non-terminal stage.

addConstraints

addConstraints(k, constraints)

Add constraints to a stage.

addContactConstraint

addContactConstraint(k, frame, mu=0.6, max_normal_force=float('inf'), eq_tol=0.0, no_slip=True, name='')

Add a contact constraint for a frame to a stage.
This includes adding a force for the contact constraint at that stage.
Unless no_slip is false, the constraint also bounds the linear velocity
of the frame to zero so that it does not slip.

getStateSize

getStateSize(k)

Get the state size of the model.

getLinearStateSize

getLinearStateSize(arg)

Get the state size of the model after state linearization.

getControlSize

getControlSize(k)

Get the control size of the model.

getConstraintSize

getConstraintSize(k)

Get the constraint size of the model.

getLinearConstraintSize

getLinearConstraintSize(arg)

Get the constraint size of the model after linearization.

getnq

getnq()

Get the configuration size of the model.
Only defined if the model was initialized using an XML file.

getnv

getnv()

Get the velocity size of the model.
Only defined if the model was initialized using an XML file.

getnu

getnu()

Get the non-force control size of the model.
Only defined if the model was initialized using an XML file.

getnjoints

getnjoints()

Get the number of actuated joints of the model.
Only defined if the model was initialized using an XML file.

hasFloatingBase

hasFloatingBase(arg)

Whether the model has a floating base.
Only defined if the model was initialized using an XML file.

getSolverMode

getSolverMode()

Get the solver mode formulation

getCost

getCost(k)

Get the current model cost at a stage.

getDynamics

getDynamics(k)

Get the current model dynamics at a stage.

getConstraints

getConstraints(k)

Get the current model constraints at a stage.

setInitState

setInitState(k, init_state)

Set initial state for stage k (must be finalized)

setInitControl

setInitControl(k, init_state)

Set initial state for stage k (must be finalized)

addForce

addForce(k, frame)

Add a linear force (ForceDim.Linear) on a frame at a stage.
Only defined if the model was initialized using an XML file.
For this method to be called, the model must not be finalized.

addWrench

addWrench(k, frame)

Add a full spatial wrench (ForceDim.Wrench, force plus moment) on a
frame at a stage.
Only defined if the model was initialized using an XML file.
For this method to be called, the model must not be finalized.

getTotalForces

getTotalForces(k)

Get the current number of forces for a stage.
Only defined if the model was initialized using an XML file.

getOrderedForceFrames

getOrderedForceFrames(k)

Vector containing frame names, ordered 1-1 with forces for stage k

printFramesAndJoints

printFramesAndJoints()

Print the frames and joints of the model.
Only defined if the model was initialized using an XML file.

getOrderedActuatedJoints

getOrderedActuatedJoints()

Get an ordered list of the actuated joints of the model.
Only defined if the model was initialized using an XML file.

getJointTypes

getJointTypes()

Types for each joint that is part of the state (including unactuated)

hasFrame

hasFrame(frame)

Whether the model has a frame with the given name

setActuatedJoints

setActuatedJoints(actuated_joints)

Set the actuated joint indices vector

forwardKinematics

forwardKinematics(q, frame)

Perform forward kinematics on the model.
Only defined if the model was initialized using an XML file.

forwardKinematicsSE3

forwardKinematicsSE3(q, frame)

Perform forward kinematics on the model and return a transformation matrix.
Only defined if the model was initialized using an XML file.

centerOfMass

centerOfMass(q)

Get model center of mass for the given configuration

frameJacobian

frameJacobian(q, frame, local=False)

Get a frame jacobian based on the XML model dynamics

jacobianCenterOfMass

jacobianCenterOfMass(q)

Get model center of mass jacobian for the given configuration

getAllDynamicParameters

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

setAllDynamicParameters

setAllDynamicParameters(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

getJointTorqueRegressor

getJointTorqueRegressor(q, v, a)

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

getFullAccelerationVector

getFullAccelerationVector(x, u, k=0)

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 Reduced Inverse mode and there are present forces)

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

getActuatedTorqueVector

getActuatedTorqueVector(x, u, k=0)

Returns the actuated joint torques that realize the given controls
Input:

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

The torques come from the inverse dynamics, accounting for the forces
present at stage k. In Reduced Inverse mode, the full acceleration
vector is reconstructed first (see getFullAccelerationVector)

Note: Only defined in Inverse and Reduced Inverse dynamics modes
Note: XML-based function

neutralPose

neutralPose()

Get a configuration for the model.
Only defined if the model was initialized using an XML file.

setCustomProperties

setCustomProperties(arg)

Set custom properties for non-XML models

hasCustomProperties

hasCustomProperties()

Whether custom properties have been set