diff --git a/mujoco_warp/_src/collision_convex.py b/mujoco_warp/_src/collision_convex.py index 8727a93db..6d730a968 100644 --- a/mujoco_warp/_src/collision_convex.py +++ b/mujoco_warp/_src/collision_convex.py @@ -35,6 +35,7 @@ from mujoco_warp._src.types import MJ_MAX_EPAHORIZON from mujoco_warp._src.types import MJ_MAXCONPAIR from mujoco_warp._src.types import MJ_MAXVAL +from mujoco_warp._src.types import NEW_GAP_SEMANTICS from mujoco_warp._src.types import Data from mujoco_warp._src.types import DisableBit from mujoco_warp._src.types import GeomType @@ -773,7 +774,10 @@ def eval_ccd_write_contact( if is_collision_sensor: cutoff = 1.0e32 else: - cutoff = 0.0 + if wp.static(NEW_GAP_SEMANTICS): + cutoff = gap + else: + cutoff = 0.0 dist, ncollision, w1, w2, multiccd_idx = ccd( opt_ccd_tolerance[worldid % opt_ccd_tolerance.shape[0]], cutoff, @@ -793,8 +797,12 @@ def eval_ccd_write_contact( epa_horizon_in[ccdid], ) - if dist >= 0.0 and pairid[1] == -1: - return 0 + if wp.static(NEW_GAP_SEMANTICS): + if dist >= gap and pairid[1] == -1: + return 0 + else: + if dist >= 0.0 and pairid[1] == -1: + return 0 # CCD operates on margin-inflated shapes (support() inflates each geom by # 0.5 * margin). The returned dist is therefore relative to the inflated diff --git a/mujoco_warp/_src/collision_core.py b/mujoco_warp/_src/collision_core.py index 0d9e41364..9210dff52 100644 --- a/mujoco_warp/_src/collision_core.py +++ b/mujoco_warp/_src/collision_core.py @@ -23,6 +23,7 @@ from mujoco_warp._src.math import safe_div from mujoco_warp._src.types import MJ_MINMU from mujoco_warp._src.types import MJ_MINVAL +from mujoco_warp._src.types import NEW_GAP_SEMANTICS from mujoco_warp._src.types import ContactType from mujoco_warp._src.types import GeomType from mujoco_warp._src.types import mat63 @@ -197,14 +198,15 @@ def write_contact( Returns 1 if the contact is active (dist < margin), 0 otherwise. """ active = dist_in < margin_in + detected = dist_in < margin_in + gap_in # skip contact and no collision sensor - if (pairid_in[0] == -2 or not active) and pairid_in[1] == -1: + if (pairid_in[0] == -2 or not detected) and pairid_in[1] == -1: return 0 contact_type = 0 - if pairid_in[0] >= -1 and active: + if pairid_in[0] >= -1 and detected: contact_type |= ContactType.CONSTRAINT if pairid_in[1] >= 0: @@ -217,7 +219,10 @@ def write_contact( contact_frame_out[cid] = frame_in contact_geom_out[cid] = geoms_in contact_worldid_out[cid] = worldid_in - includemargin = margin_in - gap_in + if wp.static(NEW_GAP_SEMANTICS): + includemargin = margin_in + else: + includemargin = margin_in - gap_in contact_includemargin_out[cid] = includemargin contact_dim_out[cid] = condim_in contact_friction_out[cid] = friction_in diff --git a/mujoco_warp/_src/collision_driver.py b/mujoco_warp/_src/collision_driver.py index de6c57253..db0f123a8 100644 --- a/mujoco_warp/_src/collision_driver.py +++ b/mujoco_warp/_src/collision_driver.py @@ -271,13 +271,14 @@ def _obb_filter( return True -def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.func def func( # Model: geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], geom_xmat_in: wp.array2d[wp.mat33], @@ -299,21 +300,25 @@ def func( rbound1, rbound2 = geom_rbound[rbound_id, geom1], geom_rbound[rbound_id, geom2] # kernel_analyzer: ignore margin_id = worldid % ngeom_margin if wp.static(ngeom_margin > 1) else 0 margin1, margin2 = geom_margin[margin_id, geom1], geom_margin[margin_id, geom2] # kernel_analyzer: ignore + gap_id = worldid % ngeom_gap if wp.static(ngeom_gap > 1) else 0 + gap1, gap2 = geom_gap[gap_id, geom1], geom_gap[gap_id, geom2] # kernel_analyzer: ignore + effective_margin1 = margin1 + gap1 + effective_margin2 = margin2 + gap2 xpos1, xpos2 = geom_xpos_in[worldid, geom1], geom_xpos_in[worldid, geom2] xmat1, xmat2 = geom_xmat_in[worldid, geom1], geom_xmat_in[worldid, geom2] if rbound1 == 0.0 or rbound2 == 0.0: if wp.static(opt_broadphase_filter & BroadphaseFilter.PLANE): - return _plane_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2, xmat1, xmat2) + return _plane_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2) else: if wp.static(opt_broadphase_filter & BroadphaseFilter.SPHERE): - if not _sphere_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2): + if not _sphere_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.AABB): - if not _aabb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _aabb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.OBB): - if not _obb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _obb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False return True @@ -378,6 +383,7 @@ def sap_project( ngeom: int, geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], nworld_in: int, @@ -398,7 +404,7 @@ def sap_project( # geom is a plane rbound = MJ_MAXVAL - radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + geom_gap[worldid % geom_gap.shape[0], geomid] center = wp.dot(direction_in, xpos) sort_index_out[worldid, geomid] = geomid @@ -444,7 +450,7 @@ def _sap_range( @cache_kernel -def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -453,6 +459,7 @@ def kernel( geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_pairid: wp.array[wp.vec2i], # Data in: geom_xpos_in: wp.array2d[wp.vec3], @@ -503,8 +510,8 @@ def kernel( continue if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or pairid[1] >= 0 ): @@ -588,7 +595,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): wp.launch( kernel=_sap_project(m.opt.broadphase), dim=(d.nworld, m.ngeom), - inputs=[m.ngeom, m.geom_rbound, m.geom_margin, d.geom_xpos, d.nworld, direction], + inputs=[m.ngeom, m.geom_rbound, m.geom_margin, m.geom_gap, d.geom_xpos, d.nworld, direction], outputs=[ projection_lower.reshape((-1, m.ngeom)), projection_upper, @@ -624,7 +631,9 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): # assumes each geom has 5 other geoms (batched over all worlds) nsweep = 5 * nworldgeom wp.launch( - kernel=_sap_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + kernel=_sap_broadphase( + m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0] + ), dim=nsweep, inputs=[ m.ngeom, @@ -632,6 +641,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_pairid, d.geom_xpos, d.geom_xmat, @@ -646,7 +656,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): @cache_kernel -def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -654,6 +664,7 @@ def kernel( geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_geom_pair: wp.array[wp.vec2i], nxn_pairid: wp.array[wp.vec2i], # Data in: @@ -674,8 +685,8 @@ def kernel( geom2 = geom[1] if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or nxn_pairid[elementid][1] >= 0 ): @@ -711,13 +722,16 @@ def nxn_broadphase(m: Model, d: Data, ctx: CollisionContext): `contype`/`conaffinity`, parent-child relationships, and explicit `` tags. """ wp.launch( - _nxn_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + _nxn_broadphase( + m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0] + ), dim=(d.nworld, m.nxn_geom_pair_filtered.shape[0]), inputs=[ m.geom_type, m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_geom_pair_filtered, m.nxn_pairid_filtered, d.geom_xpos, diff --git a/mujoco_warp/_src/collision_driver_test.py b/mujoco_warp/_src/collision_driver_test.py index 1a88a9069..558431f23 100644 --- a/mujoco_warp/_src/collision_driver_test.py +++ b/mujoco_warp/_src/collision_driver_test.py @@ -30,6 +30,7 @@ from mujoco_warp._src.collision_driver import MJ_COLLISION_TABLE from mujoco_warp._src.collision_primitive import plane_convex from mujoco_warp._src.math import upper_trid_index +from mujoco_warp._src.types import NEW_GAP_SEMANTICS from mujoco_warp.test_data.collision_sdf.utils import register_sdf_plugins _TOLERANCE = 5e-5 @@ -702,7 +703,7 @@ def test_contact_pair(self, broadphase): # 1 pair _, _, m, d = test_data.fixture( - xml=""" + xml=f""" @@ -715,7 +716,7 @@ def test_contact_pair(self, broadphase): - + """ @@ -746,7 +747,7 @@ def test_contact_pair(self, broadphase): # 1 pair: override contype and conaffinity _, _, m, d = test_data.fixture( - xml=""" + xml=f""" @@ -759,7 +760,7 @@ def test_contact_pair(self, broadphase): - + """ @@ -790,7 +791,7 @@ def test_contact_pair(self, broadphase): # 1 pair: override exclude _, _, m, d = test_data.fixture( - xml=""" + xml=f""" @@ -804,7 +805,7 @@ def test_contact_pair(self, broadphase): - + """ @@ -835,7 +836,7 @@ def test_contact_pair(self, broadphase): # 1 pair 1 exclude _, _, m, d = test_data.fixture( - xml=""" + xml=f""" @@ -853,7 +854,7 @@ def test_contact_pair(self, broadphase): - + """ @@ -1126,22 +1127,23 @@ def test_sdf_volume_collision(self, fixture): def test_ccd_margin_dist(self): """Tests that CCD contact dist matches MuJoCo when margin > 0. - Two ellipsoids are placed 0.05 m apart (not touching). With margin=0.1 on - each geom the pair margin is 0.2, so contacts are detected within the + Two ellipsoids are placed 0.05 m apart (not touching). With margin=0.01 + and gap=0.2 on each geom, the pair margin is 0.02 and pair gap is 0.4. + CCD inflates geometries by margin, detecting contacts within the speculative envelope. The reported dist must equal the true geometric separation (≈0.05), not the margin-biased value that the inflated GJK/EPA would produce. """ - xml = """ + xml = f""" - + - + @@ -1173,7 +1175,7 @@ def test_ccd_margin_dist(self): break self.assertTrue(found, f"MJ contact {i} dist={mj_dist:.4f} not matched in MJW") - # Verify no constraint forces are generated (includemargin=0, dist > 0) + # dist(≈0.05) > margin(0.02): contacts are in gap zone, no constraints self.assertEqual(mjd.nefc, 0, "Classic MuJoCo should have no active constraints") self.assertEqual(d.nefc.numpy()[0], 0, "MuJoCo Warp should have no active constraints") diff --git a/mujoco_warp/_src/constraint_test.py b/mujoco_warp/_src/constraint_test.py index b38e3cff0..82d286565 100644 --- a/mujoco_warp/_src/constraint_test.py +++ b/mujoco_warp/_src/constraint_test.py @@ -26,6 +26,7 @@ import mujoco_warp as mjw from mujoco_warp import ConeType from mujoco_warp import test_data +from mujoco_warp._src.types import NEW_GAP_SEMANTICS # tolerance for difference between MuJoCo and MJWarp constraint calculations, # mostly due to float precision @@ -270,15 +271,15 @@ def test_equality_tendon(self, jacobian): def test_efc_address_inactive_contacts(self): """Test that efc_address is -1 for inactive contacts in the gap zone.""" # Sphere at z=0.35 with radius 0.1: dist ~ 0.15 to ground plane. - # margin=0.5, gap=0.4 => includemargin = 0.1. - # dist(0.15) < margin(0.5) => contact is detected. - # dist(0.15) >= includemargin(0.1) => contact is NOT active (in gap zone). - xml = """ + # margin=0.1, gap=0.4 => detection at margin+gap=0.5, forces at margin=0.1. + # dist(0.15) < margin+gap(0.5) => contact is detected. + # dist(0.15) >= margin(0.1) => contact is NOT active (in gap zone). + xml = f""" - + - + diff --git a/mujoco_warp/_src/smooth_test.py b/mujoco_warp/_src/smooth_test.py index 556d1ed86..8fa64207b 100644 --- a/mujoco_warp/_src/smooth_test.py +++ b/mujoco_warp/_src/smooth_test.py @@ -25,6 +25,7 @@ from mujoco_warp import ConeType from mujoco_warp import DisableBit from mujoco_warp import test_data +from mujoco_warp._src.types import NEW_GAP_SEMANTICS from mujoco_warp._src.util_pkg import check_version # tolerance for difference between MuJoCo and MJWarp smooth calculations - mostly @@ -376,6 +377,8 @@ def test_transmission(self, xml): ) def test_actuator_adhesion(self, keyframe, cone, jacobian): """Tests adhesion actuator.""" + if not NEW_GAP_SEMANTICS: + self.skipTest("Skipping due to new gap semantics") mjm, mjd, m, d = test_data.fixture( "actuation/adhesion.xml", keyframe=keyframe, overrides={"opt.cone": cone, "opt.jacobian": jacobian} ) diff --git a/mujoco_warp/_src/types.py b/mujoco_warp/_src/types.py index 1ad8f9177..44fa4b3c0 100644 --- a/mujoco_warp/_src/types.py +++ b/mujoco_warp/_src/types.py @@ -20,12 +20,15 @@ import numpy as np import warp as wp +from mujoco_warp._src.util_pkg import check_version + MJ_MINVAL = mujoco.mjMINVAL MJ_MAXVAL = mujoco.mjMAXVAL MJ_MINIMP = mujoco.mjMINIMP # minimum constraint impedance MJ_MAXIMP = mujoco.mjMAXIMP # maximum constraint impedance MJ_MAXCONPAIR = mujoco.mjMAXCONPAIR MJ_MINMU = mujoco.mjMINMU # minimum friction +NEW_GAP_SEMANTICS = check_version("mujoco>=3.9.0.dev914519929") # maximum size (by number of edges) of an horizon in EPA algorithm MJ_MAX_EPAHORIZON = 24 # maximum average number of trianglarfaces EPA can insert at each iteration @@ -975,7 +978,7 @@ class Model: geom_quat: local orientation offset rel. to body (*, ngeom, 4) geom_friction: friction for (slide, spin, roll) (*, ngeom, 3) geom_margin: detect contact if dist tuple[tuple[int, int | str], ...]: """Parse a version string into comparable components. - Both '.' and '-' are treated as separators. Each component is wrapped in a - tuple: (0, int) for numeric parts, (-1, str) for non-numeric. A (0, 0) - sentinel is appended so that stable releases sort above pre-release suffixes - during Python tuple comparison (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric - components are compared lexicographically (e.g., b >= a). + Dot-separated components form the version. Hyphen-separated suffixes (e.g., + "-newton") are treated as local build identifiers and stripped before + parsing. Each component is wrapped in a tuple: (0, int) for numeric parts, + (-1, str) for non-numeric. A (0, 0) sentinel is appended so that stable + releases sort above pre-release suffixes during Python tuple comparison + (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric components are compared + lexicographically (e.g., b >= a). Args: - version_str: Version string like "3.5.0" or "3.5.0.dev869102767". + version_str: Version string like "3.5.0", "3.5.0.dev869102767", or + "3.9.0-newton". Returns: Tuple of (type_order, value) pairs for comparison, where type_order is 0 for integers and -1 for strings, followed by a (0, 0) sentinel. """ - # Split on both '.' and '-' - parts = re.split(r"[.\-]", version_str) + # Strip local build identifier (e.g., "3.9.0-newton" -> "3.9.0") + version_str = version_str.split("-", 1)[0] + parts = version_str.split(".") return tuple([(0, int(p)) if p.isdigit() else (-1, p) for p in parts] + [(0, 0)]) diff --git a/mujoco_warp/_src/util_pkg_test.py b/mujoco_warp/_src/util_pkg_test.py index 1dfabb6c7..6b5cf748e 100644 --- a/mujoco_warp/_src/util_pkg_test.py +++ b/mujoco_warp/_src/util_pkg_test.py @@ -32,9 +32,10 @@ class ParseVersionTest(parameterized.TestCase): ("3.5.0", ((0, 3), (0, 5), (0, 0), (0, 0))), ("1.20.0", ((0, 1), (0, 20), (0, 0), (0, 0))), ("3.5.0.dev869102767", ((0, 3), (0, 5), (0, 0), (-1, "dev869102767"), (0, 0))), - ("3.5.0-foobar", ((0, 3), (0, 5), (0, 0), (-1, "foobar"), (0, 0))), - ("1.0.0-alpha", ((0, 1), (0, 0), (0, 0), (-1, "alpha"), (0, 0))), - ("2.0.0-beta.1", ((0, 2), (0, 0), (0, 0), (-1, "beta"), (0, 1), (0, 0))), + # hyphen-separated suffixes are stripped as local build identifiers + ("3.5.0-foobar", ((0, 3), (0, 5), (0, 0), (0, 0))), + ("1.0.0-alpha", ((0, 1), (0, 0), (0, 0), (0, 0))), + ("3.9.0-newton", ((0, 3), (0, 9), (0, 0), (0, 0))), ) def test_parse_version(self, version_str, expected): self.assertEqual(util_pkg._parse_version(version_str), expected) @@ -67,16 +68,12 @@ class CheckVersionTest(parameterized.TestCase): ("pkg!=1.0.0", "1.0.0", False), # With dev/pre-release versions (pre-release < clean release) ("pkg>=3.5.0", "3.5.0.dev869102767", False), # dev version < base - ("pkg>=3.5.0", "3.5.0-foobar", False), # foobar < base ("pkg>3.5.0", "3.5.0.dev869102767", False), # dev version < base ("pkg>=3.5.0", "3.5.0", True), # exact match ("pkg>=3.5.0.dev", "3.5.0", True), # clean release > dev - # Lexicographic ordering: foobar > dev - ("pkg>=3.5.0-foobar", "3.5.0-foobar", True), - ("pkg>3.5.0.dev869102767", "3.5.0-foobar", True), - # b >= a - ("pkg>=1.2.3-a", "1.2.3-b", True), - ("pkg>=1.2.3-b", "1.2.3-a", False), + # hyphen-separated suffixes are stripped (local build identifiers) + ("pkg>=3.5.0", "3.5.0-foobar", True), # 3.5.0-foobar == 3.5.0 + ("pkg>=3.9.0", "3.9.0-newton", True), # 3.9.0-newton == 3.9.0 ) def test_check_version(self, spec, installed_version, expected): with mock.patch("importlib.metadata.version", return_value=installed_version): diff --git a/mujoco_warp/test_data/actuation/adhesion.xml b/mujoco_warp/test_data/actuation/adhesion.xml index c40a58cad..2205df86a 100644 --- a/mujoco_warp/test_data/actuation/adhesion.xml +++ b/mujoco_warp/test_data/actuation/adhesion.xml @@ -3,7 +3,7 @@ - +