Introducing Choreo: A New Approach to Swerve Trajectories

Technical
, ,
1

The SleipnirGroup team of developers invites all FRC teams to participate in our open beta of Choreo, a fully-featured trajectory optimization-based swerve drive autonomous path planning app.

Choreo is a tool for transforming 2D paths from rough sketches into optimal trajectories. With minimum-travel-time paths that respect your robot’s physical properties and limits, you can have a high degree of trust that your robot will be able to follow Choreo’s paths on the first try.

Download our beta release here!

Advantages

Delivers optimal solutions

Using CasADi, an industry-leading numerical optimization software, Choreo treats trajectory planning as a minimum-time problem, automatically refining both the shape of the path and the robot’s velocity and acceleration along it. CasADi is integrated into Choreo and does not require a separate installation.

Highly customizable

Choreo always respects your robot’s physical properties and limits, applying them to a simplified kinodynamic model of a swerve drive. By applying custom constraints, Choreo enables precise control of what you want your robot to do along the path. This means that you don’t have to “fudge” your path much to make it do what you want.

Easy to use

Choreo integrates with your robot project by providing JSON trajectory files that your robot reads and follows during the autonomous period of the FRC match, providing minimal overhead and points of failure. We provide a Java vendor library with a utility to load these trajectory files and a command factory to follow them.

Community support

Choreo is a fully open source project developed and maintained by the active and welcoming Sleipnir Discord server. Feel free to ask any questions there, and we will help you get Choreo integrated into your workflow. Issues with the Choreo app should be posted on the GitHub Issues tracker.

Comparison with existing path planning tools

Spline-based tools

Prior spline-based path planning tools (e.g. PathPlanner) allow users to draw a spline curve to define the shape of the path, and the tools determine the robot’s motion along that path according to the robot’s velocity and acceleration limits.

In contrast, Choreo uses a numerical optimization software to generate trajectories. With this technology, we refine the exact shape and motion of the path based on a set of user-specified requirements. These requirements currently include waypoints, custom constraints, and the physical properties of the robot, but the numerical optimization approach could be used with other constraints such as field obstacles.

Choreo’s optimization software seeks the path that passes through all waypoints, obeys all requirements, and does so in the shortest possible time. This approach means that trajectory generation is a compute-heavy process and can sometimes fail if the robot cannot physically obey all requirements. This approach unfortunately makes Choreo a poor fit for OTF/realtime path generation.

Path optimization tools

During the 2021 FIRST Robotics Competition season (INFINITE RECHARGE at Home), FRC 604 developed Quikplan, a differential-drive optimizer, for that year’s autonomous challenges. Like Choreo, Quickplan used CasADi to generate paths with minimal travel times that also obey the robot’s physics constraints. Quickplan also demonstrated obstacle avoidance.

Choreo currently only supports swerve drives, but differential drive support is planned.

History

Choreo is the evolution of several years of research and development in trajectory optimization by the contributors. Triple Helix Robotics collaborated on FRC 319’s BobTrajectory, a spline-based path planner and trajectory generator for differential-drives, from 2018 to 2021. After the 2021 season, Triple Helix took inspiration from FRC 604’s Quikplan and began developing HelixTrajectory (now SleipnirGroup’s TrajoptLib), a C++ library for optimizing swerve trajectories. HelixNavigator, the JavaFX-based trajectory optimization UI, was created to provide a simple, visual interface for HelixTrajectory. HelixNavigator later evolved into the web-on-desktop-based UI called Choreo. Choreo was first used in competition at Chezy Champs 2023 by team 8033, and soon after at Madtown Throwdown 2023 by team 6657.

Planned Improvements Before Kickoff

  • Heading angles do not currently wrap around.
  • We plan to make the workflow more integrated with robot projects.
  • We plan to make the configuration of the robot physical properties more user-friendly.
  • For best results, add a waypoint zero angular velocity constraint (second from the right in the top bar) to Start and End. The default path settings will be improved to include these.

Acknowledgements

