Mocap-equality issues in brax/MJX #536

SimpleGeometry · 2024-10-17T22:32:25Z

SimpleGeometry
Oct 17, 2024

As I understand, we cannot set mocap="true" for a body unless the body is fixed.

On the other hand, as per google-deepmind/mujoco#2058, I believe brax is fusing all bodies that aren't joints, and so equalities cannot target any bodies that are fused to their parents. Normally this makes sense because you'd just target the parent (with an offset if needed), but in this case the mocap can actually move, it's just controlled externally.

In combination, it seems like these two conditions would result in it being impossible to do EEF control for a robotic arm using a mocap + equality, under the current scheme. Is that true or is my understanding not quite right?

If not, are there any workarounds for this?
If so, is it possible that brax adds an option to remove fusion of bodies (in the XML, or in the environment flags for the backend, etc.)?

I looked around but did not spot any other threads with similar issues for fusion of bodies causing this set of conditions which breaks mocap + equality, so I hope I am not duplicating any threads.

If necessary, the XML I am working with is below.

<!-- Credit: adapted from Mujoco Menagerie -->
<mujoco model="panda">
  <compiler angle="radian" meshdir="franka_emika_panda/assets" autolimits="true"/>

  <option integrator="implicitfast" impratio="10"/>
    
  <default>
    <default class="panda">
      <joint armature="0.1" damping="5" axis="0 0 1" range="-2.8973 2.8973"/>
      <general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
      <default class="finger">
        <joint axis="0 1 0" type="slide" range="0 0.08"/>
      </default>

      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
      </default>
      <default class="collision">
        <geom type="mesh" group="3"/>
        <default class="finger_collision">
          <geom type="box" size="0.015 0.005 0.04" pos="0 0.0055 0.035" friction="10 0.005 0.0001"/>
        </default>
      </default>
    </default>
  </default>

  <asset>
    <!-- Visual meshes -->
    <mesh file="link0_0.obj"/>
    <mesh file="link0_1.obj"/>
    <mesh file="link0_2.obj"/>
    <mesh file="link0_3.obj"/>
    <mesh file="link0_4.obj"/>
    <mesh file="link0_5.obj"/>
    <mesh file="link0_7.obj"/>
    <mesh file="link0_8.obj"/>
    <mesh file="link0_9.obj"/>
    <mesh file="link0_10.obj"/>
    <mesh file="link0_11.obj"/>
    <mesh file="link1.obj"/>
    <mesh file="link2.obj"/>
    <mesh file="link3_0.obj"/>
    <mesh file="link3_1.obj"/>
    <mesh file="link3_2.obj"/>
    <mesh file="link3_3.obj"/>
    <mesh file="link4_0.obj"/>
    <mesh file="link4_1.obj"/>
    <mesh file="link4_2.obj"/>
    <mesh file="link4_3.obj"/>
    <mesh file="link5_0.obj"/>
    <mesh file="link5_1.obj"/>
    <mesh file="link5_2.obj"/>
    <mesh file="link6_0.obj"/>
    <mesh file="link6_1.obj"/>
    <mesh file="link6_2.obj"/>
    <mesh file="link6_3.obj"/>
    <mesh file="link6_4.obj"/>
    <mesh file="link6_5.obj"/>
    <mesh file="link6_6.obj"/>
    <mesh file="link6_7.obj"/>
    <mesh file="link6_8.obj"/>
    <mesh file="link6_9.obj"/>
    <mesh file="link6_10.obj"/>
    <mesh file="link6_11.obj"/>
    <mesh file="link6_12.obj"/>
    <mesh file="link6_13.obj"/>
    <mesh file="link6_14.obj"/>
    <mesh file="link6_15.obj"/>
    <mesh file="link6_16.obj"/>
    <mesh file="link7_0.obj"/>
    <mesh file="link7_1.obj"/>
    <mesh file="link7_2.obj"/>
    <mesh file="link7_3.obj"/>
    <mesh file="link7_4.obj"/>
    <mesh file="link7_5.obj"/>
    <mesh file="link7_6.obj"/>
    <mesh file="link7_7.obj"/>
    <mesh file="hand_0.obj"/>
    <mesh file="hand_1.obj"/>
    <mesh file="hand_2.obj"/>
    <mesh file="hand_3.obj"/>
    <mesh file="hand_4.obj"/>
    <mesh file="finger_0.obj"/>
    <mesh file="finger_1.obj"/>
  </asset>

  <worldbody>
    <light name="top" pos="0 0 2" mode="trackcom"/>
    <body name="link0" childclass="panda">
      <geom mesh="link0_0" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
      <geom mesh="link0_1" rgba="0.25 0.25 0.25 1" class="visual"/>
      <geom mesh="link0_2" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
      <geom mesh="link0_3" rgba="0.25 0.25 0.25 1" class="visual"/>
      <geom mesh="link0_4" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
      <geom mesh="link0_5" rgba="0.25 0.25 0.25 1" class="visual"/>
      <geom mesh="link0_7" rgba="1 1 1 1" class="visual"/>
      <geom mesh="link0_8" rgba="1 1 1 1" class="visual"/>
      <geom mesh="link0_9" rgba="0.25 0.25 0.25 1" class="visual"/>
      <geom mesh="link0_10" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
      <geom mesh="link0_11" rgba="1 1 1 1" class="visual"/>
      <body name="link1" pos="0 0 0.333">
        <inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
          fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
        <joint name="joint1"/>
        <geom rgba="1 1 1 1" mesh="link1" class="visual"/>
        <body name="link2" quat="1 -1 0 0">
          <inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
            fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
          <joint name="joint2" range="-1.7628 1.7628"/>
          <geom rgba="1 1 1 1" mesh="link2" class="visual"/>
          <body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
            <joint name="joint3"/>
            <inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
              fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
            <geom mesh="link3_0" rgba="1 1 1 1" class="visual"/>
            <geom mesh="link3_1" rgba="1 1 1 1" class="visual"/>
            <geom mesh="link3_2" rgba="1 1 1 1" class="visual"/>
            <geom mesh="link3_3" rgba="0.25 0.25 0.25 1" class="visual"/>
            <body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
              <inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
                fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
              <joint name="joint4" range="-3.0718 -0.0698"/>
              <geom mesh="link4_0" rgba="1 1 1 1" class="visual"/>
              <geom mesh="link4_1" rgba="1 1 1 1" class="visual"/>
              <geom mesh="link4_2" rgba="0.25 0.25 0.25 1" class="visual"/>
              <geom mesh="link4_3" rgba="1 1 1 1" class="visual"/>
              <body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
                <inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
                  fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
                <joint name="joint5"/>
                <geom mesh="link5_0" rgba="0.25 0.25 0.25 1" class="visual"/>
                <geom mesh="link5_1" rgba="1 1 1 1" class="visual"/>
                <geom mesh="link5_2" rgba="1 1 1 1" class="visual"/>
                <body name="link6" quat="1 1 0 0">
                  <inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
                    fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
                  <joint name="joint6" range="-0.0175 3.7525"/>
                  <geom mesh="link6_0" rgba="0.901961 0.921569 0.929412 1" class="visual"/>
                  <geom mesh="link6_1" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_2" rgba="0.25 0.25 0.25 1" class="visual"/>
                  <geom mesh="link6_3" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_4" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_5" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_6" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_7" rgba="0.039216 0.541176 0.780392 1" class="visual"/>
                  <geom mesh="link6_8" rgba="0.039216 0.541176 0.780392 1" class="visual"/>
                  <geom mesh="link6_9" rgba="0.25 0.25 0.25 1" class="visual"/>
                  <geom mesh="link6_10" rgba="0.25 0.25 0.25 1" class="visual"/>
                  <geom mesh="link6_11" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_12" rgba="0 1 0 1" class="visual"/>
                  <geom mesh="link6_13" rgba="1 1 1 1" class="visual"/>
                  <geom mesh="link6_14" rgba="0.25 0.25 0.25 1" class="visual"/>
                  <geom mesh="link6_15" rgba="0.25 0.25 0.25 1" class="visual"/>
                  <geom mesh="link6_16" rgba="1 1 1 1" class="visual"/>
                  <body name="link7" pos="0.088 0 0" quat="1 1 0 0">
                    <inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
                      fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
                    <joint name="joint7"/>
                    <geom mesh="link7_0" rgba="1 1 1 1" class="visual"/>
                    <geom mesh="link7_1" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_2" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_3" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_4" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_5" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_6" rgba="0.25 0.25 0.25 1" class="visual"/>
                    <geom mesh="link7_7" rgba="1 1 1 1" class="visual"/>
                    <body name="hand" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834">
                      <geom name="hand_geom" rgba="1 1 1 1" type="capsule" size="0.04 0.06" pos="0 0 0.035" quat="1 1 0 0" contype="0" conaffinity="0"/>
                      <body name="left_finger" pos="0 0 0.0584">
                        <inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
                        <joint name="finger_joint1" class="finger"/>
                          <geom name="left_finger_geom" rgba="0.25 0.25 0.25 1" class="finger_collision" contype="0" conaffinity="0"/>
                      </body>
                      <body name="right_finger" pos="0 0 0.0584" quat="0 0 0 1">
                        <inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
                        <joint name="finger_joint2" class="finger"/>
                          <geom name="right_finger_geom" rgba="0.25 0.25 0.25 1" class="finger_collision" contype="0" conaffinity="0"/>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
      
    <body name="eef_target" pos="0.5 0.5 0.5" mocap="true" gravcomp="1">
      <geom name="eef_target_geom" rgba="1 1 1 1" type="sphere" size="0.01" contype="0" conaffinity="0"/>
    </body>
      
    <body name="binA" pos="-0.17 0.6 0">
      <geom name="binA1" pos="0 -0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
      <geom name="binA2" pos="0 0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
      <geom name="binA3" pos="0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
      <geom name="binA4" pos="-0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0 0.2 0.8 1" contype="0" conaffinity="0"/>
    </body>
    <body name="binB" pos="0.17 0.6 0">
      <geom name="binB1" pos="0 -0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
      <geom name="binB2" pos="0 0.14 0.033" size="0.145 0.005 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
      <geom name="binB3" pos="0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
      <geom name="binB4" pos="-0.14 0 0.033" size="0.005 0.135 0.033" type="box" rgba="0.8 0 0 1" contype="0" conaffinity="0"/>
    </body>
    
    <body name="cube" pos="-0.15 0.6 0.07">
      <freejoint/>
      <geom name="cube_geom" size="0.03 0.03 0.03" type="box" rgba="0 0.4 0 1" contype="0" conaffinity="0"/>
    </body>
      
    <body name="goal_marker" pos="-0.15 0.6 0.07" gravcomp="1">
      <freejoint/>
      <geom name="goal_marker_geom" size="0.03 0.03 0.03" type="box" rgba="0.3 1 0.3 1" contype="0" conaffinity="0"/>
    </body>
      
    <body name="tablelink" pos="0 .2 0">
      <geom name="table_geom" group="4" pos="0.0 0.0 0.0" size="2.8 2.4 0.1" type="plane" mass="10" friction="0.2 0.005 0.0001" contype="0" conaffinity="0"/>
    </body>
  </worldbody>

  <equality>
    <joint joint1="finger_joint1" joint2="finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
    <weld body1="eef_target" body2="link7" relpose="0 0 0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
  </equality>
    

  <contact>
    <pair geom1="cube_geom" geom2="table_geom"/>
    <pair geom1="cube_geom" geom2="binA1"/>
    <pair geom1="cube_geom" geom2="binA2"/>
    <pair geom1="cube_geom" geom2="binA3"/>
    <pair geom1="cube_geom" geom2="binA4"/>
    <pair geom1="cube_geom" geom2="binB1"/>
    <pair geom1="cube_geom" geom2="binB2"/>
    <pair geom1="cube_geom" geom2="binB3"/>
    <pair geom1="cube_geom" geom2="binB4"/>
    <pair geom1="cube_geom" geom2="hand_geom"/>
    <pair geom1="cube_geom" geom2="left_finger_geom"/>
    <pair geom1="cube_geom" geom2="right_finger_geom"/>
      
    <pair geom1="table_geom" geom2="hand_geom"/>
    <pair geom1="table_geom" geom2="left_finger_geom"/>
    <pair geom1="table_geom" geom2="right_finger_geom"/>
    
    <!-- The finger and bin collisions aren't being registered that well so fingers will ghost through the bin if enough force is applied and the fingers are fast enough. -->
    <pair geom1="left_finger_geom" geom2="binA1"/>
    <pair geom1="left_finger_geom" geom2="binA2"/>
    <pair geom1="left_finger_geom" geom2="binA3"/>
    <pair geom1="left_finger_geom" geom2="binA4"/>
    <pair geom1="left_finger_geom" geom2="binB1"/>
    <pair geom1="left_finger_geom" geom2="binB2"/>
    <pair geom1="left_finger_geom" geom2="binB3"/>
    <pair geom1="left_finger_geom" geom2="binB4"/>
    <pair geom1="right_finger_geom" geom2="binA1"/>
    <pair geom1="right_finger_geom" geom2="binA2"/>
    <pair geom1="right_finger_geom" geom2="binA3"/>
    <pair geom1="right_finger_geom" geom2="binA4"/>
    <pair geom1="right_finger_geom" geom2="binB1"/>
    <pair geom1="right_finger_geom" geom2="binB2"/>
    <pair geom1="right_finger_geom" geom2="binB3"/>
    <pair geom1="right_finger_geom" geom2="binB4"/>
    <pair geom1="hand_geom" geom2="binA1"/>
    <pair geom1="hand_geom" geom2="binA2"/>
    <pair geom1="hand_geom" geom2="binA3"/>
    <pair geom1="hand_geom" geom2="binA4"/>
    <pair geom1="hand_geom" geom2="binB1"/>
    <pair geom1="hand_geom" geom2="binB2"/>
    <pair geom1="hand_geom" geom2="binB3"/>
    <pair geom1="hand_geom" geom2="binB4"/>
      
    <pair geom1="left_finger_geom" geom2="right_finger_geom"/>
  </contact>

  <actuator>
    <!-- Remap original ctrlrange (0, 0.08) to (0, 255): 0.08 * 500 / 255 = 0.15686 -->
    <general class="panda" name="actuator8" joint="finger_joint1" forcerange="-500 500" ctrlrange="0 255"
      gainprm="0.15686 0 0" biasprm="0 -500 -50"/>
    <general class="panda" name="actuator9" joint="finger_joint2" forcerange="-500 500" ctrlrange="0 255"
      gainprm="0.15686 0 0" biasprm="0 -500 -50"/>
  </actuator>
</mujoco>

btaba · 2024-10-24T21:18:15Z

btaba
Oct 24, 2024
Maintainer

Thanks @SimpleGeometry, I agree we likely remove (or provide the option to remove) fuse_bodies in this case. Opened #544 for now

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Mocap-equality issues in brax/MJX #536

{{title}}

Replies: 1 comment

{{title}}

Select a reply

Mocap-equality issues in brax/MJX #536

SimpleGeometry Oct 17, 2024

Replies: 1 comment

btaba Oct 24, 2024 Maintainer

SimpleGeometry
Oct 17, 2024

btaba
Oct 24, 2024
Maintainer