Moveit: Adding collision checking between two collision objects in a planning scene

[pradeepr-roboticist]

Is your feature request related to a problem? Please describe.
I would like to propose a new feature that allows collision checking between two objects ("object_A", "object_B") in the planning scene. Right now, collision queries can only happen between robot links and world objects. But, collision between world objects cannot be queried.

I recently posted this as a question at https://answers.ros.org/question/351846/collision-between-planning-scene-objects-eg-collisionobjecta-and-collisionobjectb/?answer=352800#post-id-352800.

Describe the solution you'd like
After looking at the comments for the question I posted and following advice of the answers, I feel that there should be a new public virtual function in "collision_env.h" called "checkObjectPairCollision".

I have already implemented this function in collision_env_fcl.cpp. The basic idea is to put "object_A" into a FCL world manager called "manager1". And, "object_B" into another world manager called "manager2". And, finally query the "manager1->collide(manager2, ...)".

Please visit the question given in the link above for more details.

I would like to contribute a more elaborate version of the above code. I wanted a go ahead signal before I proceeded with a pull request.

My worries are how this is going to be supported for other collision detectors (i.e., bullet).

Describe alternatives you've considered
Here are some of my ideas so far. All of them have some issue.

1) I tried digging through code related AllowedCollisionMatrix. It has methods such as "setDefaultEntry". As far as I understood, it useful only for ignoring collision between robot and a specific object in the scene.

2) A hacky solution is to put the robot in a known non-colliding state. Then, temporarily attach "CollisionObjectA" to the robot. And, see if it collides with "CollisionObjectB" using the existing functions in the API. This solution seems like the easiest but it is also ugly.

3) Define "CollisionObjectA" as link attached to a 6 DOF joint co-located at the root of the robot. Make a planning group for the 6 DOF joint. Set the 6 DOF state of the "CollisionObjectA" and query collision via the existing API (i.e., isValid ). Though it theoretically makes sense, I guess that there can be complications when solving IK for other planning groups.

4) Instantiate FCL collision manager object on my own and perform the necessary collision checks. This has the drawback of ensuring the collision object A and B in planning scene are in sync with the same in my FCL collision manager.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[felixvd]

Can you give an example of the use cases you are envisioning? I don't know how many people would use it, and this runs the risk of duplicating collision checking APIs that need to be maintained afterwards.

[pradeepr-roboticist]

There are 3 major use-cases that I have identified.

Use-case 1: Computing heuristics that exploit workspace information
This use-case is partly inspired by works such as [1], [2], [3] and [5].
In high DoF manipulator planning, workspace guided heuristics can accelerate search by a large factor.
The idea is to compute workspace-paths traced by just the attached tool (instead of finding high DoF joint-space trajectories). Once collision-free workspace paths are computed, a biased configuration space sampler can be constructed using a seed-configuration and Jacobian-inverse control. This way we can bias any sampling-based planner to produce samples on or around the collision-free workspace path (i.e., more configuration samples such that end-effector is around the workspace-path). This drastically speeds up planning in many cases. This is especially so for higher-level task planning where having rough idea of path feasibility can aid _agent assignment_ [5].

In fact, I would like to implement my planner [2], [3] in MoveIt + OMPL. I have implemented the rest of the planner in OMPL except for the collision-free workspace-path generation.

On a slightly unrelated note:
IMO, computing workspace-based heuristics is something that can't be readily outsourced to OMPL (due to the OMPL abstractions). Changes need to be made in ompl_interface. This is something I have been working on recently. I have been playing with ompl_interface mode-based-state-space class and OMPL state-space base class and exploring ways to inject a notion of workspace into OMPL planners.

Use-case 2: Evaluating placements of objects
This is a use-case motivated by a manufacturing scenario. Workpieces need to be placed onto a storage area. Being able to test potential placements of objects at certain locations will help decide the end-effector constraints required. Also, my guess is that the proposed feature will be useful for related areas such as _robot placement planning_ [4].

Use-case 3: Cartesian motion planning
In manufacturing settings such as robotic sanding/finishing/gluing, a workspace path is already computed by a coverage planner. The robot needs to follow the workspace path. Being able to identify and prune out potentially bad segments of the workspace is important and can effectively reduce search-graph complexity. In order to identify bad segments, one of the techniques is to "fly" just the tool along the given workspace path to get a rough understanding of potential bad segments (i.e., segments where the tool collides with the environment).
While the above problem can be handled with "AttachedCollisionObject" mechanism,
the major issue with using the "AttachedCollisionObject" mechanism is that you need to specify the robot joint-space configuration at which a tool (AttachedCollisionObject) is attached. In many situations, it may be hard to find IK solutions for the robot.

[1] M. Rickert, A. Sieverling and O. Brock, "Balancing Exploration and Exploitation in Sampling-Based Motion Planning," in IEEE Transactions on Robotics, vol. 30, no. 6, pp. 1305-1317, Dec. 2014, doi: 10.1109/TRO.2014.2340191.

[2] P. Rajendran, S. Thakar, A. M. Kabir, B. C. Shah and S. K. Gupta, "Context-Dependent Search for Generating Paths for Redundant Manipulators in Cluttered Environments," 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019, pp. 5573-5579, doi: 10.1109/IROS40897.2019.8967865.

