Barriers

Definition

All barriers derive from the Barrier base class.

The formalism used in this implementation is written down in https://simeon-ned.com/blog/2024/cbf/

class pink.barriers.barrier.Barrier(dim, gain=1.0, gain_function=None, safe_displacement_gain=0.0)

Abstract base class for barrier.

A barrier is a function \(h(q)\) that satisfies the following condition:

\[\frac{\partial h_j}{\partial q} \dot{q} +\alpha_j(h_j(q)) \geq 0, \quad \forall j\]

where \(\frac{\partial h_j}{\partial q}\) are the Jacobians of the constraint functions, \(\dot{q}\) is the joint velocity vector, and \(\alpha_j\) are extended class kappa functions.

On top of that, following this article barriers utilize safe displacement term is added to the cost of the optimization problem:

\[\frac{r}{2\|J_h\|^{2}}\|dq-dq_{\text{safe}}(q)\|^{2},\]

where \(J_h\) is the Jacobian of the barrier function, dq is the joint displacement vector, and \(dq_{\text{safe}}(q)\) is the safe displacement vector.

dim: Dimension of the barrier.

gain: linear barrier gain.

gain_function: function, that defines stabilization term as nonlinear function of barrier. Defaults to the (linear) identity function.

safe_displacement: Safe backup displacement.

safe_displacement_gain: positive gain for safe backup displacement.

abstract compute_barrier(configuration)

Compute the value of the barrier function.

The barrier function \(h(q)\) is a vector-valued function that represents the safety constraints. It should be designed such that the set \(\{q : h(q) \geq 0\}\) represents the safe region of the configuration space.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Value of the barrier function \(h(q)\).

abstract compute_jacobian(configuration)

Compute the Jacobian matrix of the barrier function.

The Jacobian matrix \(\frac{\partial h}{\partial q}(q)\) of the barrier function with respect to the configuration variables is required for the computation of the barrier condition.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Jacobian matrix \(\frac{\partial h}{\partial q}(q)\).

compute_qp_inequalities(configuration, dt=0.001)

Compute the linear inequality constraints for the barrier-based QP.

The linear inequality constraints enforce the barrier conditions:

\[\frac{\partial h_j} {\partial q} \dot{q} + \alpha_j(h_j(q)) \geq 0, \quad \forall j\]

where \(\frac{\partial h_j}{\partial q}\) are the Jacobians of the constraint functions, \(\dot{q}\) is the joint velocity vector, and \(\alpha_j\) are extended class K functions.

Note

Jacobian and barrier values are cached to avoid recomputation.

Parameters:

configuration (Configuration) – Robot configuration \(q\).
dt (float) – Time step for discrete-time implementation. Defaults to 1e-3.

Return type:

Tuple[ndarray, ndarray]

Returns:

Tuple containing the inequality constraint matrix (G): and vector (h).

compute_qp_objective(configuration)

Compute the quadratic objective function for the barrier-based QP.

The quadratic objective function includes a regularization term based on the safe backup policy:

\[\gamma(q)\left\| \dot{q}- \dot{q}_{safe}(q)\right\|^{2}\]

where \(\gamma(q)\) is a configuration-dependent weight and \(\dot{q}_{safe}(q)\) is the safe backup policy.

Note

If safe_displacement_gain is set to zero, the regularization term is not included. Jacobian and barrier values are cached to avoid recomputation.

Parameters:

configuration (Configuration) – Robot configuration \(q\).
dt – Time step for discrete-time implementation. Defaults to 1e-3.

Return type:

Tuple[ndarray, ndarray]

Returns:

Tuple containing the quadratic objective matrix (H) and linear: objective vector (c).

compute_safe_displacement(configuration)

Compute the safe backup displacement.

The safe backup control displacement \(dq_{safe}(q)\) is a joint displacement vector that can guarantee that system would stay in safety set.

By default, it is set to zero, since it could not violate safety set.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Safe backup joint velocities: \(\dot{q}_{safe}(q)\).

Position barrier

Frame position barrier.

class pink.barriers.position_barrier.PositionBarrier(frame, indices=None, p_min=None, p_max=None, gain=1.0, safe_displacement_gain=0.0)

A position-based barrier.

Defines a barrier function based on the position of a specified frame in the world coordinate system. It allows for the specification of minimum and maximum position bounds along selected axes.

frame: Name of the frame to monitor.

indices: Indices of the position components to consider.

p_min: Minimum position bounds.

p_max: Maximum position bounds.

compute_barrier(configuration)

Compute the value of the barrier function.

The barrier function is computed based on the position of the specified frame in the world coordinate system. It considers the minimum and maximum position bounds along the selected axes.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Value of the barrier function \(h(q)\).

compute_jacobian(configuration)

Compute the Jacobian matrix of the barrier function.

The Jacobian matrix is computed based on the position Jacobian of the specified frame. The Jacobian is transformed to align with the world coordinate system and only the selected indices are considered.

Parameters:: configuration (Configuration) – Robot configuration \(q\).
Return type:: ndarray
Returns:: Jacobian matrix \(\frac{\partial h}{\partial q}(q)\).

Body Spherical barrier

Barrier based on the distance between two frames.

class pink.barriers.body_spherical_barrier.BodySphericalBarrier(frames, d_min, gain=1.0, safe_displacement_gain=3.0)

A barrier based on the distance between two frames.

Defines a barrier function based on the Euclidean distance between two specified frames. It allows for the specification of a minimum distance threshold.

The barrier function is defined as:

\[h(q) = \|p_1(q) - p_2(q)\|^2 - d_{min}^2\]

where \(p_1(q)\) and \(p_2(q)\) are the positions of the two frames in the world coordinate system, and \(d_{min}\) is the minimum distance threshold.

frames: Tuple of two frame names.

d_min: Minimum distance threshold.

compute_barrier(configuration)

Compute the value of the barrier function.

The barrier function is computed based on the Euclidean distance between the two specified frames. It considers the minimum distance threshold.

The barrier function is given by:

\[h(q) = \|p_1(q) - p_2(q)\|^2 - d_{min}^2\]

where \(p_1(q)\) and \(p_2(q)\) are the positions of the two frames in the world coordinate system, and \(d_{min}\) is the minimum distance threshold.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Value of the barrier function: \(h(q)\).

compute_jacobian(configuration)

Compute the Jacobian matrix of the barrier function.

The Jacobian matrix is computed based on the position Jacobians of the two specified frames. The Jacobians are transformed to align with the world coordinate system.

The Jacobian matrix is given by:

\[\begin{split}\frac{\partial h}{\partial q}(q) = 2(p_1(q) - p_2(q))^T \begin{bmatrix} \frac{\partial p_1}{\partial q} (q) \\ -\frac{\partial p_2}{\partial q} (q) \end{bmatrix}\end{split}\]

where \(\frac{\partial p_1}{\partial q}(q)\) and \(\frac{\partial p_2}{\partial q}(q)\) are the position Jacobians of the two frames.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Jacobian matrix: \(\frac{\partial h}{\partial q}(q)\).

Whole Body Collision Avoidance

Self Collision Avoidance Barrier with hpp-fcl.

class pink.barriers.self_collision_barrier.SelfCollisionBarrier(n_collision_pairs, gain=1.0, safe_displacement_gain=1.0, d_min=0.02)

Self Collision Avoidance Barrier.

This class defines a barrier function based on the smooth convex collision geometry. using hpp-fcl through Pinocchio. Note that for non-smooth collision geometries behaviour is undefined.

The barrier function is defined as:

\[\begin{split}h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\ \ldots \end{bmatrix} \quad \forall i \in 0 \ldots N\end{split}\]

where \(N\) is number of collision pairs, \(p^k_i\) is the \(k-th\) body in \(i\)-th collision pair, \(d(p^1_i, p^2_i)\) is the distance between collision bodies in the pair, and \(d_{min}\) is minimal distance between any collision bodies.

Note

The number of evaluated collision pairs might not be equal to the number of all collision pairs. If the former is lower, then only the closest collision pairs will be considered.

d_min: Minimum distance between collision pairs.

compute_barrier(configuration)

Compute the value of the barrier function.

The barrier function is computed as the vector of lowest distances between collision pairs. It is defined as:

\[\begin{split}h(q) = \begin{bmatrix} \ldots \\ d(p^1_i, p^2_i) - d_{min}^2 \\ \ldots \end{bmatrix} \quad \forall i \in 1 \ldots N\end{split}\]

where \(N\) is number of collision pairs, \(p^k_i\) is the \(k-th\) body in \(i\)-th collision pair, \(d(p^1_i, p^2_i)\) is the distance between collision bodies in the pair, and \(d_{min}\) is the minimal distance between any collision bodies.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Value of the barrier function: \(h(q)\).

compute_jacobian(configuration)

Compute the Jacobian matrix of the barrier function.

The Jacobian matrix could be represented as stacked gradients of each collision pair distance function w.r.t. joints. They are computed based on joints Jacobians and normal surface vector at nearest distance points n.

The gradient, a.k.a. the i-th row in the Jacobian matrix, is given by:

\[J_i = n_1^T J^1_p + (r_1 \times n_1)^T J^1_w + n_2^T J^2_p + (r_2 \times n_2)^T J^2_w,\]

where \(n_{1,2}\) are normal vectors (note that \(n_1 = -n_2\)), \(r_{1, 2}\) are vectors from joint origin and nearest point, \(J^{1, 2}_{p, w}\) are position/orientation Jacobians of respective joint.

Parameters:

configuration (Configuration) – Robot configuration \(q\).

Return type:

ndarray

Returns:

Jacobian matrix: \(\frac{\partial h}{\partial q}(q)\).