Limits

Limits implemented as inequality constraints in the IK problem.

Kinematic limits derive from the Limit base class. They compute QP inequality constraints of the form:

\[G(q) \Delta q \leq h(q)\]

where \(q \in {\cal C}\) is the robot’s configuration and \(\Delta q \in T_q({\cal C})\) is the displacement in the tangent space at \(q\).

class pink.limits.AccelerationLimit(model, acceleration_limit)

Subset of acceleration-limited joints in a robot model.

Acceleration limits consists of two parts: the expected \(|a| \leq a_{\mathrm{max}}\), but also the following term accounting for the “breaking distance” to configuration limits. This additional inequality is detailed in [Flacco2015] as well as in [DelPrete2018].

Delta_q_prev: Latest displacement of the robot.

a_max: Maximum acceleration vector for acceleration-limited joints.

has_configuration_limit: Whether each acceleration-limited joint also has a real configuration limit, in which case the “braking distance” term applies. Continuous (unbounded) joints do not have one.

indices: Tangent indices corresponding to acceleration-limited joints.

model: Robot model.

projection_matrix: Projection from tangent space to subspace with acceleration-limited joints.

compute_qp_inequalities(configuration, dt)

Compute inequalities for acceleration limits.

Those limits are defined by two constraints. First, the finite-difference approximation of the acceleration is bounded by the robot’s acceleration-limit vector \(a_{\mathrm{max}} \in {\cal T}_q({\cal C})\) (which belongs to the tangent space at the current configuration \(q \in \mathcal{C}\)):

\[- a_{\mathrm{max}} \leq \frac{\frac{\Delta q}{\mathrm{d} t} - \frac{\Delta q_{\mathrm{prev}}}{\mathrm{d} t}}{\mathrm{d} t} \leq a_{\mathrm{max}}\]

where \(\Delta q \in T_q({\cal C})\) is the displacement computed by the inverse kinematics, and \(\Delta q_{\mathrm{prev}}\) is the displacement from our previous iteration of differential IK.

Second, our new velocity \(\Delta q / \mathrm{d} t\) should not exceed the “braking distance” until configuration limits:

\[-\sqrt{2 a_{\mathrm{max}} (q \ominus q_{\min})} \leq \frac{\Delta q}{\mathrm{d} t} \leq \sqrt{2 a_{\mathrm{max}} (q_{\max} \ominus q)}\]

This additional inequality is detailed in [Flacco2015] as well as in [DelPrete2018].

Parameters:

configuration (Configuration) – Robot configuration.
dt (float) – Integration timestep in [s].

Return type:

Optional[Tuple[ndarray, ndarray]]

Returns:

Pair \((G, h)\) representing the inequality constraint as \(G \Delta q \leq h\), or None if there is no limit.

set_last_integration(v_prev, dt)

Set the latest velocity and the duration it was applied for.

The goal of the low-acceleration task is to minimize the difference between the new velocity and the previous one.

Parameters:

v_prev (ndarray) – Latest integrated velocity.
dt – Integration timestep in [s].

Return type:

None

class pink.limits.ConfigurationLimit(model, config_limit_gain=0.5)

Maintain the configuration \(q\) between bounds.

A configuration limit can be written \(q_{min} \leq q \leq q_{max}\) when the configuration space is homeomorphic to \(\mathbb{R}^n\). For instance, this is the case when all joints are revolute and \(\mathcal{C} = (S^1)^n \cong \mathbb{R}^n\).

Since this is not always the case, the limit is defined on the tangent space of displacements (equivalently velocities) as:

\[{q \ominus q_{min}} \leq \Delta q \leq {q_{max} \ominus q}\]

When the current configuration \(q\) is within bounds, the limit ensures that the next configuration \(q \oplus \Delta q\) stays within bounds as well.

Note

This limit assumes the current configuration \(q\) is within bounds. This is why pink.configuration.Configuration.check_limits() is called with the safety break by default when solving IK. If \(q\) is out of bounds, the configuration limit still works, but it may yield a large displacement \(\Delta q\) incompatible with e.g. a velocity limit.

config_limit_gain: gain between 0 and 1 to steer away from configuration limits. It is described in “Real-time prioritized kinematic control under inequality constraints for redundant manipulators” (Kanoun, 2012). More details in this writeup.

model: Robot model the limit applies to.

joints: Joints with configuration limits.

projection_matrix: Projection from tangent space to subspace with configuration-limited joints.

compute_qp_inequalities(configuration, dt)

Compute the configuration-dependent velocity limits.

Those limits are returned as:

\[{q \ominus q_{min}} \leq \Delta q \leq {q_{max} \ominus q}\]

where \(q \in {\cal C}\) is the robot’s configuration and \(\Delta q \in T_q({\cal C})\) is the displacement in the tangent space at \(q\). These limits correspond to the derivative of \(q_{min} \leq q \leq q_{max}\).

Parameters:

configuration (Configuration) – Robot configuration.
dt (float) – Integration timestep in [s].

Return type:

Optional[Tuple[ndarray, ndarray]]

Returns:

Pair \((G, h)\) representing the inequality constraint as \(G \Delta q \leq h\), or None if there is no limit.

class pink.limits.FloatingBaseVelocityLimit(model, base_frame, max_linear_velocity, max_angular_velocity)

Velocity limits applied to the robot floating base.

compute_qp_inequalities(configuration, dt)

Linearize floating base velocity bounds.

Return type:: Optional[Tuple[ndarray, ndarray]]

class pink.limits.Limit

Abstract base class for kinematic limits.

abstractmethod compute_qp_inequalities(configuration, dt)

Compute limit as linearized QP inequalities.

Those limits are returned as:

\[G(q) \Delta q \leq h(q)\]

where \(q \in {\cal C}\) is the robot’s configuration and \(\Delta q \in T_q({\cal C})\) is the displacement in the tangent space at \(q\).

Parameters:

configuration (Configuration) – Robot configuration providing up-to-date kinematics.
dt (float) – Integration timestep in [s].

Return type:

Optional[Tuple[ndarray, ndarray]]

Returns:

Pair \((G, h)\) representing the inequality constraint as \(G \Delta q \leq h\), or None if there is no limit.

class pink.limits.VelocityLimit(model, velocity_limit=None)

Subset of velocity-limited joints in a robot model.

indices: Tangent indices corresponding to velocity-limited joints.

joints: List of velocity-limited joints.

model: Robot model.

projection_matrix: Projection from tangent space to subspace with velocity-limited joints.

velocity_limit: Velocity-limit vector used to bound the joints: the constructor argument when one is given, otherwise the model’s.

compute_qp_inequalities(configuration, dt)

Compute inequalities for velocity limits.

Those limits are defined by:

\[-\mathrm{d}t v_{max} \leq \Delta q \leq \mathrm{d}t v_{max}\]

where \(v_{max} \in {\cal T}\) is the robot’s velocity limit vector and \(\Delta q \in T_q({\cal C})\) is the displacement computed by the inverse kinematics.

Parameters:

configuration (Configuration) – Robot configuration (unused).
dt (float) – Integration timestep in [s].

Return type:

Optional[Tuple[ndarray, ndarray]]

Returns:

Pair \((G, h)\) representing the inequality constraint as \(G \Delta q \leq h\), or None if there is no limit.