Shooter trajectory optimization for 2022 and 2024 games

I read the slides for 1690’s latest software session two days ago, and I was surprised they included a shooter trajectory optimization formulation without an implementation. Since that’s a nontrivial exercise left to the reader, I decided to fill that gap. I’ve implemented shooter trajopt for the 2022 and 2024 games in C++ and Python via Sleipnir (the NLP solver that powers Choreo).

Python 2022, C++ 2022:

The cost function is linear.
The equality constraints are nonlinear.
The inequality constraints are quadratic.

Number of decision variables: 61
Number of equality constraints: 60
Number of inequality constraints: 3

Error tolerance: 1e-08

iter   time (ms)      error          cost       infeasibility
=============================================================
   0       0.292   1.241993e+01   7.526196e-01   1.581507e+01
   1       0.457   1.756843e+00   7.997123e-01   1.035325e+00
   2       0.491   2.131484e-01   8.350351e-01   9.879818e-02
   3       0.336   2.097234e-02   8.354066e-01   1.158715e-04
   4       0.186   5.022454e-02   8.134858e-01   5.942876e-02
   5       0.189   1.501405e-04   8.139413e-01   1.638808e-05
   6       0.189   2.161493e-06   8.138033e-01   2.540714e-06
   7       0.176   2.519626e-09   8.138015e-01   4.392581e-10

Solve time: 2.989 ms
  ↳ 0.596 ms (solver setup)
  ↳ 2.393 ms (8 solver iterations; 0.299 ms average)

autodiff   setup (ms)   avg solve (ms)   solves
===============================================
 ∇f(x)          0.002            0.000        0
 ∇²ₓₓL          0.095            0.069        9
 ∂cₑ/∂x         0.053            0.044        9
 ∂cᵢ/∂x         0.002            0.002        9

Exit condition: solved to desired tolerance
Velocity = 10.000 m/s
Pitch = 39.128°
Yaw = 38.093°
Total time = 0.814 s
dt = 81.380 ms

Python 2024, C++ 2024:

The cost function is nonlinear.
The equality constraints are nonlinear.
The inequality constraints are nonlinear.

Number of decision variables: 7
Number of equality constraints: 6
Number of inequality constraints: 3

Error tolerance: 1e-08

iter   time (ms)      error          cost       infeasibility
=============================================================
   0       0.217   5.067010e+01   1.785355e-01   9.147772e+00
   1       0.182   2.660976e+01   1.209213e-01   2.270646e+01
   2       0.182   6.344297e+00   1.154788e-01   6.637674e+00
   3       0.170   4.772784e+00   1.155992e-01   4.964213e+00
   4       0.169   2.654620e-01   1.108068e-01   2.817756e-01
   5       0.168   3.061266e-02   1.103543e-01   4.893935e-03
   6       0.166   2.111241e-04   1.103472e-01   9.999763e-06
   7       0.170   2.009353e-06   1.103472e-01   9.944232e-08
   8       0.167   2.865228e-09   1.103472e-01   8.052448e-13

Solve time: 2.538 ms
  ↳ 0.772 ms (solver setup)
  ↳ 1.765 ms (9 solver iterations; 0.196 ms average)

autodiff   setup (ms)   avg solve (ms)   solves
===============================================
 ∇f(x)          0.021            0.008       10
 ∇²ₓₓL          0.120            0.068       10
 ∂cₑ/∂x         0.024            0.016       10
 ∂cᵢ/∂x         0.008            0.006       10

Exit condition: solved to desired tolerance
Velocity = 14.525 m/s
Pitch = 24.159°
Yaw = 43.071°
Total time = 0.514 s
dt = 51.386 ms

Implementation details

I chose different formulations for each game. The 2022 examples use direct transcription, where there’s lists of states with dynamics constraints between them and initial/final constraints. The cost function is minimum-time. There’s also a max velocity constraint and a requirement that the final velocity have a downward component so it goes in the hoop.

The 2024 examples use single shooting, where the initial velocity vector is rolled out through the dynamics, and there’s initial/final constraints. The cost function is “final z’s sensitivity to initial conditions” like 1690 used for their 2022 trajopt. There’s also a max velocity constraint and a requirement that the final velocity have an upward component as a crude way of avoiding speaker hood collisions. Approach angle constraints or even turret hood keep-out constraints could also be included.

Both groups of examples model air resistance and a moving robot with independent aiming (e.g., swerve, differential drive + turret). Magnus force could be included as another line in the dynamics function, and turret pitch/yaw constraints should be included in a production implementation.

Using this on a robot

Sleipnir may be fast enough on a roboRIO to facilitate real-time trajectory optimization, based on my laptop generally being 23x faster at Eigen compute tasks. The solver returning “infeasible” can be interpreted as the shot being invalid from the current location. I still recommend pregenerating a lookup table so you can validate and tweak solutions. Also, as usual with model-based controls, the solutions will only be as accurate as your model.

Development builds of WPILib include Sleipnir now (used by ArmFeedforward.calculate(currentVelocity, nextVelocity) and Ellipse2d.findNearestPoint(point)), so C++ teams can play with it on a robot at least. There’s no roboRIO Python wheel because I couldn’t figure out how to cross-compile one, and Java’s poor FFI experience and lack of operator overloading makes a Java wrapper too daunting to write and difficult to express problems with respectively.

How to formulate and solve optimization problems

You may be interested in these C++ and Python tutorials.

38 Likes

This is an incredible resource, although I wish it could have come from Orbit themselves, following the spirit of the rules and of FIRST as a whole. I do understand their wish to keep their work private in order to maintain an advantage, but one can dream.

4 Likes

Wow, this is amazing!
Thanks for sharing this, it will truly help a lot of teams.
Hopefully, in the future, top level teams will have it in their hearts to release these type of things. I do understand why they don’t, and that’s okay (given they rewrite the code each year). But I still believe the right thing to do is share with the community.
Anyways, thanks for putting the effort into doing this and sharing this with the rest of the community!

For anyone interested with the 2024 code, this class is supposed to be similar to what Orbit has this year (tested with great success).

4 Likes

You’ve done something way more sophisticated than what we did! Which is probably better…
We did a simple grid search on the exit velocity of the ball, nothing too fancy. The hood angle is a function of that velocity. (I think we talked about it in the session)

6 Likes