Assignment 03 - Path Following

Assignment 03 Goals

The goal of assignment 03 is to implement path following on your differential drive robot. This requires the following subgoals:

1. Enable PF Localization

2. Mapping wheel velocities to motor PWM commands 

3. Mapping robot velocities to wheel velocities

4. Implementing point tracking control

5. Implementing path tracking control

6. Path tracking experiments

Key Deliverables

To  be submitted on brightspace under assignment 03, one submission per team:

1. Assignment 03 Report

2. Video of robot point tracking

3. Video of robot path tracking 

4. Copy of lab_3.py file

5. Copy of gui_03.py file and your PWM calibration CSV file

Use of Gen AI Tools

In the coding sections (2–5), you will first complete each section without GenAI tools. At the each of these sections when the work is completed, repeat the work by asking a GenAI tool to write the same functions. Run your unit tests on the GenAI version, and compare it with the code you wrote without Gen AI. Keep notes: in the report you will explain how the AI's solution was similar or different, and which worked better.

1. PF Localization

Before you can use a controller on a robot, you need reliable state estimates of the pose of the robot with respect to a coordinate frame. The Particle Filter localization algorithm will provide those state estimates.

Step 1: Flash the new firmware ESP32_lab3.ino to your robot. Install numpy (pip install numpy).

Step 2:  Add your motion model function from assignment 02 to the assignment 03 file gui_03.py. Also fill in your Lab 2 constants (ticks per revolution, wheel radius, wheelbase) in gui_03.py and lab_3.py.

Step 3: The current map for the PF for the corner of a room. Fine a room that has a corner with 2x2 meters of free space, (no real large furniture that will get in the way of the lidar.)

Step 4: Place the robot at the location 0.25 m from each wall, facing in a direction parallel to the wall as shown in the image below. This start location is the origin of the coordinate frame. Then click RESET LOCALIZATION (robot at start pose) in the GUI. All poses in the GUI are in meters with respect to this origin. The box "Measure from walls (tape)" shows the robot's distance from each wall, so you can check it with a tape measure.

Step 5: Drive the robot (using the gui) around the 2 x 2 meter square space. Watch the particles on the screen (red) move as well as the robot's state esimate (blue). The estimate should always be within 0.05 m of the actual robot. Confirm this is true. You will need this localization for the point tracking and path tracking controllers to work.

2. WHeel Velocities to PWM

The motors take PWM commands, not wheel speeds in rad/s. The relation between PWM and wheel speed is not linear, and it is different for each wheel and each direction.

Step 1: Lift the robot so the wheels do not touch the ground. In gui_03.py, write get_calibration_pwms(), which returns the list of PWM values to test from 0 to 255, and measure_wheel_speeds(delta_ticks_left, delta_ticks_right, dt), which returns the wheel speeds in rad/s using your Lab 2 functions. Then click CALIBRATE PWM. The GUI runs your PWM values forward and backward and saves the results to a pwm_calibration_*.csv file.

Step 2: In gui_03.py, write the function speed_to_pwm(wheel, phi) that takes a desired wheel speed and returns the PWM to send, using your calibration table. Interpolate between measured points.

Step 3: Check that:

• the sign (direction) is preserved;

• speeds below the lowest speed the wheel can reach return 0;

• a higher speed never gives a lower PWM;

• each wheel uses its own calibration.

Step 4: Ask a GenAI tool to write the same functions and compare them with yours.

Suggestions (optional): at very low speeds, send short PWM pulses instead of a constant PWM (already implemented in the GUI). If the measured wheel speed is lower than requested, add a small correction. If a wheel is stuck, give it a short PWM boost.

3. Robot Velocities to WHeel Velocities

Step 1:  Using the math from lecture slides, derive the equations that relate the left and right robot wheel velocities as a function of v and w, i.e. forward and rotational velocities of the robot respectively.

Step 2: For a function called get_wheel_velocities(v, w), that inputs the forward and rotational velocities of the robot, and returns the left and right wheel speeds phi_dot_l and phi_dot_r, write 5 unit tests called wheel_velocity_unit_test_X, where X = 1,2,3,4,5. These five unit tests should handle cases where:

1. v > 0, w=0

2. v < 0, w=0

3. v = 0, w>0

4. v = 0, w<0

5. v = 0, w=0

Step 3: In the file lab_3.py, write the function get_wheel_velocities(v, w).

Step 4: Write a function run_wheel_velocity_unit_tests() that tests your get_wheel_velocities(v, w) function against all 5 unit tests and prints to the terminal how many of the tests pass out of 5. Run the function run_wheel_velocity_unit_tests() and check that all unit tests pass.

Step 5: Ask a GenAI tool to write get_wheel_velocities(v, w), run your unit tests on it, and compare it with your version.

4. Point Tracking

Step 1:  For a function called point_tracking_controller(X_des, X), that inputs the desired and current robot pose X = [x, y, theta], and returns left and right wheel speeds phi_dot_l and phi_dot_r, write 5 unit tests called point_tracker_unit_test_X, where X = 1,2,3,4,5. These five unit tests should handle cases where:

1. X = [0,0,0], x_des > 1, y_des > 1, theta_des=0

2. X = [0,0,0], x_des > 1, y_des < -1, theta_des=0

3. X = [0,0,0], x_des < -1, y_des > 1, theta_des=0

4. X = [0,0,0], x_des < -1, y_des < -1, theta_des=0

5. X = [0,0,0], x_des > 1, y_des > 1, theta_des=pi

6. X = [1,1,0], x_des > 2, y_des > 2, theta_des=pi

The unit test can call the function robot_simulator(X_tm1, phi_dot_l, phi_dot_r, delta_T) (provided in lab_3.py) that inputs the robot pose X_tm1 for some time step t-1 as well as the left and right robot wheel speeds phi_dot_l and phi_dot_r, and the time step size delta_T that the simulator will run for, in seconds. This function returns the updated robot pose X_t for time step t. This function should also return a list of the robot poses that it visited throughout the test.

Hint: Write a while loop that at each iteration calls the necessary functions for point tracking and simulating the robot. The loop should end when the robot is within a 0.03 meters of the desired position and 0.1 radians of the desired orientation.

Step 2: In the file lab_3.py, write the function point_tracking_controller(X_des, X). Write any supporting functions you may also want. Use the point tracking controller from Lecture 05A: the coordinate transformation to ρ, α, β (slide 29), the control law v = kρρ, w = kαα + kββ (slide 36), the stability conditions kρ > 0, kβ < 0, kα − kρ > 0 to choose your gains (slide 38), and the Backwards Method when |α| > π/2 (slides 40–43). Then convert v and w to wheel speeds with your get_wheel_velocities(v, w) from Section 1.

Step 3: Write a function run_point_tracker_unit_tests() that tests your point_tracking_controller(X_des, X) function against all 5 unit tests and prints to the terminal how many of the tests pass out of 5. Run the function run_point_tracker_unit_tests() and check that all unit tests pass.

Step 4: Write and check more unit tests to convince yourself the robot works for point tracking.

Step 5: Plot the paths from each unit test on a single plot. This should be included in your report.

Step 6: Ask a GenAI tool to write point_tracking_controller(X_des, X), run your unit tests on it, and compare it with your version.

5. Path Tracking

Step 1:  For a function called path_tracking_controller(P, X), that inputs the desired path (a list of poses that constitute a path P = [X1, X2, X3...]) and current robot pose X = [x, y, theta], and returns left and right wheel speeds phi_dot_l and phi_dot_r, write 3 unit tests called path_tracker_unit_test_X, where X = 1,2,3. These three unit tests should handle cases where:

1. X = [0,0,0], P = [[0,0.1,0],[2,0.1,0]]

2. X = [0,0,0], P = [[0,0,0],[1.0,0,0],[1.0,0,pi/2],[1.0,1.0,pi/2]]

3. X = [0,0,0], P = [[0,0,pi/4],[2.0,2.0,pi/4]]

The unit test can call the function robot_simulator(X_tm1, phi_dot_l, phi_dot_r, delta_T). This function should also return a list of the robot poses that it visited throughout the test.

Hint: Write a while loop that at each iteration calls the necessary functions for path tracking and simulating the robot. The loop should end when the robot is within a 0.02 meters of the final path desired position and 0.3 radians of the final path desired orientation.

Step 2: In the file lab_3.py, write the function path_tracking_controller(P, X). Write any supporting functions you may also want. Use the path tracking solution from Lecture 06A (slide 7). At each time step: find the pose p_c,t on P closest to the robot; find the pose p_des,t a distance Δ further along the path; use p_des,t as the input to your point tracking controller from Section 4.

Step 3: Write a function run_path_tracker_unit_tests() that tests your path_tracking_controller(P, X) function against all 3 unit tests and prints to the terminal how many of the tests pass out of 3. Run the function run_path_tracker_unit_tests() and check that all unit tests pass.

Step 4: Write and check more unit tests to convince yourself the robot works for path tracking.

Step 5: Plot the paths from each unit test on separate plots, e.g. as subfigures of one figure. This should be included in your report.

Step 6: Ask a GenAI tool to write path_tracking_controller(P, X), run your unit tests on it, and compare it with your version.

6. Robot Experiments

Step 1: Replicate all of your point tracking unit tests by running point tracking control on the real robot. Note you can use the Point Tracking Control box on the GUI that allows you to enter a desired robot pose. 

Step 2: Replicate all of your path tracking unit tests by running path tracking control on the real robot. Note you can use the Path Tracking Control box on the GUI that allows you to enter a list of desired robot poses. 

Some unit tests go outside the 2x2 m free space. On the real robot, use these versions instead. Before each test, drive the robot (using the GUI) to the start pose.

Point tracking (start pose → desired pose):

1. [0,0,0] → [1,1,0]

2. [0,1.25,0] → [1,0,0]

3. [1.25,0,0] → [0,1,0]

4. [1.25,1.25,0] → [0,0,0]

5. [0,0,0] → [1,1,pi]

6. [0.25,0.25,0] → [1.5,1.5,pi]

Path tracking:

1. P = [[0,0.1,0],[1.5,0.1,0]]

2. same as the unit test

3. P = [[0,0,pi/4],[1.5,1.5,pi/4]]

Step 3: For each experiment, record the final position and orientation error, and for path tracking the largest distance from the path. Report these in your results.

7. Report Writing

Write a formal report of length 3-6 pages, using IEEE format as mentioned on the website assignment schedule page. For this assignment, be sure to include all plots mentioned above, and more plots if you think they are significant. Assume the reader knows about robotics, but that they are not familiar with our class. Sections should include:

1. Abstract - The section should provide an overview of the work. There should be a minimum of one sentence that informs the reader about the motivation, method, experiment, and results. Be sure to provide at least two significant quantifiable results from your results that a reader may find interesting.

2. Introduction - Use at least one paragraph to describe the motivation for path following. Use a paragraph to provide an overview of each section of the paper. E.g. “The method section will describe mathematical details of the controllers developed, after which the experimental design section will detail how they were validated.”.

3. Method - Describe your point tracking controller and path tracker with mathematical equations. Start with what is given to you - i.e. introduce the robot (with an image) and discuss how the robot is controlled. Use the equations from Lecture 05A (point tracking) and Lecture 06A (path tracking), and cite them. Be sure to define all new variables in text. Equations should not have words, just greek letter variables with numbers and letter subscripts. Number all equations. Describe how you mapped wheel velocities to PWM, and include a plot or table of your calibration. For each coding section, explain how the GenAI solution was similar or different from yours.

4. Experiment design - Use images, photos, figures to explain the experimental setup and how physical measurements were taken. 

5. Experimental results - Show all your plots here. Discuss assumptions problems, successes, etc. Label all plots. Each plot should be referred to at least once in your text discussions.

6. Conclusion - Present a high level understanding of the performance of your point tracking and path tracking controllers. Mention efforts that could be taken in the future to improve tracking.

7. References - Cite key references. You may want to do a little research to cite key textbooks, controllers you found.

We will use the following grading rubric for a total of 100 ponts: 

• 10 - Abstract 10

• 20 - Method (clear & correct) 20

• 50 - Results 50

• 20 - Spelling/Format/Grammar