Capsule Collision Avoidance

In trajectory planning for 3D scenarios such as robot arms and drones, capsules are commonly used to bound links, airframes, or rod-shaped obstacles. A capsule is the solid consisting of all points whose distance to a line segment (the axis) does not exceed the radius $r$. Compared with using multiple bounding spheres, a single capsule can compactly bound a slender object, and its distance computation is simple.

The following two capsule collision avoidance inequality constraints are supported:

• point_to_capsule_inequality: Point-to-capsule collision avoidance inequality constraint
• capsule_to_capsule_inequality: Capsule-to-capsule collision avoidance inequality constraint

Both constraints use the exact signed distance: when separated, it is the shortest distance between the two; when overlapping, it is the negative penetration depth. The endpoints and radius of a capsule can be constants or expressions of parameters or optimization variables, so static obstacles, dynamic obstacles, and robot arm self-collision can all be handled.

Capsule Definition

A capsule is defined through the capsule_3d interface:

capsule = capsule_3d(p1, p2, radius)

Argument	Description
p1, p2	The two endpoints of the axis, which can be lists/tuples of length 3 or 3D vectors (e.g., Matrix)
radius	Radius of the capsule, which must be nonnegative when it is a number

info

• Each element of p1, p2 and radius can be a constant or an expression of parameters or optimization variables. For example, the geometry of an obstacle capsule can be passed in through parameters, and the endpoints of a robot arm link capsule can be forward kinematics expressions of the joint angles.
• When p1 and p2 coincide, the capsule degenerates into a sphere; when radius is 0, the capsule degenerates into a line segment.

Point-to-Capsule Collision Avoidance

The point-to-capsule collision avoidance feature enables collision avoidance constraints between a point and a capsule, ensuring that the point does not enter the region occupied by the capsule.

Modeling

Below is an example of a point-to-capsule collision avoidance inequality constraint:

p2c_ineq = point_to_capsule_inequality(
    point, capsule,
    distance_to_avoid,
    use_hessian=False
)
prob.inequality(p2c_ineq)

The input arguments of the point_to_capsule_inequality interface are as follows:

• point: Coordinates $(p_x, p_y, p_z)$ of the point to be kept clear, in the same format as the endpoints of capsule_3d
• capsule: Capsule defined by capsule_3d
• distance_to_avoid: Avoidance distance, which can be a constant or an expression of parameters or optimization variables
• use_hessian: bool, whether to provide the analytic Hessian of the distance, False by default

The inequality constraint constructed by this interface is:

$$d = \min_{t \in [0, 1]} \left\lVert p - p_1 - t\,(p_2 - p_1) \right\rVert - r \ge d_{safe}$$

where $p$ is the point to be kept clear, $p_1$, $p_2$, and $r$ are the axis endpoints and radius of the capsule, respectively, and $d_{safe}$ is the avoidance distance. When the point is outside the capsule, $d$ is the distance from the point to the capsule surface; when the point is inside the capsule, $d$ is the negative penetration depth.

Capsule-to-Capsule Collision Avoidance

The capsule-to-capsule collision avoidance feature enables collision avoidance constraints between two capsules, ensuring that the two capsules do not collide.

Modeling

Below is an example of a capsule-to-capsule collision avoidance inequality constraint:

c2c_ineq = capsule_to_capsule_inequality(
    capsule_a, capsule_b,
    distance_to_avoid,
    use_hessian=False
)
prob.inequality(c2c_ineq)

The input arguments of the capsule_to_capsule_inequality interface are as follows:

• capsule1, capsule2: Two capsules defined by capsule_3d (i.e., capsule a and capsule b in the figure below); their order can be swapped
• distance_to_avoid: Avoidance distance, which can be a constant or an expression of parameters or optimization variables
• use_hessian: bool, whether to provide the analytic Hessian of the distance, False by default

The inequality constraint constructed by this interface is:

$$d = \min_{s, t \in [0, 1]} \left\lVert p_{a1} + s\,(p_{a2} - p_{a1}) - p_{b1} - t\,(p_{b2} - p_{b1}) \right\rVert - r_a - r_b \ge d_{safe}$$

That is, the shortest distance between the two axes minus the radii of the two capsules. When the two capsules are separated, $d$ is the shortest distance between their surfaces; when they overlap, $d$ is the negative penetration depth.

warning

When the two axes are parallel, the closest points between the axes are not unique, and the distance is not differentiable there. When the capsule orientations are optimization variables, in rare cases the iterations may oscillate near the parallel configuration until the maximum number of iterations is reached, although the iterates still satisfy the collision avoidance constraints. Setting early exit conditions can prevent long oscillations.

Modeling Example

Below is the setup of an inverse kinematics problem with self-collision constraints for an iiwa14 robot arm, implemented with capsule-to-capsule collision avoidance:

• 4 capsules are used to bound the robot arm: a fixed base capsule (radius 0.15 m), and 3 link capsules for the upper arm, forearm, and wrist (radii 0.164 m, 0.147 m, and 0.099 m, respectively). The endpoints of the link capsules are forward kinematics expressions of the joint angles, computed through Pinocchio's CasADi interface
• Capsule-to-capsule collision avoidance constraints are applied to 3 pairs of non-adjacent capsules (base and forearm, base and wrist, upper arm and wrist), with an avoidance distance of dsafe

# model and symbolic_data are Pinocchio's CasADi model and data, with forward kinematics already computed from the joint angle variables
BASE_CAPSULE = ((0.0, 0.0, -0.125), (0.0, 0.0, 0.125), 0.15)
LINK_CAPSULES = (
    ("upper_arm", "iiwa_link_2", "iiwa_link_4", (0, .25, .5, .75, 1), .164),
    ("forearm", "iiwa_link_4", "iiwa_link_6", (0, .25, .5, .75, 1), .147),
    ("wrist", "iiwa_link_6", "iiwa_link_ee", (0, .5, 1), .099),
)
SELF_COLLISION_PAIRS = (("base", "forearm"), ("base", "wrist"), ("upper_arm", "wrist"))
capsules = {"base": capsule_3d(*BASE_CAPSULE)}
for name, frame_a, frame_b, _, radius in LINK_CAPSULES:
    a = symbolic_data.oMf[model.getFrameId(frame_a)].translation
    b = symbolic_data.oMf[model.getFrameId(frame_b)].translation
    capsules[name] = capsule_3d(a, b, radius)
for first, second in SELF_COLLISION_PAIRS:
    prob.inequality(capsule_to_capsule_inequality(capsules[first], capsules[second], dsafe))