The previous few posts have included some information about what the 6328 software team has been working on, but we thought it would be worth going into a little more detail about our adventures with motion profiling.

Our motion profiling system is built using the standard functionality built into WPILib. We wrote a custom command which wraps the normal RamseteCommand. This gives us an easier API for setting up the profiles and means we don’t have to define all of the constraints for each one; There’s only one configuration per robot.

For each path, we wrap the motion profiling command in a command group. We do this for a couple of reasons – first, breaking each path into a separate file makes them easier to manage. Also, we can easily add in functionality before and after the profile. Before running, we set the odometry position using our coordinate system (with the origin in the upper right of the field near A11). Because we don’t care about the ending position, we set the ending velocity of the profile as high as possible and chain a command to brake after – the robot can stop quite quickly so we don’t hit a wall. The odometry can’t track accurately through that kind of skidding, but exact positioning doesn’t matter at the end of the profile.

Visualization

When iterating quickly through versions of the trajectories, testing on the robot can get quite cumbersome. We’ve been using a path visualization tool put together by college mentor Cameron (FRC 401 in high school). Here’s some more information on that system from him:

I wrote this tool a couple of years ago as a student on FRC Team 401. We were getting into advanced trajectory following (this was before spline trajectories and the Ramsete controller were part of WPILib) using Team 254’s code as a reference. I needed a way to quickly visually examine trajectories before running them on a robot to ensure they had no malformed sections (such as wheels reversing due to tight turns, forgetting to set a trajectory as reversed, etc.), check that constraints were being applied correctly, and verify waypoint geometry.

The tool generates the trajectory using WPILib’s trajectory generator, and then plots the resulting path on the screen. It then uses inverse kinematics to draw the paths of the left and right wheels, and colors these paths on a gradient based on the maximum speed throughout the path. This “heatmap” visualization makes it very clear to see which parts of the path are acceleration or deceleration, and to check that centripetal and region constraints are being applied correctly. The tool also provides the ability to “simulate” the path by drawing a “robot” (black square) which traverses the path at real-time speed. This isn’t a true simulation in that it does not account for any sort of physics, instead purely drawing what the trajectory is commanding the robot to do. This is useful for getting a visual sense of how the robot should behave, and identifying areas where the robot appears to be doing something physically impossible. Along with this real-time display, some statistics are drawn in the upper left corner about current pose and timestamp. The tool uses Swing (built in Java GUI API) to draw the graphics, and it was built very rapidly, so not much work was put into making it pretty.

The code for the visualizer can be found in my “FRCKit” library, a work in progress to bring tools like this as well as simulation and other utility libraries to more FRC teams. The project is in no form release-ready or even well organized yet, but nevertheless the source code can be found here.

As an example, here’s a video from the visualizer of a trajectory we tried for the barrel racing path:

Galactic Search

Our plan for Galactic Search is to define the four possible profiles ahead of time and select one to follow when the run starts using vision (more details on that in the coming weeks). Our work thus far has been to start defining each of those profiles. One of the questions we had to answer was whether to start each profile from the same place (which simplifies the vision pipeline) or try to optimize each starting location (potentially saving valuable time). As a proof of concept, we visualized two trajectories for each path – one with a center starting position and another with an optimized off-center starting position. We tested using a cubic spline with waypoints on each power cell, and tuned the starting directions of each. The results of our testing are below. There are also videos included, where left is off-center and right is centered.

• A/red
  • Centered = 4.7589 secs
  • Off-center = 4.6807 secs
  • Improvement = 0.0782 secs, 1.6%
  • https://youtu.be/KXahCFEloZQ 2
• A/blue
  • Centered = 4.8171 secs
  • Off-center = 4.5945 secs
  • Improvement = 0.2226 secs, 4.6%
  • https://youtu.be/V88rQ67rmKE 3
• B/red
  • Centered = 4.6224 secs
  • Off-center = 4.2806 secs
  • Improvement = 0.3418 secs, 7.4%
  • https://youtu.be/wA1ZjCr-nZ0 2
• B/blue
  • Centered = 4.3433 secs
  • Off-center = 4.2065 secs
  • Improvement = 0.1368 secs, 3.2%
  • https://youtu.be/LTRY5W3_OzA 1

Given the short length of these profiles, our conclusion is that saving even a fraction of a second is worth the extra complexity. Also note that the center starting positions are still tuned to different angles, so if we used a single starting position (including direction) the discrepancy would be even greater.

Circle Profiles

For several of the paths (slalom and barrel racing) the robot needs to maneuver in a circle around the waypoints. For our initial testing, we tried to define waypoints such that they formed a continuous curve:

That isn’t terrible, but it’s also clearly not the optimal route. We’ve been doing some work to instead define perfectly circular profiles. Unfortunately, WPILib’s trajectory generator doesn’t support something like this on its own, so we needed to give it a little (a lot) of help. All of this functionality is part of our motion profiling command I linked previously if you’re interested in the details. The path itself is defined by hundreds of waypoints spaced a tenth of an inch apart in a circle. That forms a trajectory which looks something like this:

Unfortunately, the red color coding indicates that both sides are moving at the same maximum velocity, which a) doesn’t satisfy the centripetal acceleration constraint we defined and b) isn’t how to drive in a circle. On a real robot, you get something like this (https://youtu.be/M1k2Q-ZSMdE 7). It tries to correct the position, but goes way off course. The issue is that the trajectory generator thinks the curvature of this section is 0 (it’s completely straight), meaning it doesn’t properly apply constraints or compute wheel speeds. Since we know the properties of this circle, our solution was to manually adjust the curvature of each point after the trajectory was generated, which results in this:

That’s a big improvement, but there are still some issues. First of all, the curvature changes abruptly going into the circle which means the wheels have to accelerate very quickly. We actually found that this wasn’t a major issue in testing, and the drivetrain seems to behave reasonably. However, the larger issue is that our constraints are still not being obeyed so we’re exceeding the maximum velocity and centripetal acceleration (those are all still calculated using a curvature of 0). The solution? Apply a custom constraint within the circle. To provide maximum velocities and accelerations, this constraint passes calls through to all of the standard constraints that we apply to the whole path. However, it runs the calculations using the corrected curvature. The constraint also determines the maximum chassis velocity which will prevent the outer wheels exceeding their maximum velocity. Once the curvature is fixed after generation, all of the constraints are obeyed properly:

Below are some examples of running these profiles on the robot. This is the first time we ran with proper constraints- the circle looks good but the positioning is a little off. We were able to address that with further tuning.

Here’s another example on the barrel racing path:

We’ll keep everyone updated as we make more improvements in all of these areas, and we’re happy to answer any questions.