My team is currently trying to implement motion profiling on our robot. So far, I’ve watched 254’s presentation at Worlds and from there have been trying to understand Splines.
From what I gather, you have the general form of a cubic polynomial and since you know the initial and final position and velocity, you can solve for the a, b, c, and d values. Once you know that, you can create a parametric equation that tells you the x and y position at any given time. What I don’t understand is how you go from a sequence of waypoints (x, y, heading) to finding the following:
The m0 and m1 values needed for the a, b, and c values (ie a=2x_0-2x_1+m_0+m_1)
The trajectories once your path has been defined (position and times)
I’ve been using a graph I made on Desmos to try and visualize this but it hasn’t cleared much up yet. I’ve also been trying to follow this paper but they use a different type of Spline so I don’t know if the trajectory generation carries over (nor could I really follow what they were doing).
This may be a better paper for understanding Hermite splines, since it synonymizes the input parameters with position, velocity and acceleration(for quintic hermite splines).
In the paper I link, it breaks down the cubic into the following, where H_x are basis functions satisfying certain conditions described in the paper.
p(t) = H_1(t) * p0 + H_2(t) * v0 + H_3(t) * v1 + H_4(t) * p1; t = 0 at p0 and 1 at p1
That way, the way you calculate the hermite spline is consistent and independent of your input position and velocity.
There are several tools out there already implementing some form of motion profiling. It may be more beneficial to look a those options instead of trying to roll your own.
BobTrajectory v1.0 was just released today.
Jaci’s Pathfinder is also a very popular option.
Worst case you can peek at the code bases and get an idea of what they’re doing in the background.
What would be the difference between a cubic and a quintic hermite spline (in terms of its effect on the robot)? And are you suggesting I start with a quintic implementation instead of a cubic?
I looked at Pathfinder before but it wasn’t working nicely with my teams robot so I am trying to understand the process of how the profiles are generated. (I tried looking at it’s code but got bogged down in the c) I’ll take a look at Bobs though, I haven’t heard of that one.
A cubic curve guarantees consistent heading between two curves where one ends in the initial state of the next. A quintic curve goes further and says that the rate of change of heading (curvature) is consistent between the two as well. For FRC purposes, quintic curves are useful but not necessary, as long as you’re careful with your internal waypoints.
If you’re just starting to learn about motion profiling, I recommend sticking with cubic curves, and focusing your efforts into learning the other parts of the process, the velocity profile and turning that into motion on your robot.
m_0 and m_1 are your initial and final tangent vectors. These should have the same direction as your initial and final “headings.” The method for determining their magnitude is usually derived empirically in the FRC setting. IIRC 254 takes the linear distance between the final and initial points and multiplies it by a factor larger than 1. On 1678, we have a bit of a wonky process you can take a look at here, although I would suggest going the 254 route .
Once you have generated your spline and a set of points from t = 0 to t = 1, then you should time-reparametrize the spline to a trajectory. I would suggest setting up some constraints, like max linear velocity/acceleration and max angular velocity/acceleration.
Then, do a “forwards pass” by assigning the goal initial velocity/accelerations to the first point and running through to the end with the maximum accelerations/velocities.
Then start at the end of the spline and do a “backwards pass” with the goal final velocity. This backwards pass should try for maximum acceleration, but take whatever the forward pass yielded if it is lower. This way, you should end up with a spline parametrized so that it starts and ends at a set initial/final velocity.
I was looking through 254’s code that did it and I tried adding that to this graph, but I was getting very wonky results and wasn’t sure if I was missing something.
A common approach in the real world (and what we did in 2018 in a limited way) is to let the magnitudes of m_0 and m_1 (and the curvature parameters as well) be determined by some sort of iterative gradient descent optimization.
.