\documentclass[11pt]{article}
\usepackage{latexsym}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsthm}
\usepackage{epsfig}
\usepackage[tight]{subfigure}

\newcommand{\handout}[5]{
  \noindent
  \begin{center}
  \framebox{
    \vbox{
      \hbox to 5.78in { {#1} \hfill #2 }
      \vspace{4mm}
      \hbox to 5.78in { {\large \hfill #5  \hfill} }
      \vspace{2mm}
      \hbox to 5.78in { {\em #3 \hfill #4} }
    }
  }
  \end{center}
  \vspace*{4mm}
}

\newcommand{\lecture}[5]{\handout{#1}{#2}{#3}{#4}{#5}}

\newtheorem{theorem}{Theorem}
\newtheorem{corollary}[theorem]{Corollary}
\newtheorem{lemma}[theorem]{Lemma}
\newtheorem{observation}[theorem]{Observation}
\newtheorem{proposition}[theorem]{Proposition}
\newtheorem{definition}[theorem]{Definition}
\newtheorem{claim}[theorem]{Claim}
\newtheorem{fact}[theorem]{Fact}
\newtheorem{assumption}[theorem]{Assumption}

% 1-inch margins, from fullpage.sty by H.Partl, Version 2, Dec. 15, 1988.
\topmargin 0pt
\advance \topmargin by -\headheight
\advance \topmargin by -\headsep
\textheight 8.9in
\oddsidemargin 0pt
\evensidemargin \oddsidemargin
\marginparwidth 0.5in
\textwidth 6.5in

\parindent 0in
\parskip 1.5ex
%\renewcommand{\baselinestretch}{1.25}

\begin{document}

\lecture{Adaptive Control and Reinforcement Learning (16-899C, S09)}{Lecture \#5 (01/29/2009)}{Lecturer: Nathan Ratliff, Matt Zucker}{Scribe: Don Burnette}{CHOMP: Gradient Optimization Techniques for Efficient Motion Planning}

\section{Introduction}
Covariant Hamiltonian Optimization for Motion Planning (CHOMP), is a method for generating and optimizing trajectories, without requiring that the input path be obstacle free. The algorithm consists of three main features:

\begin{itemize}
\item
In order to encourage smoothness we must measure the size of an update to our hypothesis in terms of the amount of a particular dynamical quantity (such as total velocity or total acceleration) it adds to the trajectory. 
\item
Measurements of obstacle costs should be taken in the workspace so as to correctly account for the geometrical relationship between the robot and the surrounding environment. 
\item
The same geometrical considerations used to update a trajectory should be used when correcting any joint limit violations that may occur.
\end{itemize}

\section{The CHOMP Algorithm}
\subsection{Covariant Gradient Descent}
The cost of a trajectory using two terms: an obstacle term $f_{obs}$, which measures the cost of being near obstacles, and a prior term $f_{prior}$, which measures dynamical quantities of the robot such as smoothness and acceleration. The cost function can be written as

\begin{equation}
C(\xi) = f_{prior}(\xi) + f_{obs}(\xi).
\end{equation}

The $f_{prior}$ term is merely the sum of the squared derivatives, which can be expressed in the simple quadratic form

\begin{equation}
f_{prior}(\xi) = \frac{1}{2}\xi^T A \xi + \xi^T b + c,
\end{equation}

for suitable matrix, vector, and scalar constants $A$, $b$, $c$. When constructed as defined above, $A$ will always be symmetric positive definite for all $d$. The goal is to improve the trajectory at each iteration by minimizing a local approximation of the function that suggests only smooth perturbations to the trajectory. At iteration $k$, within a region of our current hypothesis $\xi_k$, we can approximate our cost  using a first-order Taylor expansion:

\begin{equation}
C(\xi) = C(\xi_k) + g^T_k (\xi - \xi_k), 
\end{equation}

where $g_k = \nabla C(\xi_k)$. Using this expansion, our update can
be written formally as

\begin{equation}
\xi_{k+1} = \underset{\xi}{\operatorname{argmin}} \left\{C(\xi_k) +  g^T_k (\xi - \xi_k) + \frac{\lambda}{2}||\xi-\xi_k||^2_M\right\},
\label{eqn:argmin}
\end{equation}

where the notation $||\delta||^2_M$ denotes the norm of the displacement $\delta = \xi - \xi_k$ taken with respect to the Riemannian metric $M$ equal to $\delta^TM\delta$. Setting the gradient of the right hand side of equation ~\ref{eqn:argmin} to zero and solving for the minimizer results in the following more succinct update rule:

\begin{equation}
\xi_{k+1} = \xi_k - \frac{1}{\lambda}M^{-1}g_k
\end{equation}

This update rule serves to ensure that the trajectory remains smooth after each trajectory modification.

\subsection{Obstacle avoidance}
If obstacles are static, it becomes advantageous to simply precompute a signed distance field $d(x)$ which stores the distance from a point $x\in \Re^3$ to the boundary of the nearest obstacle. Values of $d(x)$ are negative inside obstacles, positive outside, and zero at the boundary. There are a few key advantages of using the signed distance field to check for collisions. 
\begin{itemize}
\item
Collision checking is
very fast, taking time proportional to the number of voxels
occupied by the robot’s “skeleton”. 
\item
Since the signed distance
field is stored over the workspace, computing its gradient via
finite differencing is a trivial operation. 
\item
Because we
have distance information everywhere, not just outside of
obstacles, we can generate a valid gradient even when the
robot is in collision, a particularly difficult feat for other
representations and distance query methods.
\end{itemize}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%FIGURE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%picture of board with 2 holes
\begin{figure}[ht!]
    \centering
    {\includegraphics[width=.5\textwidth]{potential_function.png}}
    \caption{Potential function for obstacle avoidance}
    \label{fig:potfunction}
\end{figure}
%%%%%%%%%%%%%%%%%%%%%%%%%%FIGURE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Now we can define the workspace potential function c(x),
which penalizes points of the robot for being near obstacles.
A good choice, shown in Figure~\ref{fig:potfunction}, is given by

\begin{equation}
c(x) =
\left\{
\begin{array}{rl}
-d(x) + \frac{1}{2}\epsilon, & \mbox{if $d(x) < 0$}\\
\frac{1}{2\epsilon}(d(x) - \epsilon)^2, & \mbox{if $0 \le d(x) \le \epsilon$}\\
0, & \mbox{otherwise}\\
\end{array}\right.
\end{equation}


\end{document}
