Barriers

Note

Head over to Pink for the SelfCollisionBarrier, as Pinker does not implement it currently.

Control Barrier Functions.

class pinker.barriers.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.

abstractmethod 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)\).

abstractmethod 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)\).

class pinker.barriers.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)\).

class pinker.barriers.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)\).