Implement a global path optimizer for path follow template

The current path follow template records a set of way-points and then follows them. It will also record throttle values with each way-point. However, when the way-points are gathered it is usually better the drive slowly and accurately to get way-points that are accurate and close together. The resulting path will definitely not have race-quality throttle values or possibly an optimal racing path.

It is a common practice in autonomous racing to map the track then calculate an optimal path around it that includes way-points with velocity. The Indy Autonomous Racing team from the Technical University of Munich (TUM) have open sourced their global race trajectory optimizer which is written in Python. This is a very sophisticated algorithm that can calculate a trajectory at the limits of traction/stability. TUM also has simpler optimizers; one for optimizing for minimum curvature and one for optimizing for shortest path. There are other algorithms that use reinforcement learning to calculate a trajectory, for instance https://github.com/byronbenharris/reinforcement-learning-trajectory-optimization.

In this process we need to think about:

How do we map the drive-able area?
How or if we do model vehicle parameters?
Which algorithm do we use to calculate a path from the map?
How do we calculate velocities/throttle way points on the path?

In all of this we want to anticipate what the workflow for the human will be. This needs to be easy enough for a Donkeycar user to accomplish at a new track on the day of racing. What follows is a description of 4 command line applications that can be used in a pipeline to calculate an optimal race trajectory for a given car and race course.

1. Measure Vehicle Parameters

At a minimum we need to know the maximum speed/throttle we would want to apply (the value we would apply on a long straight) and the minimum speed/throttle we would want to apply (around the tightest turn on the track). These can be determined by driving the course manually and recording the way-points. Since Donkeycar records throttle rather than velocity we can estimate the velocity using the way-point position and time values between in the recorded path.

The current path follow template does not use velocities; each way-point is tagged with the throttle value that was being applied at the time it was recorded. Throttle may be fine for our purposes. We simply use throttle as a proxy for velocity. However, if we want to include velocity in our calculations, we will need to update the path recording code to include millisecond timestamps on each way-point in the path so we can calculate velocities. The following sections reference throttle/velocity, with the hope that we will be adding optional timestamps to the recorded path.

The human driver may be hesitant to drive the car to it's limits, so we should allow them to 'tweak' the minimum and maximum values using scaling factors.

NOTE: Throttle values are recorded as a range of -1 to 1, where -1 is maximum reverse throttle, zero is no throttle, and 1 is maximum forward throttle. Those values are used to calculate a PWM value that is use to control the motors (see actuators), so there is some interaction with how the PWM values set when the car is calibrated. It is common for a user to choose a PWM value for the maximum throttle that is considerable less than what the motor can actually do. I do this myself so that I know that when I am driving manually I can go full on the joystick and still be able to control the car. With our autopilot and optimized trajectory I hope we can do better than a timid human driver like myself, so it's possible we could end up maximum throttle values > 1.

[ ] Create a command-line application that takes in a recorded path and some parameters and calculates the minimum and maximum throttle and velocity that the car achieved.
Takes a recorded path as input
Takes in parameters that can be used to modify the calculation; for instance
- how many way points should be use to calculate the velocity in order to smooth out noise.
- scaling factor for minimum
- scaling factor for maximum
Outputs maximum target throttle and velocity
Outputs minimum target throttle and velocity

These outputs will be needed we calculate the target throttle/velocity for way-points in the optimized path. Ideally the command-line application will write these out to a file that can be used as configuration input to subsequent steps.

2. Calculate the Drive-able Area

The path follow algorithm outputs path consisting of way-points and an optional throttle value. We need to have a 'map' of the drive-able race area in order to calculate an optimized trajectory. A simple way to define the drive-able area is to have a human drive in the middle of the track while recording a path (or even just hold the car and walk down the middle of the track). Then a width can be given to the path using each way-point as the center of the path. So for each way-point, calculate a tangent using the way point and the one before it and the one after it. The calculate a perpendicular to that tangent and apply the track width to get the left and right boundary at that point.

[ ] Create a command-line program to calculate drive-able area:
Takes in a path follow path file (CSV), which it assumes is the center of the racing track.
Takes in one or more parameters that are used to expand the path into a drive-able area.
Outputs drive-able area in a format suitable for use in the path optimization stage.

3. Trajectory Optimization Algorithm

The simplest algorithm that offers the most stability would be to optimize for minimum curvature. The TUM solver for minimum curvature does not calculate velocities, just the path. So we need to add code to calculate velocities at each way-point (see section 4).

The TUMFTF code can take in a lot of parameters to use in this calculation. For our purposes we should make sensible defaults for most parameters and then make it easy to change these defaults.

[ ] Create a command line program that calculates an optimal trajectory around the drive-able area on the track.
Takes in the drive-able area from (1) and uses it to calculate the optimal path around the track.
Takes in values for parameters necessary to run the TUMFTF code; most will be fixed-defaults, some may be settable.
We would like to be able to choose some distance or density between the resulting waypoints. Hopefully the TUMFTF algorithm already does this. If not we may need to add a post processing step. The 'distance' parameter is probably really the estimated travel time between a way-point in the next one such that we have more waypoints around curves, where we will go slower and less in straights where we will go faster. As such, this may be better done in the next step (4).

4. Calculating Target Way-point Throttle/Velocities

As simple way to handle velocity is to apply a scaling factor based on curvature (or inverse curvature) at each way-point; calculate curvature range and use this to map between the minimum and maximum target velocities that were measured/configured in a previous step. The most simplistic approach would be to apply the throttle/velocity for the target way-point. This is not great when using our simplistic line follow algorithm based on cross-track error. It can be improved by taking a second pass that looks ahead in the waypoint list based on the first pass target velocity for the way-point and fixes up the target velocity based on the maximum curvature it would encounter at the look ahead point, so when we are going fast we look ahead for curves and start to slow down before we get there.

The output is a new path where each way-point has an (x,y) position and a target throttle based on the optimal path and the velocity calculation. This is the path that the Donkeycar path follow autopilot will use.

[ ] Create a command line program that will add target throttle to each way-point in the optimized trajectory.
Takes the optimized trajectory as input
Takes in the min and max target throttles/velocities
Takes in some parameter for 'look-ahead' that is used in the algorithm to decide at each way-point how many way-points ahead it should look to choose it's next target throttle.
Outputs a path where each way-point is given a target throttle. This is the final path the car will use to drive the track.

Summary

So we 4 parts to this pipeline:

Determine the car's minimum and maximum target throttle/velocity values around the track.
Use a path recorded in the path follow template that is assumed to be the center of the race track and convert this to drive-able area suitable for input into the TUM solver for minimum curvature.
Integrate the TUM solver for minimum curvature to produce an optimized race line.
Add target throttle/velocity values to each way point on the optimized race line. Output the optimized race line with throttle in the path follow template format so it can be used directly in DonkeyCar.

autorope / donkeycar