Programming Assignment 1

Due Monday, February 11, 12:00pm (noon)


Overview

This homework assignment is a simple introduction to C-spaces and how they relate to the workspace. Your primary task is to setup a simple 2D robot environment and create collision detection functions for it. This will later form the basis for some of the motion planning algorithms you'll be asked to program. For now, you are only required to visualize the free C-space of the robot. The robot has 3 degrees of freedom, so this can be done by sampling and approximating the 3D surface of the boundaries of the free C-space (i.e., the boundary between collisions and non-collisions).

Your program should process an environment configuration file describing the collision geometry of objects and the robot. It then has to output a visual representation of the free C-space of the robot.

The Environment

Objects in the environment (including the robot) are composed of 2D oriented bounding boxes (OBBs). There are a number of techniques for testing whether two OBBs intersect (e.g. using the Separating Axis Theorom). Your program (or script) will read a text file containing the placement of the robot base, and the relative positions of the obstacles in the environment.

To make the problem a little more interesting, the robot is a 3-link gripper. The configuration angles are marked on the figure as AngleX. The centers of rotation for each joint are given by the green circles. The red text indicates joint indices. The range of each joint is in [0,2*pi). The smaller extent (half width) for each box of the robot is 0.1. Finally, the center of Joint 3 lies on the center of the gripper's biggest box.

The format for the environment file is:


robot_pos_x robot_pos_y length_j12 length_j23 0
obj1_pos_x obj1_pos_y obj1_extents_x obj1_extents_y obj1_orientation
...
objn_pos_x objn_pos_y objn_extents_x objn_extents_y objn_orientation

Where

You can find a sample here.

Collision Detection and Sampling the C-space

Because we do not know the complex shape of the C-space obstacles, we will approximate them through sampling. Specifically, a given joint configuration q = (Angle1, Angle2, Angle3) of the robot can be tested whether or not it is collision free by applying the forward kinematics of the robot to position the links and testing for collisions between the link geometry and the obstacles in the workspace.

This process requires both a forward kinematics and a collision detection routine to be available. You are free to use publicly available collision detection software, or implement your own. If you would like to get a head-start on upcoming programming assignments, you may utilize 3D collision detection libraries by representing the 2D OBBs internally as 3D OBBs that are constrained to lie in a plane.

For this lab, you will be sampling the C-space by discretizing along each of the joint axes and testing a regular grid in the 3D C-space volume. Your program should allow the user to adjust the resolution of the discretization along each axis independently and rerun the sampling process.

Self Collision

Even without any obstacles in the environment, the robot can possibly collide with itself depending upon shape and movement capabilities. These "self-collisions" tend to occur between non-adjacent links. Collisions between adjacent links are typically prevented by setting proper joint limits.

For each test environment, make sure to show the resulting free C-space when considering self-collisions only.

Visualizaing the C-space Obstacles

For visualization, you may render 3D triangles obtained by 1) running any flavor of "marching cubes" algorithm, 2) using the Matlab script provided below, or 3) coming up with your own method of interpreting and visualizing the C-obstacle volumes.

If you are using Matlab, the following function will take care of displaying the visualization of the free C-space.

% draws the free space of a 3D C-space
% fnCheckCol takes a 3x1 matrix specifying the configruation of the robot,
% it should return 1 for collision and 0 for no collision
function drawfreespace(fnCheckCol)

N = 100; % number of points for each dimension
xgrid = [0:(2*pi)/N:2*pi*(N-1)/N];

[GridX, GridY, GridZ] = meshgrid(xgrid,xgrid,xgrid);
Grid = [GridX(:) GridY(:) GridZ(:)]';

FreeSpace = zeros(1,N^3);
for i = 1:size(Grid,2)
FreeSpace(i) = fnCheckCol(Grid(:,i));
end

FreeSpace = reshape(FreeSpace,[N N N]);

figure;
camlight;
axis equal;
p = patch(isosurface(GridX, GridY, GridZ, FreeSpace, 0.5)); hold on;
isonormals(GridX, GridY, GridZ, FreeSpace,p);
set(p,'FaceColor',[1 0 0],'EdgeColor','none', 'FaceLighting', 'phong');

Deliverables

You can use any language or program for this, as long as there are clear instructions on how to run it. Although you won't be graded specifically for the speed of your program, you have the potential to earn bonus points if your implementation is clean, robust, and efficient.

Everything should be turned in electronically using the subversion (SVN) account provided to you on the course server.

Short Questions

Please answer the following questions:

1. Determine the C-space for a car that drives around on a huge sphere (such as the earth with no mountains or oceans). Assume the sphere is big enough so that its curvature may be neglected (e.g., the car rests flatly on the earth without wobbling). [Hint: it is not S^2 x S^1] (#10, Planning Algorithms, pg. 180)

2. Suppose five polyhedral bodies float freely in a 3D world. They are each capable of rotating and translating. If these are treated as "one" composite robot, what is the topology of the resulting C-space (assume that the bodies are not attached to each other)? What is its dimension? (#16, Planning Algorithms, pg. 180)

What to turn in

Extra Credit

Any type of visualizations are extremely helpful when debugging a planning system in high dimensions. You can receive extra credit if you do any of the following: