I would imagine that the approach you're suggesting is going to take many iterations to get everything connected, right? Here are a couple things you might try that could speed this up. (Although without knowing all the details, it's a little tough to make assurances.)
CollisionChecker
object, you can use CollisionChecker::CheckEdgeCollisionFree
method.IrisOptions
for details -- you'll want to work with the starting_ellipse
and iteration_limit
fields for this. Just make sure the hyperellipsoid is "thin" around the edge.IrisZo
algorithm, and leverage the containment_points
option to force the region to contain the two points. This is a very new feature (just got added to drake last week), so you'll have to build from source or use the nightly build.There's definitely a lot of other things you could try as well -- constructing a good set of regions for GCS is an active topic of research for us. So please let us know if any of these things work for you!