From 1301bf45d9935cc1a51ef700888496d88590dc9b Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 1 Mar 2023 12:24:45 -0500 Subject: [PATCH 1/2] Pad AABB --- CHANGELOG.md | 2 ++ smarts/core/chassis.py | 11 ++++++++--- smarts/core/scenario.py | 2 +- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7effe19278..4caaf50ac2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,8 @@ Copy and pasting the git commit messages is __NOT__ enough. ### Fixed - Missing neighborhood vehicle ids are now added to the `highway-v1` formatted observations. - Using `trip` in sstudio traffic generation no longer causes a durouter error. +- Chassis collision AABB first pass now has an additional `0.05m` tolerance to identify axis aligned collisions that would previously be missed. +- Agent to mission padding warning now occurs when there are less missions than agents rather than when there are the same number of agents as missions. ### Removed ### Security diff --git a/smarts/core/chassis.py b/smarts/core/chassis.py index d8cf5a44e8..817dd5a335 100644 --- a/smarts/core/chassis.py +++ b/smarts/core/chassis.py @@ -59,22 +59,27 @@ def _query_bullet_contact_points(bullet_client, bullet_id, link_index): contact_objects = set() + # Give 0.05 meter leeway + LEEWAY = 0.05 # `getContactPoints` does not pick up collisions well so we cast a fast box check on the physics min_, max_ = bullet_client.getAABB(bullet_id, link_index) # note that getAABB returns a box around the link_index link only, # which means it's offset from the ground (min_ has a positive z) # if link_index=0 (the chassis link) is used. - overlapping_objects = bullet_client.getOverlappingObjects(min_, max_) + overlapping_objects = bullet_client.getOverlappingObjects( + tuple(map(sum, zip(min_, (-LEEWAY, -LEEWAY, -LEEWAY)))), + tuple(map(sum, zip(max_, (LEEWAY, LEEWAY, LEEWAY)))), + ) # the pairs returned by getOverlappingObjects() appear to be in the form (body_id, link_idx) if overlapping_objects is not None: contact_objects = set(oo for oo, _ in overlapping_objects if oo != bullet_id) contact_points = [] for contact_object in contact_objects: - # Give 0.05 meter leeway + contact_points.extend( - bullet_client.getClosestPoints(bullet_id, contact_object, distance=0.05) + bullet_client.getClosestPoints(bullet_id, contact_object, distance=LEEWAY) ) return contact_points diff --git a/smarts/core/scenario.py b/smarts/core/scenario.py index 9df5bba2ad..b96fd22fc7 100644 --- a/smarts/core/scenario.py +++ b/smarts/core/scenario.py @@ -233,7 +233,7 @@ def variations_for_all_scenario_roots( # `or [None]` so that product(...) will not return an empty result # but insted a [(..., `None`), ...]. agent_missions = agent_missions or [None] - if len(agents_to_be_briefed) == len(agent_missions): + if len(agents_to_be_briefed) > len(agent_missions): warnings.warn( f"Scenario `{scenario_root}` has {len(agent_missions)} missions and" f" but there are {len(agents_to_be_briefed)} agents to assign" From 484c597399be0c79c34f10369206660676a02ec3 Mon Sep 17 00:00:00 2001 From: Tucker Date: Wed, 1 Mar 2023 12:59:02 -0500 Subject: [PATCH 2/2] Bump up from ground. --- smarts/core/chassis.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smarts/core/chassis.py b/smarts/core/chassis.py index 817dd5a335..9fe61fefa1 100644 --- a/smarts/core/chassis.py +++ b/smarts/core/chassis.py @@ -68,7 +68,7 @@ def _query_bullet_contact_points(bullet_client, bullet_id, link_index): # which means it's offset from the ground (min_ has a positive z) # if link_index=0 (the chassis link) is used. overlapping_objects = bullet_client.getOverlappingObjects( - tuple(map(sum, zip(min_, (-LEEWAY, -LEEWAY, -LEEWAY)))), + tuple(map(sum, zip(min_, (-LEEWAY, -LEEWAY, 0)))), tuple(map(sum, zip(max_, (LEEWAY, LEEWAY, LEEWAY)))), ) # the pairs returned by getOverlappingObjects() appear to be in the form (body_id, link_idx)