Collision avoidance constraints in trajectory optimization

[sea-bass]

This PR adds collision avoidance to kinematic trajectory optimization, per these resources:

• https://typeset.io/pdf/a-collision-free-mpc-for-whole-body-dynamic-locomotion-and-1l6itpfk.pdf
• https://laas.hal.science/hal-04425002

It seems to correctly enforce the constraint (traj opt won't succeed unless the waypoints/collocation points are collision free), but it doesn't work that well when the initial guess is in collision...

I suspect the same HPP-FCL Limitation where we're not getting the collision distance points is at play here... so I'm inclined to merge this as-is and fix once Pinocchio 3.0 is out.

[github-actions[bot]]

Coverage Report

File	Stmts	Miss	Cover	Missing
pyroboplan
__init__.py	0	0	100%
pyroboplan/core
__init__.py	0	0	100%
planning_context.py	8	8	0%	1, 4, 9, 13–15, 17–18
utils.py	134	12	91%	78–79, 81–84, 86–87, 228–229, 531, 542
pyroboplan/ik
__init__.py	0	0	100%
differential_ik.py	96	13	86%	158–159, 168–169, 197, 203, 209, 212, 250, 256–257, 262, 273
nullspacecomponents.py	35	0	100%
pyroboplan/models
__init_\.py	0	0	100%
panda.py	45	0	100%
twodof.py	8	0	100%
utils.py	5	0	100%
pyroboplan/planning
__init_\.py	0	0	100%
cartesian_planner.py	92	5	94%	115, 139, 143–144, 207
graph.py	130	4	96%	58, 103, 195, 261
graph_search.py	53	2	96%	57, 135
path_shortcutting.py	48	1	97%	121
prm.py	107	25	76%	109, 144, 206–209, 235–236, 242–243, 268–269, 273–274, 326–329, 332, 335, 343–345, 349–350
rrt.py	148	26	82%	152–153, 162–163, 186–187, 190, 198, 263, 359, 407–409, 413–414, 418–421, 424, 427, 435–437, 440, 443
utils.py	47	0	100%
pyroboplan/trajectory
__init__.py	0	0	100%
polynomial.py	152	39	74%	148, 150–151, 153–155, 157–159, 162–185, 188, 191, 194, 203, 230, 299
trajectory_optimization.py	183	0	100%
trapezoidal_velocity.py	168	33	80%	283–284, 329, 331–335, 337–338, 341–359, 362–364, 373
pyroboplan/visualization
__init__.py	0	0	100%
meshcat_utils.py	21	13	38%	40–41, 43, 45, 57, 81–82, 109, 131–134, 136
TOTAL	1480	181	87%

Tests	Skipped	Failures	Errors	Time
95	0 :zzz:	0 :x:	0 :fire:	29.954s :stopwatch:

[sea-bass]

How much more do you want to poke at this?

I don't know... it really doesn't work well enough for me to be happy with it, but at the same time I've been seriously spinning my wheels and am close to giving up and just merging what is there.

[sea-bass]

I added a fallback that I'm more satisfied with in the "optimize RRT path example". It will now do:

1. Plan an RRT
2. Try do a full optimization using the RRT waypoints only as initial conditions
3. If 2. fails, do another optimization, this time constraining the RRT waypoints and only optimizing trajectory segments in between them.