Instructions:  This  assignment  can  be  completed  in  teams  consisting  of  up  to  two individuals. The assignment has two parts. In Part 1, you will implement path planning algorithms on 2D occupancy grids. In Part 2, you will apply your RRT implementation to plan collision-free motions for a 7-DOF Franka robot arm in MuJoCo. The key insight is that the same RRT algorithm works in both settings — only the configuration space and collision checker change.

1.1 Background

Occupancy Grids

An occupancy grid is a discretized representation of a 2D environment. The environment is divided into a regular grid of cells, each of which is labeled as either free (the robot can occupy it) or occupied (the cell contains an obstacle). A cell’s status is stored as a binary value: 0 for free and 1 for occupied. Given a grid resolution δ  (meters per cell) and an origin (xorig, yorig ), the center of cell (i, j) in world coordinates is

x = xorig  + j . δ ,        y = yorig  + i . δ .

A* Search

A* is a graph search algorithm that finds the shortest path between a start node and a goal node. It operates on a graph where each node represents a grid cell and edges connect neighboring cells (4-connected or 8-connected).

A* maintains an open set of candidate nodes, always expanding the node with the lowest value of f (n) = g (n) + ℎ (n), where:

• g (n) is the cost of the path from the start to node n,

•  ℎ (n) is a heuristic estimate of the cost from n to the goal.

For  grid-based  planning,  common  heuristics  include  the  Euclidean  distance  and  the diagonal (Chebyshev) distance. When ℎ is admissible (never overestimates the true cost), A* is guaranteed to find the optimal path.

Rapidly-exploring Random Trees (RRT)

RRT is a sampling-based motion planning algorithm that incrementally builds a tree rooted at the start configuration by randomly sampling the space. At each iteration:

a) A random point qrand  is sampled from the configuration space.

b) The nearest node qnear  in the existing tree is found.

c) A new node qnew  is created by extending from qnear  toward qrand  by a step size η .

d) If the path from qnear  to qnew   is collision-free, qnew  is added to the tree.

The algorithm terminates when a node is added within a threshold distance of the goal. RRT is probabilistically complete: given enough time, it will find a path if one exists, but the path is generally not optimal. The RRT* variant rewires the tree to asymptotically approach an optimal path.

The power of RRT is that it is agnostic to the dimension of the configuration space. Whether q is a 2D point (x,y) or a 7-dimensional vector of joint angles  (q1, … , q7), the algorithm structure is identical. What changes between applications is:

• The configuration space (bounds on each dimension),

• The distance metric (how to measure “nearest”), and

• The collision checker (how to determine if a configuration or edge is valid).

Configuration-Space Planning for Robot Arms

For a robot arm with n joints, the configuration space is the set of all joint angle vectors

q = (q1, … , qn) ∈ ℝn. Each joint has limits qi(min) ≤ qi ≤ qi(max) . A configuration is  collision-

free if, when the robot is placed at that configuration, none of its links intersect with obstacles in the workspace (or with each other, in some formulations).

To  check  collisions,  we  use  a  physics  simulator:  set  the  joint  angles  in  MuJoCo,  call mj_forward to compute forward kinematics, and inspect the number of detected contacts. If data .ncon > 0, the configuration is in collision.

To check an edge between two configurations qa and qb , we interpolate: q(t) = (1 — t)qa + tqb for t ∈ [0, 1], and check several samples along the edge for collisions.

1.2 Part 1: 2D Grid Planning

You will be provided with occupancy grid maps stored as .npz files. Each file contains:

• grid: a 2D binary array (0 = free, 1 = occupied)

•  resolution: grid cell size in meters

• origin: (x, y) position of cell (0, 0) in world coordinates

•  start: start position (x, y) in world coordinates

• goal: goal position (x, y) in world coordinates

Requirements

a) A* Implementation: Implement the A* algorithm to find a path on the occupancy grid. Use 8-connected neighbors. Your implementation should return the sequence of waypoints from start to goal in world coordinates.

b) RRT Implementation: Implement the RRT algorithm to find a path in the contin- uous  2D  space  defined  by  the  map  bounds. Your  collision  checking  should  verify that straight-line segments between nodes do not pass through occupied cells. Your implementation should return the sequence of waypoints from start to goal in world coordinates.

c)  Output: For each provided map file map_N .npz, save two path files:

• map_N_astar .npz containing a key path with an M × 2 array of waypoints (∞, y) in world coordinates.

• map_N_rrt .npz containing a key path with an M × 2  array of waypoints  (∞, y) in world coordinates.

d) Visualization:  Generate  and  save  a  plot  for  each  map  and  method  showing  the occupancy grid, start and goal positions, and the computed path overlaid on the grid.

Evaluation

a) Validity: The path must be collision-free — no segment may cross through an occupied cell.

b)  Completeness: The path must connect the start to the goal.

c)  Path length: Sum of Euclidean distances between successive waypoints. Shorter paths are better.

d) A* optimality: A* path length relative to the optimal grid-based path length (should be near 1.0).

1.3 Part 2: Franka Robot Arm Planning

In this part, you will use RRT to plan a collision-free motion for the 7-DOF Franka Emika Panda robot arm in MuJoCo. An obstacle (a box) is placed in the robot’s workspace. You must plan a path in the 7-dimensional joint space from a start configuration to a goal configuration, avoiding collisions with the obstacle and with the robot itself.

Setup

The helper code provides:

•  franka_utils .py: loads the Franka model in MuJoCo with a box obstacle, and provides functions for collision checking and visualization.

•  franka_scene.xml: the MuJoCo scene definition (Franka arm + obstacle + table).

• Pre-defined start and goal configurations stored in franka_problems .npz. You can visualize a problem and test the collision checker by running:

python3 franka_utils .py 1

Requirements

a) RRT in Joint Space: Adapt your RRT implementation from Part 1 to work in the 7D joint space of the Franka arm. Specifically:

•  Configuration space: Sample joint angles uniformly within the joint limits pro- vided by MuJoCo (model.jnt_range).

• Distance metric: Use the Euclidean distance in joint space, ‖qa  — qb ‖2 .

•  Collision checking: Use the provided check_collision(model, data, q) function, which returns True if configuration q is in collision. To check an edge, interpolate between configurations and check samples along the edge.

b)  Output: For each problem in  franka_problems .npz, save a file  franka_N_path .npz containing a key path with a K × 7 array of joint-space waypoints.

c) Visualization: Use the provided animate_path function to generate a video or screen- shot sequence showing the robot executing the planned path.

Evaluation

a) Validity: Every waypoint and every interpolated configuration along each edge must be collision-free.

b)  Completeness: The path must connect the start to the goal configuration (within a joint-space tolerance of 0.1 rad per joint).

c)  Path length: Sum of Euclidean joint-space distances between successive waypoints.

1.4 Hints

• Helper code is provided in the assignment2 folder.

•  Start with Part 1. Get A* and RRT working on the simple 2D maps before tackling the Franka arm.

• For A*, a priority queue (e.g., Python’s heapq) is essential for efficiency.

•  For RRT, a step size η that is too large may cause the planner to miss narrow passages; too small may be very slow. Experiment with values appropriate to the space.

•  For collision checking along edges, sample configurations at small intervals (e.g., 10–20 samples per edge) and verify each is collision-free.