\documentclass{article}
\usepackage{times}
\usepackage{fullpage}
\usepackage{amsmath}
\usepackage{graphicx}
\usepackage{tweaklist}
\usepackage{pifont}
\usepackage{epsfig}
\usepackage{color,soul}
\usepackage{hyperref}

\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 7: Grasping and Object Transport}

  \textbf{Distributed: 3/12/2014, 3pm}\\
  \textbf{Due: 3/19/2014, 3pm}

\end{center}

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

Your objective in this lab is to understand grasping and object transport.
You will build an arm with a gripper for your robot. You will incorporate the
arm in your robot. You will then use the arm to pick up objects and transport 
them to desired locations.

This lab will give you the technical skills to incorporate grasping
and manipulation capabilities into your robot.  This lab will also
enhance your knowledge of the mechanics of objects in contact which is
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 Grasping 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 Hardware}: How proficient are you at modifying the hardware
  of your robot? \\ (1=Not at all proficient; 2=slightly proficient;
  3=reasonably proficient; 4=very proficient; 5=expert.)

\item {\bf Mechanics of Manipulation}: How proficient are you at
  mechanics and kinematics? \\ (1=Not at all proficient; 2=slightly
  proficient; 3=reasonably proficient; 4=very proficient; 5=expert.)

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

\end{itemize}

\vspace*{0.1in}

{\bf \large To start the lab, you should have:}
\begin{itemize}
\item The arm/gripper kit: 12 laser-cut pieces for the arm and
  gripper, 3 servos, 1 break-beam sensor, mounting hardware.
\item Your notes from the Grasping, Kinematics, and Manipulation
  lectures
\end{itemize}

\vspace*{0.1in}

In addition to this lab specification, you should have the following
handouts:
\begin{enumerate}
\item Arm Assembly Instructions
\item Sharp IS471F Datasheet, available online
\end{enumerate}

\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: Grasp Synthesis}
%
%In this part of the lab you will solidify your knowledge of synthesizing grasping 
%points for an object. Recall the algorithm described in lecture that explained how to synthesize a closure grasp with four
%frictionless point contacts. The goal is to find points on 4 polygonal edges so that
%the torque closure property is satisfied. Also recall the algorithm for computing independent
%regions of contact for two hardfinger contacts on planar objects. In this algorithm
%the goal is to identify mutually acceptable grasping regions  on the two opposing edges.  The regions must meet  the constraint that any force that falls 
%within the friction cone in the region of the first edge can be reciprocated by a `sticking' force within the region of the opposing edge. By a `sticking' force, we mean the direction is
%within the friction cone. Which algorithm
%is more robust and why? Consider the polygons in Figure~\ref{fig:grasping}.  Indicate diagrammatically and provide high level pseudocode of  algorithms that do the following: 

%\begin{figure}
%\epsfig{figure=grasping.eps, width=0.8\columnwidth}
%%%\include{figure=grasping.pdf,width=0.8\columnwidth}
%\caption{(Left): Planar object for grasp synthesis with frictionless fingers.
%(Right) Two opposing edges of an object for grasp synthesis of independent regions
%of contact with hardfingers. The edges are drawn with corresponding friction cones.}
%\label{fig:grasping}
%\end{figure}

%\begin{enumerate}

%\item
%Synthesize a torque-closure grasp for four frictionless point contacts for the left object.

%
%\item
%Synthesize independent regions of contact for two hardfinger contacts for the right object
%(where we have drawn only two opposing edges for this object.) How would you determine 
%opposing edges, given the geometry of an object?

%\end{enumerate}

%{\em To Hand In:} Your web report should include the figures showing the resulting grasps and provide high level pseudocode of  an algorithm for each of the two cases.

\section*{Part 1: Building the Arm}

In this part of the lab you will use the kit we give you to assemble
and install an arm with gripper for your robot. The exemplar robot
will be available for inspection. It shows the end result of your
assembly. The arm assembly handout contains pictorial step-by-step
assembly instructions.

Before assembling the arm it is a good idea to test the servos.
\textbf{The shoulder servo MUST always be run with the robot powered
  by the battery, not the AC adapter due to its current requirements.}
You can test the servos by starting Part 2 and testing the servos in
parallel with some of the assembly steps.

{\em Deliverables: Create a new page on your wiki called ``Grasping
  Lab Report Group N.''  Take some pictures of your arm while being
  constructed, and a picture of the final result, put these on your
  wiki page. Please record in the difficulties you encountered, if
  any.}

