Unitree H1 - replace mesh colliders with primitive geoms.

PiperOrigin-RevId: 602741813 Change-Id: I6d5a7bf5fe968deca42eaac10f3f34264ec0edc0
Pointergg · Jan 30, 2024 · 83e4e86 · 83e4e86
1 parent 022b235
commit 83e4e86
Show file tree

Hide file tree

Showing 2 changed files with 65 additions and 35 deletions.
diff --git a/unitree_h1/h1.xml b/unitree_h1/h1.xml
@@ -1,15 +1,28 @@
 <mujoco model="h1">
   <compiler angle="radian" meshdir="assets" autolimits="true"/>
 
+  <statistic meansize="0.05"/>
+
   <default>
     <default class="h1">
-      <geom type="mesh"/>
       <joint damping="1" armature="0.1"/>
       <default class="visual">
-        <geom contype="0" conaffinity="0" group="2" material="black"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="2" material="black"/>
       </default>
       <default class="collision">
         <geom group="3" mass="0" density="0"/>
+        <default class="foot">
+          <geom type="capsule" size=".014"/>
+          <default class="foot1">
+            <geom fromto="-.035 0 -0.056 .02 0 -0.045"/>
+          </default>
+          <default class="foot2">
+            <geom fromto=".02 0 -0.045 .115 0 -0.056"/>
+          </default>
+          <default class="foot3">
+            <geom fromto=".14 -.03 -0.056 .14 .03 -0.056"/>
+          </default>
+        </default>
       </default>
       <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
     </default>
@@ -43,13 +56,12 @@
   </asset>
 
   <worldbody>
-    <light mode="targetbodycom" target="torso_link" pos="1 0 2.5"/>
-    <body name="pelvis" pos="0 0 1.1" childclass="h1">
+    <light mode="targetbodycom" target="torso_link" pos="2 0 2.5"/>
+    <body name="pelvis" pos="0 0 1.06" childclass="h1">
       <inertial pos="-0.0002 4e-05 -0.04522" quat="0.498303 0.499454 -0.500496 0.501741" mass="5.39"
         diaginertia="0.0490211 0.0445821 0.00824619"/>
       <freejoint/>
       <geom class="visual" mesh="pelvis"/>
-      <geom class="collision" mesh="pelvis"/>
       <body name="left_hip_yaw_link" pos="0 0.0875 -0.1742">
         <inertial pos="-0.04923 0.0001 0.0072" quat="0.69699 0.219193 0.233287 0.641667" mass="2.244"
           diaginertia="0.00304494 0.00296885 0.00189201"/>
@@ -61,25 +73,30 @@
             diaginertia="0.00243264 0.00225325 0.00205492"/>
           <joint name="left_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
           <geom class="visual" mesh="left_hip_roll_link"/>
-          <geom class="collision" mesh="left_hip_roll_link"/>
+          <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 -0.02 0"/>
           <body name="left_hip_pitch_link" pos="0 0.11536 0">
             <inertial pos="0.00746 -0.02346 -0.08193" quat="0.979828 0.0513522 -0.0169854 -0.192382" mass="4.152"
               diaginertia="0.0829503 0.0821457 0.00510909"/>
             <joint name="left_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
             <geom class="visual" mesh="left_hip_pitch_link"/>
-            <geom class="collision" mesh="left_hip_pitch_link"/>
+            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
+            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
+            <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 -0.07 0"/>
             <body name="left_knee_link" pos="0 0 -0.4">
               <inertial pos="-0.00136 -0.00512 -0.1384" quat="0.626132 -0.034227 -0.0416277 0.777852" mass="1.721"
                 diaginertia="0.0125237 0.0123104 0.0019428"/>
               <joint name="left_knee" axis="0 1 0" range="-0.26 2.05"/>
               <geom class="visual" mesh="left_knee_link"/>
-              <geom class="collision" mesh="left_knee_link"/>
+              <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
+              <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
               <body name="left_ankle_link" pos="0 0 -0.4">
                 <inertial pos="0.06722 0.00015 -0.04497" quat="0.489101 0.503197 0.565782 0.432972" mass="0.446"
                   diaginertia="0.00220848 0.00218961 0.000214202"/>
                 <joint name="left_ankle" axis="0 1 0" range="-0.87 0.52"/>
                 <geom class="visual" mesh="left_ankle_link"/>
-                <geom class="collision" mesh="left_ankle_link"/>
+                <geom class="foot1"/>
+                <geom class="foot2"/>
+                <geom class="foot3"/>
               </body>
             </body>
           </body>
@@ -96,25 +113,30 @@
             diaginertia="0.00243264 0.00225325 0.00205492"/>
           <joint name="right_hip_roll" axis="1 0 0" range="-0.43 0.43"/>
           <geom class="visual" mesh="right_hip_roll_link"/>