Thank you to all contributors for your hard work over the past year!

SleipnirGroup Contributors

@calcmogul – Sleipnir, TrajoptLib

@Amicus1 – Choreo Frontend Lead

@JN25 – Choreo Frontend, Sleipnir

@justinb – Choreo Backend Lead, TrajoptLib, HelixNavigator creator

@dydx – Sleipnir

Triple Helix Robotics - FRC 2363

Highlander Robotics - FRC 8033

Arborbotics - FRC 6657

And many others who tested the app and contributed ideas!

80 Likes
2

Nice! It does seem difficult to get the generated trajectories to not hit any field walls in some cases though. Is there any way to avoid this?

choreoexample

Overall, will def be looking into using this for the 2024 season :smile:

1 Like
3

The way 8033 solved this this offseason was adding more translation waypoints, which isnt super clean but worked. Choreo’s backend, Trajoptlib, supports adding field obstacles which we have tested. Unfortunately having more than a couple of obstacles severely impacts generation time and reliability, but obstacles high on my list of things to work on for Choreo at the moment. The main blocker is finding a faster way to make them work or adding support for them in the gui to limit the number of obstacles in each path.

4 Likes
4

Huge thanks to the Sleipnir team for making this incredible tool. 6328 has been doing some testing with it and we’re super impressed. This truly feels like the right way to generate swerve trajectories.

11 Likes
5

I’d love to see an analysis comparing the trajectories generated by PathPlanner with those generated by Choreo, to see when it benefits more and when it’s completely fine to use splines.

3 Likes
6

With PathPlanner it’s possible to make really good paths you just have to manually tune them. With Choreo it automatically makes optimal paths. PathPlanner is as good as you can make it be. I think Choreoes trajectories also account for max wheel velocities and acceleration when generating the path but I may be wrong about that part.

1 Like
7

It’s fairly difficult to get an “all else being equal” comparison because the PathPlanner user actually overconstrains the problem into being suboptimal to actually impossible to follow, depending on how high the maximum velocity and acceleration constraints are.
The important difference is that in PathPlanner the only constraints applied explicitly are maximum chassis velocity and acceleration along the spline. Rotational velocity and acceleration were not considered in PathPlanner 2023, and IIRC only considered robot-side in 2024 (meaning rotation is done with whatever module speed is not used for translation, but the path is not required to leave enough time for rotation).

Meanwhile Choreo starts by limiting the velocity and torque applied at each wheel, with chassis velocity and angular velocity constraints (among others) applied on top of that if specified. This has two noticeable effects. One, the path will have time to hit both the rotation and translation elements of the waypoints. Two, the generated paths perform rotation during translational acceleration and deceleration, in order to maximize the portion of module speed used for translation at “cruising” speed.

Overall, the trapezoid-profile-along-spline approach can generate feasible paths if spinning is not too aggressive and/or the path constraints are well under the actual robot limits. Unfortunately, you leave a lot of performance on the table doing this, which is a limitation Choreo seeks to avoid.

8 Likes
8

To show what @apotato is talking about. Notice the translation waypoint at position 3

image
image2624×1824 362 KB

6 Likes
9

Hence I’m asking for some examples where a certain path drawn quickly on both apps will take x seconds using this and y using that. Is this a 0.1s difference? 0.5s? 2s? Just the order of magnitude is good to know…

10

Apologies if I missed this. Are you using fixed max module velocities and accelerations (essentially trapezoidal) or are you considering motor curves? Additionally, do you consider module rotation speed?

Also, are your output trajectories compatible with the PathPlanner path followers?

1 Like
11

  1. It currently uses a fixed module max speed / acceleration. Adding motor dynamics is more difficult due to swerve dynamics (both motor torque and lateral wheel friction apply force to the chassis). This requires more math and is planned for a future update, possibly before 2024 but not guaranteed.

  2. No, but they should be similar as ChoreoLib is very simple (parses and follows json files). @Amicus1 can speak to this further.

1 Like
12

