ssqpy.model.costs
costs ¶
Submodule containing classes for assigning costs to optimization stages.
This includes the general StageCost class, as well as predefined costs
for ease of use.
StageCost ¶
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
set_enabled ¶
Set enabled status for StageCost
A stage always keeps at least one cost, so this has no effect if every
cost on the stage is disabled
evaluate ¶
Evaluate function \(f(\boldsymbol{x}, \boldsymbol{u})\)
Returns: output vector, \(n \times 1\)
gradient ¶
Get cost Jacobians with respect to \(\boldsymbol{x}\) and \(\boldsymbol{u}\), evaluated at
given \((\boldsymbol{x}, \boldsymbol{u})\): \(\nabla_x \ell_k(\boldsymbol{x}, \boldsymbol{u})\) and \(\nabla_u \ell_k(\boldsymbol{x}, \boldsymbol{u})\)
Returns:
dx: gradient with respect to \(\boldsymbol{x}\), \(n_{dx} \times 1\)
du: gradient with respect to \(\boldsymbol{u}\), \(n_u \times 1\)
hessian_xx ¶
Get cost function hessian matrix with respect to \(\boldsymbol{xx}\), evaluated at given
\((\boldsymbol{x}, \boldsymbol{u})\):
\(\nabla_{xx}^2 \ell_k(\boldsymbol{x}, \boldsymbol{u})\)
Returns:
Hxx: Hessian matrix, \(n_{dx} \times n_{dx}\)
hessian_xu ¶
Get cost function hessian matrix with respect to \(\boldsymbol{xu}\), evaluated at given
\((\boldsymbol{x}, \boldsymbol{u})\):
\(\nabla_{xu}^2 \ell_k(\boldsymbol{x}, \boldsymbol{u})\)
Returns:
Hxu: Hessian matrix, \(n_{dx} \times n_u\)
hessian_uu ¶
Get cost function hessian matrix with respect to \(\boldsymbol{uu}\), evaluated at given
\((\boldsymbol{x}, \boldsymbol{u})\):
\(\nabla_{uu}^2 \ell_k(\boldsymbol{x}, \boldsymbol{u})\)
Returns:
Huu: Hessian matrix, \(n_u \times n_u\)
SquaredJointAngleCost ¶
Bases: StageCost
Predefined squared joint angle cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \boldsymbol{x}_{ang} \rVert_2^2\)
where:
- \(\boldsymbol{x}_{ang}\) is the angular (position) part of the state Note that this will ignore the first 7 elements if the model has a floating base
- \(\boldsymbol{w}\) is the weight on the angular term
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq()), appearing after the floating base (if present)
SquaredJointVelocityCost ¶
Bases: StageCost
Predefined squared joint velocity cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \boldsymbol{x}_{vel} \rVert_2^2\)
where:
- \(\boldsymbol{x}_{vel}\) is the velocity part of the state
- \(\boldsymbol{w}\) is the weight on the velocity term
Assumes:
- The model joint velocity is the final part of the state vector (size
model.nv())
SquaredControlCost ¶
Bases: StageCost
Predefined squared control cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \boldsymbol{u}_{ctrl} \rVert_2^2\)
where:
- \(\boldsymbol{u}_{ctrl}\) is the control part of the controls
- \(\boldsymbol{w}\) is the weight on the control term
Assumes:
- The model control is the first part of the control vector (size
model.nu())
SquaredIDTorqueCost ¶
Bases: StageCost
Predefined squared inverse dynamics torque cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \boldsymbol{\tau} \rVert_2^2\)
where:
- \(\boldsymbol{\tau}\) is the actuated-joint torque caused by the controls
- \(\boldsymbol{w}\) is the weight on the torque term
Note that this cost assumes inverse dynamics is being used (i.e. treats
controls as accelerations)
Assumes:
- The model is in Inverse Dynamics or Reduced Inverse Dynamics mode, such that the state is made up of configuration and velocity and the controls are made up of accelerations (including base, for Inverse Dynamics; actuated joints only, for Reduced Inverse Dynamics) and potential forces
k (the model stage) only matters when there are forces present at
that stage. Construct one instance per stage (rather than sharing a
single instance across stages via Model.addCost) whenever forces
vary between the stages a cost is added to
SquaredForceCost ¶
Bases: StageCost
Predefined squared force cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \boldsymbol{u}_{f} \rVert_2^2\)
where:
- \(\boldsymbol{u}_{f}\) is the force part of the controls
- \(\boldsymbol{w}\) is the weight on the force term
Assumes:
- The model forces make up the second and final part of the control vector,
following the control part of size
model.nu(), for any remaining elements
SquaredConfigurationErrorCost ¶
Bases: StageCost
Predefined squared configuration error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{q} - \boldsymbol{q}_d)\rVert_2^2\)
where:
- \(\boldsymbol{q}\) is the configuration part of the state
- \(\boldsymbol{q}_d\) is the desired configuration
- \(\boldsymbol{w}\) is the weight for the configuration error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FloatingBaseSquaredTranslationErrorCost ¶
Bases: StageCost
Predefined squared floating base translation error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{t}_f - \boldsymbol{t}_d)\rVert_2^2\)
where:
- \(\boldsymbol{t}_f\) is the floating base translation (in \(\mathcal{R}^3\))
- \(\boldsymbol{t}_d\) is the desired translation of the floating base (in \(\mathcal{R}^3\))
- \(\boldsymbol{w}\) is the weight on the translation error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq()) - The floating base translation is extracted from the base pose of the model state representation
FloatingBaseSquaredRotationErrorCost ¶
Bases: StageCost
Predefined squared floating base rotation error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot \log_{SO(3)}(R_f^\top R_d)\rVert_2^2\)
where:
- \(R_f\) is the floating base rotation (in \(\mathit{SO3}\))
- \(R_d\) is the desired rotation of the floating base (in \(\mathit{SO3}\))
- \(\boldsymbol{w}\) is the weight on the rotation error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq()) - The floating base orientation is extracted from the base pose of the model state representation
FloatingBaseSquaredPoseErrorCost ¶
Bases: StageCost
Predefined squared floating base pose error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} D^\top W D\)
where:
- \(D = \log_{SE(3)} T^{-1}T_d\)
- \(T\) is the floating base pose (in \(\mathit{SE3}\))
- \(T_d\) is the desired floating base pose (in \(\mathit{SE3}\))
- \(W\) is a weight matrix, made up of parameters \(w_p\) for translation and \(w_r\) for rotation
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FloatingBaseSquaredVelocityErrorCost ¶
Bases: StageCost
Predefined squared floating base velocity error cost
Defines the cost term
\(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{v}_b - \boldsymbol{v}_d)\rVert_2^2\)
where:
- \(\boldsymbol{v}_b\) is the floating base velocity (in \(\mathcal{R}^6\))
- \(\boldsymbol{v}_d\) is the desired floating base velocity (in \(\mathcal{R}^6\))
- \(\boldsymbol{w}\) is the weight on the velocity error
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())
FrameSquaredTranslationErrorCost ¶
Bases: StageCost
Predefined squared frame translation error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{t} - \boldsymbol{t}_d)\rVert_2^2\)
where:
- \(\boldsymbol{t}\) is the frame translation for a given frame of the model (in \(\mathcal{R}^3\))
- \(\boldsymbol{t}_d\) is the desired translation for that frame (in \(\mathcal{R}^3\))
- \(\boldsymbol{w}\) is the weight on the translation error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FrameSquaredTerrainHeightErrorCost ¶
Bases: StageCost
Predefined squared frame terrain height error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} w^2 (p_z - h(p_x, p_y) - d_d)^2\)
where:
- \(\boldsymbol{p}\) is the frame translation for a given frame of the model (in \(\mathcal{R}^3\))
- \(h\) is the height of the model's terrain at the frame's horizontal position
- \(d_d\) is the desired height of the frame above the terrain
- \(w\) is the weight on the height error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FrameSquaredRotationErrorCost ¶
Bases: StageCost
Predefined squared frame rotation error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert w \log_{SO(3)}R^\top R_d\rVert_2^2\)
where:
- \(R\) is the frame rotation for a given frame of the model (in \(\mathit{SO3}\))
- \(R_d\) is the desired rotation for that frame (in \(\mathit{SO3}\))
- \(\boldsymbol{w}\) is the weight on the rotation error
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FrameSquaredPoseErrorCost ¶
Bases: StageCost
Predefined squared frame pose error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} D^\top W D\)
where:
- \(D = \log_{SE(3)} T^{-1}T_d\)
- \(T\) is the frame pose for a given frame of the model (in \(\mathit{SE3}\))
- \(T_d\) is the desired pose for that frame (in \(\mathit{SE3}\))
- \(W\) is a weight matrix, made up of parameters \(w_p\) for translation and \(w_r\) for rotation
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FrameSquaredVelocityErrorCost ¶
Bases: StageCost
Predefined squared frame velocity error cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{v}_f - \boldsymbol{v}_d)\rVert_2^2\)
where:
- \(\boldsymbol{v}_f\) is the frame velocity for a given frame of the model (in \(\mathcal{R}^6\))
- \(\boldsymbol{v}_d\) is the desired velocity for that frame (in \(\mathcal{R}^6\))
- \(\boldsymbol{w}\) is the weight on the velocity error
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())
CenterOfMassTrackingCost ¶
Bases: StageCost
Predefined center of mass (CoM) tracking cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \frac{1}{2} \lVert \boldsymbol{w} \odot (\boldsymbol{c} - \boldsymbol{c}_r)\rVert_2^2\)
where:
- \(\boldsymbol{c}\) is the center of mass for the model (in \(\mathcal{R}^3\))
- \(\boldsymbol{c}_r\) is the reference point for the center of mass (in \(\mathcal{R}^3\))
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())
FramesInverseSquaredDistanceCost ¶
Bases: StageCost
Predefined frames squared inverse distance cost
Defines the cost term \(\ell(\boldsymbol{x}, \boldsymbol{u}) = \sum_{i < j} \frac{w}{2 \lVert \boldsymbol{\sigma} \odot (\boldsymbol{t}_i - \boldsymbol{t}_j)\rVert_2^2}\)
where:
- \(\boldsymbol{t}_i\) is the frame translation for frame \(i\) of the model (in \(\mathcal{R}^3\))
- \(w\) is the weight on the translation error
- \(\boldsymbol{\sigma}\) is the distance scale, a per-axis vector (in \(\mathcal{R}^3\)) determining the scaling factor for each axis; the factors work in an inverse manner, i.e. larger values place less importance on an axis, while smaller ones place more
Assumes:
- The model joint configuration is the first part of the state vector (size
model.nq())