diff --git a/CHANGELOG.md b/CHANGELOG.md index 207ccffbdb..2e6ae3a8dc 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/newton/_src/solvers/mujoco/kernels.py b/newton/_src/solvers/mujoco/kernels.py index 61ba2a9e68..468147b787 100644 --- a/newton/_src/solvers/mujoco/kernels.py +++ b/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 diff --git a/newton/_src/solvers/mujoco/solver_mujoco.py b/newton/_src/solvers/mujoco/solver_mujoco.py index 5e6369b8de..d2a8ce963e 100644 --- a/newton/_src/solvers/mujoco/solver_mujoco.py +++ b/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, diff --git a/newton/tests/test_mujoco_solver.py b/newton/tests/test_mujoco_solver.py index 169ba76afb..7787d4c90b 100644 --- a/newton/tests/test_mujoco_solver.py +++ b/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)