\section*{Part 2: Controlling the Arm}

You should begin by adding the new lab source code to your group
repository following the usual procedure.  The RSS code base contains support for your arm servos and the breakbeam sensor.  The \verb+uorc_listener+ node listens to the message \verb+/command/Arm+ for arm commands and the \verb+uorc_publisher+ publishes the current arm state on \verb+/rss/ArmStatus+.

\subsection*{Arm parameters}
The ORC board (and your code, by extension) has the ability to support
servos through ({\it fast} DIO ports on the ORC board).  The source
code you are given assumes that all of your servos are
identical, and each accepts a 16 bit PWM value which is integrated by
the servo electronics into a rotational position. However, each model
of servo that you have been given has specific maximum and minimum
angles that it can express. These correspond to maximum and minimum
PWM values. You will need to add code to handle this differentiation. In addition, you will need to
calibrate each servo's mapping from PWM to the corresponding angular
value. Instructions for how to do this will be covered in the Arm
Control subsection of the lab.

As an exercise, read through \verb+uorc_listener+ code to determine the appropriate ports for your arm servos. Note that you may elect to use different ports, so it is important that you know the location of this specification in the code.

\subsection*{Arm class libraries}
You can view the current state of the Arm by subscribing and
handling \verb+rss_msgs.ArmMsg+.  As a reminder, to subscribe to arm messages, implement a handler using the following code snipets:

\begin{verbatim}
   private Subscriber<org.ros.message.rss_msgs.ArmMsg> armSub;
   ...
   @Override public void onStart(Node node){   
      armSub = node.newSubscriber("rss/ArmStatus", "rss_msgs/ArmMsg");
      armSub.addMessageListener(new ArmListener(...));
   }
   ...
   public class ArmListener implements MessageListener<ArmMsg> {
      ...
      @Override public void onNewMessage(ArmMsg msg) {
           ...
      }
   }
\end{verbatim}

The \verb+ArmMsg+ contains six floats, the first three of which correspond to  the first three PWM I/O ports.

You can also subscribe to the BreakBeam sensor on topic \verb+/rss/BreakBeam+ with message type \verb+rss_msgs/BreakBeamMsg+.

\subsection*{Arm control}

Your goal in this part of the lab is to implement simple, reliable
control of the arm. We have provided for you a helper GUI for
exploring arm control, called \verb!ArmPoseGui!.

For each arm servo, you need to determine its \textbf{maximum} and \textbf{minimum} PWM values.

The servo cannot be physically moved past these values; if you try,
the command will either be ignored, or worse, the servo motor will
chatter against the physical limits.  You should take into account not
only the range of motion of the servo itself, but also within the
context of the arm's range of motion.

For each servo, use the slider in the \verb!ArmPoseGui! to determine
what the extreme PWM values are.  Be very careful as the arm may move
very fast when you do this.  

%Setting the PWM value to zero will
%disable a servo.  If you happen to exit the program without setting
%the values to zero your arm may still be trying to hold a position.
%You can use the class \verb+ClearArm+ to reset the arm.  Run it as
%\verb+java Grasping.ClearArm+ to reset the arm.  This is a useful
%command when you are developing your code.  We have also provided code
%that resets the servos if you exit the carmen window or use
%control-c. If for some reason, the servos are left holding a position,
%you should manually cut their power.

You need to know what PWM values correspond to actual angles, in
order to compute a conversion between angles and PWM ticks. For each
servo, move the servo to the position that you consider to be
$\theta_1 = 0$ radians using the slider in \verb!ArmPoseGui!. Note the
PWM value, call it $\mathit{PWM}_1$. Now, move the servo to some other
angle, such as $\theta_2 = \pi/2$ radians. You will have to measure
this angle carefully.  Note this PWM value as well, call it
$\mathit{PWM}_2$.  You can use these two data points to compute a
conversion between angles and PWM by fitting a line and interpolating
for desired values.  The slope of your line will be:
\begin{equation}
m = \frac{\theta_2 - \theta_1}{\mathit{PWM}_2-\mathit{PWM}_1}
\end{equation}
The theta-intercept of your line can be determined by plugging in one data point:
\begin{equation}
\theta_i = \theta_1 - m \cdot \mathit{PWM}_1
\end{equation}
Recognize that you'll need separate conversion factors for each servo motor,
including the gripper. 