[3] P. Rajendran, S. Thakar and S. K. Gupta, "User-Guided Path Planning for Redundant Manipulators in Highly Constrained Work Environments," 2019 IEEE 15th International Conference on Automation Science and Engineering (CASE), Vancouver, BC, Canada, 2019, pp. 1212-1217, doi: 10.1109/COASE.2019.8843126.

[4] R. K. Malhan, A. M. Kabir, B. Shah and S. K. Gupta, "Identifying Feasible Workpiece Placement with Respect to Redundant Manipulator for Complex Manufacturing Tasks," 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 2019, pp. 5585-5591, doi: 10.1109/ICRA.2019.8794353.

[5] S. Thakar et al., "Task Assignment and Motion Planning for Bi-Manual Mobile Manipulation," 2019 IEEE 15th International Conference on Automation Science and Engineering (CASE), Vancouver, BC, Canada, 2019, pp. 910-915, doi: 10.1109/COASE.2019.8843209.

[v4hn]

I fully agree with the request.

You can simulate a query like that by attaching one of the objects to an arbitrary robot link and check for collisions (as the attached object is considered to be part of the robot).
@henningkayser and I also needed object/object collision checking in the past and used that workaround, but we might as well provide a nicer API for it.

Of course, someone has to implement it though which is the usual bottleneck.

[felixvd]

OK, you had me at "the place part of pick-and-place".

If you want to implement this, note that World MoveIt Day is happening in less than 2 weeks, which might be a good opportunity.

[v4hn]

@pradeepr-roboticist now that I actually read your line of argumentation:

Happy to hear you are working to make your published planner available for MoveIt :medal_sports:
Please feel free to file new issues for concrete problems you encounter(ed) and I guess some of us (including @mamoll for OMPL) can provide support and related background.

There is also other work under way (just starting) to port more OMPL functionality that requires more workspace understanding in OMPL.
Please have a look at the linked thread.

Also somewhat related: https://github.com/ros-planning/moveit/pull/847

[pradeepr-roboticist]

@felixvd Great. I have put my name down for WMD2020. I have implemented a slightly general version (i.e., collision checking between two sets of objects). I am in the process of testing it. Since this is going to be my first PR to MoveIt, I was wondering how I should structure my pull request and supply testing functions.

@v4hn I'll raise new issues as I encounter problems in implementing my planner. Thanks for the link. #2092 sounds very interesting. I would love to contribute in case there is a free spot in that effort.
Regarding #847 , the final effect sought after seems to be very similar. Also #847 seems to have stopped. Is it something that can be combined with what I am proposing?

General question:
Since planning scene has a notion of two entities "robot" and the rest of the "world", it seems odd that collision checking between two collision objects requires a robot to be defined (due to the planning scene construction mechanics). Now that we are looking at this additional feature, I would like to know your comments as to whether this new feature belongs where I think it belongs (i.e., collision_env.h).

[felixvd]

Yes, collision_env.h is where such a function would live.

I guess it's a quirk that you need a robot to check the collision between two objects, but if you don't have a robot, why would you be using MoveIt to start with?

As some background: my understanding is that the planning scene was originally split into objects and robot for performance reasons (at least partially), but given how optimized collision checking in complex scenes has become because of video games etc, it is unlikely that maintaining this separation will be worth the effort (it is actually unclear if there really is a performance boost to it). I think most people would prefer if the collision environment was simpler and the collision environment not split into robot and world.

[v4hn]

Since this is going to be my first PR to MoveIt, I was wondering how I should structure my pull request and supply testing functions.

To the best of your knowledge. :-)
Try to provide a clean API extension/generalization of the current API and provide some tests similar to others you can find in MoveIt. Our overall test coverage is rather low, so improvements are very welcome..
This change will likely spark some discussion, so do not expect to see it merged right away. :sunglasses:

Regarding #847 , the final effect sought after seems to be very similar. Also #847 seems to have stopped. Is it something that can be combined with what I am proposing?

It for sure got stuck on my TODO list (item 27 w.r.t. MoveIt :cloud_with_rain: ), feel free to pick up the discussion there. It seems like a common API extension to account for both use-cases would make sense.

2092 sounds very interesting. I would love to contribute in case there is a free spot in that effort.

It's a GSoC project, so a student will work on it this summer. I'm not directly involved, but your input is probably very welcome. If you want to take part in the video meetings, comment in the issue.

Yes, collision_env.h is where such a function would live.

:+1:

Since planning scene has a notion of two entities "robot" and the rest of the "world", it seems odd that collision checking between two collision objects requires a robot to be defined

my understanding is that the planning scene was originally split into objects and robot for performance reasons

The major difference between RobotState and PlanningScene is that the former does not include any kind of collision checking capabilities and is a much simpler class, focusing on forward/inverse kinematics and handling joint information. In my opinion, that is a valid reason for the separation.

[...] I think most people would prefer if the collision environment was simpler and the collision environment not split into robot and world.

I don't see a need to give up on the notion of a "robot" in the scene as long as it is transparent w.r.t. transforms. But it would make some sense to generalize the PlanningScene class to maintain a set of robots, supporting zero as well as multiple robots. :sunglasses: