Using Point Clouds as CollisionObject's mesh

[Christian Dornhege]

Hi,

I'm wondering how to include point cloud clusters as part of a CollisionObject for cases, where no mesh or shape exists.

Is it possible to just use the points as mesh vertices or do we really need to somehow reconstruct a surface?

The issues that I could imagine would be:

1. Visualization

2. Collision tests relying on a proper mesh structure

I could live with 1. or work around it. For 2. obviously in general this is a problem as one could put in any other collision library.

However, only using MoveIt's defaults, does anyone know if that actually uses the mesh triangles or maybe only the vertices?

Are there any places in MoveIt that use the CollisionObject's geometry?

Best,

  Christian

[Sachin Chitta]

Hi Christian,

Point clouds generally pass through the Octomap when being integrated into MoveIt!. FCL, the underlying collision library, can handle the Octomap as (almost) a native input. I would recommend exploring that path before trying anything else.

Sachin

[Christian Dornhege]

Hi,

I think I wasn't clear about what we'd like to do.

Obviously all environment data will end up in the Octomap as usual.

Now, when we have observed an actual object (e.g. a cup), but don't know, what it is yet, we'd like to try to grasp or place it.

We, however, do know that this cluster of points is an object. So this should be removed from the octomap, so that we do not get collisions when interacting with these points. This removal usually happens when you add this as a moveit_msgs/CollisionObject and/or attach this to the end effector.

The goal is to only have this cluster of points as a CollisionObject, but keep all other points integrated in the Octomap to avoid collisions. What we do right now is to put the Octomap in the AllowedCollisionMatrix until we have handled the object, which is obviously dangerous.

So, maybe a reformulation of the question would be: What is the best way to interact with objects represented by point clouds in MoveIt? One possible answer would be: Run surface reconstruction to get a mesh. I'd like to know if someone can confirm/guess that this might be unnecessary as only a meshes vertices are considered.

[Acorn Pooley]

You could build a convex hull of the points in the point cloud.  Look at QHull for how to do that.  You can then use the convex hull (or extract a mesh of the convex hull) to remove those points from the octomap and add as a collision object.  You might also want to expand or shrink the convex hull depending on what you are doing with it.

Acorn

[Christian Dornhege]

This is actually what is already happening inside MoveIt, when you add a CollisionObject. Exactly as you described. This is why I would like to just add a point cloud cluster as a mesh in a CollisionObject. So I get this functionality for free without doing extra work.

What I am worried about is what happens to "the rest" of MoveIt as now I'd have a CollisionObject/mesh without triangles. Some component might rely on the triangles being there.

Am Dienstag, 22. September 2015 15:05:21 UTC+2 schrieb Christian Dornhege:

[Acorn Pooley]

Ah, I see.  I believe that you can just add a "mesh" with many points and no triangles (or maybe have to trick it by adding arbitrary triangles that use all the points).  QHull discards the triangles and just uses the points (vertices).  I'm not sure if anything else is actually using the triangles but I don't think so.  I recommend trying it out and see it if works.

Sounds like you already know all this, so not sure if this helps :)

Acorn

[Simon Schmeißer]

From looking at the octomap updater I found that it uses the bounding sphere for filtering (stl-file based) collision objects. That can get quite wrong with concave objects (ie no octomap inside a box). What I ended up doing is to filter the point cloud based on proximity to a recognized object during object recognition. That is quite fast in Halcon. I then publish only the point cloud with holes and the found collision objects. I removed about 60% of the code in octomap updater, leading to a great speed improvement as well. (No updating, simply create a new octomap from each point cloud. No need for ray tracing any more)

[Martin Günther]

Hi Simon,

On Thu, 24 Sep 2015 01:27:52 -0700 (PDT)
Simon Schmeißer <s.schm...@gmx.net> wrote:

> [...]

> I removed about 60% of the code in octomap
> updater, leading to a great speed improvement as well. (No updating,
> simply create a new octomap from each point cloud. No need for ray
> tracing any more)

But that means that you lose currently unobserved voxels. In
particular, if the robot arm occludes an object, it won't check for
collisions with that object. Right?

This isn't meant as a criticism of your approach, just saying that it's
not suitable to all cases. :)

Cheers,
Martin

[Christian Dornhege]

@Simon: I'm somewhat confused. The last time I checked the octomap updater computes the convex hull of a mesh and filters by that (I'm referring to the pointcloud_octomap_updater, which uses a point_containment_filter). It basically works like Acorn described.

Just using a bounding sphere would obviously be wrong, but I also did not observe behavior like that.

I had this part on the: "it should work" side and was more worried about other stuff.

I guess the only solution for now is trying it.

However, your workaround of pre-filtering the point clouds that go into MoveIt, might serve as a solution if there are any unexpected problems.

Am Dienstag, 22. September 2015 15:05:21 UTC+2 schrieb Christian Dornhege: