Differentiable line segment vs disk (or circle) collision detection

Consider the problem of controlling a two-link planar robot with a circular obstacle on the way. At every moment in time, the robot should avoid hitting the obstacle. If we are to use trajectory optimization for finding a sequence of actions, we need to encode ‘avoid hitting the obstacle’ as a constraint (or a cost penalty if you wish). That means we need to have a predicate that takes current system state and reports if there is collision or not, and ideally we would like this predicate to be differentiable to allow for easy integration with existing trajectory optimization tools.

Line segment vs disk intersection

There is a difference between the following two questions: “Does a given line segment intersect a given disk?” and “Is intersection of a given line segment with a given disk empty?”. Obviously, the second question is the negation of the first one. So, if we manage to formulate the first question as an intersection of inequality constraints, then the second question becomes a union of inequality constraints, which is much nastier to work with (all optimization tools search within the intersection of constraints). Therefore, our goal will be to formulate all conditions as intersections of inequalities, such that standard tools can deal with them.

No disk intersection

The first on the list is the intersection of a line segment and a disk. Let’s say we want to know when they don’t intersect (that is the kind of constraint you would want to put into trajectory optimization to find a collision-free trajectory). This is quite easy, we just need to find the distance between the line and the center of the disk and make sure that it is greater than the radius of the disk. The only subtlety is that there is no beautiful formula for the distance between a line segment and a point, but we will try to make ours as nice as possible.

To start with, let’s assume that the disk is located at the origin (if not, we can always make an appropriate coordinate transformation) and denote the radius of the disk by $R$. Let the line segment be given by its end points $P_0 = (x_0, y_0)$ and $P_1 = (x_1, y_1)$ and denote the length of the line segment by $L = |P_1 - P_0|$.

Closest point to the origin. The distance from a point to a set of point is defined as the distance from the point to the closest point in the set. In our problem, we need to find the distance from the origin to the closest point on the line segment. To accomplish that, let’s parameterize the points on the line segment as \begin{equation*} P_t = (1 - t) P_0 + t P_1, \quad \forall t \in [0, 1]. \end{equation*} Then the distance from the origin to the line segment is given by \begin{align*} d = \min_{t \in [0, 1]} |P_t|. \end{align*} We can find $d$ explicitly by minimizing \begin{equation*} |P_t|^2 = ((1-t)x_0 + tx_1)^2 + ((1-t)y_0 + ty_1)^2 \end{equation*} over $t \in [0, 1]$. Note that if we would do unconstrained optimization, we would find the distance from the origin to the line passing through the line segment. However, since the line segment is finite, we should not step outside its limits.

Account for finiteness of the line segment. To ensure that $t \in [0, 1]$, we can simply rectify the unconstrained solution so that it becomes $t = 1$ if $t > 1$ and $t = 0$ if $t < 0$. Such saturation operation can be accomplished by the saturation function defined as \begin{equation*} s(x; a, b) = r(x - a) - r(x - b) \end{equation*} where $r(x)$ is the ramp function. Note, however, that the ramp function is not differentiable, therefore one may benefit from approximating it by a sigmoid to obtain a smooth saturation function.

Minimal distance. The solution of the unconstrained problem follows from the necessary condition for optimality. Denote $u = x_1 - x_0$ and $v = y_1 - y_0$, then the optimal $t$ for the unconstrained problem is given by \begin{equation} \label{min_dist} t^* = \arg \min_{t \in \mathbb{R}} |P_t|^2 = \frac{-u}{u^2 + v^2} x_0 + \frac{-v}{u^2 + v^2} y_0. \end{equation} By passing this solution through a saturation function with limits $0$ and $1$, we obtain the optimal $t$ for the original constrained problem \begin{equation*} \hat{t} = \arg \min_{t \in [0, 1]} |P_t| = s(t^*; 0, 1) \end{equation*} and the distance from the origin to the line segment \begin{equation} \label{line_seg_to_origin_dist} d = |P_{\hat{t}}| \end{equation} respectively.

Disk intersection

It is straightforward to derive the complementary condition to \eqref{line_seg_disk_no_intersec}.

Line segment vs circle intersection

The story with the circle is a bit trickier. To appreciate the difficulty, take a look at the four cases of line segment vs circle intersection that emerge. One could of course check every possible situation, but that does not result in a differentiable constraint amenable to smooth optimization.

No circle intersection

Unfortunately, there is no way to describe the condition when an arbitrary line segment and an arbitrary circle do not intersect by a single inequality. This problem inherently requires a union of conditions because the line segment can either be completely inside the circle or completely outside the circle. However, if you additionally assume that the link is bigger than the circle (i.e., $L > 2R$), then one constraint is sufficient.

Maximal distance. To formulate the non-intersection condition, we need to compute one more quantity; namely, the maximum distance from the origin to the line segment. The maximum distance $D$ is thus defined as \begin{equation*} D = \max_{t \in [0, 1]} |P_t|. \end{equation*} To find the optimal $t$, we can reuse the minimal distance $t^*$ that we already computed in \eqref{min_dist}, but pass it through a different non-linearity, namely, through the Heaviside step function, \begin{equation*} \bar{t} = 1 - H(t^* - 0.5). \end{equation*} Convince yourself that \begin{equation*} D = P_{\bar{t}}. \end{equation*} Of course, using a smooth version of the Heaviside function leads to a smooth constraint.

Long link, small circle. If the line segment is bigger than the circle, i.e., if condition $D < R$ is never satisfied, then we obtain the same condition for the circle as we had for the disk \eqref{line_seg_disk_no_intersec}.

Circle intersection

The condition on circle intersection is inherently nicer because it can be expressed as an intersection of inequalities. Check for yourself that the following is true.

Long link, small circle. Again, if the link is longer than the circle, we can drop the condition on $D$ and apply our old result for the disk.

Conclusion

By applying a nonlinear transformation to the solution of an unconstrained point-to-line distance minimization problem, we obtain a smooth approximation to the minimal and maximal point-to-line_segment distance. Comparing these two distances to the obstacle radius, we can decide whether there is an intersection or not. Checking intersection with a circle is inherently more complicated because the circle is not a convex set in $\mathbb{R}^2$. However, the assumption that the line segment is bigger than the circle makes the line_segment-circle intersection problem equivalent to the line_segment-disk intersection problem.