Unable to Load xarm6+Allegro Hand URDF in ManiSkill3 Due to Invalid Moment of Inertia Error

[hesic73]

Last month, I attempted to use a xarm6+allegro hand robot with the following code:

"""
    Reference:
        - https://github.com/haosulab/ManiSkill/blob/main/mani_skill/agents/robots/xarm/xarm7_ability.py

"""

from typing import Dict
from copy import deepcopy

from mani_skill.envs.scene import ManiSkillScene
import sapien
import numpy as np
from mani_skill.agents.base_agent import BaseAgent, Keyframe
from mani_skill.agents.registration import register_agent
from mani_skill.agents.base_agent import DictControllerConfig

from mani_skill.agents.controllers import *

import os



@register_agent()
class Xarm6AllegroLeft(BaseAgent):
    uid = "xarm6_allegro_left"
    urdf_path = os.path.join(
        ASSET_DIR, "urdf/xarm6/xarm6_allegro_left_2023.urdf")

    keyframes = dict(
        rest=Keyframe(
            pose=sapien.Pose(),
            qpos=np.array(
                [0, -0.3, -0.155, 0, 0.36, -np.pi/2,
                 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ]
            ),
        ),
    )

    def __init__(self, scene: ManiSkillScene, control_freq: int, control_mode: str = None, agent_idx: int = None):
        self.arm_joint_names = [
            "joint1",
            "joint2",
            "joint3",
            "joint4",
            "joint5",
            "joint6",
        ]

        self.arm_stiffness = [100, 100, 64, 64, 64, 40]
        self.arm_damping = 0
        self.arm_force_limit = 50

        self.hand_joint_names = [
            'joint_0.0',
            'joint_12.0',
            'joint_4.0',
            'joint_8.0',
            'joint_1.0',
            'joint_13.0',
            'joint_5.0',
            'joint_9.0',
            'joint_2.0',
            'joint_14.0',
            'joint_6.0',
            'joint_10.0',
            'joint_3.0',
            'joint_15.0',
            'joint_7.0',
            'joint_11.0',
        ]

        self.hand_stiffness = 30
        self.hand_damping = 1
        self.hand_force_limit = 5

        self.ee_link_name = 'base_link'

        super().__init__(scene, control_freq, control_mode, agent_idx)

    @ property
    def _controller_configs(
        self,
    ) -> Dict[str,  DictControllerConfig]:
        # -------------------------------------------------------------------------- #
        # Arm
        # -------------------------------------------------------------------------- #
        arm_pd_joint_pos = PDJointPosControllerConfig(
            self.arm_joint_names,
            None,
            None,
            self.arm_stiffness,
            self.arm_damping,
            self.arm_force_limit,
            normalize_action=False,
        )
        arm_pd_joint_delta_pos = PDJointPosControllerConfig(
            self.arm_joint_names,
            -0.1,
            0.1,
            self.arm_stiffness,
            self.arm_damping,
            self.arm_force_limit,
            use_delta=True,
        )
        arm_pd_joint_target_delta_pos = deepcopy(arm_pd_joint_delta_pos)
        arm_pd_joint_target_delta_pos.use_target = True

        # PD ee position
        arm_pd_ee_delta_pose = PDEEPoseControllerConfig(
            self.arm_joint_names,
            -0.1,
            0.1,
            0.1,
            self.arm_stiffness,
            self.arm_damping,
            self.arm_force_limit,
            ee_link=self.ee_link_name,
            urdf_path=self.urdf_path,
        )

        arm_pd_ee_target_delta_pose = deepcopy(arm_pd_ee_delta_pose)
        arm_pd_ee_target_delta_pose.use_target = True

        # -------------------------------------------------------------------------- #
        # Hand
        # -------------------------------------------------------------------------- #
        hand_target_delta_pos = PDJointPosControllerConfig(
            self.hand_joint_names,
            -0.1,
            0.1,
            self.hand_stiffness,
            self.hand_damping,
            self.hand_force_limit,
            use_delta=True,
        )
        hand_target_delta_pos.use_target = True

        controller_configs = dict(
            pd_joint_delta_pos=dict(
                arm=arm_pd_joint_delta_pos, gripper=hand_target_delta_pos
            ),
            pd_joint_pos=dict(arm=arm_pd_joint_pos,
                              gripper=hand_target_delta_pos),
            pd_ee_delta_pose=dict(
                arm=arm_pd_ee_delta_pose, gripper=hand_target_delta_pos
            ),
            pd_ee_target_delta_pose=dict(
                arm=arm_pd_ee_target_delta_pose, gripper=hand_target_delta_pos
            ),
        )

        # Make a deepcopy in case users modify any config
        return deepcopy_dict(controller_configs)

Initially, I borrowed the URDF file from dynamic handover, but encountered an error while parsing it. Details of this issue can be found here. Isaac Sim could load this file, and I assumed this was normal since dynamic handover used IsaacGym. However, I recently discovered that DexPoint uses essentially the same URDF definition, which is SAPIEN-based. I would like to understand why I cannot load this definition correctly.

The error message is as follows:

File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/gymnasium/envs/registration.py", line 802, in make
    env = env_creator(**env_spec_kwargs)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/utils/registration.py", line 82, in make
    env = env_spec.make(**kwargs)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/utils/registration.py", line 35, in make
    return self.cls(**_kwargs)
  File "/home/sichengh/24summer/MultiGraspDex/multi_grasp_dex/tasks/one_cube.py", line 77, in __init__
    super().__init__(
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/envs/sapien_env.py", line 274, in __init__
    obs, _ = self.reset(seed=2022, options=dict(reconfigure=True))
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/envs/sapien_env.py", line 684, in reset
    self._reconfigure(options)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/envs/sapien_env.py", line 543, in _reconfigure
    self._load_agent(options)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/envs/sapien_env.py", line 326, in _load_agent
    agent: BaseAgent = agent_cls(
  File "/home/sichengh/24summer/MultiGraspDex/multi_grasp_dex/robots/xarm6_allegro_left.py", line 96, in __init__
    super().__init__(scene, control_freq, control_mode, agent_idx)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/agents/base_agent.py", line 91, in __init__
    self._load_articulation()
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/agents/base_agent.py", line 160, in _load_articulation
    self.robot: Articulation = loader.load(asset_path)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/utils/building/urdf_loader.py", line 68, in load
    articulation_builders, actor_builders, cameras = self.parse(
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/mani_skill/utils/building/urdf_loader.py", line 25, in parse
    articulation_builders, actor_builders, cameras = super().parse(
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/sapien/wrapper/urdf_loader.py", line 678, in parse
    return self._parse_urdf(urdf_string)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/sapien/wrapper/urdf_loader.py", line 656, in _parse_urdf
    articulation_builders.append(self._parse_articulation(root, fix_base))
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/sapien/wrapper/urdf_loader.py", line 480, in _parse_articulation
    self._build_link(link, link_builder)
  File "/home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/sapien/wrapper/urdf_loader.py", line 262, in _build_link
    assert all([x > 0 for x in eigs]), "invalid moment of inertia"
AssertionError: invalid moment of inertia

I am using the latest ManiSkill3 version:

$ pip show mani-skill
Name: mani_skill
Version: 3.0.0b5
Summary: ManiSkill3: A Unified Benchmark for Generalizable Manipulation Skills
Home-page: https://github.com/haosulab/ManiSkill
Author: ManiSkill contributors
Author-email: 
License: 
Location: /home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages
Requires: dacite, fast-kinematics, GitPython, gymnasium, h5py, huggingface-hub, imageio, imageio, IPython, mplib, numpy, pyyaml, rtree, sapien, scipy, tabulate, tqdm, transforms3d, trimesh
Required-by:

[StoneT2000]

DexPoint likely uses a older version of SAPIEN that does not check for this. The URDF most likely has a invalid defined value for inertia. Unfortunately this error does not print which link has this issue. I am quite sure IsaacGym simply ignores this issue and attempts to simulate it anyway. Since past work had no issue with a faulty moment of inertia, it is probably fine if you delete this tag from the urdf.

For a easy fix you can edit /home/sichengh/anaconda3/envs/dex/lib/python3.9/site-packages/sapien/wrapper/urdf_loader.py and add right before the assert

print(link.name)

then once you figure out which link has the issue, either fix the inertia value or just disable it for now by deleting the tag.

[hesic73]

I attempted the method mentioned in the issue on dynamic handover:

I am currently migrating the Allegro hand model mounted on an xArm6 from one simulator to another. During this transition, I encountered a critical error that prevents the simulation from running, due to an issue with the moment of inertia values specified in the URDF file.

The URDF section for link_2.0 is defined as follows:

<inertia ixx="7.04217e-05" iyy="3.95744e-05" izz="6.61125e-05" ixy="-9.64342e-05" ixz="5.8796e-05"
               iyz="-3.62996e-05"/>

Link

Upon calculating the eigenvalues of the inertia tensor derived from these values:

import numpy as np

I = np.array([[7.04217e-05, -9.64342e-05,  5.87960e-05],
              [-9.64342e-05, 3.95744e-05, -3.62996e-05],
              [5.87960e-05, -3.62996e-05, 6.61125e-05]])

eigenvalues, eigenvectors = np.linalg.eig(I)

print("Eigenvalues of I:", eigenvalues)

The computed eigenvalues are:

Eigenvalues of I: [ 1.90343185e-04 -4.39462409e-05  2.97116560e-05]

This includes a negative value, which is physically implausible for a moment of inertia tensor.

Could someone verify whether there might be a typo or error in these values?

There are several joints with negative inertia eigenvalues. I recall resolving this by copying the content of an Allegro hand URDF file to replace the old one. My file looks like this:

xarm6.zip

By the way, I noticed your recent work also uses a similar hand+arm robot in SAPIEN. Is it possible to share the model? Thank you!

[StoneT2000]

I just tried loading the xarm6 you sent, looks like it works?

[StoneT2000]

output.mp4

[hesic73]

Yes, this is the updated version. If I use the URDF file provided in DexPoint/dynamic_handover, the error occurs.
xarm6_description.zip

[StoneT2000]

I don't quite follow what the issue is? The old dynamic handover project uses a older version of sapien that is likely not checking for the inertia problem whereas the newer version is. You can just use the new xarm6, it looks and behaves correctly.

[hesic73]

Although the current URDF appears to work, I'm unsure if there might be potential issues. If possible, I would like to use the exact same definition as prior works. Since Isaac Sim and older versions of sapien support the "invalid" inertia, I was previously uncertain if this was a URDF definition problem or related to ManiSkill. Now you have confirmed that it is due to the former. Thank you for your help!