Commit ff42bfeb authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Reintroduce delete file doc/continuous-collision-checking.tex

parent 07f9c324
% Copyright (c) 2014 CNRS
% Authors: Florent Lamiraux
% This file is part of hpp-core
% hpp-core is free software: you can redistribute it
% and/or modify it under the terms of the GNU Lesser General Public
% License as published by the Free Software Foundation, either version
% 3 of the License, or (at your option) any later version.
% hpp-core is distributed in the hope that it will be
% useful, but WITHOUT ANY WARRANTY; without even the implied warranty
% General Lesser Public License for more details. You should have
% received a copy of the GNU Lesser General Public License along with
% hpp-core If not, see
% <>.
\documentclass {article}
\usepackage {graphics}
\usepackage {color}
\usepackage {amssymb}
\newcommand\body{{\cal B}}
\newcommand\CS{{\cal C}}
\title {Continuous Collision Checking}
\author {Florent Lamiraux}
\begin {document}
\section {Definitions and Notation}
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
\end {enumerate}
Let us denote by
\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.
\section {Constant velocity}
\centerline {
\input {kinematic-chain.pdf_t}
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
\begin {equation}\label{eq:relative-position}
P_{0/m-1} = M_{m-2/m-1} M_{m-3/m-2}\cdots M_{1/2}M_{0/1}\left(\begin{array}{c}m_0\\1\end{array}\right)
\end {equation}
where \begin{itemize}
\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.
Differentiating~(\ref{eq:relative-position}), we get
\begin {eqnarray}\label{eq:ineq-vel1}
\left(\begin{array}{c}\dot{P}_{0/m-1}\\0\end{array}\right) &=&\left(\begin{array}{cc}\crossprod{\omega_{m-2/m-1}} R_{m-2/m-1} & \linvel_{m-2/m-1} \\ 0 \ 0 \ 0 & 0\end{array}\right) \cdots M_{1/2} M_{0/1} \left(\begin{array}{c}m_0\\1\end{array}\right) \\
&& + M_{m-2/m-1}\left(\begin{array}{cc}\crossprod{\omega_{m-3/m-2}} R_{m-3/m-2} & \linvel_{m-3/m-2} \\ 0 \ 0 \ 0 & 0\end{array}\right) \cdots M_{0/1}\left(\begin{array}{c}m_0\\1\end{array}\right) \\
&& + \cdots \\
&& + M_{m-2/m-1} \cdots M_{1/2}\left(\begin{array}{cc}\crossprod{\omega_{0/1}} R_{0/1} & \linvel_{0/1} \\ 0 \ 0 \ 0 & 0\end{array}\right)\left(\begin{array}{c}m_0\\1\end{array}\right)
\end {eqnarray}
\item $\crossprod{\omega_{i/i+1}}$ is the antisymmetric matrix
corresponding to the cross product by vector
$\omega_{i/i+1}\in\reals^3$, representing the angular velocity of
$J_i$ with respect to $J_{i+1}$,
\item $\linvel_{i/i+1} = \dot{T}_{i/i+1}$ is the linear velocity of the origin of
$J_i$ in the reference frame of $J_{i+1}$.
\subsection {A few properties of rigid-body transformations}
Let $M_1=\left(\begin{array}{cc}R_1 & T_1\\0\ 0\ 0 & 1\end{array}\right)$,
$M_2=\left(\begin{array}{cc}R_2 & T_2\\0\ 0\ 0 & 1\end{array}\right)$, and
$M_3=\left(\begin{array}{cc}R_3 & T_3\\0\ 0\ 0 & 1\end{array}\right)$ be three homogeneous matrices such that
M_3 = M_1 M_2 = \left(\begin{array}{cc}R_1\ R_2 & R_1\ T_2 + T_1\\0\ 0\ 0 & 1\end{array}\right)
We notice that
\|T_3\| \leq \|T_1\| + \|T_2\|
Let $m\in\reals^3$, and $p\in\reals^3$ such that
\left(\begin{array}{c}p\\1\end{array}\right) = M_1\ \left(\begin{array}{c}m\\1\end{array}\right)
\|p\| \leq \|T_1\| + \|m\|
\subsection {Upper-bound computation}
From properties~(\ref{eq:ineq1}-\ref{eq:ineq2}) and expression~(\ref{eq:ineq-vel1}-\ref{eq:ineq-vel4}), we get
\begin {eqnarray*}
\|\dot{P}_{0/m-1}\| &\leq&
\|{\color{red}\linvel_{0/1}}\| + \|{\color{red}\omega_{0/1}}\|\ \|m_0\|\\
&& + \|{\color{red}\linvel_{1/2}}\| + \|{\color{red}\omega_{1/2}}\| \left(\|m_0\| + \|T_{0/1}\|\right) \\
&& + \|{\color{red}\linvel_{2/3}}\| + \|{\color{red}\omega_{2/3}}\| \left(\|m_0\| + \|T_{0/1}\| + \|T_{1/2}\|\right) \\
&& + \cdots \\
&& + \|{\color{red}\linvel_{m-2/m-1}}\| + \|{\color{red}\omega_{m-2/m-1}}\|\ \left(\|m_0\| + \|T_{1/2}\| + \cdots + \|T_{m-2/m-1}\|\right)
\end {eqnarray*}
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:
r_0 = \sup \left\{\|m_0\|, m_0\in\body_a\right\},
we get
\begin {eqnarray*}
\|\dot{P}_{0/m-1}\| &\leq&
\|{\color{red}\linvel_{0/1}}\| + \|{\color{red}\omega_{0/1}}\|\ r_0\\
&& + \|{\color{red}\linvel_{1/2}}\| + \|{\color{red}\omega_{1/2}}\| \left(r_0 + \|T_{0/1}\|\right) \\
&& + \|{\color{red}\linvel_{2/3}}\| + \|{\color{red}\omega_{2/3}}\| \left(r_0 + \|T_{0/1}\| + \|T_{1/2}\|\right) \\
&& + \cdots \\
&& + \|{\color{red}\linvel_{m-2/m-1}}\| + \|{\color{red}\omega_{m-2/m-1}}\|\ \left(r_0 + \|T_{1/2}\| + \cdots + \|T_{m-2/m-1}\|\right)
\end {eqnarray*}
\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
M &=& J_{i+1}\texttt{->positionInParentFrame ()} \\
T &=& M [0:3, 3]
$T$ is the coordinate of the origin of $J_{i+1}$ expressed in frame $J_{i}$.
\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
\mathbf{u} &=& M [0:3, 0]
$\mathbf{u}$ is the direction of translation of $J_{i+1}$ expressed in frame
$J_{i}$. Then
\|T_{i/i+1}\| \leq \max (\|T+v_{min}\mathbf{u}\|, \|T+v_{max}\mathbf{u}\|)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment