%
%  2004/Lecture notes/Lecture10/Lecture10.tex
%
\documentclass[pdf,mmoll,slideColor%,colorBG
]{prosper}
\RequirePackage{color}

\usepackage[T1]{fontenc}
\usepackage{amsmath,amssymb,natbib}
%\usepackage[T1,noTS1]{mathtime}
\SetMathAlphabet{\mathbf}{normal}{\encodingdefault}{ptm}{b}{n}
\usepackage{subfigure}
\usepackage{rotating}
\usepackage{graphicx}
%\usepackage[dvips]{color}
\usepackage{epsfig}
\graphicspath{{../figs/}}

\newcommand{\ee}{\end{equation}} 
\newcommand{\be}{\begin{equation}} 
\newcommand{\ec}{\end{center}} 
\newcommand{\bc}{\begin{center}} 
\newcommand{\eea}{\end{eqnarray}} 
\newcommand{\bea}{\begin{eqnarray}} 
%\newcommand{\sumprime}{\mathop{{\sum}'}} 
\newcommand{\bd}{\begin{description}} 
\newcommand{\ed}{\end{description}} 
\newcommand{\bi}{\begin{itemize}} 
\newcommand{\ei}{\end{itemize}} 
\newcommand{\pa}{\partial} 
\newcommand{\hor}{\left(\frac{h}{R_0}\right)} 

\newcommand{\code}[0]{\ttfamily}
\newcommand{\co}[0]{\hbox{c}}
\newcommand{\si}[0]{\hbox{s}}
\newcommand{\pprime}[0]{\prime\prime}
\newcommand{\ppprime}[0]{\prime\prime\prime}
\newcommand{\conv}{\operatorname{conv}}
\newcommand{\pos}{\operatorname{pos}}
\newcommand{\lin}{\operatorname{lin}}
\newcommand{\vertop}{\operatorname{vert}}
\newcommand{\spanop}{\operatorname{span}}
\newcommand{\supp}{\operatorname{supp}}
\newcommand{\intoper}{\operatorname{int}}
\newcommand{\half}{\operatorname{half}}
\newcommand{\rad}{\operatorname{rad}}
\newcommand{\diam}{\operatorname{diam}}
\newcommand{\push}{\operatorname{push}}
\newcommand{\sqz}{\operatorname{sqz}}
\newcommand{\sgn}{\operatorname{sgn}}
%\renewcommand{\Re}{\Bbb{R}}
\renewcommand{\Re}{\mathbf{R}}
\newcommand{\vv}[1]{\mathbf{#1}}
\newcommand{\uu}[1]{\hat {\vv #1}}
\newcommand{\defterm}[1]{\textbf{#1}}
\newcommand{\rot}{\operatorname{rot}}

\newcommand{\btimes}{\times}
\newcommand{\bomega}{\omega}

 
\newcommand{\bis}{\begin{itemstep}} 
\newcommand{\eis}{\end{itemstep}} 

\makeatletter
\define@key{PDF}{Movie}{\pdf@addtoks{#1}{Movie}}
\define@key{PDF}{Activation}{\pdf@addtoks{#1}{Activation}}
\newcommand{\moviewithpreview}[3]{% args: width, preview, movie
   \pdfmark[{\includegraphics[width=#1]{#2}}]{%
   pdfmark=/ANN,Subtype=/Movie,Movie=<< /F (#3) >>,%
   Activation=<< /ShowControls true /Mode /Repeat >>}}
\newcommand{\movie}[3]{% args:width, height, movie
   \pdfmark[{\hbox to #1 {\vbox to #2 { }}}]{%
   pdfmark=/ANN,Subtype=/Movie,Movie=<< /F (#3) /Poster true >>,%
   Activation=<< /ShowControls true /Mode /Repeat >>}}
\makeatother

\title{10.  Cspace transform and path planning}
\subtitle{ Mechanics of Manipulation}
\author{Matt Mason}
\institution{Carnegie Mellon}
\email{matt.mason@cs.cmu.edu \\
http://www.cs.cmu.edu/\symbol{"7E}mason}
\slideCaption{Lecture 10. \hfill Mechanics of Manipulation }

\begin{document}

\maketitle

%=======================================================================
%
% SLIDE 2
%

\begin{slide}[R]{Lecture 10.  Cspace transform and path planning.}
\vspace{-0.5in}
\includegraphics[height=3.0in]{TOC10}
\end{slide}

%=======================================================================
%
% SLIDE 3
%
%

\begin{slide}[R]{Outline.}

\begin{trivlist}
\item Background
\begin{itemize}
\item[]Pick and place
\item[]Problems
\item[]Practice
\end{itemize}
\item Cspace transform
\begin{itemize}
\item[]An example
\item[]Simple cases
\end{itemize}
\item Path planning
\begin{itemize}
\item[]Visibility graph
\item[]Best first
\item[]Nonholo
\end{itemize}
\end{trivlist}

\end{slide}


%=======================================================================
%
% SLIDE 4
%
%

\begin{slide}[R]{Kinematic manipulation}
\begin{trivlist}
\item[]Don't all manipulation problems involve force?
\item[]We can treat lots of manipulation problems as pure kinematics problems.
\item[]We can address kinematic issues faced by all manipulation tasks.
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 5
%
%

\begin{slide}[R]{Pick and place}
\begin{trivlist}
\item[] Let  {\code nextblock()} describe next block to be moved;
\item[] Let {\code start(block)} give the initial
location of {\code block};
\item[]Let {\code goal(block)} give the
goal location of {\code block}.
\item[]\defterm{Pick and place} follows the pattern:
\begin{tabbing}
MMMMMM \= MM \= MMMMMMMMMMMMMMMM\= \kill
       \> \code FOR block = nextblock() \\
       \> \>  \code MOVETO start(block) \> ; pick block \\
       \> \>  \code CLOSE \\
       \> \>  \code MOVETO goal(block) \> ; place block \\
       \> \>  \code OPEN 
\end{tabbing}
\end{trivlist}

\end{slide}


%=======================================================================
%
% SLIDE 6
%
%


\begin{slide}[R]{Pick and place assumptions}
\begin{trivlist}
\item[]Each object is attached either to the fixed frame or the moving
(effector) frame. 
\item[]Pick and place manipulation assumes:
\begin{itemize}
\item[] {\code CLOSE} attaches the object rigidly to the effector;
\item[]{\code OPEN} attaches the object to the fixed frame;
\item[]The robot follows the path exactly.
\end{itemize}
\item[]Each assumption is a postponed problem:
\begin{itemize}
\item[]Grasp planning:  choosing effector and finger motions that will
      produce a stable grasp for each object.
\item[]Stable placement.
\item[]Compliant motion:  errors in sensing and control lead to
collisions interleaved with compliant motions.
\end{itemize}
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 7
%
%

\begin{slide}[R]{Pick and place in practice}
\begin{trivlist}
\item[]Pick and place is common in factory automation.  (SONY video)

\item[]\moviewithpreview{9cm}{SmartAssy.eps}{SmartAssy.mov}

\item[](There's a lot more than just pick and place going on!!)
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 8
%
%

\begin{slide}[R]{Moving problems offline}
\begin{trivlist}
\item[]Grasp planning:  design a different gripper for each part;

\item[]Known initial pose:  parts orienting machine;

\item[]Stable placement:  design for assembly.

\item[]\emph{Collision avoidance}:
\begin{itemize}
\item[]Every assembly is vertical;

\item[]Space above is guaranteed clear---a motion bus;

\item[]{\code DEPROACH}; {\code MOVETO}; {\code APPROACH}.
\end{itemize}

\end{trivlist}
Industrial manipulation has all the problems of general purpose
robotics, but they deal with many of them offline.  

\end{slide}

%=======================================================================
%
% SLIDE 9
%
%
\begin{slide}[R]{The Cspace transform}
\begin{trivlist}
\item[]Let $\vv q$ be the configuration of some moving object $A$;
\item[]let $\vv Q$ be the set of all $\vv q$, the \emph{configuration space};
\item[]let $\vv B$ be some fixed object;
\item[]let $CO_A(B)$ be the set of all $\vv q$ such that $A$ at $\vv q$ intersects $B$.
\item[]$CO_A(B)$ is the \defterm{Cspace obstacle}
\item[]\emph{Path planning} ( {collision avoidance} ) often means finding a path $\vv q(t)$ from given $\vv q_{\hbox{init}}$ to given $\vv q_{\hbox{goal}}$ without passing through the interior of $CO_A(B)$.
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 10
%
%
\begin{slide}[R]{Example 1:  point in plane}
\begin{minipage}[t]{9cm}
\begin{trivlist}
\item[]The moving object $A$ is a point in the plane;
\item[]the fixed objects $B_i$ are polygons;
\item[]define \defterm{visibility graph}:  for every vertex pair, include the line segment if it is in free space;
\item[]search the visibility graph.
\end{trivlist}
\begin{figure}[h!]
\includegraphics[width=9cm]{pp1}
\end{figure}\end{minipage}
\end{slide}

%=======================================================================
%
% SLIDE 11
%
%
\begin{slide}[R]{Example 2:  disk in plane}
\begin{minipage}[t]{9cm}
\begin{trivlist}
\item[]Especially good for cylindrical indoor mobile robots!
\item[]The moving object $A$ is a point in the plane;
\item[]Fixed objects are polygons (circular arcs easily incorporated);
\item[]Visibility graph includes all bi-tangents in free space.
\end{trivlist}
\begin{figure}[h!]
\includegraphics[width=11cm]{pp2}
%\includegraphics[height=3cm]{*file*}
\end{figure}\end{minipage}
\end{slide}

%=======================================================================
%
% SLIDE 12
%
%
\begin{slide}[R]{Example 3:  Translating polygon in plane}
%\begin{minipage}[t]{9cm}
\begin{trivlist}
\item[]
\[
        \hbox{collision} \leftrightarrow 
                \exists_{\vv{a}\in A, \vv{b}\in B} \; \vv{a} + \vv{q} = \vv{b}
\]
\item[]The Cspace obstacle is
\begin{align*}
        CO_{A}(B) & = \{ \vv{q} \; | \; \exists_{\vv{a} \in A, \vv{b} \in B} \;
        \vv{a}+\vv{q} = \vv{b}\} \\ 
        & = \{ \vv{b} - \vv{a} \; | \; \vv{a} \in A, \vv{b} \in B \} \\
        & = B \ominus A
\end{align*}
(``$\ominus$'' is \emph{Minkowski difference}.)
For $A$ and $B$ convex:
\begin{equation*}
        CO_{A}(B) = \conv( \vertop( B ) \ominus \vertop( A ))
\end{equation*}

\end{trivlist}
\begin{figure}[h!]
%\includegraphics[width=4cm]{*file*}
\includegraphics[height=2cm]{pp3}
\end{figure}
%\end{minipage}
\end{slide}


%=======================================================================
%
% SLIDE 13
%
%
\begin{slide}[R]{Planar polygon with rotation}
\begin{figure}[h!]
\includegraphics[width=9cm]{pp4}
%\includegraphics[height=3cm]{*file*}
\end{figure}
\end{slide}

%=======================================================================
%
% SLIDE 14
%
%
\begin{slide}[R]{Two link planar arm}
\begin{figure}[h!]
\includegraphics[width=9cm]{pp5}
%\includegraphics[height=3cm]{*file*}
\end{figure}
\end{slide}

%=======================================================================
%
% SLIDE 15
%
%
\begin{slide}[R]{Path planning}
\begin{trivlist}
\item[]Visibility graph works well in two dimensions, but not three or more.
\item[]Voronoi graphs have been generalized to higher dimensions (see Choset).
\item[]Free space graphs.
\item[]Maze searching algorithms.
\item[]Random trees.
\item[]Best First Planner
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 16
%
%

\begin{slide}[R]{Best First Planner}
\begin{trivlist}
\item[]Used by Barraquand and Latombe to search high dimensional space.

\item[]Discretized search in Cspace with potential fields.

\begin{itemize}
\item[]Obstacles have high potential, goal is low potential.

\item[]Potential is used as search heuristic, not as control.
\end{itemize}

\item[]Searches in Cspace, but doesn't construct Cspace obstacles.
\end{trivlist}

\end{slide}

%=======================================================================
%
% SLIDE 16.5
%
%

\begin{slide}[R]{Best First Planner}
\begin{trivlist}
\item
\begin{tabbing}
MMMMMM \= MM \= MM \= MM \= MM \= MMMMMMMMMMMMMMMM\= \kill
        \> \code procedure BFP \\
        \> \> \code  open $\leftarrow$ $\{q_{init}\}$ \\
        \> \> \code  mark $q_{init}$ visited \\
        \> \> \code  while open $\neq$ $\{\}$ \\
        \> \> \> \code  q $\leftarrow$ best( open ) \\
        \> \> \> \code  if q = $q_{goal}$ return( success )\\
        \> \> \> \code  for n $\in$ neighbors( q ) \\
        \> \> \> \> \code  if n unvisited and not a collision \\
        \> \> \> \> \> \code  insert( n, open ) \\
        \> \> \> \> \> \code  mark n visited \\
        \> \> \code  return( failure )
\end{tabbing}
\end{trivlist}
\end{slide}

%=======================================================================
%
% SLIDE 17
%
%
\begin{slide}[R]{Example of Best First Planner}
\begin{minipage}[t]{6cm}
\begin{trivlist}
\item[]Searches from $\vv q_{\hbox{init}}$, taking best nodes according to potential field.
\item[]Discards collisions.
\item[]If lucky, goes straight to goal.
\item[]More generally, has to fill in local minima in potential field.
\end{trivlist}
\end{minipage}
\hfill
\begin{minipage}[t]{4cm}
\begin{figure}[h!]
\includegraphics[width=4cm]{bestsearch}
%\includegraphics[height=3cm]{*file*}
\end{figure}\end{minipage}
\end{slide}


%=======================================================================
%
% SLIDE 18
%
%
\begin{slide}[R]{Nonholonomic path planning}
\begin{itemize}
\item[]Suppose we have a unicycle and we want to plan collision free paths.
\item[]What's wrong with Best First Planner?
\begin{tabbing}
MMMMMM \= MM \= MM \= MM \= MM \= MMMMMMMMMMMMMMMM\= \kill
        \> \code procedure BFP \\
        \> \> \code  open $\leftarrow$ $\{q_{init}\}$ \\
        \> \> \code  mark $q_{init}$ visited \\
        \> \> \code  while open $\neq$ $\{\}$ \\
        \> \> \> \code  q $\leftarrow$ best( open ) \\
        \> \> \> \code  if q = $q_{goal}$ return( success )\\
        \> \> \> \code  for n $\in$ neighbors( q ) \\
        \> \> \> \> \code  if n unvisited and not a collision \\
        \> \> \> \> \> \code  insert( n, open ) \\
        \> \> \> \> \> \code  mark n visited \\
        \> \> \code  return( failure )
\end{tabbing}

\end{itemize}

\end{slide}

%=======================================================================
%
% SLIDE 19
%
%
\begin{slide}[R]{ Approach}
\begin{itemize}
\item[]Use actions that satisfy velocity constraints.
\item[]Search sequences of actions.
\item[]Prune collisions.
\item[]Prune sequences that go where we've been before.
\end{itemize}

\end{slide}

%=======================================================================
%
% SLIDE 20
%
%
\begin{slide}[R]{NonHolo Planner}
\begin{itemize}
\item[]Define a grid in configuration space.
\item[]Let $\delta t$ be small time increment.
\item[]Let {\code grid( q )}  returns the grid node closest  {\code q}.
\item[]Let {\code actions} be a finite set of actions.  
\item[]Let {\code int( $q$, a, $\delta t$ )} simulate the system from $q$ using action {\code a} for time $\delta t$.
\end{itemize}

\end{slide}

%=======================================================================
%
% SLIDE 20.5
%
%
\begin{slide}[R]{NonHolo Planner}
\begin{itemize}
\item[]
\begin{tabbing}
MMMMMM \= MM \= MM \= MM \= MM \= MMMMMMMMMMMMMMMM\= \kill
        \> \code  procedure NHP \\
        \> \> \code  open $\leftarrow$ $\{q_{init}\}$ \\
        \> \> \code  mark grid( $q_{init}$ ) visited \\
        \> \> \code  while open $\neq$ $\{\}$ \\
        \> \> \> \code  q $\leftarrow$ best( open ) \\
        \> \> \> \code  if q $\approx$ $q_{goal}$ return( success )\\
        \> \> \> \code  for a $\in$ actions \\
        \> \> \> \> \code  n $\leftarrow$ int( q, a, $\delta t$ ) \\
        \> \> \> \> \code  if n not a collision and grid( n ) not visited \\
        \> \> \> \> \> \code  insert( n, open ) \\
        \> \> \> \> \> \code  mark grid( n ) visited \\
        \> \> \code  return( failure )
\end{tabbing}

\end{itemize}

\end{slide}

%=======================================================================
%
% SLIDE 21
%
%
\begin{slide}[R]{Example:  NHP for unicycle}
\begin{minipage}[t]{4cm}
\begin{itemize}
\item[]Let the grid be 10cm by 10cm by  5 degrees.
\item[]Let $\delta t$ be 0.1 seconds.
\item[]Let {\code actions} be {\code fwd}, {\code rev}, {\code ccw}, {\code cw}.  Speeds are 2m / sec and 90 degrees / sec.
\item[]Let {\code best} return node with shortest path so far, with big penalty for switching actions.
\end{itemize}
\end{minipage}
\hfill
\begin{minipage}[t]{4cm}
\begin{figure}[h!]
\vspace{5cm}
Draw here!
%\includegraphics[width=4cm]{*file*}
%\includegraphics[height=3cm]{*file*}
\end{figure}\end{minipage}
\end{slide}

%=======================================================================
%
% SLIDE last
%

\begin{slide}[R]{Next:  Gr\"ubler and Salisbury.}
\vspace{-0.5in}
\includegraphics[height=3.0in]{TOC11}
\end{slide}
\end{document}



\end{document}
