Skip to content

Commit

Permalink
Remove margin of Aloha collision spheres, name them.
Browse files Browse the repository at this point in the history
PiperOrigin-RevId: 629125332
Change-Id: I7555c44f60f05878eae211a2fbab41ae8a29e8ca
  • Loading branch information
yuvaltassa authored and copybara-github committed Apr 29, 2024
1 parent cc3868e commit aff360a
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 38 deletions.
28 changes: 14 additions & 14 deletions aloha/aloha.xml
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,12 @@
</default>
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" mass="0" group="2" material="black"/>
<geom type="mesh" mass="0" group="2" material="black" contype="0" conaffinity="0"/>
</default>
<default class="collision">
<geom group="3" type="mesh" condim="6" friction="1 5e-3 5e-4" solref=".01 1"/>
<default class="sphere_collision">
<geom type="sphere" size="0.0006" rgba="1 0 0 1" margin=".015" gap=".015"/>
<geom type="sphere" size="0.0006" rgba="1 0 0 1"/>
</default>
</default>
</default>
Expand Down Expand Up @@ -158,9 +158,9 @@
mesh="vx300s_8_custom_finger_left"/>
<geom pos="0.0141637 0.0211727 0.06" class="collision" quat="1 1 1 -1" type="mesh"
mesh="vx300s_8_custom_finger_left"/>
<geom pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<geom name="left/left_g0" pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom name="left/left_g1" pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom name="left/left_g2" pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<site name="left/left_finger" pos="0.015 -0.06 0.02"/>
</body>
<body name="left/right_finger_link" pos="-0.0191 -0.0141637 0.0211727" quat="1 1 1 1">
Expand All @@ -171,9 +171,9 @@
mesh="vx300s_8_custom_finger_right"/>
<geom pos="0.0141637 -0.0211727 0.0597067" class="collision" quat="1 -1 -1 -1" type="mesh"
mesh="vx300s_8_custom_finger_right"/>
<geom pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<geom name="left/right_g0" pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom name="left/right_g1" pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom name="left/right_g2" pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<site name="left/right_finger" pos="0.015 0.06 0.02"/>
</body>
</body>
Expand Down Expand Up @@ -245,9 +245,9 @@
mesh="vx300s_8_custom_finger_left"/>
<geom pos="0.0141637 0.0211727 0.06" class="collision" quat="1 1 1 -1" type="mesh"
mesh="vx300s_8_custom_finger_left"/>
<geom pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<geom name="right/left_g0" pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom name="right/left_g1" pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom name="right/left_g2" pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<site name="right/left_finger" pos="0.015 -0.06 0.02"/>
</body>
<body name="right/right_finger_link" pos="-0.0191 -0.0141637 0.0211727" quat="1 1 1 1">
Expand All @@ -258,9 +258,9 @@
mesh="vx300s_8_custom_finger_right"/>
<geom pos="0.0141637 -0.0211727 0.0597067" class="collision" quat="1 -1 -1 -1" type="mesh"
mesh="vx300s_8_custom_finger_right"/>
<geom pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<geom name="right/right_g0" pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom name="right/right_g1" pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom name="right/right_g2" pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<site name="right/right_finger" pos="0.015 0.06 0.02"/>
</body>
</body>
Expand Down
44 changes: 20 additions & 24 deletions aloha/mjx_aloha.patch
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
--- aloha.xml 2024-04-19 17:27:25.000000000 +0100
+++ mjx_aloha.xml 2024-04-19 17:11:27.000000000 +0100
--- aloha.xml 2024-04-29 19:01:00.000000000 +0100
+++ mjx_aloha.xml 2024-04-29 18:58:32.000000000 +0100
@@ -1,7 +1,7 @@
<mujoco model="aloha">
<compiler angle="radian" meshdir="assets" autolimits="true"/>
Expand Down Expand Up @@ -41,12 +41,8 @@
<default class="left_finger">
<joint range="0 0.041" axis="0 0 -1"/>
</default>
@@ -81,12 +81,18 @@
</default>
</default>
<default class="visual">
- <geom type="mesh" contype="0" conaffinity="0" mass="0" group="2" material="black"/>
+ <geom type="mesh" mass="0" group="2" material="black" contype="0" conaffinity="0"/>
@@ -84,10 +84,16 @@
<geom type="mesh" mass="0" group="2" material="black" contype="0" conaffinity="0"/>
</default>
<default class="collision">
- <geom group="3" type="mesh" condim="6" friction="1 5e-3 5e-4" solref=".01 1"/>
Expand All @@ -55,14 +51,14 @@
+ <geom condim="3" solimp="2 1 0.01" solref="0.01 1" friction="1 0.005 0.0001"/>
+ </default>
<default class="sphere_collision">
- <geom type="sphere" size="0.0006" rgba="1 0 0 1" margin=".015" gap=".015"/>
+ <geom type="sphere" size="0.0006" rgba="1 0 0 1"/>
+ </default>
<geom type="sphere" size="0.0006" rgba="1 0 0 1"/>
</default>
+ <default class="primitive_collision">
+ <geom contype="0" conaffinity="1" rgba="1 0 0 0.5" group="3"/>
</default>
+ </default>
</default>
</default>
</default>
@@ -137,7 +143,7 @@
<inertial pos="0.0395662 -2.56311e-07 0.00400649" quat="0.62033 0.619916 -0.339682 0.339869"
mass="0.251652" diaginertia="0.000689546 0.000650316 0.000468142"/>
Expand All @@ -89,9 +85,9 @@
mesh="vx300s_8_custom_finger_left"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="-0.01 -0.0192 0.015 0.015 -0.0852 0.0228" class="primitive_collision"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="0.035 -0.0192 0.015 0.02 -0.0852 0.0228" class="primitive_collision"/>
<geom pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<geom name="left/left_g0" pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom name="left/left_g1" pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom name="left/left_g2" pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
@@ -169,8 +178,10 @@
<joint name="left/right_finger" class="right_finger"/>
<geom pos="0.0141637 -0.0211727 0.0597067" class="visual" quat="1 -1 -1 -1" type="mesh"
Expand All @@ -101,9 +97,9 @@
mesh="vx300s_8_custom_finger_right"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="-0.01 0.0192 0.015 0.015 0.0852 0.0228" class="primitive_collision"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="0.035 -0.0192 0.015 0.02 0.0852 0.0228" class="primitive_collision"/>
<geom pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<geom name="left/right_g0" pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom name="left/right_g1" pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom name="left/right_g2" pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
@@ -224,7 +235,7 @@
<inertial pos="0.0395662 -2.56311e-07 0.00400649" quat="0.62033 0.619916 -0.339682 0.339869"
mass="0.251652" diaginertia="0.000689546 0.000650316 0.000468142"/>
Expand All @@ -130,9 +126,9 @@
mesh="vx300s_8_custom_finger_left"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="-0.01 -0.0192 0.015 0.015 -0.0852 0.0228" class="primitive_collision"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="0.035 -0.0192 0.015 0.02 -0.0852 0.0228" class="primitive_collision"/>
<geom pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
<geom name="right/left_g0" pos="0.013 -0.0892 0.0268" class="sphere_collision"/>
<geom name="right/left_g1" pos="0.0222 -0.0892 0.0268" class="sphere_collision"/>
<geom name="right/left_g2" pos="0.0182 -0.0845 0.0266" class="sphere_collision"/>
@@ -256,8 +270,10 @@
<joint name="right/right_finger" class="right_finger"/>
<geom pos="0.0141637 -0.0211727 0.0597067" class="visual" quat="1 -1 -1 -1" type="mesh"
Expand All @@ -142,9 +138,9 @@
mesh="vx300s_8_custom_finger_right"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="-0.01 0.0192 0.015 0.015 0.0852 0.0228" class="primitive_collision"/>
+ <geom pos="0 0 0" type="capsule" size="0.005" fromto="0.035 -0.0192 0.015 0.02 0.0852 0.0228" class="primitive_collision"/>
<geom pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
<geom name="right/right_g0" pos="0.013 0.0892 0.0268" class="sphere_collision"/>
<geom name="right/right_g1" pos="0.0222 0.0892 0.0268" class="sphere_collision"/>
<geom name="right/right_g2" pos="0.0182 0.0845 0.0266" class="sphere_collision"/>
@@ -283,5 +299,5 @@
<joint joint1="right/left_finger" joint2="right/right_finger" polycoef="0 1 0 0 0"/>
</equality>
Expand Down

0 comments on commit aff360a

Please sign in to comment.