Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Discrepancies in Mass Matrix Values Between Pinocchio and MuJoCo #796

Open
luismuschal opened this issue Apr 29, 2024 · 3 comments
Open

Discrepancies in Mass Matrix Values Between Pinocchio and MuJoCo #796

luismuschal opened this issue Apr 29, 2024 · 3 comments

Comments

@luismuschal
Copy link

Environment:

  • MuJoCo Version: 2.1.1
  • Operating System: macOS (M2 chip)
  • Related Libraries: Robosuite, Pinocchio
  • Workaround for Version Compatibility (project depends on 2.1.0): GitHub Issue Link

Description:
I am observing different values in the diagonal elements of the mass matrices computed by MuJoCo and Pinocchio when using robosuite. This offset is always constant. This occurs with both a Schunk Robot and a 1-DOF rotational joint setup. However, the torques due to nonlinear effects (sim.data.qfrc_bias) are consistent between the systems.
When I use the following xml-file created from robosuite:

<body name="robot0_link0" pos="0 0 0">
                <inertial pos="0 0 0" mass="0" diaginertia="0 0 0"/>
                <geom name="robot0_link0_visual" pos="0 0 0.052" quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robot0_link0"/>
                <geom name="robot0_link0_collision" pos="0 0 0.052" quat="0 0 1 0" type="mesh" rgba="0 0.5 0 1" mesh="robot0_link0"/>
                <body name="robot0_right_hand" pos="0 0 0.154" quat="0 0 0 1">
                    <inertial pos="0 0 0" mass="0.29364" diaginertia="0.01 0.01 0.01"/>
                    <joint name="robot0_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.947 2.947" armature="5" damping="0.0001" frictionloss="0.1"/>
                    <geom name="robot0_link_1_visual" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0.8 1" mesh="robot0_link1"/>
                    <geom name="robot0_link_1_collision" type="mesh" rgba="0 0.5 0 1" mesh="robot0_link1"/>
                    <body name="gripper0_null_gripper" pos="0 0 0">
                        <inertial pos="0 0 0" mass="0.3" diaginertia="0.01 0.01 0.01"/>
                        <site name="gripper0_ft_frame" pos="0 0 0" size="0.01" group="1" rgba="1 0 0 1"/>
                        <body name="gripper0_eef" pos="0 0 0" quat="0.707107 0 0 -0.707107">
                            <inertial pos="0 0 0" quat="0.707107 0 0 -0.707107" mass="0" diaginertia="0 0 0"/>
                            <site name="gripper0_grip_site" pos="0 0 0" size="0.01" group="1" rgba="1 0 0 0.5"/>
                            <site name="gripper0_ee_x" pos="0.1 0 0" quat="0.707105 0 0.707108 0" size="0.005 0.1" group="1" type="cylinder" rgba="1 0 0 0"/>
                            <site name="gripper0_ee_y" pos="0 0.1 0" quat="0.707105 0.707108 0 0" size="0.005 0.1" group="1" type="cylinder" rgba="0 1 0 0"/>
                            <site name="gripper0_ee_z" pos="0 0 0.1" size="0.005 0.1" group="1" type="cylinder" rgba="0 0 1 0"/>
                            <site name="gripper0_grip_site_cylinder" pos="0 0 0" size="0.005 10" group="1" type="cylinder" rgba="0 1 0 0.3"/>
                        </body>
                    </body>
                </body>
            </body>
            <body name="mount0_base" pos="0 0 0.01">
                <inertial pos="0 0 0.01" mass="0" diaginertia="0 0 0"/>
                <body name="mount0_controller_box" pos="0 0 0">
                    <inertial pos="-0.325 0 -0.38" mass="46.64" diaginertia="1.71363 1.27988 0.809981"/>
                    <geom name="mount0_controller_box_col" size="0.11 0.2 0.265" pos="-0.325 0 -0.38" type="box" rgba="0.5 0.5 0 1"/>
                </body>
                <body name="mount0_pedestal_feet" pos="0 0 0">
                    <inertial pos="-0.1225 0 -0.758" mass="167.09" diaginertia="8.16095 9.59375 15.0785"/>
                    <geom name="mount0_pedestal_feet_col" size="0.385 0.35 0.155" pos="-0.1225 0 -0.758" type="box" rgba="0.5 0.5 0 1"/>
                </body>
                <body name="mount0_torso" pos="0 0 0">
                    <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08"/>
                    <geom name="mount0_torso_vis" size="0.05 0.05 0.05" pos="0 0 -0.05" type="box" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1"/>
                </body>
                <body name="mount0_pedestal" pos="0 0 0">
                    <inertial pos="0 0 0" quat="0.659267 -0.259505 -0.260945 0.655692" mass="60.864" diaginertia="6.0869 5.81635 4.20915"/>
                    <geom name="mount0_pedestal_vis" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="mount0_pedestal"/>
                    <geom name="mount0_pedestal_col" size="0.18 0.31" pos="-0.02 0 -0.29" type="cylinder" rgba="0.5 0.5 0 1"/>
                </body>
            </body>
        </body>

