\documentclass{article}
\usepackage{times}
\usepackage{fullpage}
\usepackage{amsmath}
\usepackage{graphicx}
\usepackage{tweaklist}
\usepackage{pifont}

\setlength{\parindent}{0in}
\setlength{\parskip}{2pt} % was 6pt
\setlength{\topsep}{1pt} % was ?
% \renewcommand{\itemhook}{\setlength{\topsep}{0pt}%
%  \setlength{\itemsep}{0pt}\setlength{\leftmargin}{.25in}}
\renewcommand{\enumhook}{
  \setlength{\parskip}{0pt}
  \setlength{\topsep}{0pt}
  \setlength{\itemsep}{0pt}
  \setlength{\leftmargin}{.25in}}

\def\vec#1{{\bf #1}}
\def\solutions{solutions}
%%
%% Uncomment the \def\solutions line to include solutions
%%
% \def\solutions{\relax}

\begin{document}
\begin{center}
  Massachusetts Institute of Technology \bigskip

  \textbf{\Large{Robotics: Science and Systems I}}

  {\Large Lab 6: Motion Planning and Global Navigation}

%  \textbf{\#\#\#DATE\#\#\#}
 \textbf{Begins: 3/5/2014, 3pm}\\
 \textbf{Due: 3/12/2014, 3pm}
\end{center}

\vspace*{-0.1in}
\section*{Objectives and Lab Overview}

Your objective in this lab is to understand planning and global navigation.
You will control your robot to a goal in a maze-like environment, using a
motion planner to establish a sequence of waypoints within a given map,
around obstacles, and to the goal. Given a map and the geometry of your robot,
you will first compute the configuration space that allows you to reason 
about your robot as a moving point among complex obstacles. You will then
develop a motion planning program that will search the configuration space
for a path for the robot. This path will be transformed into waypoints in 
the physical space. Your robot will then follow the path defined by the 
computed waypoints to navigate from its starting configuration to the 
goal configuration.

This lab will give you the technical skills to incorporate motion
planning and waypoint navigation algorithms into your robot. 
This lab will also enhance your knowledge of geometric algorithms, 
an important aspect of interfacing computation to the physical world.

\vspace*{0.1in}
{\bf \large Time Accounting and Self-Assessment:} 

Make a dated entry called ``Start of Global Navigation Lab'' on your Wiki's
Self-Assessment page.  Before doing any of the lab parts below, answer
the following questions:
\begin{itemize}

\item {\bf Programming}: How proficient are you at writing large programs in
Java (as of the start of this lab)? \\ (1=Not at all proficient; 2=slightly
proficient; 3=reasonably proficient; 4=very proficient; 5=expert.)

\item {\bf Computational Geometry}: How proficient are you at working with
computational geometry (representing polygons, computing intersections, etc)?
\\ (1=Not at all proficient; 2=slightly proficient; 3=reasonably proficient;
4=very proficient; 5=expert.)

\item {\bf Motion Planning}: How proficient are you at crafting robot motion
planning algorithms? \\ (1=Not at all proficient; 2=slightly proficient;
3=reasonably proficient; 4=very proficient; 5=expert.)

\item {\bf Waypoint Navigation}: How proficient are you at using the navigation
software on your robot? \\ (1=Not at all proficient; 2=slightly proficient;
3=reasonably proficient; 4=very proficient; 5=expert.)

\end{itemize}

{\bf \large To start the lab, you should have:}
\begin{itemize}
\item This handout (6-A)
\item Your notes from the Planning, Localization, and Mapping lectures
\end{itemize}

\section*{Physical Units}
We remind you to use MKS units (meters, kilograms, seconds, radians, watts,
etc.) throughout the course and this lab.  In particular, this means that {\em
whenever you state a physical quantity, you must state its units}.  Also show
units in your intermediate calculations.

\newpage


\section*{Part 1: Team organization}

This week's assignment has several parts that can be implemented in parallel
provided you define appropriate interfaces (APIs) between the project
components. To begin, read the project specifications carefully and work as a
group to define your approach to solving the assignment. Consider the overall
project goal and break it down into software modules. Develop an algorithm for
each software module. Define the APIs between all the software modules.  Now
make programming assignments for each software module so that you can
parallelize the implementation of this project. Develop a project timeline and
be sure to include time for module development and testing, time for software
integration, and time for project testing and evaluation.

\medskip

{\it Deliverables:} A report on the Wiki, named ``GlobalNavigation Lab Report
Group N''. Include a description of each module, your proposed
algorithm for it, the APIs between the modules, the task allocations within
your team, and the proposed timeline.

\section*{Part 2: Programming Environment}

We have supplied a number of Java classes and three map files:
\begin{itemize} 
\item {\tt global-nav-maze-2011-basic.map} (required for checkoff), 
\item {\tt global-nav-maze-2011-med.map} (required for checkoff), and
\item {\tt global-nav-maze-2011-hard.map} (extra credit) 
\end{itemize}
These maps all correspond to the maze environment we have constructed and differ only
in their {\tt goalPoint}.  The Java and map files can be found in the lab distribution 
directory. Copy them from \verb!~/RSS-I-pub/labs/lab6! and check them in to your group repository.

{\bf Read the Javadoc for all the code we have provided.}

As a warm-up exercise you will use the code we have provided to read in 
the map and display it in the GUI.

\begin{enumerate}
\item Create a new Java class called {\tt GlobalNavigation} which implements \verb+NodeMain+. Read the {\tt
PolygonMap} API to find out how to read a map file and how to access the
components of the map.  Also, look at the data-types returned in the {\tt
PolygonMap} accessor methods.
\item In {\tt GlobalNavigation.onStart()}, create an instance of {\tt PolygonMap},
reading in a map file whose name is set as the parameter "mapFileName".  This can be done by accessing the ROS parameter server:

\begin{verbatim}
	ParameterTree paramTree = node.newParameterTree();
	mapFileName = paramTree.getString(node.resolveName("~/mapFileName"));
\end{verbatim}

\item Implement a method {\tt GlobalNavigation.displayMap()} which displays all
the contents of the map in {\tt MapGUI}.  You can start the ROS nodes from the command line or use the provided \verb+.launch+ files.
\end{enumerate}

Now continue by using the code provided to you for the convex hull algorithm
(in {\tt GeomUtils}) to compute and draw the convex hull of a set of
points. Recall that the convex hull of a set of points is the smallest convex
polygon containing all the points. You can visualize the convex hull by
imagining gift-wrapping the set of points.
\begin{enumerate}
\item Implement a method {\tt GlobalNavigation.testConvexHull()}. In this
method you will create instances of {\tt java.awt.geom.Point2D.Double}s to test
the convex hull algorithm given in GeomUtils.
\item Look at the API for the {\tt GeomUtils} class and find the specifications
for calling the convex hull method.
\item Plot out a (non-trivial!) set of test
points. Then, in your code, create a corresponding array of Java points and
test the convex hull algorithm.
\item Use {\tt MapGUI} to visualize the input points and the polygon returned
by the convex hull (temporarily disable display of the map).
\end{enumerate}

{\it Deliverables:} Screen shots of both the map and convex hull in {\tt
MapGUI}. Be sure to include a list of the coordinates of the points whose convex hull you computed.

\section*{Part 3: Building a Configuration Space }

In this part of the assignment, you will implement construction of the
configuration space.  Your C-space module will take as input a list of convex
polygonal obstacles ({\tt PolygonObstacle}), which will be specified by the
map.  You will also have to specify the geometry of your robot as a parameter,
as the computation of configuration space depends both on the geometry of the
obstacles and the geometry of the robot. The goal is to compute the
corresponding configuration space for the robot.

\medskip

Recall that the configuration space of an object with respect to a robot allows
you to reason about point robots by growing the obstacles in the map. The
configuration space depends on the shape of your robot and its degrees of
freedom. Your robots can rotate and translate in the plane. The true c-space
will thus be a 3-dimensional object with algebraic surfaces. Although we have
algorithms for computing such 3D c-spaces, in this assignment we will simplify
the problem to include translations only.  We can abstract out your robot's
rotational degrees of freedom by growing the robot. Think of including the
robot in a disk. Then the intersection between your robot and obstacles is
independent of the orientation of the robot and thus we can omit considering
the rotational degree of freedom of the disk robot in the c-space calculation.
Now we will consider a further simplification. Although there are algorithms
for computing c-spaces for objects with algebraic surfaces (a circle is an
algebraic surface, not a polygon), the algorithm described in class works for
the simplified case of polygonal-shaped robots. Thus we will do a further
simplification to approximate the disk robot by a polygon. One option is to
inscribe the disk in a square. Another is to do a polygonal approximation of
$n$ edges to the circle defining the robot.  

\medskip

\textbf{Note: These approximations should work for the basic and intermediate paths, 
but will not work for the hard path (extra credit).  The hard path requires your 
C-Space to account for the angle of the robot.}

\bigskip

Recall the algorithm for computing a c-space obstacle $C(O)$ relative to a
robot $R$ is

\[C(O) = ConvexHull (vert (O) \oplus ( \ominus vert (R)))\] 

In other words, after reflecting the robot you compute the convex hull of the
Minkowski sum of the obstacle vertices and the reflected robot vertices. You
will have to do this computation for each obstacle in the world.  Remember, we
have already provided Java code for computing the ConvexHull of a set of
points.

Design and implement code which accomplishes the following:
\begin{enumerate}
\item Given two polygons, compute their Minkowski sum.
\item Given a polygon A and a reference point r, reflect A about r (note that
the reflection of a square about its center point is the same square, and the
same holds for any $n$-edge polygonal approximation of a circle about its
center point when $n$ is even.  Thus, if you choose to implement either of
those cases, you may omit the reflection code, but document your choice in your
lab report.)
\item Given an obstacle polygon, a robot polygon, and the robot reference
point, compute the configuration space obstacle for the polygon.
\item Given a map with several obstacles, compute the configuration space of
this environment. You may or may not choose to include a representation of the
map boundary in your configuration space.  If you choose not to, you will have
to handle it at a higher level (you must somehow guarantee that your robot will
not drive into the maze walls).
\item Add code to your {\tt GlobalNavigation} class to compute and display the
configuration space of the map file specified on the command line.  Display
both the real obstacles and their corresponding configuration space
obstacles. (disable your convex hull test code now).
\item Test this part of the assignment on the map we have provided, which
corresponds to the maze set up for you in the lab. Then, test the algorithms 
on the two other environments ({\tt practice-maze-01.map} and \\ {\tt practice-maze-02.map}).
The format of the map file is documented in the Javadoc for the {\tt
PolygonMap} class.
\item {\bf Extra Credit: Implement a more complete configuration space
computation algorithm.}  For example, consider a robot which can be an
arbitrary convex polygon with an arbitrary reference point.  Or, relax the
constraint that the robot and obstacles must be specified as convex polygons.
\end{enumerate}

\medskip

{\it Hint:} Reflecting the robot about its center point or one of its vertices
makes the results of the c-space computation easier to visualize.

\medskip

What happens when you supply overlapping obstacles in your c-space computation
algorithm?  Consider an environment in which two adjacent convex obstacles form
an L-shaped object. How does your algorithm handle this case?

\medskip

{\it Deliverables:} Your wiki page should include a screenshot of the corresponding c-space
obstacles for each of the three maps you tested and answers to all questions above.

\vspace*{-0.15in}

\section*{Part 4: Motion Planning}
\label{motion-planning}

\begin{quote}
  \emph{A good plan is like a road map: it shows the final destination and usually the best way to get there.}\\ --- H. Stanley Judd
\end{quote}

Following Part 3, you will build a program that searches the configuration
space for a collision-free path from the given start location to the desired
goal location. Recall that the shortest path goes along the edges of the
visibility graph.  For this lab, you do not have to compute the {\em shortest}
path---{\em any short} path which takes the robot from the start to the goal
will suffice.  Note that we said short---the robot's path should not be
ridiculously longer than the shortest path.  Also, your path planner should be
{\em complete}: if a path exists, your planner should find it (in finite time).
The result of this part will be a sequence of position waypoints $(x_1, y_1),
(x_2, y_2), \dots, (x_n, y_n)$ where $(x_1, y_1)$ is the initial robot position
and $(x_n, y_n)$ is the goal location. These points represent a collision-free
path from start to goal.

\medskip

For this part you can implement any motion planner of your choice. In lecture
we described in detail an algorithm for {\em grid-based motion planning} with
potential fields. Alternatively, you can implement a different planner such as
the visibility graph motion planning algorithm, the Vornoi diagram motion
planning algorithm, or other grid-based methods. Your notes describe these
algorithms. Any standard algorithms text book will have some guidance on motion
planning algorithms; you can also find extensive documentation for almost
any such algorithm on the web.

\medskip

\textbf{Note: There are diagonal surfaces in the maze.
In order to handle them better, we recommend an 8-connected graph instead of a
4-connected graph if you choose to use grid-based methods.}

\medskip

{\it Deliverables}: Your wiki page should include a description of your
motion planning algorithm (with pointers to the corresponding code),
and the result of running your algorithm on the map that corresponds to 
the maze we constructed for you (write code to illustrate the results of the algorithm in the GUI, 
including the computed path from start to goal). You should also run the algorithm on the two additional
environments, and include the results. What is the time
complexity for your algorithm in big-$O$ notation? How much space does your
algorithm use?

\section*{Checkpoint -- Demonstrate Your Motion Plan }
For the checkpoint, you will need to demonstrate your motion planning algorithm in action.
You should be prepared to show and explain:
\begin{itemize}
\item Your C-space map generated from the map provided with the lab.
\item The path generated by your planning algorithm. The path should be drawn
on the GUI.
\end{itemize}
{\it Beware that you should probably have already started you waypoint navigation design and implementation efforts before this checkpoint.}

\section*{Part 5: Waypoint Navigation}

\begin{quote}
  \emph{A good plan executed today is better than a perfect plan executed at some indefinite point in the future.}\\ --- General George Patton, Jr.
\end{quote}

Waypoint robot navigation refers to controlling your robot to traverse a set of
points in sequence.

\medskip

The final part of this lab is to implement the motion plan you computed for the
maze in the hangar: given the starting configuration for your robot and the
desired goal location, execute the computed motion plan on your robot. Your
robot will take the output of your planner which is a sequence of position
waypoints $(x_1, y_1), (x_2, y_2), \dots, (x_n, y_n)$ where $(x_1, y_1)$ is the
initial robot position and $(x_n, y_n)$ is the goal location. The robot will do
waypoint navigation by traveling linearly from point $(x_i, y_i)$ to point
$(x_{i+1}, y_{i+1})$. Remember that the output of the planner contains points
in c-space. How do you map these points to the physical space?

\begin{figure}[h]
	\centering\includegraphics[width=0.4\linewidth]{GlobalNav-Pictures/path_planning_01_600h.png}
\end{figure}

{\it Deliverables:} Your wiki page should include a description of your
waypoint navigation implementation and tests on the three motion plans you
computed in part~\ref{motion-planning} and the answers to the questions above.
Record a video of your robot traversing the maze.  Using a ruler,
measure the accuracy of your navigation by measuring how close to the desired
goal location your robot reached. Also, numerically estimate the maximum amount
of error you observe at any waypoint. Does the error accumulate? What do you
think contributes most to the error: the translational or the rotational
component of the robot motion? How many times did you try your waypoint
navigation algorithm, and was the observed error at the goal location
consistent across the runs? How many times did it work, once you stabilized the
waypoint navigation code? What were the failure modes you observed on your robot,
if any?

\end{document}