search

openai / mujoco-py

MuJoCo is a physics engine for detailed, efficient rigid body simulations with contacts. mujoco-py allows using MuJoCo from Python 3.
Other
2.79k stars 810 forks source link

Discrepancies in Mass Matrix Values Between Pinocchio and MuJoCo #796

Open luismuschal opened 2 months ago

luismuschal commented 2 months ago

Environment:

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:

saran-t commented 2 months ago

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.