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).
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
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.