Now, create a new file called \verb!Grasping.java! in which you will
place the code for this lab.  Begin by writing a simple Java program
that receives the arm messages. Using the appropriate conversion
factors for each servo, write \verb!handle(ArmMsg msg)!, which
moves each servo through its full range of motion, moving all servos
concurrently. This handler should repeat the motion indefinitely. Note
that this will require implementing a (fairly simple) finite state
machine inside your arm message handler.

\vspace*{0.05in} 

\textbf{One caveat:} You should be careful about moving any servo
through too large a range of motion in a single step. You might want
to experiment with how large a range of motion each servo can
tolerate, but a good rule of thumb is that \textbf{no servo should
  move more than 1 radian per iteration}. Moving faster could cause
the servos to skip, fuses to blow or worse, an unexpected motion could
slam the arm into the ground destroying it.  This slew rate control
can be accomplished by implementing a clamped feed-forward control
step for each servo.

\vspace*{0.05in}

\textbf{Hint:} You may want to write a joint controller class and
create subclasses for each of the shoulder, wrist, and gripper joints.
This will help you to capture the common methods for servo control,
while enabling specific behaviors for each joint.

\vspace*{0.05in}
{\em Deliverables: Your wiki should include:
\begin{itemize}
\item Your minimum and maximum PWM measurements for each servo
\item Your angle measurements and your angle-to-PWM conversions for each arm
\end{itemize}
}

\subsection*{Arm control and inverse kinematics}

Your goal in this part of the lab is to characterize the gripper position in
terms of joint angles. Notice that you have two revolute joints (the shoulder
and the elbow) that control the position of the end effector. There will, in general,
be two sets of solutions mapping between the joint angles and end-effector position in body coordinates. You will encounter this ambiguity in your computation, and you must choose one solution (based on continuity, servo bounds, etc).

\begin{itemize}
\item Measure the length of each arm segment. Note: use the distal end of the
  gripper as the end of your kinematic chain.
  
\item Determine the forward kinematic equation that maps joint angles to end
  effector positions.
  
\item Determine the inverse kinematic equation that maps end effector
  positions to joint angles.
  
\item Choose an end effector position in the $x, z$ plane in the robot
  frame. For each of several end effector positions, compute the
  appropriate joint angles, move the servos to those angles, and
  measure the position of the end effector in body coordinates.
  
\item Place an object in the gripper, and close the gripper. (You
  should be able to close the gripper using a Java program. Do not
  force the gripper jaws closed by hand.) Repeat the measurement
  process with the object in the gripper.
\end{itemize}

\vspace*{0.05in}

{\em

Deliverables: Your wiki should contain a set of explicit assumptions
you made in building an inverse kinematic arm controller. You should
also discuss how accurate your controller is, and how you might
correct it.  Are there any failure modes and what are they, if any?

\begin{itemize}
\item Your measurements of your arm
\item Your mathematical model of the inverse kinematics 
\item The expected and measured end effector positions with and without an
  object in the grasp.
\end{itemize}
}

\vspace*{0.05in} 

\textbf{Note:} The controller for the PWM servos is feed-forward.  As a result, the \verb+uorc_publisher+ only publishes the last commanded values for the servos (and not the true position of the servo). However, this information may be useful to account for time delays between the nodes running on the workstation and the nodes running on the netbook.  We suggest you use the feedback from \verb+uorc_publisher+ to determine when the arm has reached a position. 

\vspace*{0.05in} 

\textbf{Optional:} The ORC
board does not contain enough input lines to allow us to equip the
servos with encoders and so that you could use PD controller that you
implemented in earlier labs. However, you do have an additional
sensor: the camera. How might you incorporate the camera to correct
for arm controller errors?

\subsection*{Checkpoint}

The staff will walk around to do a checkoff. We
will be looking to see that:
\begin{itemize}
 \item Your arm is constructed and mounted on the robot
 \item You can control your arm via the ArmPoseGUI
 \item You can control your arm via inverse kinematics
\end{itemize}

\section*{Part 3: Grasping and Transporting an Object}

\subsection*{Arm gymnastics}

In this part of the lab you will build arm
behaviors.  The arm control libraries can be used to program ``arm
gymnastics''.  Write a program that controls the arm through a
sequence of moves: open-gripper, close-gripper, move-up, bend-elbows,
touch-the-ground. To do this you will have to calibrate the arm to
differentiate between an open and closed arm, and to detect when the
arm touches the ground. % You can do this by using current as a way to
detect impediments.  Make sure you slew the commanded servo positions
(only move at most one radian per iteration), otherwise you will
destroy your arm when it mistakenly hits the ground (which is not
  fun).


Write a program to implement:

\begin{enumerate}
\item
open-gripper
\item
close-gripper
\item
move-up with a desired angle
\item
bend-elbow with a desired angle
\item
move-to-ground
\end{enumerate}

and then demonstrate how you can sequence these behaviors as ``arm gymnastics''.

\vspace*{0.05in}
{\em Deliverables: Your lab report should show a video sequence of the arm
gymnastics and an explanation for how you controlled each movement.}

%\subsection*{Checkpoint: Wednesday, April 9}
%The staff will walk around the lab
%to do a mid-lab check-off.  We would like to see your
%inverse kinematic equations for the arm and a demonstration of your
%robot performing the arm gymnastics sequence.

\subsection*{Grasp and Transport}
In this part of the lab, your robot will pick up an object and move it a
specified distance.  To begin, place the arm of your robot on the
floor, in an open position. Then, manually place an object (one of the
colored cubes) in the gripper. This action should be detected by the
break-beam sensor, which should then trigger a grasping behavior for
the arm. Once the object is grasped, the arm should be lifted and the
object should be transported some distance forward.  You may choose
any distance and direction for this displacement.

\vspace*{0.05in}
To complete this functionality, write software to do the following:

\begin{enumerate}

\item
Initialize the arm and move the joints to their pre-grasping position.
Servo the gripper to a open position where the break-beam sensor has a
clear field of view.

\item
Wait for an object to penetrate the grasp region of the gripper by
monitoring the break-beam sensor.  (Remember: the break beam sensor
should be connected to slow digital I/O port 7.)

\item
Grasp the penetrating object. 

This part is a little tricker than simply closing the hand.  You will
have to calibrate your gripper for two things: (1) to decide how tight
to close it around the object, and (2) to make sure that you maintain
complete closure of your object so that when you lift it off the
ground it will not fall out of the hand.

If you want to check whether the object has fallen out, how will you
do so? Is this a reliable method? Can you think of a more reliable
one? (Hint: a different sensor)

\item
Lift the grasped object off the ground.

Your lifting method should detect and recover from error. Error occurs
when your hand drops the object. Implement recovery by trying to grasp
once again.  The break-beam will also give you an empty hand signal in
this case.

\item
Move the robot to deposit the object at the new location.

\item
Place the object on the ground and move the robot back to its original
starting point.  Measure the error between the desired location of the
object and its true placement for several trials.

\end{enumerate}

You may find it helpful to begin by drawing a diagram of your finite
state machine, and identifying which components of your system are
active in each state.

\vspace*{0.05in}

{\bf Hint 1:} The break-beam sensor will work best at a static pose,
with the gripper partially open. As the gripper changes pose, the
orientation of the sensors will change and you will likely experience
false-positives.

{\bf Hint 2:} We have provided the utility class
\verb!SensorAverage.java!. You
may find this handy for filtering out sensor transients and
stabilizing the perceptual states of your grasping FSM.

\vspace*{0.05in}

{\em Deliverables: Your wiki report should include a video of this
  task and an explanation for your implementations. Please include a
  discussion of your calibration parameters used for impediments (for
  closing the hand with and without the object) and for detecting when
  the object slips out of the grasp.  How reliable is your control of
  arm gymnastics? How reliable is the control for grasping? How
  accurate is the displacement of the object? Discuss the failure
  modes of this functionality. Please also give us a pointer to the
  code and answers to the questions above.  }

\section*{Part 4: Searching For and Retrieving an Object}

\begin{quote}
  \emph{I have done this approach two hundred and thirteen times on the simulator. We are NOT where we should be.}\\ --- Col. Robert Iverson, The Core
\end{quote}

Your goal in this part of the lab is to integrate the object
pick-and-carry implementation from the previous section, with your
visual servoing code from the Visual Servoing Lab. (We're ecological
roboticists -- we recycle.) The basic idea is to visually servo to a
block of a specific color and maintain an appropriate fixation
distance, such that you can then retrieve and transport the
object. You are free to use your own code from the Visual Servoing Lab
or the solution code. The issues of colour calibration, blob
centering, etc., are the same regardless of whether you use your
solution or ours.

\vspace*{0.03in}

