Merge branch 'fix-featherstone-soft-contact' into 'main'

Fix soft contact handling in FeatherstoneIntegrator

See merge request omniverse/warp!702

CHANGELOG.md

@@ -54,6 +54,7 @@
 - Fix a bug with autograd array access validation in functions from different modules.
 - Fix a rare crash during error reporting on some systems due to glibc mismatches.
 - Handle `--num_tiles 1` in `example_render_opengl.py` ([GH-306](https://github.com/NVIDIA/warp/issues/306)).
+- Fix the computation of body contact forces in `FeatherstoneIntegrator` when bodies and particles collide.
 - Fix bug in `FeatherstoneIntegrator` where `eval_rigid_jacobian` could give incorrect results or reach an infinite
   loop when the body and joint indices were not in the same order. Added `Model.joint_ancestor` to fix the indexing
   from a joint to its parent joint in the articulation.

warp/sim/integrator_euler.py

@@ -761,6 +761,7 @@ def eval_particle_contacts(
     contact_body_vel: wp.array(dtype=wp.vec3),
     contact_normal: wp.array(dtype=wp.vec3),
     contact_max: int,
+    body_f_in_world_frame: bool,
     # outputs
     particle_f: wp.array(dtype=wp.vec3),
     body_f: wp.array(dtype=wp.spatial_vector),
@@ -809,7 +810,11 @@ def eval_particle_contacts(
     body_v = wp.spatial_bottom(body_v_s)
 
     # compute the body velocity at the particle position
-    bv = body_v + wp.cross(body_w, r) + wp.transform_vector(X_wb, contact_body_vel[tid])
+    bv = body_v + wp.transform_vector(X_wb, contact_body_vel[tid])
+    if body_f_in_world_frame:
+        bv += wp.cross(body_w, bx)
+    else:
+        bv += wp.cross(body_w, r)
 
     # relative velocity
     v = pv - bv
@@ -840,12 +845,14 @@ def eval_particle_contacts(
     ft = wp.normalize(vt) * wp.min(kf * wp.length(vt), abs(mu * c * ke))
 
     f_total = fn + (fd + ft)
-    t_total = wp.cross(r, f_total)
 
     wp.atomic_sub(particle_f, particle_index, f_total)
 
     if body_index >= 0:
-        wp.atomic_add(body_f, body_index, wp.spatial_vector(t_total, f_total))
+        if body_f_in_world_frame:
+            wp.atomic_sub(body_f, body_index, wp.spatial_vector(wp.cross(bx, f_total), f_total))
+        else:
+            wp.atomic_add(body_f, body_index, wp.spatial_vector(wp.cross(r, f_total), f_total))
 
 
 @wp.kernel
@@ -1814,7 +1821,9 @@ def eval_body_joint_forces(model: Model, state: State, control: Control, body_f:
         )
 
 
-def eval_particle_body_contact_forces(model: Model, state: State, particle_f: wp.array, body_f: wp.array):
+def eval_particle_body_contact_forces(
+    model: Model, state: State, particle_f: wp.array, body_f: wp.array, body_f_in_world_frame: bool = False
+):
     if model.particle_count and model.shape_count > 1:
         wp.launch(
             kernel=eval_particle_contacts,
@@ -1841,6 +1850,7 @@ def eval_particle_body_contact_forces(model: Model, state: State, particle_f: wp
                 model.soft_contact_body_vel,
                 model.soft_contact_normal,
                 model.soft_contact_max,
+                body_f_in_world_frame,
             ],
             # outputs
             outputs=[particle_f, body_f],
@@ -1897,7 +1907,7 @@ def compute_forces(model: Model, state: State, control: Control, particle_f: wp.
     eval_body_contact_forces(model, state, particle_f)
 
     # particle shape contact
-    eval_particle_body_contact_forces(model, state, particle_f, body_f)
+    eval_particle_body_contact_forces(model, state, particle_f, body_f, body_f_in_world_frame=False)
 
     # muscles
     if False:

warp/sim/integrator_featherstone.py

@@ -592,7 +592,7 @@ def jcalc_integrate(
         p_s = wp.vec3(joint_q[coord_start + 0], joint_q[coord_start + 1], joint_q[coord_start + 2])
 
         # linear vel of origin (note q/qd switch order of linear angular elements)
-        # note we are converting the body twist in the space frame (w_s, v_s) to compute center of mass velcity
+        # note we are converting the body twist in the space frame (w_s, v_s) to compute center of mass velocity
         dpdt_s = v_s + wp.cross(w_s, p_s)
 
         # quat and quat derivative
@@ -1621,7 +1621,7 @@ def simulate(self, model: Model, state_in: State, state_out: State, dt: float, c
             eval_particle_ground_contact_forces(model, state_in, particle_f)
 
             # particle shape contact
-            eval_particle_body_contact_forces(model, state_in, particle_f, body_f)
+            eval_particle_body_contact_forces(model, state_in, particle_f, body_f, body_f_in_world_frame=True)
 
             # muscles
             if False: