drake icon indicating copy to clipboard operation
drake copied to clipboard

IrisInConfigurationSpace using a CollisionChecker

Open jwnimmer-tri opened this issue 2 years ago • 35 comments

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

jwnimmer-tri avatar Feb 20 '23 23:02 jwnimmer-tri

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.

hongkai-dai avatar Feb 28 '23 22:02 hongkai-dai

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.

jwnimmer-tri avatar Feb 28 '23 22:02 jwnimmer-tri


> 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.

sadraddini avatar Mar 01 '23 03:03 sadraddini

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.

RussTedrake avatar Mar 01 '23 10:03 RussTedrake

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).

jwnimmer-tri avatar Mar 01 '23 14:03 jwnimmer-tri

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

calderpg-tri avatar Mar 01 '23 15:03 calderpg-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());

jwnimmer-tri avatar Mar 01 '23 15:03 jwnimmer-tri

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 avatar Mar 05 '23 06:03 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?

hongkai-dai avatar Mar 06 '23 04:03 hongkai-dai

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.

jwnimmer-tri avatar Mar 06 '23 04:03 jwnimmer-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.

calderpg-tri avatar Mar 06 '23 14:03 calderpg-tri

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.

hongkai-dai avatar Mar 08 '23 22:03 hongkai-dai

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.

calderpg-tri avatar Mar 08 '23 22:03 calderpg-tri

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

jwnimmer-tri avatar Mar 08 '23 22:03 jwnimmer-tri

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.

hongkai-dai avatar Mar 08 '23 22:03 hongkai-dai

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.

sadraddini avatar Mar 09 '23 18:03 sadraddini

@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.

jwnimmer-tri avatar Mar 09 '23 19:03 jwnimmer-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).

calderpg-tri avatar Mar 09 '23 20:03 calderpg-tri

FYI @hongkai-dai on the distance query functions:

jwnimmer-tri avatar Mar 09 '23 22:03 jwnimmer-tri

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?

hongkai-dai avatar Mar 10 '23 17:03 hongkai-dai

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;

jwnimmer-tri avatar Mar 13 '23 21:03 jwnimmer-tri

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).

hongkai-dai avatar Mar 13 '23 21:03 hongkai-dai

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.

jwnimmer-tri avatar Mar 13 '23 21:03 jwnimmer-tri

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.

hongkai-dai avatar Mar 13 '23 22:03 hongkai-dai

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?

jwnimmer-tri avatar Mar 15 '23 01:03 jwnimmer-tri

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 avatar Jun 07 '23 20:06 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.

hongkai-dai avatar Aug 30 '23 00:08 hongkai-dai

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 avatar Feb 21 '24 14:02 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.

AlexandreAmice avatar May 06 '24 16:05 AlexandreAmice

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?

hongkai-dai avatar Aug 22 '24 22:08 hongkai-dai