HW 4 – Waypoint following
Due: 2019.10.28 End of day
G. Fainekos – Intro to Robotics – Fall 2019
For this homework problem, you will control the bicycle robot to follow a sequence of waypoints. You
are allowed to use any control algorithm you like for controlling the motion of the robot between
waypoints. The recommended control algorithms are “Moving to a point” ([Corke 2011] Ch. 4.2.1),
“Following a line” ([Corke 2011] Ch. 4.2.2), and “Following a path” ([Corke 2011] Ch. 4.2.3).
However, you will be graded by how close you
remain to the path defined by the waypoints.
For example, consider the sequence of
waypoints in Figure 1. This is a sequence of 4
waypoints with the robot originally placed at
pose (0, 0, 0). If you use the control algorithm
that just directly goes to the next waypoint
(“Moving to a point”) with velocity set to
constant value 1, then the resulting robot
motion has a total error of 13.2301 (Figure 2).
On the other hand, if the “Following a line”
algorithm is used, then the total error is
9.0899 (Figure 3). Thus, your goal is to traverse
the waypoints as close as possible to the lines
defined by the waypoints.
Figure 2. Traversing the sequence of waypoints WP1 using
the “Moving to point” algorithm. Circle indicates a 0.5 radius
around each waypoint. Total error: 13.2301
LOC: ~14 in template (comments not counted)
Figure 3. Traversing the sequence of waypoints WP1 using
the “Following a line” algorithm. Circle indicates a 0.5 radius
around each waypoint. Total error: 9.0899.
LOC: ~32 in template (comments not counted)
Figure 1. The sequence of waypoints WP1 in the script file
Food for thought: You may want to slow down the bicycle in corners or you may want to start turning
the bicycle earlier (while still making sure that you enter the neighborhood of the waypoint).
When have you visited a waypoint?
The robot is considered to have visited a waypoint when its distance to the waypoint is less than 0.5
(value of variable d_wp in script file hw4_bicycle_initialize_data.m). Note that in your
implementation you may want to consider a smaller distance so that that scoring algorithm does not
miss the fact that the robot actually visited the waypoint. You are provided an error computation
function (totalTrajError.m) so that you can evaluate your solutions.
Files and templates provided
1. hw4_bicycle_run_script.m : starting
point for the homework: instantiates objects,
runs simulation, plots the results and evaluates
2. hw4Controller.m : A controller class
template. You will need to modify the method
“compute”. The template just provides a
controller method for moving to a target
position with maximum velocity without a
stopping criterion when reaching the waypoint.
See figure on the right.
Other useful files:
1. BicycleModelEulerSimulator.m: A simulator class for simple bicycle models. It uses
the forward Euler method for simulation. It is more lightweight than the simulator class
RobotBicycleSimulator.m which we introduced in class.
2. totalTrajError.m : computes the total deviation (error) between the robot trajectory and
lines defined by the waypoints. Note: if you miss a waypoint, then totalTrajError returns
3. plotWP.m : quickly plot the waypoints
Max points: 100
If you miss waypoints, then penalty 5 pt for each waypoint missed
If your algorithm returns error more than the average of my “follow the line” and “move to
point with max velocity” algorithms, than maximum 85 pt
If your algorithm returns error less than the average of my “follow the line” and “move to point”
algorithms, than maximum 100 pt
Remark: My implementations of “follow the line” and “move to point” are the textbook versions of the
algorithm; no optimizations or parameter tuning. You can potentially get better score by tuning the
parameters. The algorithm for following a path can potentially provide you with better results since it
has a look-ahead component (this algorithm is also called PurePursuit and it is implemented in the buildin Matlab code robotics.PurePursuit for differential drive robots).
If you wish you can post anonymously on Piazza the WP set and your error score for comparison with
The control algorithms that achieve 2.5% better performance in time and cost than our best control
algorithm will receive 5 bonus points.
Submit your modified class hw4Controller.m as hw4Controller.txt
o Your code will be checked for plagiarism! No copying.
If you plan to implement the PI control loop, then you have to integrate in your control loop. For
a way to integrate read:
HW 4 – Waypoint following