and the .urdf for pinocchio:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from schunk.xacro                   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="schunk_lwa4p">
  <link name="world"/>
  <!-- joint between world and arm_1_link -->
  <joint name="arm_1_joint" type="revolute">
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0.154"/>
    <parent link="world"/>
    <child link="arm_1_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="370" lower="-2.947" upper="2.947" velocity="1.26"/>
    <!--safety_controller k_position="20" k_velocity="50" soft_lower_limit="${-6.28 + 0.01}" soft_upper_limit="${6.28 - 0.01}" /-->
  </joint>
  <link name="arm_1_link">
    <!--<xacro:default_inertial/>-->
    <inertial>
      <mass value="0.29364"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <joint name="gripper_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0"/>
    <parent link="arm_1_link"/>
    <child link="gripper_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="370" lower="-2.947" upper="2.947" velocity="1.26"/>
    <!--safety_controller k_position="20" k_velocity="50" soft_lower_limit="${-6.28 + 0.01}" soft_upper_limit="${6.28 - 0.01}" /-->
  </joint>
  <link name="gripper_link">
    <!--<xacro:default_inertial/>-->
    <inertial>
      <mass value="0.3"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>
</robot>

I get a value of [[5.02]] using the following code from robosuite (base_controller.py)

mass_matrix = np.ndarray(shape=(len(self.sim.data.qvel) ** 2,), dtype=np.float64, order="C")
mujoco_py.cymj._mj_fullM(self.sim.model, mass_matrix, self.sim.data.qM)
mass_matrix = np.reshape(mass_matrix, (len(self.sim.data.qvel), len(self.sim.data.qvel)))
self.mass_matrix = mass_matrix[self.qvel_index, :][:, self.qvel_index]

and [[0.02]] for the pinocchio implementation:

pinocchio::Data data(model);
pinocchio::crba(model, data, this->q_m);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
Eigen::MatrixXd massMatrix = data.M;

Expected vs. Actual Results:

  • Expected: Matching mass matrices across both platforms.
  • Actual: Diagonal elements of the matrices do not match.
@saran-t
Copy link

saran-t commented Apr 29, 2024

This isn't a mujoco-py issue, you would be better off asking on https://github.com/google-deepmind/mujoco or (more likely) https://github.com/ARISE-Initiative/robosuite.

Also you are using a version of MuJoCo that's several years old by this point.

@melissajcruz
Copy link

Hello, was there any resolution/work-around for this issue? I'm noticing the same issues between MuJoCo 3.1.4 and Pincchio 2.7.0 with the lower half of Unitree's h1 from the mujoco_menagerie.

@luismuschal
Copy link
Author

Hey, in my case the problem was that the armature was not considered in the Pinocchio model. I resolved it by manually adding the armature to the diagonal elements of the mass matrix.

Note that full support for the robot armature in RNEA, CRBA, and ABA was introduced in Pinocchio 3.0.0 (changelog). In Pinocchio 2.7.0, even if rotor parameters are set, they are ignored (related issue).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@saran-t @melissajcruz @luismuschal