{ "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 45}, {"x": 300, "y": 0, "heading": -90}, {"x": 0, "y": 0, "heading": 135}, {"x": -300, "y": 0, "heading": -90}, {"x": 0, "y": 0, "heading": 45} ] }

A Dive into WPILib Trajectories



Declan Freeman-Gleason, Team 4915

Watch a video of this presentation

Purpose of this Presentation

  1. Provide an understanding of the purpose and context of WPILib Trajectories.
  2. Show how trajectory generation and following works, not how to use it.
  3. Stay at a high level that can still act as a good starting point.
{ "reverse": false, "waypoints": [ {"x": -300, "y": -20, "heading": -20}, {"x": 300, "y": 20, "heading": -20} ] }

History

Spring 2015 254 Motion Planning Presentation
→
Spring 2016 Pathfinder Released
→
Summer 2018 254 Releases 2018 Code
→

Late 2018-2019 Some Teams Port and Use 2018 254 Traj. Code
→
Summer 2019 WPILib Contributor Ports 2018 254 Traj. Code
→
Now (Fall 2019) WPILib Trajectories Work
→

How do we make a trajectory?

Step 1: Specify "Waypoints"


From these waypoints
We want this

Step 2: Find Splines

  • Can we approximate these waypoints using a convenient mathematical function?
  • Enter the Hermite Spline:
    • These are just a polynomial of some degree.
    • We want to find the coefficients. How?
    • $P(t) = H_1p_0 + H_2p_1 + H_3\dot{p}_0 + H_4\dot{p}_1$
    • $t \in [0, 1]$
    • $p_1 =$ a vector containing the final x and y
    • $p_0 =$ a vector containing the initial x and y
    • $\dot{p}_0$ and $\dot{p}_1 =$ tangents to the initial and final position vectors
    • $H_i$ are the Hermite Blending functions; these are found with a little algebraic manipulation
    • E.g. $H_1 = 2t^3-3t^2+1$
    • Every waypoint pair gets one polynomial
    • We use quintic or cubic* splines
    • Now we just have to sample at a bunch of different $t$s, right?
    * Both allow for continous curvature, but cubic makes you give up control of heading at the inner waypoints.

Step 3: Sample Trajectory States

We want to get the intermediate points from $P(t)$.

Sample With a dt = 0.1

Reparameterize and Sample With a ds = 35

Pathfinder does this, but...

It's slow, because we must numerically integrate to find arc length

$\int_{0}^{1} \sqrt{\frac{dx}{dt}^2 + \frac{dy}{dt}^2} dt$

Reversively Subdivide Until We Get Under Max $dx$, $dy$, and $d\theta$

$dx \approx$ forward, $dy \approx$ side "skid", $d\theta \approx$ turning

No integrals! Much faster than Pathfinder

Step 4: Find Whole-Robot Velocities

Given some basic constraints like $v_{max}$, $a_{max}$ and $a_{min}$, and some other constraints, we want a motion profile.
$v$ { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ] } { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ] } $t$ { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ] } { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ] } { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ] }

Step 4: Find Whole-Robot Velocities

