Optional frame prefix when appending models

[stephane-caron]

Consider the use case of attaching two copies of the same arm to a box:

import pinocchio as pin
from robot_descriptions.loaders.pinocchio import load_robot_description

left_arm = load_robot_description("ur3_description", root_joint=None)
right_arm = load_robot_description("ur3_description", root_joint=None)
model = pin.Model()
visual_model = pin.GeometryModel()

model, visual_model = pin.appendModel(
    model,
    left_arm.model,
    visual_model,
    left_arm.visual_model,
    model.getFrameId("universe"),
    pin.SE3.Identity(),
)
model, visual_model = pin.appendModel(
    model,
    right_arm.model,
    visual_model,
    right_arm.visual_model,
    model.getFrameId("universe"),
    pin.SE3.Identity(),
)

This example fails at the second appendModel with:

ValueError: The two models have conflicting frame names.

The reason for this is that loading a URDF always creates models with a joint named "root_joint"; and also, every model includes a "universe" joint:

In [9]: set([frame.name for frame in right_arm.model.frames]) & set([frame.name for frame in left_arm.model.frames])
Out[9]: {'root_joint', 'universe'}

Is there already a good way to address this use case?

Quick and dirty way

A quick and dirty way that seems to work:

def prefix_frames(model, visual_model, prefix):
    for frame in model.frames:
        frame.name = f"{prefix}_{frame.name}"
    for i, name in enumerate(model.names):
        model.names[i] = f"{prefix}_{name}"
    for geom in visual_model.geometryObjects:
        geom.name = f"{prefix}_{geom.name}"

prefix_frames(left_arm.model, left_arm.visual_model, "left")
prefix_frames(right_arm.model, right_arm.visual_model, "right")

Then call appendModels as above.

One idea would be to add a prefix argument to appendModels that would take care of that. What do you think?