It seems people think alike. Something that inspired me in the past also had a similar implementation.

[1]https://www.microsoft.com/en-us/research/publication/motion-...

That was cool! Watched the demo video, didn't go through the paper yet. I'm running a simple heuristic to test for candidate solutions and running them through Rapier (https://rapier.rs), but simulated annealing to efficiently search the configuration space would be a nice extension.