Support Additional Constraints for IRIS-ZO

[cohnt]

The IRIS-NP algorithm (implemented as IrisInConfigurationSpace in Drake) allows the user to specify additional constraints for the generated region to satisfy via the prog_with_additional_constraints field of IrisOptions. It would be great if IRIS-ZO could support this as well. I think the main design question is whether there should be a subclass of CollisionChecker that supports additional constraints, or if they should be specified via a field in IrisZoOptions.

cc @AlexandreAmice @wernerpe

[cohnt]

@sherm1 I'm happy to handle this, but could use some advice on which approach to take for the implementation. Who's responsible for CollisionChecker? (Maybe they could have some insight into whether it's acceptable to make a new subclass.)

[sherm1]

Looks like @calderpg-tri and @SeanCurtis-TRI could advise on CollisionChecker.

[calderpg-tri]

On a fundamental basis, I think CollisionChecker is the wrong place to put this. Inside TRI we use an additional interface, PlanningSpace, to combine collision checking (generally via an owned CollisionChecker) with additional constraints (joint limits, stability, etc) for planning purposes.*

* I do intend to make much of our internal planning tools public in planning/sampling_based/dev soon, probably within the next month, but I don't think the specific API of PlanningSpace (versus the role it serves) is that important here.

[calderpg-tri]

To elaborate a bit more - the CollisionChecker API right now is tricky enough to implement consistently and performantly against a range of underlying collision-checking methods. For example, adding additional behavior to CalcRobotClearance to support arbitrary constraints would, I think, be basically impossible to do in any principled way.

Likewise, much of the complexity of CollisionChecker is a consequence of managing per-thread resources. Adding arbitrary constraints with their own complex resource management needs would make it much harder to document and maintain safe behavior. In our implementation of PlanningSpace that supports arbitrary constraints, the behavior is much more complicated than the un-constrained implementations, and we don't even try to expose the constraints back to the user - all MathematicalProgram operations that use those constraints are entirely handled with the PlanningSpace which manages per-thread resources.

[cohnt]

Thank you for sharing! Definitely sounds like we shouldn't go making a new CollisionChecker instance for this. Long-term, it would be very cool if the IRIS-type algorithms could take in a PlanningSpace, since they're pretty well-equipped to handle those additional constraints. But for now, I'll just upgrade the algorithm to handle this separately from CollisionChecker.

For now, I'll probably plan to use a similar setup to IrisInConfiurationSpace, where the user-provided options contain an auxiliary MathematicalProgram prog_with_additional_constraints, where the user can add additional constraints that should be satisfied.

@wernerpe does this sound good to you? Note that we can also handle arbitrary functions via EvaluatorConstraint. We can also integrate nicely with the multithreading, since Evaluator reports its threadsafe status.

[calderpg-tri]

We can also integrate nicely with the multithreading, since Evaluator reports its threadsafe status.

It's important to note here that many (perhaps most) of the "interesting" constraints can't be thread-safe due to needing an MbP context, and thus tying your ability to parallelize to whether or not all of the provided constraints can be evaluated in parallel is likely going to be very limiting. The way our constrained PlanningSpace works is that constraints are specified via

/// A function type to build constraints using the provided standalone
/// CollisionCheckerContext.
///
/// Note: the only mutable operation that constraints may perform on the
/// provided context is setting positions and/or velocities.
///
/// Note: the returned Constraint objects may share (or alias) the provided
/// context; this is expected for any constraint that needs access to the plant.
/// However, the factory function itself must not share (or alias) the provided
/// context after it returns, as any later uses of the provided context may
/// cause thread safety or lifetime issues.
///
/// Note: we make no guarantees about when the factory function will be invoked
/// to create constraints, except that constraints will be created prior to use.
/// The factory function will be copied by the constrained planning space, and
/// any clones of the planning space will copy it as well, so the factory
/// function must be valid to call at any point the planning space or its clones
/// are still alive.
using ConstraintsFactoryFunction =
    std::function<std::vector<std::shared_ptr<drake::solvers::Constraint>>(
        const std::shared_ptr<drake::planning::CollisionCheckerContext>&)>;

and then the PlanningSpace implementation ensures that each thread has its own copy of constraints as needed to parallelize all queries.

[cohnt]

I think we can live with that. There are definitely a few interesting constraints we care about that we can formulate pretty easily as threadsafe constraints. For example, all the fancy constraints for the constrained bimanual IRIS-NP regions I've been making are just mathematical functions, so I think we can use EvaluatorConstraint for those and tag them as threadsafe?

For the more complicated ones stemming from MbP, a better option is probably using IRIS-NP2 (#21822), which I'll also be working on implementing. That one doesn't use parallelization -- it instead solves optimization problems, so it can interface with the user-provided constraints directly.