-          <geom class="collision" mesh="right_hip_roll_link"/>
+          <geom class="collision" type="cylinder" size="0.05 0.03" quat="1 1 0 0" pos="0 0.02 0"/>
           <body name="right_hip_pitch_link" pos="0 -0.11536 0">
             <inertial pos="0.00746 0.02346 -0.08193" quat="0.979828 -0.0513522 -0.0169854 0.192382" mass="4.152"
               diaginertia="0.0829503 0.0821457 0.00510909"/>
             <joint name="right_hip_pitch" axis="0 1 0" range="-1.57 1.57"/>
             <geom class="visual" mesh="right_hip_pitch_link"/>
-            <geom class="collision" mesh="right_hip_pitch_link"/>
+            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 -0.02 0 0.02"/>
+            <geom class="collision" type="capsule" size="0.03" fromto="0.02 0 -0.4 0.02 0 0.02"/>
+            <geom class="collision" type="cylinder" size="0.05 0.02" quat="1 1 0 0" pos="0 0.07 0"/>
             <body name="right_knee_link" pos="0 0 -0.4">
               <inertial pos="-0.00136 0.00512 -0.1384" quat="0.777852 -0.0416277 -0.034227 0.626132" mass="1.721"
                 diaginertia="0.0125237 0.0123104 0.0019428"/>
               <joint name="right_knee" axis="0 1 0" range="-0.26 2.05"/>
               <geom class="visual" mesh="right_knee_link"/>
-              <geom class="collision" mesh="right_knee_link"/>
+              <geom class="collision" type="capsule" size="0.025" fromto="0.02 0 -0.4 0.02 0 0"/>
+              <geom class="collision" type="sphere" size="0.05" pos="0 0 -0.115"/>
               <body name="right_ankle_link" pos="0 0 -0.4">
                 <inertial pos="0.06722 -0.00015 -0.04497" quat="0.432972 0.565782 0.503197 0.489101" mass="0.446"
                   diaginertia="0.00220848 0.00218961 0.000214202"/>
                 <joint name="right_ankle" axis="0 1 0" range="-0.87 0.52"/>
                 <geom class="visual" mesh="right_ankle_link"/>
-                <geom class="collision" mesh="right_ankle_link"/>
+                <geom class="foot1"/>
+                <geom class="foot2"/>
+                <geom class="foot3"/>
               </body>
             </body>
           </body>
@@ -125,33 +147,36 @@
           diaginertia="0.487315 0.409628 0.127837"/>
         <joint name="torso" axis="0 0 1" range="-2.35 2.35"/>
         <geom class="visual" mesh="torso_link"/>
-        <geom class="collision" mesh="torso_link"/>
         <geom class="visual" material="white" mesh="logo_link"/>
-        <site name="imu" size="0.01" pos="-0.04452 -0.01891 0.27756"/>
+        <geom name="head" class="collision" type="capsule" size="0.06" fromto="0.05 0 0.68 0.05 0 0.6"/>
+        <geom name="helmet" class="collision" type="sphere" size="0.073" pos="0.045 0 0.68"/>
+        <geom name="torso" class="collision" type="box" size="0.07 0.1 0.22" pos="0 0 0.25"/>
+        <geom name="hip" class="collision" type="capsule" size="0.05" fromto="0 -0.1 -0.05 0 0.1 -0.05"/>
+        <site name="imu" pos="-0.04452 -0.01891 0.27756"/>
         <body name="left_shoulder_pitch_link" pos="0.0055 0.15535 0.42999" quat="0.976296 0.216438 0 0">
           <inertial pos="0.005045 0.053657 -0.015715" quat="0.814858 0.579236 -0.0201072 -0.00936488" mass="1.033"
             diaginertia="0.00129936 0.000987113 0.000858198"/>
           <joint name="left_shoulder_pitch" axis="0 1 0" range="-2.87 2.87"/>
           <geom class="visual" mesh="left_shoulder_pitch_link"/>
-          <geom class="collision" mesh="left_shoulder_pitch_link"/>
           <body name="left_shoulder_roll_link" pos="-0.0055 0.0565 -0.0165" quat="0.976296 -0.216438 0 0">
             <inertial pos="0.000679 0.00115 -0.094076" quat="0.732491 0.00917179 0.0766656 0.676384" mass="0.793"
               diaginertia="0.00170388 0.00158256 0.00100336"/>
             <joint name="left_shoulder_roll" axis="1 0 0" range="-0.34 3.11"/>
             <geom class="visual" mesh="left_shoulder_roll_link"/>
-            <geom class="collision" mesh="left_shoulder_roll_link"/>
+            <geom name="left_shoulder" class="collision" type="capsule" size="0.04" fromto="0 0.01 0.008 0 -0.07 -0.02"/>
             <body name="left_shoulder_yaw_link" pos="0 0 -0.1343">
               <inertial pos="0.01365 0.002767 -0.16266" quat="0.703042 -0.0331229 -0.0473362 0.708798" mass="0.839"
                 diaginertia="0.00408038 0.00370367 0.000622687"/>
               <joint name="left_shoulder_yaw" axis="0 0 1" range="-1.3 4.45"/>
               <geom class="visual" mesh="left_shoulder_yaw_link"/>
-              <geom class="collision" mesh="left_shoulder_yaw_link"/>
+              <geom class="collision" type="capsule" size="0.03" fromto="0 0 0.15 0 0 -0.2"/>
               <body name="left_elbow_link" pos="0.0185 0 -0.198">
                 <inertial pos="0.15908 -0.000144 -0.015776" quat="0.0765232 0.720327 0.0853116 0.684102" mass="0.669"
                   diaginertia="0.00601829 0.00600579 0.000408305"/>
                 <joint name="left_elbow" axis="0 1 0" range="-1.25 2.61"/>
                 <geom class="visual" mesh="left_elbow_link"/>
-                <geom class="collision" mesh="left_elbow_link"/>
+                <geom class="collision" type="capsule" size="0.025" fromto="0 0 0 0.28 0 -0.015"/>
+                <geom class="collision" type="sphere" size="0.033" pos="0.28 0 -0.015"/>
               </body>
             </body>
           </body>
@@ -161,25 +186,25 @@
             diaginertia="0.00129936 0.000987113 0.000858198"/>
           <joint name="right_shoulder_pitch" axis="0 1 0" range="-2.87 2.87"/>
           <geom class="visual" mesh="right_shoulder_pitch_link"/>
-          <geom class="collision" mesh="right_shoulder_pitch_link"/>
           <body name="right_shoulder_roll_link" pos="-0.0055 -0.0565 -0.0165" quat="0.976296 0.216438 0 0">
             <inertial pos="0.000679 -0.00115 -0.094076" quat="0.676384 0.0766656 0.00917179 0.732491" mass="0.793"
               diaginertia="0.00170388 0.00158256 0.00100336"/>
             <joint name="right_shoulder_roll" axis="1 0 0" range="-3.11 0.34"/>
             <geom class="visual" mesh="right_shoulder_roll_link"/>
-            <geom class="collision" mesh="right_shoulder_roll_link"/>
+            <geom name="right_shoulder" class="collision" type="capsule" size="0.04" fromto="0 -0.01 0.008 0 0.07 -0.02"/>
             <body name="right_shoulder_yaw_link" pos="0 0 -0.1343">
               <inertial pos="0.01365 -0.002767 -0.16266" quat="0.708798 -0.0473362 -0.0331229 0.703042" mass="0.839"
                 diaginertia="0.00408038 0.00370367 0.000622687"/>
               <joint name="right_shoulder_yaw" axis="0 0 1" range="-4.45 1.3"/>
               <geom class="visual" mesh="right_shoulder_yaw_link"/>
-              <geom class="collision" mesh="right_shoulder_yaw_link"/>
+              <geom class="collision" type="capsule" size="0.03" fromto="0 0 0.15 0 0 -0.2"/>
               <body name="right_elbow_link" pos="0.0185 0 -0.198">
                 <inertial pos="0.15908 0.000144 -0.015776" quat="-0.0765232 0.720327 -0.0853116 0.684102" mass="0.669"
                   diaginertia="0.00601829 0.00600579 0.000408305"/>
                 <joint name="right_elbow" axis="0 1 0" range="-1.25 2.61"/>
                 <geom class="visual" mesh="right_elbow_link"/>
-                <geom class="collision" mesh="right_elbow_link"/>
+                <geom class="collision" type="capsule" size="0.025" fromto="0 0 0 0.28 0 -0.015"/>
+                <geom class="collision" type="sphere" size="0.033" pos="0.28 0 -0.015"/>
               </body>
             </body>
           </body>
@@ -188,6 +213,11 @@
     </body>
   </worldbody>
 
+  <contact>
+    <exclude body1="torso_link" body2="left_shoulder_roll_link"/>
+    <exclude body1="torso_link" body2="right_shoulder_roll_link"/>
+  </contact>
+
   <actuator>
     <motor class="h1" name="left_hip_yaw" joint="left_hip_yaw" ctrlrange="-200 200"/>
     <motor class="h1" name="left_hip_roll" joint="left_hip_roll" ctrlrange="-200 200"/>
@@ -209,16 +239,4 @@
     <motor class="h1" name="right_shoulder_yaw" joint="right_shoulder_yaw" ctrlrange="-18 18"/>
     <motor class="h1" name="right_elbow" joint="right_elbow" ctrlrange="-18 18"/>
   </actuator>
-
-  <keyframe>
-    <key name="home"
-      qpos="
-    0 0 0.98
-    1 0 0 0
-    0 0 -0.4 0.8 -0.4
-    0 0 -0.4 0.8 -0.4
-    0
-    0 0 0 0
-    0 0 0 0"/>
-  </keyframe>
 </mujoco>
diff --git a/unitree_h1/scene.xml b/unitree_h1/scene.xml
@@ -20,4 +20,16 @@
     <light pos="0 0 3.5" dir="0 0 -1" directional="true"/>
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
   </worldbody>
+
+  <keyframe>
+    <key name="home"
+      qpos="
+    0 0 0.98
+    1 0 0 0
+    0 0 -0.4 0.8 -0.4
+    0 0 -0.4 0.8 -0.4
+    0
+    0 0 0 0
+    0 0 0 0"/>
+  </keyframe>
 </mujoco>