If you recall, the \verb!BlobTracking! class contains the
\verb!apply(Image src, Image dest)! method, which extracts all the
blobs of the appropriate hue from the \verb!src! image, and highlights
the blobs in the \verb!dest! image.  Your \verb!BlobTracking! class is
not calibrated to the new object, so you must first re-calibrate.

\vspace*{0.03in} 

Recall from the Visual Servoing Lab that this is accomplished by
holding the object you wish to calibrate within the camera's view,
while outputting the HSB histogram in the \verb!VisionGUI!.  If the
object you wish to track is the dominant feature in the scene, then
the dominant hue in the histogram should be the hue of your object.
Once you have identified the hue of your block, edit the target hue
level used by your classifier (in the solution code for
\verb!VisualServo.java!, this is done by setting the
\verb!target_hue_level! parameter with \verb!Param.set!, so that no
physical changes need to be made to \verb!BlobTracking.java!.  

In \verb+Grasping.java+, subscribe to the video messages on \verb+/rss/video+.  Instantiate a \verb+BlobTracking+ object and pass in the video.  Publish some debug output on \verb+/rss/blobVideo+, as suggested below, which shows the highlighted blobs.

\begin{verbatim}
  Publisher<org.ros.message.sensor_msgs.Image> vidPub;
  ...
  vidPub = node.newPublisher("/rss/blobVideo", "sensor_msgs/Image");
  ...
  Image dest = ...;
  ...
  org.ros.message.sensor_msgs.Image pubImage = 
       new org.ros.message.sensor_msgs.Image();
  pubImage.width = width;
  pubImage.height = height;
  pubImage.encoding = "rgb8";
  pubImage.is_bigendian = 0;
  pubImage.step = width*3;
  pubImage.data = dest.toArray();
  vidPub.publish(pubImage);
\end{verbatim}

Test your blob tracker by placing your object in the field of view of the
camera, and watch the display. You should see your
object highlighted in the camera panel.

\vspace*{0.03in}

The next important piece is the visual servoing, which requires that
you know the size of the object in the field of view to determine the
appropriate stand-off: if the object appears too small, you need to
drive closer, and if the object appears too large, you would need to
back up. Your ability to determine the distance to the object depends
on knowing how large the object is. Let us assume that the radius
returned by the blob tracker is a reasonable approximation of the
object width. Measure the object's width, and modify the target radius
size used by your blob tracker (again, the solutions utilize the
\verb!Param! class for setting the \verb!target_radius!
parameter). Test your visual servoing code by having your robot servo
to the object as you did in the Visual Servoing Lab.

\vspace*{0.03in} 

The final parameter you need to calibrate is the stand-off
distance. You need to determine how far the block is from the center
of the camera when the block is inside the gripper break-beam.  You
should be able to measure this parameter directly by placing the
object in the break-beam.

\vspace*{0.03in}

Once you have determined that you are able to visually servo to the
object, you need to co-ordinate the object pick-and-carry
implementation from Part 3 with your visual servoing code. In
particular, once the break-beam sensor detects the object, you should
stop the robot translating and stop processing the visual servoing
commands. At the same time, you should start closing the gripper
preparatory to lifting the object.

{\em Deliverables: Your wiki report should contain:
\begin{itemize}
\item A screenshot of your block
\item Your calibration histogram
\item Your calibration parameters (hue, size, stand-off distance)
\item A description of each module and algorithm in each, the APIs between the
  modules
\item A description of your robot operation. How well does your visual
  servoing work in this lab compared to the Visual Servoing Lab?  Include a video of your robot
  running fully autonomously, as in the previous lab part.
\item The failure modes of this functionality
\item The task allocations within your team
\end{itemize}
}

\vspace*{0.05in}

\textbf{Optional:} You might consider using a few different stand-off
distances and implementing your visual servoing code in the following
manner:
\begin{enumerate}
\item Retract the arm fully, so that it is out of the field of view of
  the camera
\item Visually servo to the block with a stand-off distance such that
  you are close, but not yet gripping the block, for example, roughly
  .5m away.
\item Lower the arm so that the gripper is at the right height to grip
  the block
\item Visually servo to the block with the correct stand-off to be
  able to grip the block, and monitor the break-beam sensor
\item Start lifting once the break-beam sensor detects an obstacle
\end{enumerate}

Why might we recommend this visual servoing method? 

\section*{Wrap Up}
Report the time spent on each part of the lab in person-hours and
indicate what elements were done independently or in pairs, triples or
as a full group.

\end{document}





