Cornell University: ECE 4960

Lab 10: Path Planning and Execution

Objective

The objective of this lab is to have your robot move from an unknown location to a goal location in your map as quickly as possible. You may do so using any means necessary - of course we recommend sticking to the tools you have already developed in previous labs and heard of in the lectures. This may involve any combination or subset of the following:

Open loop control
Obstacle avoidance
PID control
Proximity, TOF, accelerometer, gyroscope, magnetometer readings
Local path planning (e.g. Bug 0-2 algorithms)
Local localization (given odometry)
Localization using the prediction and/or the update step
Graph search algorithms

We will be judging your solution by your ability to juggle fast versus predictable behavior. For example, the faster your robot moves, the noisier it is. Onboard computation is relatively slow, but will save Bluetooth transmission time. Off-board computation can be optimized given particular data structures and queries. Graph-based path planning may produce efficient plans, but may take longer to compute, than simply executing local planners given your particular map. Please discuss your overall approach given the particular characteristics of your robot and setup. Please add a graphical description of your approach and note specifics (e.g. runtime) where possible.

Parts Required

1 x Fully assembled robot, with Artemis, batteries, TOF sensor, and IMU.
1 x Room mapped out in Lab 7.

Prelab

Map Discretization

We will be using a binary occupancy grid to represent the map. An occupancy grid is used to represent a robot workspace as a discrete grid. A binary occupancy grid uses true values (1) to represent the occupied workspace (obstacles) and false values (0) to represent the free workspace. You will need to discretize the map and define the occupancy grid manually based on your continuous map representation.

The image below shows an example discretization. The occupancy grid is depicted as an overlay over the continuous map with the indices along each axis. The shaded cells are marked as obstacles and the unshaded cells are free. The planning algorithm (global planner) uses this binary 2D matrix to plan a path from the start cell to the goal cell.

Coordinate Transformation

You will need to write functions to transform between the map frame and the grid frame, taking into account the discretization. The frames are axis aligned i.e there is no rotation between them. You may re-purpose the Mapper class from the Localization accordingly, use a third-party open source package or simply write your own functions to do this.

In the image above, the map frame of the world is depicted as a green point and the grid frame is the center of the cell [0,0].

Change Virtual Robot Start Pose :new:

In the previous labs, the robot always starts at [0,0,0] to prevent the neeed to apply a transformation between the odometry and map frames. There is also a validation function in the VirtualRobot class to check if that is the case . Since we want to plan paths from arbitrary initial positions, we need to remove this validation and then modify the simulator configuration to change the initial pose of the virtual robot.

STEP 1: Remove validation function that checks if the robot starts at [0,0,0]

Open robot_interface.py in the scripts folder in Visual Studio Code (or any other text editor).

Comment the following lines (Lines 48-49) in the __init__ function of class VitualRobot

 if(is_world_file_valid() == False):
     raise Exception("Please fix the world configuration file.")

Save the changes.

STEP 2: Set the initial start pose of the virtual robot

Open the simulator configuration file playground.world in the world folder in Visual Studio Code (or any other text editor).
Change Line 32 to reflect the pose of the robot. The format is [x y z a] with units [meters meters meters degrees].
- For example, to intialize the robot at x = 1 m, y= 2 m and a = 90 deg, the line should read:
```
 turtlebot(pose [ 1.000 2.000 0.000 90.000 ])
```
Save the changes and restart the simulator.

Change the simulator world :new:

Since the virtual robot is more “adept” at localizing that the real robot, you should use the larger, default world provided in the simulator. You are not required to change the simulator world, but may do so for comparing your real robot implementation with the virtual robot, or just to make things more interesting. If you do want to experiment with the simulator world, you can use the following code snippet in your Jupyter notebook:

# get_sim_model is a function defined in loc_utils.py
for i in range(0, start_points.shape[0]):
    get_sim_model(start_points[i], end_points[i]) 

The start_points and end_points are the start and end points of each line segment describing your wrld, respectively.

Copy the output text that the above code generates.
Open playground.world in the world folder in Visual Studio Code (or any other text editor).
Remove lines 35-41 (i.e all the lines after # Map configuration) that describe the default world.
Paste the generated text.
Save your changes and restart the simulator.

Implementation Tips

You can create a 1D list that defines the occupancy grid in a text editor (ex: Visual Studio Code) and use Numpy to resize it into the required 2D matrix. For example, we can encode the above occupancy grid with 17 rows and 23 columns as a 1D binary list as follows:

grid=[1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
      1,0,0,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,
      1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,1,1,1,1,
      1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1,
      1,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,1,
      1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
      1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
      1,1,1,1,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,
      0,0,0,0,1,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0,0,0,1,
      0,0,0,0,1,0,0,0,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,
      0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0]

Note that the above list is formatted for readability. It can then be resized into a 2D numpy matrix:

import numpy as np
grid = np.array(grid, dtype=np.uint8)
grid.resize(17,23)

Lab

Use the code from here, to generate a start and a goal location. Please make sure to try some examples, that requires the robot to circumnavigate one or more obstacles.
- Run setup.sh to install the necessary dependencies and refer to the Jupyter notebook on how to use the code in planner_query.py.
Place your robot at the start position, and let the robot finds its way as quickly as possible to the goal location. Have the robot indicate when it has successfully entered the particular grid occupancy cell. Discuss your results in terms of speed, runtime, and accuracy. Feel free to also discuss what you would do to improve your system if you had more time.

Write-up

To demonstrate that you’ve successfully completed the lab, please upload a brief lab report (<1.000 words), with code snippets (not included in the word count), photos, and/or videos documenting that everything worked and what you did to make it happen. Please make sure you cite ALL the open-source packages used in your implementation.