Ability to attach collision objects to collision objects?

[rr-tom-noble]

Is your feature request related to a problem? Please describe.

In our software, we have a need to attach tools to a robot.

Each tool is composed of separate parts, e.g. tool, connector, toolbit, some of which can be switched out with one another, requiring us to represent them as separate collision objects to avoid a combinatorial explosion of collision objects as more parts are added to the chain.

I think this is also nicer for modularity.

Describe the solution you'd like

It'd be nice to allow collision objects to be attached to other collision objects.

I'm unsure if this is already a feature in moveit. If so, we may be duplicating code (see alternatives).

It seems like this would be nearly identical to attaching the object to a link (i.e. specifying the name of the parent object and the transform relative to the parent object). When a query is made for the frame of the object relative to a frame which includes its parent object(s) as part of the transformation chain, the parent object transforms are also taken into account.

It'd probably also make sense for some operations to propagate to child objects, e.g.

• When an object is removed from the scene, all child objects are removed from the scene
• When an object is detached, all child objects are detatched

Describe alternatives you've considered

Our current solution involves keeping track of which collision objects are defined relative to other collision objects, and their relative transformations.

We attach all collision objects to the end-effector link of the robot, using the relative object to object transforms to compute the attach pose of children objects with respect to the end-effector link.

Additional context

No response

[TSNoble]

A couple of questions:

• The CollisionObject message already contains a header field. If each sub-object's header is set to a parent object, would the child object's poses update when the parent object's pose updates?

• If the above is true, does this also apply when the parent object is attached to a link, and the link moves?

• If the above is true, does the collision object code automatically update all objects whose parent is foo if the object foo is deleted / detached?

I feel like this may already be a feature that we're just not making use of.

[rhaschke]

There is no hierarchy of attached collision objects supported. They are stored in a flat map:

moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.h

Line 1737 in dfb3877

std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;

[rr-tom-noble]

@rhaschke Thanks for confirming. I think I'll take a deeper look through the code some time this weekend, and come up with a more comprehensive proposal regarding how the implementation might look.

[TSNoble]

AttachedBody

Members

• A std::map<std::string, AttachedBody> children_ member should be added to allow for lookups in object hierarchies
• A const AttachedBody* parent_object_ member should be added to allow children objects to query their parent, and to identify when an object is the root (via nullptr)

Constructors

• The existing constructor should set AttachedBody* parent to nullptr, to identify it as a root link and preserve existing behaviour
• A separate constructor with AttachedBody* parent should be added.
  • pose is interpreted as relative to the parent object
  • parent_link_model_ can be set using getRootObject()->getAttachedLink() (see Additional Methods)
  • touch_links_ can be set using getRootObject()->getTouchLinks() (see Additional Methods)
  • detach_posture_ can be set using getRootObject()->getDetachPosture()
  • I wonder if there's a need to disable collisions between parents and their direct children automatically? Maybe touch_links_ should be expanded to include these object names?

Unchanged Methods

The following methods can be unchanged, as these do not involve querying child or parent objects:

• getName()
• getPose()
• getAttachedLinkName()
• getAttachedLink()
• getShapes()
• getShapePoses()
• getTouchLinks()
• getDetachPosture()
• getSubframes()
• setSubframeTransforms()
• getSubframeTransform()
• hasSubframeTransform()
• setPadding()
• setScale()

Additional Methods

The following methods should be added to allow parent objects to lookup their children and vice versa:

• getParentObjectName() -> returns the name of the direct parent of this object
• getParentObject() -> returns the direct parent of this object
• isRootObject() -> true if parent_object_ is nullptr, false otherwise
• getRootObjectName() -> returns the name of the parent of this object that has no parent object
• getRootObject() -> return the parent object of this object that has no parent object
• getChildObjectNames() -> returns names of direct children of this object
• getChildObjects() -> returns the direct children of this object
• `getChildObject() -> accepts any child or sub-child object name of this object, returns object
• `getAllChildObjectName() -> returns all child or sub-child object names of this object
• getAllChildObjects() -> returns all child or sub-child objects of this object