We can just do basic motion profiling, but we want arbitrary constraints!
Constraints convert trajectory "states" to min/max velocity and acceleration.
E.g. limit velocity within a region; limit control effort (voltage).
Let's try limiting in a region of the field.
We want to find the maximum admissible velocity for each trajectory state.
Forwards pass. Set the velocity of each state to $min(v_{user max}, v_{accel max}, v_{constraint max})$.
Forwards pass has computed all accels between states, but some of them are larger than our max accel.
We do a backwards pass that uses all computed accels and max accels to compute acceptable new max velocities.
Then we use $a$, $s$, and $v$ to compute $t$ for all states. We now have a complete time-parameterized velocity profile!
$v$ $t$ { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ], "regionConstraints": [ {"maxVel": 50, "xmin": 100, "xmax": 200, "ymin": -30, "ymax": 30} ] } { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ], "regionConstraints": [ {"maxVel": 50, "xmin": 100, "xmax": 200, "ymin": -30, "ymax": 30} ] }
{ "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 600, "y": 0, "heading": 0} ], "regionConstraints": [ {"maxVel": 50, "xmin": 100, "xmax": 200, "ymin": -30, "ymax": 30} ] }

Step 5: Find Wheel Velocities

We want to turn whole-robot velocities into wheel velocities. We use inverse kinematics.
$v_l = v_c - \omega{}r_b$ and $v_r = v_c + \omega{}r_b$
$v_c$:
$v_l$:
$v_r$:
{ "reverse": false, "waypoints": [ {"x": -300, "y": -20, "heading": -20}, {"x": 300, "y": 20, "heading": -20} ] } { "reverse": false, "waypoints": [ {"x": -300, "y": -20, "heading": -20}, {"x": 300, "y": 20, "heading": -20} ] } { "reverse": false, "waypoints": [ {"x": -300, "y": -20, "heading": -20}, {"x": 300, "y": 20, "heading": -20} ] }
{ "reverse": false, "waypoints": [ {"x": -300, "y": -20, "heading": -20}, {"x": 300, "y": 20, "heading": -20} ] }

Step 6: Odometry

We could use $v_r$ and $v_l$ as the references (setpoints) for the wheels and be done...
Pathfinder calls it a day at this point. But what if our robot gets bumped?
If we can keep track of where we are ("odometry"), we can use that to compensate.
Let's take the distances traveled by each side $d_l$ and $d_r$, and convert them the distance traveled by the whole robot, $d_c$.
$p = p + \langle cos(d\theta) d_c, sin(d\theta) d_c \rangle$. Note that wpilib uses a more sophisticated method which takes turning into account.
{ "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 340, "y": -120, "heading": 0} ] }

Step 7: Global Following

Now that we know where we are, we will use a "global pose controller" to keep us on the path.
The best option is the RAMSETE nonlinear, time-varying unicycle controller.
Given our pose, a target pose, and our current wheel velocities, RAMSETE makes new target wheel velocities to get us back on the path.
To follow a trajectory, we:
  • Start a timer at $t = 0$
  • We get the trajectory state at the current $t$, and get the target wheel velocities modified by RAMSETE for that trajectory state.
  • We feed those velocities to the wheel followers (usually P controllers), and we're done!

Step 8: Putting it all together

  1. Take waypoints and parameterize a spline to go through each waypoint.
  2. Generate a velocity profile from each point in the spline with user-specified constraints.
  3. Get the time for each trajectory state.
  4. Get the trajectory state for the current time and track out position on the field using odometry.
  5. We take modify the left and right velocities with RAMSETE to keep us on the path.
  6. We feed the left and right velocities to the wheel P controllers.

Demos

{ "reverse": false, "waypoints": [ {"x": 65.625, "y": -45.50, "heading": 0}, {"x": 112.90023622, "y": -45.25, "heading": 0}, {"x": 190, "y": -90, "heading": 320}, {"x": 260.75, "y": -45.509, "heading": 90} ], "regionConstraints": [ {"maxVel": 15, "xmin": 0, "ymin": -173, "xmax": 100, "ymax": 173} ] }

Bonus slide: Trajectory Reversal

reversed = false reversed = true
Traj. 1 { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 300, "y": -60, "heading": 0} ] } { "reverse": true, "waypoints": [ {"x": 0, "y": 0, "heading": 180}, {"x": 300, "y": -60, "heading": 180} ] }
Traj. 2 { "reverse": false, "waypoints": [ {"x": 0, "y": 0, "heading": 180}, {"x": 300, "y": -60, "heading": 180} ] } { "reverse": true, "waypoints": [ {"x": 0, "y": 0, "heading": 0}, {"x": 300, "y": -60, "heading": 0} ] }