Given a robot as a tree of joints moving in a workspace, and given a path for
this robot between two configurations, we wish to establish whether the path
is collision free with the environment or for self-collision.
For each pair body~a - body~b, we will validate intervals by
\begin{enumerate}
\item computing the distance between bodies at a given parameter, and
\item bound from above the velocity of all points of body~a in the frame of
body~b.
\end{enumerate}
Let us denote by
\begin{itemize}
\item$J_a$ and $J_b$ the two joints holding
\item bodies $\body_a$ and $\body_b$ of the pair to check for collision,
\item$J_0= J_a, J_1, \cdots, J_{m-1}= J_b$, the list of joints linking $J_a$ to $J_b$,
\item$\CS$ the configuration space of the robot,
\item$\path: [0, T]\rightarrow\CS$, the path to check for collision,
\item$\conf_{i}=\path(0)$ and $\conf_{g}=\path(T)$ the end configurations
of the path to check.
\end{itemize}
\section{Constant velocity}
\begin{figure}[ht]
\centerline{
\input{kinematic-chain.pdf_t}
}
\end{figure}
In this section, we assume that along the path $\path$, each joint $J_i$ rotates or translates at constant linear and/or angular velocity in the reference frame of its neighbor. We thus denote for $i=1,\cdots,m-1$,
\begin{itemize}
\item$\linvel_{i-1/i}$, the constant linear velocity, and
\item$\angvel_{i-1/i}$, $i=1,\cdots,m-1$ the constant angular velocity of joint
$J_{i-1}$ in the reference frame of joint $J_{i}$.
\end{itemize}
\section{Upper bound on relative velocity}
Let $P_0$ be a point fixed in reference frame $J_0$ of coordinates $p_0$ in the
local frame of $J_0$. The coordinate of $P_0$ in the frame of $J_{m-1}$ is given
\item$M_{i/i+1}=\left(\begin{array}{cc}R_{i/i+1}& T_{i/i+1}\\0\ 0\ 0&1\end{array}\right)$ is the homogeneous matrix representing the position of Joint $J_i$ in the reference frame of $J_{i+1}$,
\item$M_{i/i+1}\in SO(3)$ is a rotation matrix, and
\item$T_{i/i+1}\in\reals^3$ is a translation vector.
\end{itemize}
Differentiating~(\ref{eq:relative-position}), we get
Notice that red variables correspond to joint variable derivatives and depend on the path, while black expressions are constant for a given kinematic chain.
If we define the radius of body $\body_a$ as the maximum distance of all points of the body to the center of the joint:
\section{If $J_a$ is not ancestor nor descendant of $J_{b}$}
If the robot is a tree of joints, $J_a$ and $J_b$ may lie on different branches
and therefore not be ancestor nor descendant of one another. In this case, in the sequence, $J_0$,..., $J_{m-1}$, any joint can be the child or the parent of its
predecessor in the list. Let $J_i$ and $J_{i+1}$ be two consecutive joints in $J_0$,..., $J_{m-1}$. Notice that
$$
\|T_{i/i+1}\|=\|T_{i+1/i}\|
$$
Without loss of generality, we can then assume that $J_{i+1}$ is the child of $J_i$. We define
\begin{eqnarray*}
M &=& J_{i+1}\texttt{->positionInParentFrame ()}\\
T &=& M [0:3, 3]
\end{eqnarray*}
$T$ is the coordinate of the origin of $J_{i+1}$ expressed in frame $J_{i}$.
\begin{itemize}
\item if $J_{i+1}$ is a rotation or SO(3) joint,
$$\|T_{i/i+1}\|=\|T\|,$$
\item if $J_{i+1}$ is a translation joint bounded in interval $[v_{min}, v_{max}]$,
$\|T_{i/i+1}\|$ is the maximum of two values computed as follows: let
\begin{eqnarray*}
\mathbf{u}&=& M [0:3, 0]
\end{eqnarray*}
$\mathbf{u}$ is the direction of translation of $J_{i+1}$ expressed in frame
