Parent frame of GeometryObject changes after calling BuildReducedModel

[joao-pm-santos96]

Bug description

Hello developers! Can you please help me?

When I call BuildReducedModel, the parentFrame of my GeometryObjects change to a non-existing frame. I this the issue is here: https://github.com/stack-of-tasks/pinocchio/blob/0caf0ca4d07e63834cdc420c703993662c59e01b/include/pinocchio/algorithm/model.hxx#L569-L570

Expected behavior

They shouldn't change.

As always, thank you for your time!

[jcarpent]

Thanks @joao-pm-santos96 for raising this issue. Could you provide a small tiny example to reproduce it locally in such a way we prepare a unit tests covering your case?

[joao-pm-santos96]

Of course! In here https://github.com/stack-of-tasks/pinocchio/blob/0caf0ca4d07e63834cdc420c703993662c59e01b/unittest/model.cpp#L458-L464

I would also add

BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame);

[joao-pm-santos96]

Also, building upon https://github.com/stack-of-tasks/pinocchio/blob/master/examples/build-reduced-model.py , here something that fails:

import pinocchio as pin
import numpy as np

from os.path import *

# Goal: Build a reduced model from an existing URDF model by fixing the desired joints at a specified position.

# Load UR robot arm
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = pinocchio_model_dir + '/example-robot-data/robots'
mesh_dir = pinocchio_model_dir
# You should change here to set up your own URDF file
urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)

# Check dimensions of the original model
print('standard model: dim=' + str(len(model.joints)))
for jn in model.joints:
    print(jn)
print('-' * 30)

# Create a list of joints to lock
jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

# Get the ID of all existing joints
jointsToLockIDs = []
for jn in jointsToLock:
    if model.existJointName(jn):
        jointsToLockIDs.append(model.getJointId(jn))
    else:
        print('Warning: joint ' + str(jn) + ' does not belong to the model!')

# Set initial position of both fixed and revoulte joints
initialJointConfig = np.array([0,0,0,   # shoulder and elbow
                                1,1,1]) # gripper)

# Option 1: Only build the reduced model in case no display needed:
model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)

# Option 2: Build the reduced model including the geometric model for proper displaying of the robot
model_reduced, visual_model_reduced = pin.buildReducedModel(
    model, visual_model, jointsToLockIDs, initialJointConfig)

parent_1 = visual_model.geometryObjects[-2].parentFrame
parent_2 = visual_model_reduced.geometryObjects[-2].parentFrame

assert(parent_1 == parent_2)

[jcarpent]

@MegMll Would you have time to provide a fix for this issue?

[MegMll]

Yes, i will look into it this week.

[MegMll]

Hello @joao-pm-santos96, it's now fixed in #2160.

[joao-pm-santos96]

Thank you!