Correct way to do multi-robot collision detection?

[myersjm]

Hello,

I have a URDF file containing a single robot arm, but want to do collision checking with 2 instances of the robot arm to reflect my real world setup. I read in #1686 that the best way to do this is to merge the 2 robots into a single collision model, using appendModel. However, I have encountered the following issues:

1. This appendModel function does not merge the model, collision_model, and visual_model all at once, so I had to call it twice, overwriting the merged 'model' in order to acquire the merged visual_model and collision_model. See the following code excerpt:

# 1) Create models for each robot instance:
model1, collision_model1, visual_model1 = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir)
model2, collision_model2, visual_model2 = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir)

# ... [code left out to make this first issue more clear -- see point 2 below]

# 3) Because appendModel(model1, model2, collision_model1, collision_model2, visual_model1, visual_model2, ...) does not work,
#     I had to call it twice to combine the different geometry models, which I don't know if was the correct way:
model, visual_model = pin.appendModel(model1, model2, visual_model1, visual_model2, 0, se3_transform)
model, collision_model = pin.appendModel(model1, model2, collision_model1, collision_model2, 0, se3_transform)

2. An additional thing I had to do before appending the models above was manually rename joints, frames, and geometry objects in the model, collision_model, and visual_model for each robot instance because it gave an error when they were named the same; I added a namespace prefix to the beginning of these (such as 'robot1' or 'robot2'). See the following code:

def add_namespace_prefix_to_models(model, collision_model, visual_model, namespace):
    # Rename geometry objects in collision model:
     for geom in collision_model.geometryObjects:
         geom.name = f'{namespace}/{geom.name}'

     # Rename geometry objects in visual model:
     for geom in visual_model.geometryObjects:
         geom.name = f'{namespace}/{geom.name}'

     # Rename frames in model:
     for f in model.frames:
         f.name = f'{namespace}/{f.name}'

     # Rename joints in model:
     for k in range(len(model.names)):
         model.names[k] = f'{namespace}/{model.names[k]}'

Is this the correct way to merge models for a multi-robot setup? I am concerned that I have neglected dependencies or that there are strange data structure things that I am unaware of that could cause obscure issues down the road. The code I have given does seem to work for visualization and collision detection, but I don't know if it is the right way intended by the authors of this library. Thank you for any help!

[jcarpent]

Dear @myersjm,

This is the correct way to proceed. You must first change the entities' names, which may conflict when merging the robots.
Then you can proceed to the merging of the two robots.
In the future, we plan to add helper functions to do that automatically, but this remains to be done.

[jmirabel]

I have a piece of code that loads a robot and prefix the joint, frame and geometry names, both in C++ and Python. I'll try to wrap it up when I have some time.

[myersjm]

Okay, thanks for getting back to me!