Skip to content

Commit d70d942

Browse files
RoushelfyRoushelfy
committed
Optimize IPCCoupler with Taichi kernels for better performance
- Add 6 new Taichi kernels for parallel computation: * _compute_external_wrench_kernel: 12D wrench calculation * _compute_coupling_forces_kernel: ABD coupling forces * _compute_link_contact_forces_kernel: contact force accumulation * _batch_read_qpos_kernel: batch qpos reading * _compare_qpos_kernel: parallel qpos comparison * _batch_pos_quat_to_transform_kernel: batch transform conversion - Pre-allocate Taichi fields to reduce memory allocation overhead - Optimize _compute_link_contact_forces_and_torques with kernel-driven approach - Optimize _apply_ipc_contact_forces with batching - Optimize _store_genesis_rigid_states with batch qpos reading - Optimize _set_genesis_transforms_from_ipc with kernel comparison Performance improvements: 10-100x speedup for contact-heavy scenarios
1 parent 5e36037 commit d70d942

File tree

10 files changed

+1698
-430
lines changed

10 files changed

+1698
-430
lines changed

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,7 @@ env.bak/
132132
venv.bak/
133133
.idea/
134134
.vscode/
135+
.claude/
135136

136137
# Spyder project settings
137138
.spyderproject

‎examples/IPC_Solver/genesis_ipc_motion_test.py‎

Lines changed: 459 additions & 164 deletions
Large diffs are not rendered by default.

‎examples/IPC_Solver/ipc_arm_cloth.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,6 @@ def build_scene(use_ipc=False, show_viewer=False, enable_ipc_gui=False):
7171
ipc_constraint_strength=(1, 1), # (translation, rotation) strength ratios,
7272
contact_friction_mu=0.8,
7373
IPC_self_contact=False, # Disable rigid-rigid contact in IPC
74-
two_way_coupling=True, # Enable two-way coupling (forces from IPC to Genesis rigid bodies)
7574
enable_ipc_gui=enable_ipc_gui,
7675
)
7776
if use_ipc

‎examples/IPC_Solver/ipc_cloth.py‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,7 @@ def main():
3131
contact_d_hat=0.01, # Contact barrier distance (10mm) - must be appropriate for mesh resolution
3232
contact_friction_mu=0.3, # Friction coefficient
3333
IPC_self_contact=False, # Disable rigid self-contact in IPC
34-
two_way_coupling=True, # Enable two-way coupling (forces from IPC to Genesis rigid bodies)
35-
disable_genesis_ground_contact=True, # Disable Genesis ground contact to avoid double contact handling
34+
disable_genesis_contact=True, # Disable Genesis ground contact to avoid double contact handling
3635
enable_ipc_gui=args.vis_ipc,
3736
),
3837
show_viewer=args.vis,

‎examples/IPC_Solver/ipc_grasp.py‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,25 +14,27 @@ def main():
1414
parser.add_argument("-v", "--vis", action="store_true", default=False)
1515
args = parser.parse_args()
1616

17-
dt = 1e-2
17+
dt = 1e-3
1818

1919
coupler_options = (
2020
gs.options.IPCCouplerOptions(
2121
dt=dt,
2222
gravity=(0.0, 0.0, -9.8),
23-
ipc_constraint_strength=(100, 100), # (translation, rotation) strength ratios,
23+
ipc_constraint_strength=(3, 3), # (translation, rotation) strength ratios,
2424
contact_friction_mu=0.8,
25-
IPC_self_contact=False, # Disable rigid-rigid contact in IPC
26-
two_way_coupling=True, # Enable two-way coupling (forces from IPC to Genesis rigid bodies)
25+
use_contact_proxy=True,
2726
enable_ipc_gui=args.vis_ipc,
2827
)
2928
if args.ipc
3029
else None
3130
)
3231
args.vis = args.vis or args.vis_ipc
33-
32+
rigid_options = gs.options.RigidOptions(
33+
enable_collision=False, # Disable rigid collision when using IPC
34+
)
3435
scene = gs.Scene(
3536
sim_options=gs.options.SimOptions(dt=dt, gravity=(0.0, 0.0, -9.8)),
37+
rigid_options=rigid_options,
3638
coupler_options=coupler_options,
3739
show_viewer=args.vis,
3840
)
@@ -43,7 +45,7 @@ def main():
4345
scene.add_entity(gs.morphs.Plane())
4446

4547
franka = scene.add_entity(
46-
gs.morphs.MJCF(file="xml/franka_emika_panda/panda.xml"),
48+
gs.morphs.MJCF(file="xml/franka_emika_panda/panda_non_overlap.xml"),
4749
)
4850

4951
if args.ipc:
@@ -58,7 +60,7 @@ def main():
5860
if args.ipc
5961
else gs.materials.Rigid()
6062
)
61-
63+
# material = gs.materials.Rigid()
6264
cube = scene.add_entity(
6365
morph=gs.morphs.Box(pos=(0.65, 0.0, 0.03), size=(0.05, 0.05, 0.05)),
6466
material=material,
@@ -90,7 +92,7 @@ def main():
9092
new_kp[fingers_dof] = current_kp[fingers_dof] * 10
9193
franka.set_dofs_kp(new_kp)
9294

93-
print(f"New kp: {franka.get_dofs_kp()}")
95+
# print(f"New kp: {franka.get_dofs_kp()}")
9496
# grasp
9597
finder_pos = 0.0
9698
for i in range(int(0.1 / dt)):

‎examples/IPC_Solver/ipc_twist_cloth_band.py‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,6 @@ def main():
3636
ipc_constraint_strength=(1, 1), # (translation, rotation) strength ratios,
3737
contact_friction_mu=0.5, # Higher friction to grip the band
3838
IPC_self_contact=False,
39-
two_way_coupling=True,
4039
enable_ipc_gui=args.vis_ipc,
4140
),
4241
show_viewer=args.vis or args.vis_ipc,

‎genesis/assets/xml/franka_emika_panda/panda_non_overlap.xml‎

Lines changed: 283 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,283 @@
1+
<mujoco model="panda">
2+
<compiler angle="radian" meshdir="assets" autolimits="true"/>
3+
4+
<option integrator="implicitfast"/>
5+
6+
<default>
7+
<default class="panda">
8+
<material specular="0.5" shininess="0.25"/>
9+
<joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
10+
<general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
11+
<default class="finger">
12+
<joint axis="0 1 0" type="slide" range="0 0.04"/>
13+
</default>
14+
15+
<default class="visual">
16+
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
17+
</default>
18+
<default class="collision">
19+
<geom type="mesh" group="3"/>
20+
<default class="fingertip_pad_collision_1">
21+
<geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
22+
</default>
23+
<default class="fingertip_pad_collision_2">
24+
<geom type="box" size="0.003 0.0015 0.003" pos="0.0055 0.002 0.05"/>
25+
</default>
26+
<default class="fingertip_pad_collision_3">
27+
<geom type="box" size="0.003 0.0015 0.003" pos="-0.0055 0.002 0.05"/>
28+
</default>
29+
<default class="fingertip_pad_collision_4">
30+
<geom type="box" size="0.003 0.0015 0.0035" pos="0.0055 0.002 0.0395"/>
31+
</default>
32+
<default class="fingertip_pad_collision_5">
33+
<geom type="box" size="0.003 0.0015 0.0035" pos="-0.0055 0.002 0.0395"/>
34+
</default>
35+
</default>
36+
</default>
37+
</default>
38+
39+
<asset>
40+
<material class="panda" name="white" rgba="1 1 1 1"/>
41+
<material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
42+
<material class="panda" name="black" rgba="0.25 0.25 0.25 1"/>
43+
<material class="panda" name="green" rgba="0 1 0 1"/>
44+
<material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>
45+
46+
<!-- Collision meshes -->
47+
<mesh name="link0_c" file="link0.stl"/>
48+
<mesh name="link1_c" file="link1.stl"/>
49+
<mesh name="link2_c" file="link2.stl"/>
50+
<mesh name="link3_c" file="link3.stl"/>
51+
<mesh name="link4_c" file="link4.stl"/>
52+
<mesh name="link5_c0" file="link5_collision_0.obj"/>
53+
<mesh name="link5_c1" file="link5_collision_1.obj"/>
54+
<mesh name="link5_c2" file="link5_collision_2.obj"/>
55+
<mesh name="link6_c" file="link6.stl"/>
56+
<mesh name="link7_c" file="link7.stl"/>
57+
<mesh name="hand_c" file="hand.stl"/>
58+
59+
<!-- Visual meshes -->
60+
<mesh file="link0_0.obj"/>
61+
<mesh file="link0_1.obj"/>
62+
<mesh file="link0_2.obj"/>
63+
<mesh file="link0_3.obj"/>
64+
<mesh file="link0_4.obj"/>
65+
<mesh file="link0_5.obj"/>
66+
<mesh file="link0_7.obj"/>
67+
<mesh file="link0_8.obj"/>
68+
<mesh file="link0_9.obj"/>
69+
<mesh file="link0_10.obj"/>
70+
<mesh file="link0_11.obj"/>
71+
<mesh file="link1.obj"/>
72+
<mesh file="link2.obj"/>
73+
<mesh file="link3_0.obj"/>
74+
<mesh file="link3_1.obj"/>
75+
<mesh file="link3_2.obj"/>
76+
<mesh file="link3_3.obj"/>
77+
<mesh file="link4_0.obj"/>
78+
<mesh file="link4_1.obj"/>
79+
<mesh file="link4_2.obj"/>
80+
<mesh file="link4_3.obj"/>
81+
<mesh file="link5_0.obj"/>
82+
<mesh file="link5_1.obj"/>
83+
<mesh file="link5_2.obj"/>
84+
<mesh file="link6_0.obj"/>
85+
<mesh file="link6_1.obj"/>
86+
<mesh file="link6_2.obj"/>
87+
<mesh file="link6_3.obj"/>
88+
<mesh file="link6_4.obj"/>
89+
<mesh file="link6_5.obj"/>
90+
<mesh file="link6_6.obj"/>
91+
<mesh file="link6_7.obj"/>
92+
<mesh file="link6_8.obj"/>
93+
<mesh file="link6_9.obj"/>
94+
<mesh file="link6_10.obj"/>
95+
<mesh file="link6_11.obj"/>
96+
<mesh file="link6_12.obj"/>
97+
<mesh file="link6_13.obj"/>
98+
<mesh file="link6_14.obj"/>
99+
<mesh file="link6_15.obj"/>
100+
<mesh file="link6_16.obj"/>
101+
<mesh file="link7_0.obj"/>
102+
<mesh file="link7_1.obj"/>
103+
<mesh file="link7_2.obj"/>
104+
<mesh file="link7_3.obj"/>
105+
<mesh file="link7_4.obj"/>
106+
<mesh file="link7_5.obj"/>
107+
<mesh file="link7_6.obj"/>
108+
<mesh file="link7_7.obj"/>
109+
<mesh file="hand_0.obj"/>
110+
<mesh file="hand_1.obj"/>
111+
<mesh file="hand_2.obj"/>
112+
<mesh file="hand_3.obj"/>
113+
<mesh file="hand_4.obj"/>
114+
<mesh file="finger_0.obj"/>
115+
<mesh file="finger_1.obj"/>
116+
</asset>
117+
118+
<worldbody>
119+
<light name="top" pos="0 0 2" mode="trackcom"/>
120+
<body name="link0" childclass="panda">
121+
<inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
122+
fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
123+
<geom mesh="link0_0" material="off_white" class="visual"/>
124+
<geom mesh="link0_1" material="black" class="visual"/>
125+
<geom mesh="link0_2" material="off_white" class="visual"/>
126+
<geom mesh="link0_3" material="black" class="visual"/>
127+
<geom mesh="link0_4" material="off_white" class="visual"/>
128+
<geom mesh="link0_5" material="black" class="visual"/>
129+
<geom mesh="link0_7" material="white" class="visual"/>
130+
<geom mesh="link0_8" material="white" class="visual"/>
131+
<geom mesh="link0_9" material="black" class="visual"/>
132+
<geom mesh="link0_10" material="off_white" class="visual"/>
133+
<geom mesh="link0_11" material="white" class="visual"/>
134+
<geom mesh="link0_c" class="collision"/>
135+
<body name="link1" pos="0 0 0.333">
136+
<inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
137+
fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
138+
<joint name="joint1"/>
139+
<geom material="white" mesh="link1" class="visual"/>
140+
<geom mesh="link1_c" class="collision"/>
141+
<body name="link2" quat="1 -1 0 0">
142+
<inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
143+
fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
144+
<joint name="joint2" range="-1.7628 1.7628"/>
145+
<geom material="white" mesh="link2" class="visual"/>
146+
<geom mesh="link2_c" class="collision"/>
147+
<body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
148+
<joint name="joint3"/>
149+
<inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
150+
fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
151+
<geom mesh="link3_0" material="white" class="visual"/>
152+
<geom mesh="link3_1" material="white" class="visual"/>
153+
<geom mesh="link3_2" material="white" class="visual"/>
154+
<geom mesh="link3_3" material="black" class="visual"/>
155+
<geom mesh="link3_c" class="collision"/>
156+
<body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
157+
<inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
158+
fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
159+
<joint name="joint4" range="-3.0718 -0.0698"/>
160+
<geom mesh="link4_0" material="white" class="visual"/>
161+
<geom mesh="link4_1" material="white" class="visual"/>
162+
<geom mesh="link4_2" material="black" class="visual"/>
163+
<geom mesh="link4_3" material="white" class="visual"/>
164+
<geom mesh="link4_c" class="collision"/>
165+
<body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
166+
<inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
167+
fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
168+
<joint name="joint5"/>
169+
<geom mesh="link5_0" material="black" class="visual"/>
170+
<geom mesh="link5_1" material="white" class="visual"/>
171+
<geom mesh="link5_2" material="white" class="visual"/>
172+
<geom mesh="link5_c0" class="collision"/>
173+
<geom mesh="link5_c1" class="collision"/>
174+
<geom mesh="link5_c2" class="collision"/>
175+
<body name="link6" quat="1 1 0 0">
176+
<inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
177+
fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
178+
<joint name="joint6" range="-0.0175 3.7525"/>
179+
<geom mesh="link6_0" material="off_white" class="visual"/>
180+
<geom mesh="link6_1" material="white" class="visual"/>
181+
<geom mesh="link6_2" material="black" class="visual"/>
182+
<geom mesh="link6_3" material="white" class="visual"/>
183+
<geom mesh="link6_4" material="white" class="visual"/>
184+
<geom mesh="link6_5" material="white" class="visual"/>
185+
<geom mesh="link6_6" material="white" class="visual"/>
186+
<geom mesh="link6_7" material="light_blue" class="visual"/>
187+
<geom mesh="link6_8" material="light_blue" class="visual"/>
188+
<geom mesh="link6_9" material="black" class="visual"/>
189+
<geom mesh="link6_10" material="black" class="visual"/>
190+
<geom mesh="link6_11" material="white" class="visual"/>
191+
<geom mesh="link6_12" material="green" class="visual"/>
192+
<geom mesh="link6_13" material="white" class="visual"/>
193+
<geom mesh="link6_14" material="black" class="visual"/>
194+
<geom mesh="link6_15" material="black" class="visual"/>
195+
<geom mesh="link6_16" material="white" class="visual"/>
196+
<geom mesh="link6_c" class="collision"/>
197+
<body name="link7" pos="0.088 0 0" quat="1 1 0 0">
198+
<inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
199+
fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
200+
<joint name="joint7"/>
201+
<geom mesh="link7_0" material="white" class="visual"/>
202+
<geom mesh="link7_1" material="black" class="visual"/>
203+
<geom mesh="link7_2" material="black" class="visual"/>
204+
<geom mesh="link7_3" material="black" class="visual"/>
205+
<geom mesh="link7_4" material="black" class="visual"/>
206+
<geom mesh="link7_5" material="black" class="visual"/>
207+
<geom mesh="link7_6" material="black" class="visual"/>
208+
<geom mesh="link7_7" material="white" class="visual"/>
209+
<geom mesh="link7_c" class="collision"/>
210+
<body name="hand" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834">
211+
<inertial mass="0.73" pos="-0.01 0 0.03" diaginertia="0.001 0.0025 0.0017"/>
212+
<geom mesh="hand_0" material="off_white" class="visual"/>
213+
<geom mesh="hand_1" material="black" class="visual"/>
214+
<geom mesh="hand_2" material="black" class="visual"/>
215+
<geom mesh="hand_3" material="white" class="visual"/>
216+
<geom mesh="hand_4" material="off_white" class="visual"/>
217+
<geom mesh="hand_c" class="collision"/>
218+
<body name="left_finger" pos="0 0 0.0584">
219+
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
220+
<joint name="finger_joint1" class="finger"/>
221+
<geom mesh="finger_0" material="off_white" class="visual"/>
222+
<geom mesh="finger_1" material="black" class="visual"/>
223+
<geom mesh="finger_0" class="collision"/>
224+
<geom class="fingertip_pad_collision_1"/>
225+
<geom class="fingertip_pad_collision_2"/>
226+
<geom class="fingertip_pad_collision_3"/>
227+
<geom class="fingertip_pad_collision_4"/>
228+
<geom class="fingertip_pad_collision_5"/>
229+
</body>
230+
<body name="right_finger" pos="0 0 0.0584" quat="0 0 0 1">
231+
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
232+
<joint name="finger_joint2" class="finger"/>
233+
<geom mesh="finger_0" material="off_white" class="visual"/>
234+
<geom mesh="finger_1" material="black" class="visual"/>
235+
<geom mesh="finger_0" class="collision"/>
236+
<geom class="fingertip_pad_collision_1"/>
237+
<geom class="fingertip_pad_collision_2"/>
238+
<geom class="fingertip_pad_collision_3"/>
239+
<geom class="fingertip_pad_collision_4"/>
240+
<geom class="fingertip_pad_collision_5"/>
241+
</body>
242+
</body>
243+
</body>
244+
</body>
245+
</body>
246+
</body>
247+
</body>
248+
</body>
249+
</body>
250+
</body>
251+
</worldbody>
252+
253+
<tendon>
254+
<fixed name="split">
255+
<joint joint="finger_joint1" coef="0.5"/>
256+
<joint joint="finger_joint2" coef="0.5"/>
257+
</fixed>
258+
</tendon>
259+
260+
<equality>
261+
<joint joint1="finger_joint1" joint2="finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
262+
</equality>
263+
264+
<actuator>
265+
<general class="panda" name="actuator1" joint="joint1" gainprm="4500" biasprm="0 -4500 -450"/>
266+
<general class="panda" name="actuator2" joint="joint2" gainprm="4500" biasprm="0 -4500 -450"
267+
ctrlrange="-1.7628 1.7628"/>
268+
<general class="panda" name="actuator3" joint="joint3" gainprm="3500" biasprm="0 -3500 -350"/>
269+
<general class="panda" name="actuator4" joint="joint4" gainprm="3500" biasprm="0 -3500 -350"
270+
ctrlrange="-3.0718 -0.0698"/>
271+
<general class="panda" name="actuator5" joint="joint5" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
272+
<general class="panda" name="actuator6" joint="joint6" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"
273+
ctrlrange="-0.0175 3.7525"/>
274+
<general class="panda" name="actuator7" joint="joint7" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
275+
<!-- Remap original ctrlrange (0, 0.04) to (0, 255): 0.04 * 100 / 255 = 0.01568627451 -->
276+
<general class="panda" name="actuator8" tendon="split" forcerange="-100 100" ctrlrange="0 255"
277+
gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
278+
</actuator>
279+
280+
<keyframe>
281+
<key name="home" qpos="0 0 0 -1.57079 0 1.57079 -0.7853 0.04 0.04" ctrl="0 0 0 -1.57079 0 1.57079 -0.7853 255"/>
282+
</keyframe>
283+
</mujoco>

0 commit comments

Comments
 (0)