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 · 1 comment
Open

Discrepancies in Mass Matrix Values Between Pinocchio and MuJoCo #796

luismuschal opened this issue Apr 29, 2024 · 1 comment

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.

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
2 participants
@saran-t @luismuschal