simple collision checking in MBP/MBT

[dmsj]

Description

Preparing for migration to MBP/MBT (what's the difference, really?), I've been playing around with a simple collision detection use-case.

a simple mock-up in RBT:

bool isCollisionFree( Eigen::VectorXd config, double collision_epsilon
) {
    KinematicsCache<double> kinematics_cache = rigid_body_tree_->doKinematics(config);

    Eigen::VectorXd dist;
    Eigen::Matrix3Xd xA, xB, normal;
    std::vector<int> idxA, idxB;

    rigid_body_tree_->collisionDetect(kinematics_cache, dist, normal, xA, xB, idxA, idxB);
    if (!(dist.array() > collision_epsilon).all()) {
         return false;
    }
    return true;
}

Doing the same in MBP/MBT has been... not as straight forward. Is the intended approach something like this?

/// (from box_test.cc)
std::unique_ptr<AbstractValue> contact_results_value =
      plant.get_contact_results_output_port().Allocate();

const ContactResults<double>& contact_results =
      contact_results_value->GetValueOrThrow<ContactResults<double>>();

// Compute the poses for each geometry in the model.
plant.get_contact_results_output_port().Calc(
      plant_context, contact_results_value.get());

const PointPairContactInfo<double>& contact_info =
      contact_results.contact_info(0);

ContactResults seems intended for dynamics / simulation applications as opposed to quick collision checking. It would be nice to have a clean, fast, and thread-safe(!) (more on that later) collision-checking interface, similar to what is requested in https://github.com/RobotLocomotion/drake/issues/7251

Am I headed down the wrong rabbit-hole here?

[SeanCurtis-TRI]

Possibly down the wrong rabbit hole.

First of all, it seems one of the things you want is not currently in the API: a quick boolean "is anything hitting" query (for a given value of "everything"). Its absence is known and can be added relatively easily.

Second some questions:

• If you're porting from RBT, everything you care about in the world is "owned" by RBT and, now, MBP, yes?
• Are you evaluating this query inside another system? Hopefully yes. If not, it becomes slightly more elaborate.

The answer should really be in the following flavor:

• the SceneGraph supports the geometric query (or will in the case of the optimized boolean query).
• it has an abstract-value-valued output port containing a QueryObject -- it is the object through which you make queries (as one might expect).
• To make a query, you evaluate the SceneGraph's output port to get a const reference to a QueryObject and then call the appropriate collision method on that object. Done. You don't, in fact, have to know anything about the MBP to find out if things are hitting.

Some thoughts:

• In the absence of the bool CollisionsExist() method, you would call the ComputePointPairPenetration() method and see if the resulting vector is empty or full.
• It is very important that you get a QueryObject reference, use it, and then throw it out. For the scope of a single function, it's reasonable to hang onto it and query again and again. But don't hang onto it for a longer scope.

[dmsj]

First of all, it seems one of the things you want is not currently in the API: a quick boolean "is anything hitting" query (for a given value of "everything"). Its absence is known and can be added relatively easily.

Yes! This would be great, and make everything easier.

If you're porting from RBT, everything you care about in the world is "owned" by RBT and, now, MBP, yes?

More or less - we have an object which owns an RBT and provides some sugar to make it easy to query - e.g. make IK solves, check collisions, etc...

Are you evaluating this query inside another system? Hopefully yes. If not, it becomes slightly more elaborate.

Yes - we make this call quite often, to check validity for motion, inside of sampling based planners, etc...

To make a query, you evaluate the SceneGraph's output port to get a const reference to a QueryObject and then call the appropriate collision method on that object. Done. You don't, in fact, have to know anything about the MBP to find out if things are hitting.

Ok - so something like this? (pseudo-ish code from the BouncingBall)

// assuming some SceneGraph<T>* scene_graph, and 
// int geometry_query_port_ = this->DeclareAbstractInputPort().get_index();
template <typename T>
bool CollisionsExist<T>(const systems::Context<T>& context) {
  const geometry::QueryObject<T>& query_object =
      this->EvalAbstractInput(context, geometry_query_port_)
          ->template GetValue<geometry::QueryObject<T>>();

  std::vector<PenetrationAsPointPair<T>> penetrations =
      query_object.ComputePointPairPenetration();
  if (penetrations.size() > 0) {
       return false;
  }
  return true;
}

Can you elaborate a little more on the "appropriate collision method"? Is this something wrapped up in ComputePointPairPenetration?

It is very important that you get a QueryObject reference, use it, and then throw it out. For the scope of a single function, it's reasonable to hang onto it and query again and again. But don't hang onto it for a longer scope.

I think this shouldn't be a problem if we do something like the above.

[SeanCurtis-TRI]

Your pseudo-ish code is dead on.

By "appropriate collision method" I just meant (for now) ComputePointPairPenetration() and, in the future, CollisionsExist(). Didn't mean to make the waters murkier, sorry about that..

So, it seems that, for now, you can move forward and when the new methods lands, you can change the call in your code to use the more optimal, strictly-boolean approach.

A quick thought:

Given a world full of N geometries, there are O(N²) pairs of geometries that could be colliding at any time. However, there are various rules that immediately exclude some of those pairs from ever being considered. Furthermore, there are mechanisms by which you can explicitly omit other pairs from consideration. Are you familiar with these rules and mechanisms?

On the subject of mechanisms that limit the scope of the query, eventually, there will be methods that allow you to explicitly enumerate a set of geometries over which the query is performed. These methods don't exist now and I'm not sure when we'll get to adding them. Right now they're an intention, but not a plan. Obviously, if that becomes a problem, let us know, more data helps us prioritize.

[dmsj]

Given a world full of N geometries, there are O(N²) pairs of geometries that could be colliding at any time. However, there are various rules that immediately exclude some of those pairs from ever being considered. Furthermore, there are mechanisms by which you can explicitly omit other pairs from consideration. Are you familiar with these rules and mechanisms?

Short answer - no. For RBT, we have a bunch of collision_filter groups set, in our URDF, and we're happy with that RBT code. I haven't looked into it at all for MBT/P/SceneGraph. Is there any easy way to define collision_filter groups now?

A related question - will the QueryObject work for all contexts? Even sub-systems? Is that the right way to filter? Can I make a context which is limited to two sub-systems?

[SeanCurtis-TRI]

So, the collision_filter_groups you have in the URDF are not currently parsed into MBP/SG (all part of the growing pains). But there is a collision filtering mechanism.

• Rules: As with RBT, geometries attached to the same body will not be considered for collision and geometries attached to "adjacent" bodies will not be either. Geometries that are "anchored" to the world will not be tested against each other. All others will.
• Until the collision filter groups are parsed in MBP/SG you'll have to do it yourself programmatically. as follows:
  • The collision filter group in URDF does
    • defines one or more sets of bodies (groups)
    • specifies non-colliding relations between those groups
  • The programmatic analogy is:
    • Defining the set of body via: MultiBodyPlant::CollectRegisteredGeometries. This returns a GeometrySet -- which will serve as your collision filter group.
    • Defining the relationship:
    • Prevent collisions between the members of two groups via SceneGraph::ExcludeCollisionsBetween.
    • Prevent collisions between the members of a single group (self collisions) via SceneGraph::ExcludeCollisionsWithin.

This will get better when the parser is smarter.

The second question may not be a well-posed question. It's difficult for me to answer because it implies a mental model that is not correct and I'm not sure what the mental model is, just that it isn't correct. The normal case is:

1. I define a LeafSystem. It has an input port that will connect to the SceneGraph's QueryObject output port.
2. I create a diagram that includes an MBP, a SG, and my leaf system, where I connect up all the input/output ports.
3. My LeafSystem has some output that depends on a boolean collision query -- it is defined in a CalcMyDependsOnCollisionOutput method.
4. The signature of that method is given a Context. That context is, in fact, the context for the leaf system. It has the ability to evaluate my declared input port and acquire the QueryObject from the SceneGraph.

How does that match up to your mental model/workflow?

[dmsj]

Thanks! That's helpful. I'll play around a bit and see if I can get something working.

How does that match up to your mental model/workflow?

It helps it quite a bit. The iiwa examples are not nicely structured that way, but after looking more carefully at the simpler examples, such as BouncingBall, I have a clearer picture.

I was haphazardly constructing a plant and not explicitly defining it as a LeafSystem, which makes that structure a bit murkier.

Specifically - I had the following:

// adding geometry etc...

// build the diagram
auto diagram = builder.Build();

// Create a context for this system:
std::unique_ptr<drake::systems::Context<double>> diagram_context =
        diagram->CreateDefaultContext();
diagram->SetDefaultContext(diagram_context.get());
// Do I need to bother with this subsystem context? 
drake::systems::Context<double>& plant_context =
    diagram->GetMutableSubsystemContext(kuka_plant, diagram_context.get());

I am still a bit unsure of what is the right minimal program structure to use MBP. The two different contexts were throwing me for a loop, since the diagram does not contain any geometry except for kuka_plant.

[SeanCurtis-TRI]

Building the diagram is one thing. Evaluating the diagram is another. (What Drake calls "analysis".)

The most common analysis tool is the Simulator. Ultimately, we're talking time integration. And the CalcMyDependsOnCollisionOutput() method hypothesized above will get exercised iff there is something in the diagram that depends on that value and requests that value. If nothing depends/requests, the function will not be evaluated.

So, the important question, what is the context in which this function would be called? Is it simulation, or are you doing something outside of simulation? If the latter, you'll need to maintain your own context (and that's a bit more arduous where you have to distinguish between the diagram context and the leaf system sub context. The code example you have above is suggestive of this latter case. Although, in my example, instead of grabbing the kuka_plant's context, you'd get your my_leaf context and evaluate its output on that context.

[dmsj]

In this particular use case, we haven't needed to run a Simulator, since we're only interested in raw collision checking - e.g. no time dependence. Analyzing a diagram without a simulator is a pretty common use case for us. Example uses: (1) displaying a world configuration - I'd love to have SimpleTreeVisualizer for SceneGraph, or an easy way to configure the world and look at it. This is probably a bad way, but I was trying to avoid any physics - i.e. having to put in a controller, etc...

// // Get joints so that we can set initial conditions.
    for(int i=1; i<kuka_plant.num_positions()+1; i++){
        std::string joint_name = "iiwa_joint_" + std::to_string(i);
        const RevoluteJoint<double>& joint_i = kuka_plant.GetJointByName<RevoluteJoint>(joint_name);
        joint_i.set_angle(&plant_context, 1.5);
        joint_i.set_angular_rate(&plant_context, 0.0);
}

I'm obviously missing the easy way to set initial conditions for a simulation.
(2) collision checking (as we discussed above) (3) IK, traj_opt, etc... for static worlds

[SeanCurtis-TRI]

So, here's what I think you'd need to do.

Build a diagram like in bouncing_ball.cc (beware, there are multiple bouncing balls.) You'd swap MBP for the custom bouncing ball plant.

After building the diagram and allocating the full diagram context, you'd acquire three sub-contexts:

1. One for the plant which you'll use for configuring the robot (like you've done above).
2. One for the scene graph which you'll use to grabbing a query object for performing the query.
3. And one for the LcmPublisherSystem that is buried inside the call to geometry::ConnectDrakeVisualizer(). This last is how you'll visualize the current configuration. You'll have to spelunk down into the diagram to grab the LcmPublisherSYstem instance. GIven the instance and its context, you can call Publish() on that system, passing in its context. That should cause it to send a drake lcm message out to a visualizer like drake_visualizer.

This last

[RussTedrake]

Sorry to be late to the party. I think this is bringing up an important use case that is missing from the current design. Having SceneGraph act as a Drake::System is super important. But I think we should be able to make collision queries in drake without using the Systems framework api (much like we did in RBT), too. Is there a reason that we can't provide a few method entry points that do these queries without going through the query ports?

[sherm1]

@RussTedrake, I'm working through a similar issue with access to MultibodyTree (in WIP PR #9489). Because it uses a Context (mostly for caching), it needs a tiny bit of System functionality to operate at all. MultibodyPlant is a much heavier-weight object than is actually needed for MultibodyTree's basic functioning. I'm proposing to address this with a MinimalMultibodySystem that exists solely to serve MBTree's needs (basically allocate the Context resources it needs) and has no relationship with MBPlant at all. It has no input and output ports, exposes its MBTree directly, and all API access is done using the MBTree. So although there is a Drake System present, none of the usual complexity is involved.

I think we could do something very similar with SceneGraph, if there is low-level functionality that is valuable to access independently of the System framework.

[RussTedrake]

My two cents: Allocating a context and assigning a state doesn’t seem like too much of a burden (we had the equivalent with doKinematics returning a KinematicsCache in RBT). But once you have that context, you should be able to make geometry queries with it without allocating an output port, etc?

