giskardpy.qp.enforcement_strategy

giskardpy.qp.enforcement_strategy#

Enforcement strategies that turn high-level constraints into QP matrix rows, slack columns, and bounds (EnforcementStrategy and subclasses).

Classes#

`EnforcementStrategy`	Turns a block of constraints into the QP building blocks that enforce them: the
`ExpressionEnforcementStrategy`	Base for strategies that map per-constraint expressions onto QP rows and read their
`IntegralStrategy`	Enforces a constraint on the integral of an expression's derivative over the prediction
`VelocityStrategy`	The constraint will be applied to the derivative of the expression.
`SystemDynamicsStrategy`	The constraints produced by this class describe the discrete-time relationships between variables

Functions#

normalize_slack_weight(weight, normalization_factor[, ...]) → krrood.symbolic_math.symbolic_math.Scalar

Scales a slack weight so constraints with different units become comparable.

Module Contents#

giskardpy.qp.enforcement_strategy.normalize_slack_weight(weight: krrood.symbolic_math.symbolic_math.Scalar, normalization_factor: float, control_horizon: int = 1) → krrood.symbolic_math.symbolic_math.Scalar#

Scales a slack weight so constraints with different units become comparable.

The control horizon spreads the weight over the time steps a constraint is active.

class giskardpy.qp.enforcement_strategy.EnforcementStrategy#

Bases: abc.ABC

Turns a block of constraints into the QP building blocks that enforce them: the constraint matrix over the free variables, the slack columns and their limits, the bounds, and the row names.

Subclasses define how a constraint is mapped onto the prediction horizon.

degrees_of_freedom: list[semantic_digital_twin.world_description.degree_of_freedom.DegreeOfFreedom]#: Free variables of the robot the constraints act on.

constraints: list[giskardpy.qp.constraint.GiskardConstraint]#: Constraints enforced by this strategy.

qp_controller_config: giskardpy.qp.qp_controller_config.QPControllerConfig#: Controller configuration providing horizon length and time step.

abstract create_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the constraint matrix mapping the free variables onto the constraint rows.

abstract create_slack_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the matrix coupling the slack variables to the constraint rows.

abstract create_names() → list[str]#: Creates a debug name for every constraint row.

abstract create_slack_variables() → giskardpy.qp.dof_limits.DirectLimits#: Creates the limits and weights of the slack variables introduced by this strategy.

property number_of_free_variables: int#: Number of degrees of freedom controlled by this strategy.

property number_of_velocity_columns: int#: Number of velocity decision variable columns across the horizon.

property number_of_jerk_columns: int#: Number of jerk decision variable columns across the horizon.

property position_variables: krrood.symbolic_math.symbolic_math.Vector#: Symbolic position variables of the degrees of freedom.

property velocity_variables: krrood.symbolic_math.symbolic_math.Vector#: Symbolic velocity variables of the degrees of freedom.

property acceleration_variables: krrood.symbolic_math.symbolic_math.Vector#: Symbolic acceleration variables of the degrees of freedom.

class giskardpy.qp.enforcement_strategy.ExpressionEnforcementStrategy#

Bases: EnforcementStrategy, abc.ABC

Base for strategies that map per-constraint expressions onto QP rows and read their bounds through a getter.

Provides the inequality and equality bound builders shared by the expression-based strategies.

abstract create_bounds(bounds_getter: Callable[[giskardpy.qp.constraint.GiskardConstraint], krrood.symbolic_math.symbolic_math.Scalar]) → krrood.symbolic_math.symbolic_math.Vector#: Builds the constraint bounds, reading the relevant bound of each constraint via the getter.

lower_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardInequalityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#: The lower bound this strategy enforces for an inequality constraint.

upper_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardInequalityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#: The upper bound this strategy enforces for an inequality constraint.

equality_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardEqualityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#: The bound this strategy enforces for an equality constraint.

create_lower_bounds() → krrood.symbolic_math.symbolic_math.Vector#: Builds the lower bounds of the inequality constraints.

create_upper_bounds() → krrood.symbolic_math.symbolic_math.Vector#: Builds the upper bounds of the inequality constraints.

create_equality_bounds() → krrood.symbolic_math.symbolic_math.Vector#: Builds the bounds of the equality constraints.

class giskardpy.qp.enforcement_strategy.IntegralStrategy#

Bases: ExpressionEnforcementStrategy

Enforces a constraint on the integral of an expression’s derivative over the prediction horizon, so a single row covers the whole horizon.

Equality constraints have the form: .. math:

f(q) = b

where

\[target - f = \Delta t \ \sum_{k=0}^{N-3} J_{f} + \Delta t \epsilon\]

|   k1   |   k2   |   k1   |   k2   |   k1   |   k2   | prediction horizon
|v1 v2 v3|v1 v2 v3|j1 j2 j3|j1 j2 j3|   eps  |   eps  | free variables / slack
|--------+--------+--------+--------+--------+--------|
|  J1*dt |  J1*dt |  J3*dt | J3*dt  |   dt   |   dt   |
|-----------------------------------------------------|

create_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the constraint matrix by repeating the expression jacobian across the velocity horizon and padding the jerk columns with zeros.

create_slack_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the diagonal slack matrix with one slack variable per constraint.

create_slack_variables() → giskardpy.qp.dof_limits.DirectLimits#: Creates one slack variable per constraint, bounded by the constraint’s slack limits, with normalized weights.

reachable_change_of_constraint(constraint: giskardpy.qp.constraint.GiskardConstraint) → krrood.symbolic_math.symbolic_math.Scalar#: The largest change of a constraint’s expression that may be demanded within the control horizon: its normalization factor sustained over that horizon.

lower_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardInequalityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#

A positive lower bound demands motion, so it is capped to the reachable change.

