Programming Assignment 3

Due Monday, April 14, 12:00 pm (noon)

Overview

In this assignment you'll be planning for an articulated robot to solve the Tower of Hanoi puzzle in a 3D workspace. This will require putting together some of the course topics on sampling, planning, and smoothing. The particular approach used to solve the puzzle and plan for the robot will be all up to you, we'll only give suggestions as to how to go about solving it. The final deliverable is a program that takes as an input the robot location, the location of the three pegs, the source peg that contains 3 disks of different sizes, and the destination peg that they should be moved to such that the constraints are satisfied. The program should then output a trajectory for the arm in terms of joint angles that moves the robot to solve the puzzle. The robot you'll be using to perform this task is a PUMA manipulator.

The 3D Workspace

Planning for a high-DOF manipulator in a 3D workspace is challenging due to the complexity of the geometry, robot kinematics, and corresponding C-space structure and topology. It is also a significant effort to assemble the necessary software infrastructure to support planning in a 3D environment. Debugging planning software within a 3D workspace environment is interesting and often investing the time to visualize information graphically can save a lot of future development and debugging time. The focus of this homework assignment is to develop a simulation of a manipulator arm to perform a series of pick-and-place operations in a 3D world.

To help simplify the development process, we will be using a planning testbed called OpenRAVE to support collision detection, kinematics, and 3D rendering. OpenRAVE has been specifically designed to assist researchers to develop and test planning algorithms. OpenRAVE also provides a very convenient communication channel between its 3D simulation environment and Matlab/Octave, thus it is easy to script a demo and change parameters online without having to recompile, restart the program, or reload the test scene. You can find more about OpenRAVE in the followingonline tutorial.

Although you are not required to use OpenRAVE for this homework assignment, it is highly recommended you do so. Using OpenRAVE will save you a lot of time and will give you the opportunity to learn a powerful package that may be useful for your future research.

Manipulation Planning

Manipulation planning focuses on path planning with the goal of manipulating objects. The objective for this assignment is to develop the high level planning software for a simulated manipulator arm to perform a sequence of pick-and-place operations involving removing and placing rings of various sizes onto different pegs. The task is greatly simplified in that your simulations will be focused on generating only the high-level arm and end-effector trajectories at a kinematic level. For example, we do not expect the grasps you choose to be stable and robust. For this assignment, the end-effector of the PUMA should approach the disk and close the gripper so as to maintain some form of contact. You can kinematically attach the object to the gripper (via RobotBase::Grab) and begin moving it around rigidly along with the end-effector. Releasing the object is also straightforward: simply open the gripper at the desired position and orientation of the object.

In this version of the Hanoi puzzle, each "disk" is actually a square with a hole in the middle, making it relatively easy to grasp with the PUMA gripper. In particular, we will only require you to consider grasp approach directions that are aligned with the edges of the square (hence, each disk can be approached from the side along four different primary grasp directions). Note that due to kinematic or obstacle constraints, it may be infeasible to grasp a particular disk from some approach directions; therefore make sure to iterate across all four possibilities before giving up on an object. In OpenRAVE, we have provided you with an inverse kinematics solver to the PUMA arm. Once you have picked a location that the gripper should be in order to grasp the disk, you can run the IK solver to find feasible joint angles of the arm (see RobotBase::Manipulation::_pIkSolver).

Towers of Hanoi

The Towers of Hanoi puzzle you'll be required to solve has 3 pegs and 3 disks. In the beginning, the 3 disks all lie on one chosen peg with the bigger disk at the bottom. The goal is to move all 3 disks to a destination peg such that a bigger disk is never put atop a smaller disk. In the OpenRAVE environment file, they are 4cm thick with side lengths of 6cm, 4cm, and 3cm. To make the environment more interesting, there is a table in front of the PUMA. (For those people not using OpenRAVE, you should add a table surface or other additional obstacles to make the planning queries more interesting.)

Although OpenRAVE supports physics simulation through ODE, it should be turned off for this assignment (default is off). This means that whenever the arm puts a disk in a peg, it should place it as far down as possible on the peg, right atop the surface of the table or previous disk (if there is one).

The final program should accept 4 types of input

Robot pose - translation of the robot, the orientation will always be the identity.
Peg poses - the position and direction of three pegs in the workspace.
Source peg - the source peg where the disks start in.
Dest peg - the destination peg where the disks should be placed.

When using OpenRAVE: simply reading the environment file will be sufficient to accepting all four input parameters. The environment file specifies the initial pose of the robot and the initial poses of the pegs. The three pegs will always be named "srcpeg", "destpeg", and "peg". The peg whose name is "srcpeg" should be where you place the disks at the start of the simulation. Be sure to initialize the disks to their start positions (i.e. do not rely on the initial position specified in the environment file!)

Working with OpenRAVE

Please read about OpenRAVE at the official OpenRAVE wiki . One of the features that makes OpenRAVE powerful is its ability to interface to Matlab. You can setup the Matlab scripting environment by following the directions here. After installion, you should follow the simple tutorial on scripting. Then, you should download the starter pack for the OpenRAVE Hanoi puzzle. Copy the hanoi.env.xml file in the opernave/bin directory. You can launch it by ./openrave -f hanoi.env. Or in Octave/Matlab by orEnvLoadScene('hanoi.env').

For Linux, once you unzip the hanoipuzzle, execute the following lines to build it and get OpenRAVE to recognize the plugin:

aclocal
automake -a
autoconf
./configure --prefix=$(ABSOLUTE_OPENRAVE_DIR)
make install

Run ./openrave -listplugins to check if your plugin loaded properly.

For Windows, just add the msvc8 proj file into the solution directory of openrave

Note that you can also make the plugin without the prefix, but in order to load it into OpenRAVE, orEnvLoadPlugin will need to be called from the scripting environment.

Deliverables/Requirements

A planning algorithm used to solve the pick and place problem for robots. If you are using OpenRAVE, the planner should be derived from PlannerBase, and it should parse the information in PlannerParameters to extract its source and destination goals. The startup code has a skeleton class for a "HanoiPlanner". By following these simple guidelines, your planning algorithm can be used by any other OpenRAVE module! Note that OpenRAVE is distributed with several example planner implemenations: RRTs, BiRRTs, and Randomized A*. Although these planners are good for gathering ideas, you should NOT use these planners in your assignment. The goal is to give you the chance to create and implement your own planner and test it. It should be significantly different from the example planners.
The final robot trajectories must be smooth. Feel free to use some of the techniques discussed in class, or experiment with your own method for smoothing.
Working code for a PUMA arm to solve the Hanoi puzzle. The inputs are the relative transformation of robot base, the 3D transformations of three pegs, the source peg that the three disks are originally placed on, and the destination peg that the disks need to be relocated to.
A brief writeup of what you did for this homework, and some instructions on how to use your system (e.g. how to specify inputs to the planner, etc). The input method should be intuitive and easy.
A video showing the PUMA arm solving the Hanoi Tower puzzle. OpenRAVE has video recording functionality which can be accessed by clicking on Options->Record Video.

Extra Credit

As always, there is extra credit available for students. Some extra credit ideas are:

Implement one of the nicer smoothing algorithms that optimizes clearance from Roland Geraerts, et Al., or experiment with visibility/Voronoi techniques we've covered
Allow the robot to approach and grasp the disks from more the primary four directions (e.g. from above, or at angles), and integrate these possibilities in your grasp selection and planning algorithm.
Run simulations on stacks of more than three disks, and/or with more than three pegs.
Integrate some optimality criteria into the planning process not beyond just the number of manipulation operations (e.g. considering such things as C-space and workspace path metrics, time or energy optimality, etc.)