[sherm1]

Right, at least that's how it will be with MBTree. Should also be possible to use the SceneGraph (or underlying engine) API directly once the bookkeeping has been done. Let's discuss, @SeanCurtis-TRI.

[RussTedrake]

cc @caelan -- who also needs collision queries, and ideally a convenience wrapper like allCollisions available for him to port from RBT:

https://github.com/caelan/drake/commit/c31d45a70568c76e9d9455784270cf7f12f0c923

[SeanCurtis-TRI]

The need for a boolean collision query is captured in #10227. The original crux of this issue seemed to have started with that and led to a number of other conversations. If the ultimate actionable conclusion of this conversation is that we need a CollisionsExist() method, then I'm in favor of closing this.

@dmsj As the original author of this issue, how do you feel about that? Also, one last question: I realize that I never got around to something in the original conversation. Your original code sample had a "collision epsilon". Issue #10227 doesn't really incorporate that -- or, it does so implicitly by setting it to zero. Do you need a threshold? In that case, maybe what you really need is a: ThingsAreCloserThan(epsilon) method.

[EricCousineau-TRI]

Is this done?

[SeanCurtis-TRI]

There was nor response back in July. We can probably justify closing it and allow @dmsj to re-open if he feels there's some worthwhile outstanding issue to resolve.

[EricCousineau-TRI]

SGTM. Closing for now.