A negative one only permits motion and is kept, however far away it is.

upper_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardInequalityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#

A negative upper bound demands motion, so it is capped to the reachable change.

A positive one only permits motion and is kept, however far away it is.

equality_bound_of_constraint(constraint: giskardpy.qp.constraint.GiskardEqualityConstraint) → krrood.symbolic_math.symbolic_math.Scalar#: An equality bound always demands motion, so it is capped to the reachable change by the constraint’s own bound cap.

create_bounds(bounds_getter: Callable[[giskardpy.qp.constraint.GiskardConstraint], krrood.symbolic_math.symbolic_math.Scalar]) → krrood.symbolic_math.symbolic_math.Vector#: Builds the bounds, one per constraint.

create_names() → list[str]#: Returns the constraint names, one row per constraint.

class giskardpy.qp.enforcement_strategy.VelocityStrategy#

Bases: ExpressionEnforcementStrategy

The constraint will be applied to the derivative of the expression. Position constraints are implemented by constraining the integral of the expressions’ derivative over a prediction horizon. All other constraints are applied directly to that derivative of the expression. As a result, position constraints are cheaper, as they only require a single constraint.

Equality constraints have the form: .. math:

f(q) = b

where

\[target - f = \Delta t \ \sum_{k=0}^{N-1} J_{f}\]

|   t1   |   t2   |   t3   |   t1   |   t2   |   t3   |   t1   |   t2   |   t3   | prediction horizon
|v1 v2 v3|v1 v2 v3|v1 v2 v3|a1 a2 a3|a1 a2 a3|a1 a2 a3|j1 j2 j3|j1 j2 j3|j1 j2 j3| free variables
|--------------------------------------------------------------------------------|
|  Jv*dt |        |        |  Ja*dt |        |        |  Jj*dt |        |        |
|  Jv*dt |        |        |  Ja*dt |        |        |  Jj*dt |        |        |
|--------------------------------------------------------------------------------|
|        |  Jv*dt |        |        |  Ja*dt |        |        |  Jj*dt |        |
|        |  Jv*dt |        |        |  Ja*dt |        |        |  Jj*dt |        |
|--------------------------------------------------------------------------------|

create_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the constraint matrix applying the expression jacobian at each velocity step of the horizon, padding the jerk columns with zeros.

create_slack_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the diagonal slack matrix with one slack variable per constraint and velocity step.

create_slack_variables() → giskardpy.qp.dof_limits.DirectLimits#: Creates one slack variable per constraint and velocity step with normalized weights.

create_bounds(bounds_getter: Callable[[giskardpy.qp.constraint.GiskardConstraint], krrood.symbolic_math.symbolic_math.Scalar]) → krrood.symbolic_math.symbolic_math.Vector#: Builds the bounds, one per constraint and control step.

create_names() → list[str]#: Creates a name per constraint and control step, prefixed with the time step.

class giskardpy.qp.enforcement_strategy.SystemDynamicsStrategy#

Bases: EnforcementStrategy

The constraints produced by this class describe the discrete-time relationships between variables in the prediction horizon \(N\) using a semi-implicit euler integration method:

\[ \begin{align}\begin{aligned}v_k = v_{k-1} + a_{k} \, \Delta t\\a_k = a_{k-1} + j_{k} \, \Delta t\end{aligned}\end{align} \]

Where v, a and j are velocity, acceleration and jerk, respectively, and k is the time step. Acceleration variables are removed using substitution. The first two rows links the MPC to the current state:

\[ \begin{align}\begin{aligned}-v_{c} - a_{c} \, \Delta t = -v_0 + j_0 \, \Delta t^2\\v_{c} = - v_1 + 2 v_0 + j_1 \, \Delta t^2\end{aligned}\end{align} \]

Rows from 2 until k-2 have this form:

\[0 = - v_k + 2 v_{k-1} - v_{k-2} + j_k \, \Delta t^2\]

The final two rows have this form:

\[ \begin{align}\begin{aligned}0 = 2 v_{k-1} - v_{k-2} + j_k \, \Delta t^2\\0 = - v_{k-2} + j_k \, \Delta t^2\end{aligned}\end{align} \]

For a prediction horizon of 5 with 1 degree of freedom, the matrix looks like this:

|  equality_bounds |   |           equality constraint matrix          |   |    v_0    |
|------------------|   |-----------------------------------------------|   |    v_1    |
| - v_c - a_c * dt |   | -1  |     |     |  1  |     |     |     |     |   |    v_2    |
|       v_c        |   |  2  | -1  |     |     |  1  |     |     |     |   | j_0*dt**2 |
|        0         | = | -1  |  2  | -1  |     |     |  1  |     |     | @ | j_1*dt**2 |
|        0         |   |     | -1  |  2  |     |     |     |  1  |     |   | j_2*dt**2 |
|        0         |   |     |     | -1  |     |     |     |     |  1  |   | j_3*dt**2 |
|------------------|   |-----------------------------------------------|   | j_4*dt**2 |

This means that the QP does not optimize jerk, but jerk \(*\Delta t^2\), this improves the conditioning of the constraint matrix, which helps some solvers.

create_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Builds the equality matrix encoding the velocity, acceleration, and jerk integration over the horizon.

create_slack_matrix() → krrood.symbolic_math.symbolic_math.Matrix#: Returns an empty slack matrix, as the system dynamics need no slack variables.

create_slack_variables() → giskardpy.qp.dof_limits.DirectLimits#: Returns empty limits, as the system dynamics introduce no slack variables.

create_names() → list[str]#: Creates a name for every velocity/jerk integration row.

create_equality_bounds() → krrood.symbolic_math.symbolic_math.Vector#: Builds the equality bounds linking the first horizon steps to the current state.