IrisInConfigurationSpace using a CollisionChecker

[jwnimmer-tri]

We currently offer these two functions:

// N.B. In discussions below, we decided to _not_ change MakeIrisObstacles as part of this ticket.
~ConvexSets MakeIrisObstacles(~
~    const QueryObject<double>& query_object,~
~    std::optional<FrameId> reference_frame = std::nullopt);~

HPolyhedron IrisInConfigurationSpace(
    const multibody::MultibodyPlant<double>& plant,
    const systems::Context<double>& context,
    const IrisOptions& options = IrisOptions());

In both, the collision geometry comes from a query object, either directly (in the first) or the plant input port (in the second).

The drake::planning::CollisionChecker offers a more natural interface for collision queries used for motion planning, and most particularly the ability to configure collision padding on a per-body-pair basis.

It would make IRIS a lot more useful to more people if we could run it using padding, and therefore if it accepted a CollisionChecker as an alternative source of geometry and kinematics.

It probably still makes sense to keep the QueryObject flavor around, for the simpler cases that don't need the extra details.

\CC @calderpg-tri @RussTedrake FYI

[hongkai-dai]

To make sure we are on the same page, I think you are proposing these API

ConvexSets MakeIrisObstacles(const CollisionChecker& collision_checker, std::optional<FrameId> reference_frame = std::nullopt);

HPolyhedron IrisInConfigurationSpace(const CollisionChecker& collision_checker,  const IrisOptions& options = IrisOptions());

In the function IrisInConfigurationSpace with a CollisionChecker, I suppose we do not need to pass in a Context object as the input argument? Because CollisionChecker already has a context CollisionChecker::plant_context() in it (but the documentation says the function might be tossed)? https://github.com/RobotLocomotion/drake/blob/6f1a88111c7b04217d461e906521a9ac7f6e58ed/planning/collision_checker.h#L239-L245.

[jwnimmer-tri]

The documentation of the existing IrisInConfigurationSpace says that the initialIRIS seed configuration is passed via the Context.

In the new function, I think asking the user to call CollisionChecker::UpdatePositions to pass the seed would be awkward at best. Perhaps the new function can just take an Eigen q vector as argument for the seed.

... (but the documentation says the function might be tossed)?

FYI even if the plant_context() function goes away, you could call model_context().plant_context(). Only the accessor sugar would go away, not the actual object.

[sadraddini]

> ConvexSets MakeIrisObstacles(const CollisionChecker& collision_checker, std::optional<FrameId> reference_frame = std::nullopt);
> 
> HPolyhedron IrisInConfigurationSpace(const CollisionChecker& collision_checker,  const IrisOptions& options = IrisOptions());

Looks good. I think passing const Eigen::VectorXd& q_seed is more natural than passing the context for the first.

[RussTedrake]

To be clear, the normal preference for passing a context instead of just a state is to handle additional features like parameters. If this is not needed here (e.g. if the geometries/kinematics are all specified already at the time of the call, so the effect of parameters is irrelevant), then q_seed should be sufficient. Otherwise we are losing something.

I admit that supporting changes in kinematics through the parameters might not be a big loss. Just want to make sure you understand Chesterton's fence.

[jwnimmer-tri]

It's a really good point. If users need non-default kinematic parameter for their motion planning, I think we need to teach CollisionChecker to handle that directly (by letting users set non-default Parameters on the checker itself).

[calderpg-tri]

Are there parameters that couldn't be set through the existing CollisionChecker::PerformOperationAgainstAllModelContexts mechanism?

[jwnimmer-tri]

Great point. That's precisely the way to do it, and will cover all cases. All parameters are sufficiently provided by the CollisionChecker. That (plus a q vector argument for convenience) should be good enough:

ConvexSets MakeIrisObstacles(
    const CollisionChecker& collision_checker,
    std::optional<FrameId> reference_frame = std::nullopt);

HPolyhedron IrisInConfigurationSpace(
    const CollisionChecker& collision_checker,
    const Eigen::Ref<const Eigen::VectorXd>& q_seed,
    const IrisOptions& options = IrisOptions());

[hongkai-dai]

BTW, I would think both MakeIrisObstacles and IrisInCOnfigurationSpace would need q_seed? Currently MakeIrisObstacles documentation says https://github.com/RobotLocomotion/drake/blob/42d86973c41725ad4ef1848547374fa21263009f/geometry/optimization/iris.h#L133-L136, I think we also need a configuration to fix the geometry poses in MakeIrisObstacles.

[hongkai-dai]

Also I would like to make sure that we really need this function MakeIrisObstacles that takes CollisionChecker as an input argument. As far as I can see the implementation is just a two-liner

ConvexSets MakeIrisObstacles(const CollisionChecker& collision_checker, const Eigen::Ref<const Eigen::VectorXd>& q_seed, std::optional<FrameId> reference_frame) {
  collision_checker.UpdatePosition(q_seed);
  return MakeIrisObstacles(collision_checker.model_context().GetQueryObject(), reference_frame);
}

if the implementation is really this short, why the user cannot just type it themselves?

[jwnimmer-tri]

if the implementation is really this short, why the user cannot just type it themselves?

The query object does not account for the collision checker's configured padding matrix.

[calderpg-tri]

More importantly, IRIS using a CollisionChecker provides for IRIS using a collision checker that is not SceneGraphCollisionChecker (e.g. Anzu's VoxelizedEnvironmentCollisionChecker), where the robot and/or environment geometry is not necessarily part of the collision checker's SceneGraph.

[hongkai-dai]

Sorry for being silent on this thread.

Sorry I am very new to CollisionChecker class. My understanding is that CollisionChecker contains two types of geometries, the "model geometries" and "checker geometries". collision_checker.model_context().GetQueryObject() contains both model and check geometries. What it doesn't contain is the padding for each pair of geometries.

Padding is added for each pair of geometries. On the other hand, in MakeIrisObstacles, we want to convert each geometry (for example G1) to a convex set, which would be the same convex set for any pair of geometries (G1, G2), (G1, G3), ... I think I am confused here on how to handle the padding.

[calderpg-tri]

Only in SceneGraphCollisionChecker does the context contain all geometries. For any other implementation of CollisionChecker, the robot and/or environment geometries being considered could be arbitrarily different from those in the context.

While it is possible to extract the complete robot and environment geometry (in different ways) from every extant collision checker, that's not the intended use. What we are really talking about here is IRIS using CollisionChecker queries (i.e. binary and distances), rather than its internals.

[jwnimmer-tri]

We might need an offline sync here. That doesn't match with my understanding.

[hongkai-dai]

While it is possible to extract the complete robot and environment geometry (in different ways) from every extant collision checker, that's not the intended use. What we are really talking about here is IRIS using CollisionChecker queries (i.e. binary and distances), rather than its internals.

I can see how that works out for IrisInConfigurationSpace (it would require writing a bunch of functions, such at SamePointConstraint that takes CollisionChecker instead of a plant and a context). But I don't see how that works for MakeIrisObstacles, which really only need the collision geometry of each individual geometry.

I have sent a hangout invitation, we can sync up then.

[sadraddini]

I can see how that works out for IrisInConfigurationSpace (it would require writing a bunch of functions, such at SamePointConstraint that takes CollisionChecker instead of a plant and a context). But I don't see how that works for MakeIrisObstacles, which really only need the collision geometry of each individual geometry.

I agree with this. I was confused about what MakeIrisObstacles is - I thought it is about C-space. When it comes to configuration in workspace paddings of collision checkers are not relevant.

[jwnimmer-tri]

@sadraddini Ok, thanks! I'll re-scope this issue to just the IrisInConfigurationSpace function.

---

@hongkai-dai @calderpg-tri for our chat later today, refer to this section of code from IrisInConfigurationSpace:

https://github.com/RobotLocomotion/drake/blob/0cb9faa6902e2c4558f33b35d2ef1cdbe05001fe/geometry/optimization/iris.cc#L643-L661

We convert every geometry to a convex set. We can easily switch that from every GeomtryId to every BodyIndex, that's not difficult. But when per-pair padding, do we need per-pair convex sets now (possibly memoized if padding if often the same)? And how to we extract the voxel grid from the voxel checker and make it into a convex set?

Or is that the wrong idea, and we just need to implement it from scratch as a nonlinear program that only ever does signed distance queries, with no convex sets? Wouldn't that defeat the purpose?

We can chat in a few hours.

[calderpg-tri]

Or is that the wrong idea, and we just need to implement it from scratch as a nonlinear program that only ever does signed distance queries, with no convex sets? Wouldn't that defeat the purpose?

This is my understanding. The purpose is to produce the same kinds of C-space collision-free regions as the existing IrisInConfigurationSpace, just using CollisionChecker queries instead of knowledge of the specific collision geometries. An advantage of this formulation is that no additional work is required to support any additional geometry types supported by other CollisionChecker implementations (e.g. voxel grids, octomap, etc).

[jwnimmer-tri]

FYI @hongkai-dai on the distance query functions:

• MaxNumDistances to find out how many distance measurements we might need to optimize against.
• CalcRobotClearance which returns a table of RobotClearance for the actual distances, using a given q.

[hongkai-dai]

Current CollisionChecker CalcRobotClearance function returns a table for the distance in every pair of geometries. On the other hand, in IRIS we would like to solve many small mathematical programs, each mathematical program only considers one pair of collision geometry (IRIS wants to find a posture such that the robot is in collision for any pair of geometries, so it is easier to write a for loop to iterate for each pair of geometry and impose the constraint that the robot clearance for that pair of geometry is <0). Is it possible to have CollisionChecker to compute the clearance for a specific pair of geometries?

[jwnimmer-tri]

We can't go down to the level of geometry (i.e., shapes) with a CollsionChecker.

We could probably offer a query that returns a subset of the clearance, filtered either for: (1) measurements that mention a particular BodyIndex (e.g., the end-effector collising with anything else, either robot or environment) (2) measurements that mention two specific BodyIndex values (e.g., link6+link7, or link6+world, etc.).

It sounds like what you'd propose doing is: (a) Get the full clearance table. Sort by distance (just like we do today -- "As a surrogate for the true objective, the pairs are sorted by the distance between each collision pair from the sample point configuration..."). (b) Loop over the body pairs in the clearance table, calculating clearance for just that one body-pair as part of the nonlinear optimization program.

In that case, you'd want API function (2) as part of step (b) --

/** ...
Returns clearance rows that match the given pair of body indices.
The two indices must differ, and at least one must be a robot body. */
RobotClearance CalcRobotClearance(
    const Eigen::VectorXd& q,
    std::pair<BodyIndex, BodyIndex> bodies,
    double influence_distance) const;

[hongkai-dai]

The more granularity we have in computing the signed distance, the better it is for the IRIS nonlinear program. So I would say (2) (signed distance between two bodies) is better than (1) (signed distance between a body and everything else).

What I have in mind now is that I will write a constraint

min(CalcRobotClearance(...)) < 0

The min operation will be problematic for nonlinear optimization, as it does not have a smooth gradient when a=b in min(a, b). I could use soft_min instead of min, but that only alleviates the gradient problem, doesn't completely resolves the issue. The less entries I have in the min function, the nonlinear optimization probably works better (hence the best situation is that CalcRobotClearance returns one distance for a single pair of geometries, but as you said that is not doable).

[jwnimmer-tri]

Even if we had a query for low-level pairs of shapes, I think the gradient of closest distance would not always be smooth? The shortest distance segment between two shapes can still jump between different faces / edges / points as we reposture things. It's just intrinsic to the problem of using anything except extremely primitive shapes like ellipsoids for proximity.

[hongkai-dai]

Right, when one geometry crosses the median axis of the penetrated geometry, then the distance is non-smooth. My understanding is that sometimes we do use spheres as collision avoidance hence we do not need to worry about the median axis problem. Nevertheless, I think if we can reduce the number of non-smoothness then the code could run better. I think if CollisionChecker can return a table of clearance between to bodies that should be a good starting point.

[jwnimmer-tri]

Yup.

If we wanted to test the premise (but with slower runtime perf), we could do that today with the current Clearance API. We'd just filter by body-index in the IRIS code, instead of passing the body index as an argument to a new API. It would run a lot slower, but I think it would still pass all the unit tests and we could see if SNOPT likes the gradients?

[hongkai-dai]

Sorry I was silent on this issue for a while.

Currently we do not have the API to return the clearance between a specific pair of bodies. So I will use CalcRobotClearance which computes the distance between every pair of bodies, and then select out the specified body pair in the new IRIS implementation. If the SNOPT is happy with this body-wise distance, then we can add the API to CollisionChecker.

[hongkai-dai]

I just had a meeting with Tom and Akio from Woven, they are interested in using IRIS with a TSDF. I think once we have the IRIS-NP that uses CollisionChecker we should be able to support their use case.

I can push on this.

[AlexandreAmice]

FWIW, if one were to add this feature iris.h would then depend on planning/collision_checker. I specifically put the features of #20831 in planning to avoid having this dependency as having planning rely on geometry is fine to me but having geometry depend on planning seems wrong.

[AlexandreAmice]

This issue is coming back up in my work with @Michaelszeng. The use case is being able to build IRIS regions with filtered collision geometries. As mentioned above, it would also be extremely helpful in the IrisFromCliqueCover work.

Currently, since the convex sets are always built on all geometry queries in the plant, then we can't build IRIS regions using filtered geometries. This means we need to build a new plant for these See the code Jeremy already highlighted here.

Based on this thread, it seems like supporting this has a lot more subtleties than I would have originally thought, so I am not sure it makes sense for me to push on this on my own.

[hongkai-dai]

I missed the action item for this issue in today's GCS standup meeting. I suppose this issue is still relevant, even if we work on #21822? Do we want to wait for #21821 (the refactorization) to finish and then work on using CollisionChecker?

[RussTedrake]

The plan from today, as I understand it, is to commit #21822 with CollisionChecker in the api, but without supporting many of the CollisionChecker features that we most wanted. So this issue is still relevant, but I think the goal should now be to support IRIS-NP2 instead of IRIS-NP.

[cohnt]

Just some additional detail -- IRIS-ZO should support any CollisionChecker pretty much out of the box (@wernerpe correct me if I'm wrong).

IRIS-NP2 will likely only be able to support SceneGraphCollisionChecker, due to the need to construct the counterexample programs based on individual robot geometries (voxels would be very inefficient if not outright intractable, for example). Of course, one could imagine tracing the gradient of the C-space signed distance function to abstract away from obstacle pairs at some point in the future.

Also, there are a couple features in CollisionChecker that may be challenging to include in IRIS-NP2. Negative padding is the big one that comes to mind -- I don't know how to do that in the counterexample search program. (But positive padding should be fine.)

[RussTedrake]

I understood that the work Hongkai had imagined here would have been to support the min distance queries directly in the collision checker base class (for all implementations). @hongkai-dai -- maybe you can weigh in?

[hongkai-dai]

Right, I was imagine to use the CollisionChecker class (with its minimum distance query and the gradient) to find the closest in-collision configuration, which was used in the original IRIS-NP algorithm. I tried to work on it a while ago, but the MinimumDistanceUpperBoundConstraint doesn't work as nicely as I expected (somehow it can fail to find the in-collision configuration in a toy example with just two spheres.)

[calderpg-tri]

closest in-collision configuration ... but the MinimumDistanceUpperBoundConstraint doesn't work as nicely as I expected ...

Depending on the behavior you want, e.g. if a nearby in-collision config, rather than closest in some sense, you might be able to use the gradient from ComputeCollisionAvoidanceDisplacement to iteratively adjust from a free to colliding config. In my experience, manually iterating with ComputeCollisionAvoidanceDisplacement to go from colliding->free config is significantly faster (although somewhat less likely to succeed) than posing the same projection as an optimization problem. I suspect that the same may hold for free->colliding.