Merging models transformation issue

[HarisCavkic]

Hello all,

Issue description

I am using pinocchio to load urdf models, do collision checking and visualising with meshcat. Im currently facing following issue:
When I load ur5 robot urdf (ur5_robot.urdf from you example robot data) and I load robotiq 85 gripper model (https://github.com/Daniella1/urdf_files_dataset/blob/main/urdf_files/ros-industrial/xacro_generated/robotiq/robotiq_2f_85_gripper_visualization/urdf/robotiq_arg2f_85_model.urdf), everything works fine.
The problem arises when I want to merge these two models using pinocchio.appendModel, concretely when there is transformation present. This means when I call the appendModel with identity matrix everything is ok, except the fact that the gripper is not correctly placed, but when I add transformation (x,y,z,r,p,y => np.array([0,0.085,0,0,0,1.5708]), that I then transform to pinocchio.SE3) and call pinocchio.appendModel part of the gripper does get correctly transformed, but other part (robotiq_85_left_knuckle_link, robotiq_85_left_finger_link, robotiq_85_right_inner_knuckle_link, robotiq_85_right_finger_tip_link, robotiq_85_right_knuckle_link and robotiq_85_left_finger_link) stays in initial position, i.e., the same as when identity matrix is the transformation matrix.

For context this is in the SE3 object:
Rotation R: [[-3.6732051e-06 -1.0000000e+00 0.0000000e+00]
[ 1.0000000e+00 -3.6732051e-06 0.0000000e+00]
[ 0.0000000e+00 0.0000000e+00 1.0000000e+00]]
Translation t: [0. 0.085 0. ]

Expected behavior

Whole gripper is transformed properly.

Reproduction steps

def pose_to_transformation_matrix(pose, rot_type="rad"):
    x, y, z, roll, pitch, yaw = pose
    if rot_type == "rad":
        # Create transformation matrix
        transformation_mat = np.eye(4)
        transformation_mat[:3, -1] = [x, y, z]
        # Create rotation matrix
        R = pin.rpy.rpyToMatrix(roll, pitch, yaw)
        transformation_mat[:3, :3] = R
        return transformation_mat
    else:
        raise ValueError("Only radians are supported for now.")


def merge_models(robot_model, robot_geom_model, robot_visual_model, gripper_model, gripper_collision_model,
                 gripper_visual_model, gripper_transform):
    transformation_mat = pose_to_transformation_matrix(gripper_transform, rot_type="rad")
    R = transformation_mat[:3, :3]
    t = transformation_mat[:3, -1]
    o_T_d = pin.SE3(R, t)

    wrist_id = robot_model.getFrameId("wrist_3_link")
    _, robot_visual_model = pin.appendModel(robot_model, gripper_model, robot_visual_model,
                                            gripper_visual_model, wrist_id,
                                            o_T_d)

    robot_model, robot_geom_model = pin.appendModel(robot_model, gripper_model,
                                                    robot_geom_model, gripper_collision_model,
                                                    wrist_id, o_T_d)
    # update data
    geom_data = pin.GeometryData(robot_geom_model)
    # Create data required by the algorithms
    data = robot_model.createData()

    # Recompute the placements of all joints in the model
    pin.framesForwardKinematics(robot_model, data, pin.randomConfiguration(robot_model))
    pin.computeJointJacobians(robot_model, data)
    pin.updateFramePlacements(robot_model, data)

    return robot_model, robot_geom_model, robot_visual_model

def main():
    gripper_model_path = ...
    robot_model_path = ...
    # Load model gripper
    gripper_model, gripper_geom_model, gripper_visual_model = pin.buildModelsFromUrdf(gripper_model_path, package_dirs=models_dir)

    # Add collisition pairs
    gripper_geom_model.addAllCollisionPairs()

    # Load model robot
    robot_model, robot_geom_model, robot_visual_model = pin.buildModelsFromUrdf(robot_model_path,
                                                                                package_dirs=models_dir)
    # Add collisition pairs
    robot_geom_model.addAllCollisionPairs()

    # Add namespace to frames so there is no conflict regarding frame names
    add_namespace_prefix_to_models(gripper_model, gripper_geom_model, gripper_visual_model, "gripper")
    gripper_transform = np.array([0, 0.085, 0, 0, 0, 1.5708])
    robot_model, robot_geom_model, robot_visual_model = merge_models(robot_model, robot_geom_model, robot_visual_model,
                                                                     gripper_model, gripper_geom_model, gripper_visual_model, gripper_transform)

Additional context

It is weird that one part of the gripper gets transformed and other not at all, although they are all hanging on the base. For context here is the frame hierarchy (after merging):

Frame universe , has following children: ['universe', 'root_joint']
Frame root_joint , has following children: ['world']
Frame world , has following children: ['world_joint']
Frame world_joint , has following children: ['base_link']
Frame base_link , has following children: ['base_link-base_fixed_joint', 'shoulder_pan_joint']
Frame base_link-base_fixed_joint , has following children: ['base']
Frame shoulder_pan_joint , has following children: ['shoulder_link']
Frame shoulder_link , has following children: ['shoulder_lift_joint']
Frame shoulder_lift_joint , has following children: ['upper_arm_link']
Frame upper_arm_link , has following children: ['elbow_joint']
Frame elbow_joint , has following children: ['forearm_link']
Frame forearm_link , has following children: ['wrist_1_joint']
Frame wrist_1_joint , has following children: ['wrist_1_link']
Frame wrist_1_link , has following children: ['wrist_2_joint']
Frame wrist_2_joint , has following children: ['wrist_2_link']
Frame wrist_2_link , has following children: ['wrist_3_joint']
Frame wrist_3_joint , has following children: ['wrist_3_link']
Frame wrist_3_link , has following children: ['ee_fixed_joint', 'wrist_3_link-tool0_fixed_joint', 'gripper/root_joint']
Frame ee_fixed_joint , has following children: ['ee_link']
Frame wrist_3_link-tool0_fixed_joint , has following children: ['tool0']
Frame gripper/root_joint , has following children: ['gripper/robotiq_85_base_link']
Frame gripper/robotiq_85_base_link , has following children: ['gripper/robotiq_85_left_inner_knuckle_joint', 'gripper/robotiq_85_left_knuckle_joint', 'gripper/robotiq_85_right_inner_knuckle_joint', 'gripper/robotiq_85_right_knuckle_joint']
Frame gripper/robotiq_85_left_inner_knuckle_joint , has following children: ['gripper/robotiq_85_left_inner_knuckle_link']
Frame gripper/robotiq_85_left_inner_knuckle_link , has following children: ['gripper/robotiq_85_left_finger_tip_joint']
Frame gripper/robotiq_85_left_finger_tip_joint , has following children: ['gripper/robotiq_85_left_finger_tip_link']
Frame gripper/robotiq_85_left_knuckle_joint , has following children: ['gripper/robotiq_85_left_knuckle_link']
Frame gripper/robotiq_85_left_knuckle_link , has following children: ['gripper/robotiq_85_left_finger_joint']
Frame gripper/robotiq_85_left_finger_joint , has following children: ['gripper/robotiq_85_left_finger_link']
Frame gripper/robotiq_85_right_inner_knuckle_joint , has following children: ['gripper/robotiq_85_right_inner_knuckle_link']
Frame gripper/robotiq_85_right_inner_knuckle_link , has following children: ['gripper/robotiq_85_right_finger_tip_joint']
Frame gripper/robotiq_85_right_finger_tip_joint , has following children: ['gripper/robotiq_85_right_finger_tip_link']
Frame gripper/robotiq_85_right_knuckle_joint , has following children: ['gripper/robotiq_85_right_knuckle_link']
Frame gripper/robotiq_85_right_knuckle_link , has following children: ['gripper/robotiq_85_right_finger_joint']
Frame gripper/robotiq_85_right_finger_joint , has following children: ['gripper/robotiq_85_right_finger_link']

For more context when add gripper urdf manually into the ur5 urdf with following joint connection, the transformations are good.

   <!-- connect gripper to arm -->
 <joint name="arm_gripper_joint" type="fixed">
   <parent link="wrist_3_link"/>
   <child link="robotiq_85_base_link"/>
   <origin rpy="0.0 0.0 1.5708" xyz="0.0 0.085 0.0"/>
 </joint>

Screenshots

System

• OS: macOS Ventura 13.6.7
• Pinocchio (pin) version: 2.7.0
• Python 3.9.19
• matplotlib==3.8.3
• numpy==1.26.4
• scipy==1.13.0
• meshcat==0.3.2

[jcarpent]

Solved via #2295