How does this tool work with a more full autonomous (aka commands past driving, like grabbing a piece)? Should you:
A) Make path segments to drive from one point to another, and schedule your commands (say grab piece) in parallel?
B) Make one big path, and just time when commands should run in parallel?

I really like PathPlanners marker system where you can add in commands in parallel, and stop commands to wait until a command finishes.

Related to that, if you have 2 separate paths you daisy chain, do you need to tell them that the initial velocity is >0?

1 Like
13

We’ll probably end up doing something like PathPlanner’s system of stop points, where you can split your trajectory at waypoints where linear and angular velocity are zero. This would be a basic way to draw one path and be able to schedule commands in parallel with segments or in pauses between trajectories. This is your option A.
Something like PathPlanner’s marker system is pretty nice when you’re not necessarily stopping. One difficulty with implementing a similar system here is that outside of waypoints, you wouldn’t be able to trigger a command when the robot passes through a particular pose (because the exact path shape isn’t under your control). Placing command markers based on time elapsed after/before a waypoint could work though.

So a hypothetical command-marker system would probably have two options:
Stop Points: These would be commands fired at points that are constrained to 0 linear/angular velocity. They could be wait-until-finish or wait-specified-time before moving along the path again. Stop Point commands that are wait-specified-time and require the drive subsystem would be interrupted after that time, other wait-specified time commands would continue until interrupted by the end of the last path segment or another command marker.
Mid-path: These would be commands fired a specified time before/after a waypoint (and not necessarily a 0-velocity waypoint). They wouldn’t stop motion, and they could be interrupted by hitting another marker that requires the same subsystem (because how else could this be handled).

Each marker would have at least one command tag associated with it. Command tags are basically strings, but selected from a project-wide list to avoid mistyping and allow multiple commands to be associated with markers. Robot-code-side, you would populate a map of strings to Supplier (best to create new command instances for each auto for various reasons), similar to PathPlanner.

I’m kind of designing this system as I type it, and there’s probably issues with this idea that I don’t know about, so let me know if this makes sense for your usecase(s).

As for your second question, we currently don’t support starting one path with the end state of another, because you have to regenerate both if one changes (unless you have the start of one and end of the other fully constrained, i.e. to a stop point).

7 Likes
14

Rather than reinventing the wheel, have you considered using the work that Path Planner has already done?

It sounds to me like the biggest difference here (simplifying a lot) is a new spline and interpolation algorithm, why not couple that with the rest of the systems already in PathPlanner?

3 Likes
15

Behold my irrefutable scientific data with a rock-solid sample size of 1

The same amount of time (~2min) was spent creating and adjusting the paths, the constraints were matched between PathPlanner and Choreo. Obviously, more time could be spent with the PathPlanner path to get better results, but that’s not what this is about. They also are not exactly the same path. I tried to do the same thing (big loop), but did not copy one into the other exactly.

Choreo (6.2s):

choreo

PathPlanner (6.5s) (6.8s with accel constraint corrected):

pathplanner

9 Likes
16

FYI, the default choreo configs give a robot with a max velocity of 3.57 m/s and a max accel of 3.44 m/s^2, but this assumes no spinning.

17

Messed up the accel calculation a little bit then, this actually makes the PathPlanner path slower at 6.8 with that changed.

2 Likes
18

This is super cool

2 Likes
19

We are very very likely to use PathPlanner for the live-computed trajectory features. Your work here looks fantastic on paper and we’d love to give it a go. That said, we really don’t want to tune and run two different path followers. Please consider adopting or collaborating on the same file format. It would be a big improvement for ease of adoption.

5 Likes
20

@mjansen4857 reached out on Discord, we’re looking at ways to do interoperability. PathPlanner importing a Choreo trajectory as a PathPlannerTrajectory without any event markers is an easy first step towards that. For what it’s worth, 6995 also plans to use OTF pathing with PathPlanner, but I wasn’t about to go ping mjansen and say “hey make your thing load this other tool’s paths please”.

7 Likes