Align Urdf Mjcf

Align URDF and MJCF

Goal

Treat the URDF as the source of truth unless the user states otherwise. Produce the smallest MJCF change that makes the requested arm scope numerically equivalent at the home pose and remains compatible with the repository's model-composition code.

Establish the Contract

1. Read repository instructions and inspect the dirty worktree before editing.
2. Identify the source URDF, target MJCF, mesh directory, and any model-composition utilities or tests.
3. State the requested scope explicitly:
   • Which links and joints belong to the arm?
   • Does the terminal body represent a physical link or only an attachment frame?
   • Must gripper, finger, tip, tool, or top bodies be excluded?
   • How is “link length” defined? Default to the norm of the parent-to-child joint translation, but also require the full translation vector to match.
4. Preserve unrelated user changes and assets.
5. Create a short, verifiable task plan before editing.

For the YAM arm-only contract, retain exactly six arm joints named joint1 through joint6. Keep the terminal body named gripper -- the end-effector mount, named after the URDF's joint6 child link. Do not copy the URDF gripper mass, gripper geometry, or tip bodies into the arm MJCF.

Inventory the Models

Use rg, rg --files, and an XML parser to collect:

• Link names, parent-child relationships, and joint types.
• Joint origins, axes, limits, and names.
• Visual origins, mesh filenames, scales, and colors.
• Masses, centers of mass, inertial-frame orientations, and inertia components.
• MJCF compiler settings, mesh assets, nested body transforms, inertials, joints, geoms, sites, and top-level equality/contact sections.
• Composition assumptions such as “append a gripper to the deepest body.”

Confirm actual mesh filenames on disk. Follow the repository's URI convention in URDF, such as package://yam/... when required. In MJCF, resolve the same files through meshdir or absolute paths used by the composition utility.

Align the URDF with World Axes

Normalize the URDF before synchronizing the MJCF:

1. Name the arm links exactly base, link1 through linkN, and the joints joint1 through jointN in kinematic order. Update every parent, child, transmission, control, and configuration reference when renaming.
2. Resolve every mesh URI against the filesystem and match the actual filename, capitalization, and extension. Use the repository's required URI form, such as package://yam/...; do not invent or silently rename a mesh file.
3. Calculate all global link, visual, collision, and inertial poses at the home configuration and state the assumed world-axis convention.
4. Render or otherwise present the base mesh with visible world axes and ask the user: “Does the base visually face the intended robot-forward direction?” If not, ask which world direction it should face or what yaw rotation is required. Treat this as a required checkpoint before changing heading; do not infer forward direction from CAD coordinates or link names.

When the user requests a heading correction while keeping the global base link frame fixed:

1. Construct the requested rigid heading rotation R_heading, normally a yaw about world Z.
2. Apply R_heading to the base visual and collision transforms. Rotate the base center of mass and inertial-frame orientation consistently so its dynamics continue to describe the rotated physical base.
3. Rotate every non-base home-pose link frame by the same rigid transform about the base origin. Encode the rotation once in the base -> link1 joint origin, for example T_base_link1_new = R_heading @ T_base_link1_old when base and world axes coincide. Leave downstream relative joint transforms unchanged.
4. Keep the first joint's local axis components unchanged when its entire joint frame rotates with the new origin; then verify the resulting world axis numerically. Re-express the axis only when the intended physical world axis is different. Never rotate both the joint frame and its local axis blindly.
5. Preserve masses, joint ranges, and parent-to-child translation norms unless the user requests another physical change.
6. Recompute all global home poses and verify that the base frame is unchanged, the base mesh and every non-base link received exactly R_heading, downstream relative kinematics are unchanged, and all joint world axes are correct. Present the corrected visual heading to the user for confirmation.

Map URDF Semantics to MJCF

Use these equivalences:

Represent the base as an explicit MJCF body when it has mass or inertia. A worldbody geom alone cannot represent the URDF base dynamics.

For an arm-only terminal mount:

• Give the mount body the URDF joint6 child-frame pose and joint6.
• Use a tiny valid placeholder inertia only when MuJoCo requires it, for example mass 1e-6 and diagonal inertia 1e-9 1e-9 1e-9.
• Add no terminal mesh, gripper mass, finger joint, or tip body.
• Let the existing composition utility attach the selected external gripper.

Preserve and Change Poses Correctly

Interpret URDF RPY as:

R = Rz(yaw) @ Ry(pitch) @ Rx(roll)

Interpret MuJoCo quaternions as w x y z. Compare rotations as matrices because q and -q represent the same rotation.

Compute home-pose global transforms recursively:

T_world_child = T_world_parent @ T_parent_child

Apply frame changes deliberately:

• To remove a synthetic root link while preserving every global pose, compose its transform into the new root's children, visuals, and inertials as required.
• To rotate only a base mesh, change the base geom/visual transform; do not rotate the base body frame.
• To rotate all descendant link frames while leaving the base frame fixed, apply the desired world rotation to the first child global transform, solve back for its new parent-relative transform, and leave downstream relative transforms unchanged when the same rigid rotation should affect the entire subtree.
• Recompute joint axes in their declared local frames after changing frame orientations. Do not guess an axis from its global appearance.

After every transform edit, recompute all global link poses at the home configuration and verify that only the requested frames moved.

Match Geometry and Link Lengths

For each included link:

1. Copy the exact visual-to-link translation and rotation into the MJCF geom.
2. Point the MJCF mesh asset to the same physical file as the URDF mesh.
3. Preserve mesh scale and RGBA.
4. Match the complete parent-to-child joint translation vector.
5. Report link length as the norm of that vector, while treating vector equality as the stronger validation.

MuJoCo may recenter mesh vertices during compilation and store a mesh reference transform. Do not declare a geometry mismatch by comparing compiled geom_xpos directly with a URDF visual frame. First account for MuJoCo's mesh reference transform, or compare the source geom transform and underlying mesh file identity.

Convert Mass and Inertia

Copy mass and center of mass exactly for each physical arm link.

URDF gives a symmetric inertia tensor in its inertial frame:

I_urdf = [[ixx, ixy, ixz],
          [ixy, iyy, iyz],
          [ixz, iyz, izz]]

Rotate it into the link/body frame using the URDF inertial-origin rotation:

I_body = R_inertial @ I_urdf @ R_inertial.T

Convert I_body to MuJoCo principal form:

1. Run a symmetric eigendecomposition eigenvalues, eigenvectors = eigh(I_body).
2. Ensure the eigenvector matrix is a proper rotation with determinant +1; flip one eigenvector if necessary.
3. Convert the eigenvector rotation matrix to a normalized MuJoCo w x y z quaternion.
4. Write the eigenvalues as diaginertia.
5. Reconstruct I_check = R_quat @ diag(diaginertia) @ R_quat.T and compare it with I_body.

Prefer high-precision quat plus diaginertia values. MuJoCo can diagonalize fullinertia, but that compilation step may introduce a larger reconstruction residual. Do not specify an inertial quaternion together with fullinertia.

Build the MJCF Chain

Nest bodies in kinematic order:

world
└── base
    └── link1 / joint1
        └── link2 / joint2
            └── ...
                └── gripper / joint6   (end-effector mount; empty in the arm-only MJCF)

Place each joint at pos="0 0 0" inside its child body when the body's transform already represents the URDF joint origin. Copy joint type, axis, range, and name exactly. Keep actuator-force metadata only if the existing MJCF convention requires it.

For YAM, expect six arm meshes: base.stl and link1.stl through link5.stl. Exclude gripper.stl, tip_left.stl, and tip_right.stl from the arm MJCF.