The following methods should be added to allow for taking object hierarchy transforms into account. This should be done by taking the relevant poses, and iterating up to the root object, applying each object's transformation. For root objects, this is identical to the corresponding non-root-frame method:

• getShapePostureLinksInRootObjectFrame()
• getGlobalSubframeTransformsInRootObjectFrame()
• getSubframeTransformsInRootObjectFrame()`
• getCollisionBodyTransformInRootObjectFrame()`

Method Modifications

The following methods should be modified to use the appropriate inRootFrame() pose, rather than their relative pose:

• getGlobalPose()
• `getShapePosesInLinkFrame()
• getGlobalSubframeTransforms()
• getSubframeTransfomInLinkFrame()
• getGlobalSubframeTransform()
• getGlobalCollisionBodyTransforms()

Alternatively, since AttachedBody stores global and link frames as members, rather than looking them up on the fly, perhaps computeTransform should be modified to update child objects, and the members can be left alone.

Additional Thoughts

It'd be good to choose a suitable naming convention for denoting child objects. We could use the same convention as with submframes, however I think this could cause un-necessary ambiguity e.g. in name1/name2, is name2 a child object or a subframe?

I think my preference would be to choose a different denoter. A few ideas:

• object1|object2/subframe
• object1->object2/subframe
• object1>object2/subframe

I think my preference would be the third option, since it's single-character, so less effort to parse, and is easier to distinguish than a pipe character, but I'm open to suggestions.

[TSNoble]

@rhaschke Could you take a look over this proposal when you have time?

I think I'd like to approach this work as a series of PRs, since it seems like it might require changes across quite a few files. I think I'd like to do things in the following order

• Make the above changes to AttachedBody
• Modify RobotState methods to allow for querying using sub-object notation
• Modify PlanningScene to account for hierarchical collision objects which are not yet attached (any frame computations, propagation of mutating operations to sub-objects, etc..)
• Modify PlanningScene / msgs to allow for add / move / remove of hierarchical collision object easily

This way, each PR should be easier to review and can be thoroughly tested at each step. Since the functionality will only be effectively available when the final PR goes in, this allows users to continue using collision objects without needing to consider hierarchies, whilst the tests from the previous PRs should provide confidence that enabling this feature will not break existing functionality.

[TSNoble]

Also brief thoughts on the follow-on PR:

• I think RobotState can maintain a flat map of collision objects, since the AttachedBody class will handle the nested structure
• I think parts of the RobotState code that check for subframes do so by iterating over an AttachedBody's subframes and doing a transform lookup at the RobotState level, in addition to doing the same for the AttachedBodies themselves. It seems like the code could be simplified by adding knowsFrameTransform() and getFrameTranform() methods to AttachedBody, which queries both its own frame and subframes (and child objects with the additional changes). This code could also use the inbuilt methods of AttachedBody to get the transform to the link.

[TSNoble]

And some brief thoughts on mutating collision object operations in the planning scene:

• Adding a collision object should attach it to a parent if the frame_id in the header is a collision object
• Moving an object should move all of its children
• Removing an object should remove all of its children
• Attaching an object should attach all of its children
• Detatching an object should detach all of its children

I'm unsure what the desired behaviour would be if an attempt is made to attach a child object to a link. On the one hand, this feels like invalid behaviour, since if you're modelling collision objects in a hierarchy, you probably want to consider the group of objects as a whole. An error indicating that the root object should be attached could be raised. Alternatively, the child object could be de-associated with its parent object, and attached as usual.

It'd also be nice to be able to propagate colour information to child objects. Though this feels like behaviour that should be optional, as I can imagine use-cases where you'd want to visually distinguish between parts of a compound object. (Also I don't think this piece of work should cover that side of things, but might be nice to raise as an issue once the core functionality is implemented)