Fix contact visualization in mujoco collision pipeline

CHANGELOG.md

@@ -23,6 +23,7 @@
 - Fix body `gravcomp` not being written to the MuJoCo spec, causing it to be absent from XML saved via `save_to_mjcf`
 - Fix `eq_solimp` not being written to the MuJoCo spec for equality constraints, causing it to be absent from XML saved via `save_to_mjcf`
 - Fix WELD equality constraint quaternion written in xyzw format instead of MuJoCo's wxyz format in the spec, causing incorrect orientation in XML saved via `save_to_mjcf`
+- Fix `update_contacts` not populating `rigid_contact_point0`/`rigid_contact_point1` when using `use_mujoco_contacts=True`
 
 ## [1.0.0] - 2026-03-10
 

newton/_src/solvers/mujoco/kernels.py

@@ -680,12 +680,16 @@ def convert_mjw_contacts_to_newton_kernel(
     mjc_body_to_newton: wp.array(dtype=wp.int32, ndim=2),
     pyramidal_cone: bool,
     mj_nacon: wp.array(dtype=wp.int32),
+    mj_contact_pos: wp.array(dtype=wp.vec3),
     mj_contact_frame: wp.array(dtype=wp.mat33f),
     mj_contact_dim: wp.array(dtype=int),
     mj_contact_geom: wp.array(dtype=wp.vec2i),
     mj_contact_efc_address: wp.array2d(dtype=int),
     mj_contact_worldid: wp.array(dtype=wp.int32),
     mj_efc_force: wp.array2d(dtype=float),
+    mj_geom_bodyid: wp.array(dtype=int),
+    mj_xpos: wp.array2d(dtype=wp.vec3),
+    mj_xquat: wp.array2d(dtype=wp.quatf),
     # outputs
     rigid_contact_count: wp.array(dtype=wp.int32),
     rigid_contact_shape0: wp.array(dtype=wp.int32),
@@ -698,6 +702,7 @@ def convert_mjw_contacts_to_newton_kernel(
     """Convert MuJoCo contacts to Newton contact format.
 
     Uses mjc_geom_to_newton_shape to convert MuJoCo geom indices to Newton shape indices.
+    Contact positions are converted from MuJoCo world frame to Newton body-local frame.
     """
     contact_idx = wp.tid()
     n_contacts = mj_nacon[0]
@@ -712,11 +717,32 @@ def convert_mjw_contacts_to_newton_kernel(
     geoms_mjw = mj_contact_geom[contact_idx]
 
     normal = mj_contact_frame[contact_idx][0]
+    pos_world = mj_contact_pos[contact_idx]
 
     rigid_contact_shape0[contact_idx] = mjc_geom_to_newton_shape[world, geoms_mjw[0]]
     rigid_contact_shape1[contact_idx] = mjc_geom_to_newton_shape[world, geoms_mjw[1]]
     rigid_contact_normal[contact_idx] = -normal
 
+    # Convert contact position from world frame to body-local frame for each shape.
+    # MuJoCo contact.pos is the midpoint in world frame; we transform it into each
+    # body's local frame to match Newton's convention (see collide.py write_contact).
+    body_a = mj_geom_bodyid[geoms_mjw[0]]
+    body_b = mj_geom_bodyid[geoms_mjw[1]]
+
+    X_wb_a = wp.transform_identity()
+    X_wb_b = wp.transform_identity()
+    if body_a > 0:
+        X_wb_a = wp.transform(mj_xpos[world, body_a], quat_wxyz_to_xyzw(mj_xquat[world, body_a]))
+    if body_b > 0:
+        X_wb_b = wp.transform(mj_xpos[world, body_b], quat_wxyz_to_xyzw(mj_xquat[world, body_b]))
+
+    dist = mj_contact_dist[contact_idx]
+    point0_world = pos_world - 0.5 * dist * normal
+    point1_world = pos_world + 0.5 * dist * normal
+
+    rigid_contact_point0[contact_idx] = wp.transform_point(wp.transform_inverse(X_wb_a), point0_world)
+    rigid_contact_point1[contact_idx] = wp.transform_point(wp.transform_inverse(X_wb_b), point1_world)
+
     if contact_force:
         efc_address0 = mj_contact_efc_address[contact_idx, 0]
         has_force = efc_address0 >= 0

newton/_src/solvers/mujoco/solver_mujoco.py

@@ -3548,12 +3548,16 @@ def update_contacts(self, contacts: Contacts, state: State | None = None) -> Non
                 self.mjc_body_to_newton,
                 self.mjw_model.opt.cone == int(self._mujoco.mjtCone.mjCONE_PYRAMIDAL),
                 mj_data.nacon,
+                mj_contact.pos,
                 mj_contact.frame,
                 mj_contact.dim,
                 mj_contact.geom,
                 mj_contact.efc_address,
                 mj_contact.worldid,
                 mj_data.efc.force,
+                self.mjw_model.geom_bodyid,
+                mj_data.xpos,
+                mj_data.xquat,
             ],
             outputs=[
                 contacts.rigid_contact_count,

newton/tests/test_mujoco_solver.py

@@ -8097,5 +8097,71 @@ def test_weld_constraint_quat_spec_conversion(self):
             os.unlink(xml_path)
 
 
+class TestUpdateContactsPointPositions(unittest.TestCase):
+    """Test that update_contacts populates rigid_contact_point0/point1."""
+
+    def test_contact_points_populated(self):
+        """Drop a box onto a ground plane with use_mujoco_contacts and verify contact points are nonzero."""
+        builder = newton.ModelBuilder()
+        SolverMuJoCo.register_custom_attributes(builder)
+
+        builder.add_ground_plane()
+        body = builder.add_body(
+            xform=wp.transform(p=wp.vec3(0.0, 0.0, 1.0), q=wp.quat_identity()),
+        )
+        builder.add_shape_box(body, hx=0.25, hy=0.25, hz=0.25)
+        model = builder.finalize()
+
+        solver = SolverMuJoCo(
+            model,
+            solver="newton",
+            integrator="implicitfast",
+            iterations=10,
+            ls_iterations=20,
+            use_mujoco_contacts=True,
+        )
+
+        state_0 = model.state()
+        state_1 = model.state()
+        control = model.control()
+        contacts = newton.Contacts(
+            rigid_contact_max=solver.mjw_data.naconmax,
+            soft_contact_max=0,
+            device=model.device,
+        )
+        newton.eval_fk(model, model.joint_q, model.joint_qd, state_0)
+
+        dt = 1.0 / 200.0
+        found_contacts = False
+        for _ in range(200):
+            state_0.clear_forces()
+            solver.step(state_0, state_1, control, contacts, dt)
+            solver.update_contacts(contacts, state_0)
+            state_0, state_1 = state_1, state_0
+
+            n = contacts.rigid_contact_count.numpy()[0]
+            if n > 0:
+                found_contacts = True
+                point0 = contacts.rigid_contact_point0.numpy()[:n]
+                point1 = contacts.rigid_contact_point1.numpy()[:n]
+
+                hx, hy, hz = 0.25, 0.25, 0.25
+                for i in range(n):
+                    # point0 is on the ground plane (body-local = world for static body).
+                    # z should be near 0 (ground surface), x/y within box footprint.
+                    self.assertAlmostEqual(point0[i][2], 0.0, delta=0.02)
+                    self.assertLessEqual(abs(float(point0[i][0])), hx + 0.01)
+                    self.assertLessEqual(abs(float(point0[i][1])), hy + 0.01)
+
+                    # point1 is in box body frame.
+                    # z should be near -hz (bottom face), x/y within box half-extents.
+                    self.assertAlmostEqual(point1[i][2], -hz, delta=0.02)
+                    self.assertLessEqual(abs(float(point1[i][0])), hx + 0.01)
+                    self.assertLessEqual(abs(float(point1[i][1])), hy + 0.01)
+                break
+
+        self.assertTrue(found_contacts, "No contacts detected after 200 steps")
+
+
 if __name__ == "__main__":
     unittest.main(verbosity=2)