ssqpy.model.constraints

constraints

Submodule containing classes for assigning constraints to optimization
stages. This includes the general StageConstraints class, as well as
predefined constraints for ease of use.

StageConstraints

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

names

names()

Get all constraints' names for StageConstraints.

set_enabled

set_enabled(enabled)

Set enabled status for StageConstraints

enabled

enabled()

Whether this constraint is enabled

output_size

output_size()

Get the constraints function output size.

lower_bound

lower_bound()

Get the constraints' lower bound vector.

upper_bound

upper_bound()

Get the constraints' upper bound vector.

constraint_diff_size

constraint_diff_size()

Size of the difference vector: \(\boldsymbol{c}_k(\boldsymbol{x}_1, \boldsymbol{u}_1) \ominus \boldsymbol{c}_k(\boldsymbol{x}_2, \boldsymbol{u}_2)\)
Where \(\ominus\): the defined right_minus operator

right_minus

right_minus(x, y)

The \(\ominus\) operator, which outputs the difference of two constraint
vectors in tangent space

evaluate

evaluate(x, u)

Evaluate function \(f(\boldsymbol{x}, \boldsymbol{u})\)
Returns: output vector, \(n \times 1\)

gradient

gradient(x, u)

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

hessprod_xx

hessprod_xx(x, u, mu)

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

hessprod_xu

hessprod_xu(x, u, mu)

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

hessprod_uu

hessprod_uu(x, u, mu)

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

JointAngleConstraint

JointAngleConstraint(model, lb, ub, name='')

Bases: StageConstraints

Predefined joint angle constraint

Creates a constraint on the joint angles of the model

Note: this constraint is separate to any potential innate constraints
\(\boldsymbol{q}_l \leq \boldsymbol{q} \leq \boldsymbol{q}_u\)

where:

• \(\boldsymbol{q}_l\), \(\boldsymbol{q}_u\): Lower and upper configuration vectors
• \(\boldsymbol{q}\): The model's joint configuration vector

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq()), appearing after the floating base (if present)

JointVelocityConstraint

JointVelocityConstraint(model, lb, ub, name='')

Bases: StageConstraints

Predefined joint velocity constraint

Creates a constraint on the joint velocities of the model

Note: this constraint is separate to any potential innate constraints
\(\boldsymbol{q}_{v,l} \leq \boldsymbol{q}_v \leq \boldsymbol{q}_{v, u}\)

where:

• \(\boldsymbol{q}_{v,l}\), \(\boldsymbol{q}_{v,u}\): Lower and upper joint velocity vectors
• \(\boldsymbol{q}_v\): The model's joint velocity vector

Assumes:

• The model joint velocity is the final part of the state vector (size model.nv())

JointControlConstraint

JointControlConstraint(model, lb, ub, name='')

Bases: StageConstraints

Predefined joint control constraint

Creates a constraint on the joint part of the control vector

This can either act as a torque constraint (FD mode) or an acceleration
constraint (ID mode)

Note: this constraint is separate to any potential innate constraints
\(\boldsymbol{u}_{c,l} \leq \boldsymbol{u}_c \leq \boldsymbol{u}_{c, u}\)

where:

• \(\boldsymbol{u}_{c,l}\), \(\boldsymbol{u}_{c,u}\): Lower and upper joint control vectors
• \(\boldsymbol{u}_c\): The joint part of the control vector

Assumes:

• The model control is the first part of the control vector (size model.nu())

FloatingBaseTranslationBound

FloatingBaseTranslationBound(model, lb, ub, name='')

Bases: StageConstraints

Floating base position bound constraint

Creates a linear bound constraint on the position of the floating base:
\(\boldsymbol{b}_l \leq p_f \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors on the base position (in \(\mathcal{R}^3\))
• \(p_f\): Floating base position extracted from the system state

Assumes:

• The Model contains a floating-base where the base position is the first part of the configuration vector (first 3 elements of size model.nq())

FloatingBaseRotationBound

FloatingBaseRotationBound(model, Rref, lb=..., ub=..., name='')

Bases: StageConstraints

Predefined floating base orientation bound

Creates a nonlinear bound constraint on the orientation of the floating base:
\(\boldsymbol{b}_l \leq \log_{SO3}(R_{ref}^\top R_f) \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors on the orientation error (in \(\mathcal{R}^3\))
• \(R_f\): Floating base rotation matrix
• \(R_{ref}\): Reference rotation matrix

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The floating base orientation is represented as a rotation matrix in SE(3) and can be extracted from the base pose in the system state

FloatingBasePoseBound

Bases: StageConstraints

Predefined floating base SE(3) pose bound

Creates a nonlinear bound constraint on the full pose (position + orientation)
of the floating base:
\(\boldsymbol{b}_l \leq \log_{SE3}(T_{ref}^{-1} T_f) \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors on the pose error (in \(\mathcal{R}^6\))
• \(T_f\): Floating base pose in SE(3)
• \(T_{ref}\): Reference pose in SE(3)

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The floating base pose is represented as an SE(3) transform and can be extracted from the base state in the system model

FloatingBaseLinearVelocityBound

FloatingBaseLinearVelocityBound(model, lb, ub, name='')

Bases: StageConstraints

Predefined floating base linear velocity bound

Creates a linear bound constraint on the linear velocity of the floating base:
\(\boldsymbol{b}_l \leq \boldsymbol{v}_b \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{v}_b\): Floating base linear velocity vector (in \(\mathcal{R}^3\))

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The model joint velocity is the final part of the state vector (size model.nv())

FloatingBaseAngularVelocityBound

FloatingBaseAngularVelocityBound(model, lb, ub, name='')

Bases: StageConstraints

Predefined floating base angular velocity bound

Creates a linear bound constraint on the angular velocity of the floating base:
\(\boldsymbol{b}_l \leq \boldsymbol{\omega}_b \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{\omega}_b\): Floating base angular velocity vector (in \(\mathcal{R}^3\))

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The model joint velocity is the final part of the state vector (size model.nv())

FrameTranslationBound

FrameTranslationBound(model, frame, lb, ub, name='')

Bases: StageConstraints

Predefined 3D position bound for a given frame

Creates a linear bound constraint on the position of a frame of the given
model: \(\boldsymbol{b}_l \leq \boldsymbol{p}_f \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{p}_f\): Frame position vector (in \(\mathcal{R^3}\))

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())

FrameTerrainHeightBound

FrameTerrainHeightBound(model, frame, lb, ub, name='')

Bases: StageConstraints

Predefined terrain height bound for a given frame

Creates a bound constraint on the height of a frame above the model's
terrain: \(b_l \leq p_z - h(p_x, p_y) \leq b_u\)

where:

• \(b_l\), \(b_u\): Lower and upper bounds on the height above the terrain
• \(\boldsymbol{p}_f\): Frame position vector (in \(\mathcal{R^3}\))
• \(h\): Terrain height at the frame's horizontal position

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())

FrameRotationBound

FrameRotationBound(model, frame, Rref, lb=..., ub=..., name='')

Bases: StageConstraints

Predefined 3D orientation bound for a given frame

Creates a nonlinear bound constraint on the orientation of a frame:
\(\boldsymbol{b}_l \leq \log_{SO3}R_{ref}^\top R_f \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors on the orientation error (in \(\mathcal{R}^3\))
• \(R_f\): Frame rotation matrix
• \(R_{ref}\): Reference rotation matrix

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())

FramePoseBound

Bases: StageConstraints

Predefined SE(3) pose bound for a given frame

Creates a nonlinear bound constraint on the pose of a frame:
\(\boldsymbol{b}_l \leq \log_{SE3}(T_{ref}^{-1} T_f) \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors on the pose error (in \(\mathcal{R}^6\))
• \(T_f\): Frame pose in SE(3)
• \(T_{ref}\): Reference pose in SE(3)

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())

FrameLinearVelocityBound

FrameLinearVelocityBound(model, frame, lb, ub, name='')

Bases: StageConstraints

Predefined 3D velocity bound for a given frame

Creates a linear bound constraint on the velocity of a frame of the given
model: \(\boldsymbol{b}_l \leq \boldsymbol{v}_f \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{v}_f\): Frame velocity vector (in \(\mathcal{R^3}\))

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The model joint velocity is the final part of the state vector (size model.nv())

FrameAngularVelocityBound

FrameAngularVelocityBound(model, frame, lb, ub, name='')

Bases: StageConstraints

Predefined angular velocity bound for a given frame

Creates a linear bound constraint on the angular velocity of a frame of the
given model:
\(\boldsymbol{b}_l \leq \boldsymbol{\omega}_f \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{\omega}_f\): Frame angular velocity vector (in \(\mathcal{R}^3\))

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())
• The model joint velocity is the final part of the state vector (size model.nv())

CenterOfMassBound

CenterOfMassBound(model, lb, ub, name='')

Bases: StageConstraints

Predefined center of mass bound

Creates a linear bound constraint on the center of mass of the model:
\(\boldsymbol{b}_l \leq \boldsymbol{c} \leq \boldsymbol{b}_u\)

where:

• \(\boldsymbol{b}_l\), \(\boldsymbol{b}_u\): Lower and upper bound vectors (in \(\mathcal{R}^3\))
• \(\boldsymbol{c}\) is the center of mass of the model

Assumes:

• The model joint configuration is the first part of the state vector (size model.nq())

ContactConstraint

Bases: StageConstraints

Predefined contact constraint for a given frame
Creates a set of constraints that amount to a contact force constraint for
a frame
This consists of the following 4-point linearized friction cone constraints:

• \(f_n \gt 0\)
• \(|f_1| \leq \mu f_n\)
• \(|f_2| \leq \mu f_n\) As well as the following constraint on the 3D position of the frame the contact is on:
• \(p_z = h_z\) And, unless no_slip is false, the following no-slip constraint on the linear velocity of the frame:
• \(\boldsymbol{v}_f = \boldsymbol{0}\) where:
• \(f_n\) is the normal of the contact force (\(f_z\))
• \(f_1\) is the first component of the tangential contact force (\(f_x\))
• \(f_2\) is the second component of the tangential contact force (\(f_y\))
• \(\mu\) is the static friction coefficient at the contact
• \(p_z\) is the \(z-\)component of the translation of the frame the contact is on
• \(h_z\) is the height of the terrain at point \((p_x, p_y)\)
• \(\boldsymbol{v}_f\) is the linear velocity of the frame the contact is on Note that this constraint cannot be manually added in the Python library, use the Model's addContactConstraint method instead.