Team 604 Presents: Our 2020-2021 IR@Home Robot

Here are our at home challenge videos: 2021 FRC604 Infinite Recharge at Home - YouTube

You can see our whole autonomous stack (from ball detection to trajectory tracking) in our autonomous video: FIRST Team 604 - 2021 At Home Autonomous Award Video - YouTube

As we did not have access to our build space at school and had limited in-person meetings, we focused a lot on the software of this year’s robot.

We created a ball detection system that allowed us to accurately determine “field relative” locations of the power cells using PhotonVision.

We also developed a time-optimal trajectory planner that we call Quikplan.

Quikplan runs on the driver station (to take advantage of its compute resources) and optimizes an initial path to take the least amount of time while obeying a kinodynamic model of our robot and obstacle constraints. It is analogous to a racecar driver finding the optimal racing line on a race track.

Here are some gifs showing it in action.

This gif shows Quikplan optimizing the bounce path from an initial non-optimal path that was drawn by hand.


Here is a gif of Quikplan simulaing an optimized path for the slalom challenge with our 2020-2021 robot (39" x 34" including bumpers). The wide turns Quikplan makes here are optimal in order to preserve as much velocity as possible, but the turn radius is limited by traction. This trajectory takes 7.373s.


Here is another simulated path but with a greater wheel coefficient of friction. The greater mu allows Quikplan to make the turns less wide as the robot has more traction. This results in a faster time of 6.379s.


Here we are telling Quikplan that the motors have a lower maximum RPM than they really do. This makes the turns less wide because the robot is no longer trying to maintain as high of a speed around the turn, so we are no longer traction limited. Of course, this also results in a slower time of 8.885s even though the path distance is shorter.


And finally we can also make the robot really small (20” x 20”) which allows Quikplan to take the turns tighter and pass through the gaps straighter, significantly reducing the path’s total time. This path is 5.990s.


This is incredible! Is there a GitHub link somewhere we can look at or something?

This is such a flex. I’ve been wanting to play with trajectory optimization using CasADi for a while, but I’ve never had the time to spend on learning it. I’m super jealous.

You should do a write-up for this based on the many details you’ve given on the FRC Discord. :slight_smile:


Very cool! It takes a lot of work to make something like this work in the real world.

What kind of trajectory optimization algorithm did you use?


Right now the library is still quite messy and underdeveloped so we are not ready to share it with the public yet. We are working on cleaning it up for release. However, we are happy to answer any questions about the math, process, and library used.

1 Like

We made our own trajectory optimizer using CasADi with an IPOPT backend. We are using a 7 state differential drive robot model with left and right motor jerk as control inputs.

The trajectory is represented by a “chain” of robot states (the number of states is a fixed constant) with the time between them being summed up in our cost function: we are essentially optimizing dt. With the full state at each control interval, we can account for physical parameters like robot mass and rotational inertia, as well as physical limits like longitudinal and lateral wheel traction.

We also apply constraints between the robot states to make sure the trajectory obeys the robot’s differential drive dynamics.

To make sure the paths don’t intersect with obstacles, we model the robot geometry as a rectangle and use a line segment-point distance calculation to ensure we stay a set distance away from them.


After cleaning it up we are ready to release the code for Quikplan! Here is the Github repo: