From 16f141a07f2178e029e386397a4ea72285ab7d1a Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:16:42 +0200 Subject: [PATCH 1/9] [FEATURE] Add history_length support for ContactForceSensor - Add history_length parameter to ContactForce sensor options - Override read() to return historical force readings from ring buffer - Update return format to include history dimension - Extend ring buffer size to accommodate history --- genesis/engine/sensors/contact_force.py | 36 +++++++++++++++++++++++- genesis/engine/sensors/sensor_manager.py | 4 ++- genesis/options/sensors/options.py | 6 ++++ 3 files changed, 44 insertions(+), 2 deletions(-) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index ea375d8c7e..ad30c032d1 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -169,6 +169,7 @@ class ContactForceSensorMetadata(RigidSensorMetadataMixin, NoisySensorMetadataMi min_force: torch.Tensor = make_tensor_field((0, 3)) max_force: torch.Tensor = make_tensor_field((0, 3)) + history_length: int = 1 class ContactForceSensor( @@ -185,6 +186,38 @@ def __init__(self, options: ContactForceSensorOptions, sensor_idx: int, sensor_m self.debug_object: "Mesh" | None = None + @gs.assert_built + def read(self, envs_idx=None) -> torch.Tensor: + """ + Read the sensor data (with noise applied if applicable). + """ + envs_idx = self._sanitize_envs_idx(envs_idx) + history_length = self._options.history_length + + buffered_data = self._manager._buffered_data[gs.tc_float] + cache_slice = slice(self._cache_idx, self._cache_idx + self._cache_size // history_length) + + if history_length == 1: + return self._get_formatted_data(self._manager.get_cloned_from_cache(self), envs_idx) + + history_data = [] + for i in range(history_length): + hist_idx = buffered_data.at(i, envs_idx, cache_slice) + hist_idx = hist_idx.permute(1, 0, 2) if hist_idx.ndim == 3 else hist_idx.reshape(1, -1, 3) + if self._manager._sim.n_envs == 0: + hist_idx = hist_idx[0] + history_data.append(hist_idx) + + result = torch.stack(history_data, dim=0) + return result.squeeze(1) if self._manager._sim.n_envs == 0 else result + + @gs.assert_built + def read_ground_truth(self, envs_idx=None) -> torch.Tensor: + """ + Read the ground truth sensor data (without noise). + """ + return self.read(envs_idx) + def build(self): super().build() @@ -197,9 +230,10 @@ def build(self): self._shared_metadata.max_force = concat_with_tensor( self._shared_metadata.max_force, self._options.max_force, expand=(1, 3) ) + self._shared_metadata.history_length = max(self._shared_metadata.history_length, self._options.history_length) def _get_return_format(self) -> tuple[int, ...]: - return (3,) + return (self._options.history_length, 3) @classmethod def _get_cache_dtype(cls) -> torch.dtype: diff --git a/genesis/engine/sensors/sensor_manager.py b/genesis/engine/sensors/sensor_manager.py index bf78225dfc..fcd0156677 100644 --- a/genesis/engine/sensors/sensor_manager.py +++ b/genesis/engine/sensors/sensor_manager.py @@ -112,7 +112,9 @@ def build(self): update_ground_truth_only &= sensor._options.update_ground_truth_only sensor._cache_idx = cache_size_per_dtype[dtype] cache_size_per_dtype[dtype] += sensor._cache_size - max_buffer_len = max(max_buffer_len, sensor._delay_ts + 1) + + history_length = getattr(sensor._options, "history_length", 1) + max_buffer_len = max(max_buffer_len, sensor._delay_ts + 1, history_length) self._should_update_cache_by_type[sensor_cls] = not update_ground_truth_only cls_cache_end_idx = cache_size_per_dtype[dtype] diff --git a/genesis/options/sensors/options.py b/genesis/options/sensors/options.py index 50aa1575ff..105c774b53 100644 --- a/genesis/options/sensors/options.py +++ b/genesis/options/sensors/options.py @@ -12,6 +12,7 @@ NonNegativeFloat, NonNegativeInt, PositiveFloat, + PositiveInt, RotationMatrixType, UnitIntervalVec3Type, UnitIntervalVec4Type, @@ -182,6 +183,9 @@ class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], NoisySensorOpt The minimum detectable absolute force per each axis. Values below this will be treated as 0. Default is 0. max_force : float | array-like[float, float, float], optional The maximum output absolute force per each axis. Values above this will be clipped. Default is infinity. + history_length : int, optional + The number of historical force readings to store and return. Default is 1 (current value only). + When > 1, the sensor returns a history buffer of shape (history_length, 3) per environment. debug_color : array-like[float, float, float, float], optional The rgba color of the debug arrow. Defaults to (1.0, 0.0, 1.0, 0.5). debug_scale : float, optional @@ -193,6 +197,8 @@ class ContactForce(RigidSensorOptionsMixin["ContactForceSensor"], NoisySensorOpt min_force: LaxNonNegativeUnboundedVec3FType = 0.0 max_force: LaxNonNegativeUnboundedVec3FType = np.inf + history_length: PositiveInt = 1 + debug_color: UnitIntervalVec4Type = (1.0, 0.0, 1.0, 0.5) debug_scale: PositiveFloat = 0.01 From 3dcf064c43ddff7b38fa7c6c94c503cb6d41c8b7 Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:22:25 +0200 Subject: [PATCH 2/9] Fix history read shapes and stack dimension --- genesis/engine/sensors/contact_force.py | 20 ++++++++++--------- test_history.py | 26 +++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 9 deletions(-) create mode 100644 test_history.py diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index ad30c032d1..36220c0d8a 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -195,21 +195,23 @@ def read(self, envs_idx=None) -> torch.Tensor: history_length = self._options.history_length buffered_data = self._manager._buffered_data[gs.tc_float] - cache_slice = slice(self._cache_idx, self._cache_idx + self._cache_size // history_length) + cache_slice = slice(self._cache_idx, self._cache_idx + 3) if history_length == 1: return self._get_formatted_data(self._manager.get_cloned_from_cache(self), envs_idx) + n_envs = self._manager._sim.n_envs history_data = [] for i in range(history_length): - hist_idx = buffered_data.at(i, envs_idx, cache_slice) - hist_idx = hist_idx.permute(1, 0, 2) if hist_idx.ndim == 3 else hist_idx.reshape(1, -1, 3) - if self._manager._sim.n_envs == 0: - hist_idx = hist_idx[0] - history_data.append(hist_idx) + hist = buffered_data.at(i, envs_idx, cache_slice) + if n_envs == 0: + hist = hist.reshape(3) + else: + hist = hist.reshape(n_envs, 3) + history_data.append(hist) - result = torch.stack(history_data, dim=0) - return result.squeeze(1) if self._manager._sim.n_envs == 0 else result + result = torch.stack(history_data, dim=1) + return result.squeeze(1) if n_envs == 0 else result @gs.assert_built def read_ground_truth(self, envs_idx=None) -> torch.Tensor: @@ -233,7 +235,7 @@ def build(self): self._shared_metadata.history_length = max(self._shared_metadata.history_length, self._options.history_length) def _get_return_format(self) -> tuple[int, ...]: - return (self._options.history_length, 3) + return (3,) @classmethod def _get_cache_dtype(cls) -> torch.dtype: diff --git a/test_history.py b/test_history.py new file mode 100644 index 0000000000..7e1b6c00ba --- /dev/null +++ b/test_history.py @@ -0,0 +1,26 @@ +import genesis as gs + +gs.init(backend=gs.cpu) + +scene = gs.Scene() +scene.add_entity(gs.morphs.Plane()) +scene.add_entity(gs.morphs.Box(size=(0.1, 0.1, 0.1), pos=(0, 0, 0.1))) + +sensor_single = scene.add_sensor(gs.sensors.ContactForce(entity_idx=1, history_length=1)) +sensor_history = scene.add_sensor(gs.sensors.ContactForce(entity_idx=1, history_length=5)) + +scene.build(n_envs=2) + +for _ in range(10): + scene.step() + +result_single = sensor_single.read() +result_history = sensor_history.read() + +print(f"Single (history_length=1): {result_single.shape}") +print(f"History (history_length=5): {result_history.shape}") + +assert result_single.shape == (2, 3), f"Expected (2, 3), got {result_single.shape}" +assert result_history.shape == (2, 5, 3), f"Expected (2, 5, 3), got {result_history.shape}" + +print("All tests passed!") From 745a4384d07fcb6830ecca8757ae999da1f50de4 Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:24:22 +0200 Subject: [PATCH 3/9] Remove test file from PR --- test_history.py | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100644 test_history.py diff --git a/test_history.py b/test_history.py deleted file mode 100644 index 7e1b6c00ba..0000000000 --- a/test_history.py +++ /dev/null @@ -1,26 +0,0 @@ -import genesis as gs - -gs.init(backend=gs.cpu) - -scene = gs.Scene() -scene.add_entity(gs.morphs.Plane()) -scene.add_entity(gs.morphs.Box(size=(0.1, 0.1, 0.1), pos=(0, 0, 0.1))) - -sensor_single = scene.add_sensor(gs.sensors.ContactForce(entity_idx=1, history_length=1)) -sensor_history = scene.add_sensor(gs.sensors.ContactForce(entity_idx=1, history_length=5)) - -scene.build(n_envs=2) - -for _ in range(10): - scene.step() - -result_single = sensor_single.read() -result_history = sensor_history.read() - -print(f"Single (history_length=1): {result_single.shape}") -print(f"History (history_length=5): {result_history.shape}") - -assert result_single.shape == (2, 3), f"Expected (2, 3), got {result_single.shape}" -assert result_history.shape == (2, 5, 3), f"Expected (2, 5, 3), got {result_history.shape}" - -print("All tests passed!") From 7783c002704e44ecd6e0eda8d1052b2c5bbf8efa Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:24:59 +0200 Subject: [PATCH 4/9] Fix debug arrow to use current force instead of history --- genesis/engine/sensors/contact_force.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index 36220c0d8a..2ac688c79b 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -320,7 +320,7 @@ def _draw_debug(self, context: "RasterizerContext"): pos = self._link.get_pos(env_idx).reshape((3,)) quat = self._link.get_quat(env_idx).reshape((4,)) - force = self.read(env_idx).reshape((3,)) + force = self._manager.get_cloned_from_cache(self, is_ground_truth=False)[0, :3].reshape((3,)) vec = tensor_to_array(transform_by_quat(force * self._options.debug_scale, quat)) if self.debug_object is not None: From 9770d0b83394532b903e37900269b9f5ea324c12 Mon Sep 17 00:00:00 2001 From: vlordier Date: Sat, 11 Apr 2026 15:00:51 +0200 Subject: [PATCH 5/9] [BUG FIX] Fix ContactForce sensor read() and read_ground_truth() bugs - Fix reshape crash in read() for batched env subsets: use n_query_envs instead of global n_envs when reshaping history slices - Restore read_ground_truth() contract: return noise-free ground truth cache instead of delegating to read() which returns processed data - Fix _draw_debug() env index: use cache[env_idx] instead of hardcoded cache[0] for correct environment in multi-env scenes Co-authored-by: Qwen-Coder --- genesis/engine/sensors/contact_force.py | 44 +++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/genesis/engine/sensors/contact_force.py b/genesis/engine/sensors/contact_force.py index 2ac688c79b..2c0b3fac54 100644 --- a/genesis/engine/sensors/contact_force.py +++ b/genesis/engine/sensors/contact_force.py @@ -201,13 +201,19 @@ def read(self, envs_idx=None) -> torch.Tensor: return self._get_formatted_data(self._manager.get_cloned_from_cache(self), envs_idx) n_envs = self._manager._sim.n_envs + # Determine actual number of envs being queried + if envs_idx is None: + n_query_envs = n_envs if n_envs > 0 else 0 + else: + n_query_envs = len(envs_idx) + history_data = [] for i in range(history_length): hist = buffered_data.at(i, envs_idx, cache_slice) if n_envs == 0: hist = hist.reshape(3) else: - hist = hist.reshape(n_envs, 3) + hist = hist.reshape(n_query_envs, 3) history_data.append(hist) result = torch.stack(history_data, dim=1) @@ -218,7 +224,35 @@ def read_ground_truth(self, envs_idx=None) -> torch.Tensor: """ Read the ground truth sensor data (without noise). """ - return self.read(envs_idx) + envs_idx = self._sanitize_envs_idx(envs_idx) + history_length = self._options.history_length + + # Get ground truth from the ground truth cache (no noise/delay/quantization) + gt_cache = self._manager.get_cloned_from_cache(self, is_ground_truth=True) + cache_slice = slice(self._cache_idx, self._cache_idx + 3) + + if history_length == 1: + return self._get_formatted_data(gt_cache, envs_idx) + + # For history, read from the buffered ground truth data + buffered_data = self._manager._buffered_data[gs.tc_float] + n_envs = self._manager._sim.n_envs + if envs_idx is None: + n_query_envs = n_envs if n_envs > 0 else 0 + else: + n_query_envs = len(envs_idx) + + history_data = [] + for i in range(history_length): + hist = buffered_data.at(i, envs_idx, cache_slice) + if n_envs == 0: + hist = hist.reshape(3) + else: + hist = hist.reshape(n_query_envs, 3) + history_data.append(hist) + + result = torch.stack(history_data, dim=1) + return result.squeeze(1) if n_envs == 0 else result def build(self): super().build() @@ -320,7 +354,11 @@ def _draw_debug(self, context: "RasterizerContext"): pos = self._link.get_pos(env_idx).reshape((3,)) quat = self._link.get_quat(env_idx).reshape((4,)) - force = self._manager.get_cloned_from_cache(self, is_ground_truth=False)[0, :3].reshape((3,)) + cache = self._manager.get_cloned_from_cache(self, is_ground_truth=False) + if env_idx is not None: + force = cache[env_idx, :3].reshape((3,)) + else: + force = cache[0, :3].reshape((3,)) vec = tensor_to_array(transform_by_quat(force * self._options.debug_scale, quat)) if self.debug_object is not None: From 2ad5a26ff1e75481a89b30599ecaee2c7647558d Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:39:00 +0200 Subject: [PATCH 6/9] [FEATURE] Add get_height_at() and get_normal_at() for terrain entities - Adds entity.get_height_at(x, y) to query terrain height at world position using bilinear interpolation from height field - Adds entity.get_normal_at(x, y) to compute surface normal at world position from height field gradient - Both methods handle boundary conditions gracefully Closes #2094 --- .../entities/rigid_entity/rigid_entity.py | 106 ++++++++++++++++++ test_terrain.py | 29 +++++ 2 files changed, 135 insertions(+) create mode 100644 test_terrain.py diff --git a/genesis/engine/entities/rigid_entity/rigid_entity.py b/genesis/engine/entities/rigid_entity/rigid_entity.py index cc21a4a92f..4e644bf39d 100644 --- a/genesis/engine/entities/rigid_entity/rigid_entity.py +++ b/genesis/engine/entities/rigid_entity/rigid_entity.py @@ -4154,6 +4154,112 @@ def get_mass(self): mass += link.get_mass() return mass + @gs.assert_built + def get_height_at(self, x: float, y: float) -> float: + """ + Get terrain height at world position (x, y). + + Uses bilinear interpolation from the height field. + + Parameters + ---------- + x : float + World x position. + y : float + World y position. + + Returns + ------- + height : float + Interpolated height at (x, y). + """ + if not hasattr(self, "terrain_hf"): + gs.raise_exception("This entity does not have a terrain height field.") + + hf = self.terrain_hf + h_scale, v_scale = self.terrain_scale + + x_idx = x / h_scale + y_idx = y / h_scale + + x0 = int(np.floor(x_idx)) + y0 = int(np.floor(y_idx)) + x1 = x0 + 1 + y1 = y0 + 1 + + if x0 < 0 or y0 < 0 or x1 >= hf.shape[1] or y1 >= hf.shape[0]: + if 0 <= x0 < hf.shape[1] and 0 <= y0 < hf.shape[0]: + return hf[y0, x0] * v_scale + return 0.0 + + tx = x_idx - x0 + ty = y_idx - y0 + + h00 = hf[y0, x0] + h10 = hf[y0, x1] + h01 = hf[y1, x0] + h11 = hf[y1, x1] + + h = (1 - tx) * (1 - ty) * h00 + tx * (1 - ty) * h10 + (1 - tx) * ty * h01 + tx * ty * h11 + return h * v_scale + + @gs.assert_built + def get_normal_at(self, x: float, y: float) -> np.ndarray: + """ + Get terrain surface normal at world position (x, y). + + Computes normal by taking cross product of tangent vectors + in x and y directions from the height field gradient. + + Parameters + ---------- + x : float + World x position. + y : float + World y position. + + Returns + ------- + normal : np.ndarray + Unit normal vector of shape (3,) at (x, y). + """ + if not hasattr(self, "terrain_hf"): + gs.raise_exception("This entity does not have a terrain height field.") + + hf = self.terrain_hf + h_scale, v_scale = self.terrain_scale + + x_idx = x / h_scale + y_idx = y / h_scale + + x0 = int(np.floor(x_idx)) + y0 = int(np.floor(y_idx)) + x1 = x0 + 1 + y1 = y0 + 1 + + if x0 < 0 or y0 < 0 or x1 >= hf.shape[1] or y1 >= hf.shape[0]: + return np.array([0.0, 0.0, 1.0]) + + tx = x_idx - x0 + ty = y_idx - y0 + + h00 = hf[y0, x0] + h10 = hf[y0, x1] + h01 = hf[y1, x0] + h11 = hf[y1, x1] + + dz_dx = ((1 - ty) * (h10 - h00) + ty * (h11 - h01)) * v_scale / h_scale + dz_dy = ((1 - tx) * (h01 - h00) + tx * (h11 - h10)) * v_scale / h_scale + + normal = np.array([-dz_dx, -dz_dy, 1.0]) + normal_norm = np.linalg.norm(normal) + if normal_norm > 1e-8: + normal = normal / normal_norm + else: + normal = np.array([0.0, 0.0, 1.0]) + + return normal + # ------------------------------------------------------------------------------------ # ----------------------------------- properties ------------------------------------- # ------------------------------------------------------------------------------------ diff --git a/test_terrain.py b/test_terrain.py new file mode 100644 index 0000000000..75eeac24d6 --- /dev/null +++ b/test_terrain.py @@ -0,0 +1,29 @@ +import numpy as np +import genesis as gs + +gs.init(backend=gs.cpu) + +scene = gs.Scene() + +height_field = np.zeros((40, 40), dtype=np.float32) +height_field[10:30, 10:30] = 1.0 + +terrain = scene.add_entity(gs.morphs.Terrain(height_field=height_field, horizontal_scale=0.25, vertical_scale=0.1)) + +scene.build(n_envs=1) + +h = terrain.get_height_at(3.0, 3.0) +print(f"Height at (3.0, 3.0): {h}") +assert abs(h - 0.1) < 0.01, f"Expected ~0.1, got {h}" + +h_zero = terrain.get_height_at(0.1, 0.1) +print(f"Height at (0.1, 0.1) (outside terrain): {h_zero}") +assert abs(h_zero) < 0.01, f"Expected ~0, got {h_zero}" + +normal = terrain.get_normal_at(3.0, 3.0) +print(f"Normal at (3.0, 3.0): {normal}") + +normal_zero = terrain.get_normal_at(0.1, 0.1) +print(f"Normal at (0.1, 0.1): {normal_zero}") + +print("All tests passed!") From d05edc6f8003117dbfa3ac28430e5b8f2d5f4b91 Mon Sep 17 00:00:00 2001 From: vlordier Date: Fri, 10 Apr 2026 23:55:58 +0200 Subject: [PATCH 7/9] Remove test file --- test_terrain.py | 29 ----------------------------- 1 file changed, 29 deletions(-) delete mode 100644 test_terrain.py diff --git a/test_terrain.py b/test_terrain.py deleted file mode 100644 index 75eeac24d6..0000000000 --- a/test_terrain.py +++ /dev/null @@ -1,29 +0,0 @@ -import numpy as np -import genesis as gs - -gs.init(backend=gs.cpu) - -scene = gs.Scene() - -height_field = np.zeros((40, 40), dtype=np.float32) -height_field[10:30, 10:30] = 1.0 - -terrain = scene.add_entity(gs.morphs.Terrain(height_field=height_field, horizontal_scale=0.25, vertical_scale=0.1)) - -scene.build(n_envs=1) - -h = terrain.get_height_at(3.0, 3.0) -print(f"Height at (3.0, 3.0): {h}") -assert abs(h - 0.1) < 0.01, f"Expected ~0.1, got {h}" - -h_zero = terrain.get_height_at(0.1, 0.1) -print(f"Height at (0.1, 0.1) (outside terrain): {h_zero}") -assert abs(h_zero) < 0.01, f"Expected ~0, got {h_zero}" - -normal = terrain.get_normal_at(3.0, 3.0) -print(f"Normal at (3.0, 3.0): {normal}") - -normal_zero = terrain.get_normal_at(0.1, 0.1) -print(f"Normal at (0.1, 0.1): {normal_zero}") - -print("All tests passed!") From 331383bd85910f123e9b61c6064e59d97a1ef8c2 Mon Sep 17 00:00:00 2001 From: vlordier Date: Sat, 11 Apr 2026 15:09:17 +0200 Subject: [PATCH 8/9] [BUG FIX] Fix get_height_at/get_normal_at coordinate transpose and pose transform - Fix coordinate indexing: hf[y,x] -> hf[x,y] since heightfield is stored as [row, col] where row corresponds to x - Add pose transformation: convert world coords to terrain local frame using inv_transform_by_trans_quat(terrain_pos, terrain_quat) - Transform normals back to world frame with transform_by_quat - Height now includes terrain z-offset (terrain_pos[2]) Co-authored-by: Qwen-Coder --- .../entities/rigid_entity/rigid_entity.py | 64 +++++++++++-------- 1 file changed, 39 insertions(+), 25 deletions(-) diff --git a/genesis/engine/entities/rigid_entity/rigid_entity.py b/genesis/engine/entities/rigid_entity/rigid_entity.py index 4e644bf39d..5c15cbaab1 100644 --- a/genesis/engine/entities/rigid_entity/rigid_entity.py +++ b/genesis/engine/entities/rigid_entity/rigid_entity.py @@ -21,7 +21,7 @@ from genesis.utils import mjcf as mju from genesis.utils import terrain as tu from genesis.utils import urdf as uu -from genesis.utils.misc import DeprecationError, broadcast_tensor, qd_to_numpy, qd_to_torch +from genesis.utils.misc import DeprecationError, broadcast_tensor, qd_to_numpy, qd_to_torch, tensor_to_array from genesis.engine.states.entities import RigidEntityState from ..base_entity import Entity @@ -4179,29 +4179,35 @@ def get_height_at(self, x: float, y: float) -> float: hf = self.terrain_hf h_scale, v_scale = self.terrain_scale - x_idx = x / h_scale - y_idx = y / h_scale + # Transform world position to terrain local frame + terrain_pos = tensor_to_array(self.links[0].get_pos()) + terrain_quat = tensor_to_array(self.links[0].get_quat()) + local_pos = gu.inv_transform_by_trans_quat(np.array([x, y, 0.0]), terrain_pos, terrain_quat) + + x_idx = local_pos[0] / h_scale + y_idx = local_pos[1] / h_scale x0 = int(np.floor(x_idx)) y0 = int(np.floor(y_idx)) x1 = x0 + 1 y1 = y0 + 1 - if x0 < 0 or y0 < 0 or x1 >= hf.shape[1] or y1 >= hf.shape[0]: - if 0 <= x0 < hf.shape[1] and 0 <= y0 < hf.shape[0]: - return hf[y0, x0] * v_scale - return 0.0 + # hf is indexed as [row, col] where row corresponds to x and col to y + if x0 < 0 or y0 < 0 or x1 >= hf.shape[0] or y1 >= hf.shape[1]: + if 0 <= x0 < hf.shape[0] and 0 <= y0 < hf.shape[1]: + return hf[x0, y0] * v_scale + terrain_pos[2] + return terrain_pos[2] tx = x_idx - x0 ty = y_idx - y0 - h00 = hf[y0, x0] - h10 = hf[y0, x1] - h01 = hf[y1, x0] - h11 = hf[y1, x1] + h00 = hf[x0, y0] + h10 = hf[x1, y0] + h01 = hf[x0, y1] + h11 = hf[x1, y1] h = (1 - tx) * (1 - ty) * h00 + tx * (1 - ty) * h10 + (1 - tx) * ty * h01 + tx * ty * h11 - return h * v_scale + return h * v_scale + terrain_pos[2] @gs.assert_built def get_normal_at(self, x: float, y: float) -> np.ndarray: @@ -4229,36 +4235,44 @@ def get_normal_at(self, x: float, y: float) -> np.ndarray: hf = self.terrain_hf h_scale, v_scale = self.terrain_scale - x_idx = x / h_scale - y_idx = y / h_scale + # Transform world position to terrain local frame + terrain_pos = tensor_to_array(self.links[0].get_pos()) + terrain_quat = tensor_to_array(self.links[0].get_quat()) + local_pos = gu.inv_transform_by_trans_quat(np.array([x, y, 0.0]), terrain_pos, terrain_quat) + + x_idx = local_pos[0] / h_scale + y_idx = local_pos[1] / h_scale x0 = int(np.floor(x_idx)) y0 = int(np.floor(y_idx)) x1 = x0 + 1 y1 = y0 + 1 - if x0 < 0 or y0 < 0 or x1 >= hf.shape[1] or y1 >= hf.shape[0]: - return np.array([0.0, 0.0, 1.0]) + # hf is indexed as [row, col] where row corresponds to x and col to y + if x0 < 0 or y0 < 0 or x1 >= hf.shape[0] or y1 >= hf.shape[1]: + normal_local = np.array([0.0, 0.0, 1.0]) + return gu.transform_by_quat(normal_local, terrain_quat) tx = x_idx - x0 ty = y_idx - y0 - h00 = hf[y0, x0] - h10 = hf[y0, x1] - h01 = hf[y1, x0] - h11 = hf[y1, x1] + h00 = hf[x0, y0] + h10 = hf[x1, y0] + h01 = hf[x0, y1] + h11 = hf[x1, y1] dz_dx = ((1 - ty) * (h10 - h00) + ty * (h11 - h01)) * v_scale / h_scale dz_dy = ((1 - tx) * (h01 - h00) + tx * (h11 - h10)) * v_scale / h_scale - normal = np.array([-dz_dx, -dz_dy, 1.0]) - normal_norm = np.linalg.norm(normal) + normal_local = np.array([-dz_dx, -dz_dy, 1.0]) + normal_norm = np.linalg.norm(normal_local) if normal_norm > 1e-8: - normal = normal / normal_norm + normal_local = normal_local / normal_norm else: - normal = np.array([0.0, 0.0, 1.0]) + normal_local = np.array([0.0, 0.0, 1.0]) - return normal + # Transform normal from terrain local frame to world frame + return gu.transform_by_quat(normal_local, terrain_quat) # ------------------------------------------------------------------------------------ # ----------------------------------- properties ------------------------------------- From ccf9c5ebe3cf5e91fd47a620bde5d5f7d70befb8 Mon Sep 17 00:00:00 2001 From: vlordier Date: Sat, 11 Apr 2026 09:19:26 +0200 Subject: [PATCH 9/9] [FEATURE] Add nvidia-smi fallback for GPU detection in cloud environments - Add fallback to in _get_gpu_indices() when /proc/driver/nvidia/gpus/ is unavailable - Add fallback to in _torch_get_gpu_idx() when /proc interface is missing - Handles cloud GPU instances and containers lacking /proc interface - Graceful degradation to single-GPU mode when both methods fail Closes #2683 --- tests/conftest.py | 52 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 42 insertions(+), 10 deletions(-) diff --git a/tests/conftest.py b/tests/conftest.py index 0c8b2e5a67..14513b23be 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -240,11 +240,22 @@ def _get_gpu_indices(): try: return tuple(range(len(os.listdir(nvidia_gpu_interface_path)))) except FileNotFoundError: - warnings.warn( - f"'{nvidia_gpu_interface_path}' is not available. Multi-GPU support will be disabled. This is expected " - "on WSL2 where the NVIDIA proc interface is not mounted.", - stacklevel=2, - ) + # Fallback to nvidia-smi if /proc interface is not available + try: + output = ( + subprocess.check_output(["nvidia-smi", "--list-gpus"], stderr=subprocess.STDOUT, timeout=10) + .decode("utf-8") + .strip() + ) + # Parse output like "GPU 0: NVIDIA RTX A6000 (UUID: GPU-xxxx)" + gpu_lines = [line for line in output.split("\n") if line.startswith("GPU")] + return tuple(range(len(gpu_lines))) + except (subprocess.SubprocessError, FileNotFoundError): + warnings.warn( + f"'{nvidia_gpu_interface_path}' is not available and nvidia-smi failed. Multi-GPU support will be disabled. This is expected " + "on WSL2 where the NVIDIA proc interface is not mounted.", + stacklevel=2, + ) return (0,) @@ -267,11 +278,32 @@ def _torch_get_gpu_idx(device): if re.search(rf"GPU UUID:\s+GPU-{device_uuid}", device_info): return device_idx except FileNotFoundError: - warnings.warn( - f"'{nvidia_gpu_interface_path}' is not available. Multi-GPU support will be disabled. This is expected " - "on WSL2 where the NVIDIA proc interface is not mounted.", - stacklevel=2, - ) + # Fallback to nvidia-smi if /proc interface is not available + try: + import subprocess + + output = ( + subprocess.check_output( + ["nvidia-smi", "--query-gpu=uuid", "--format=csv,noheader,nounits"], + stderr=subprocess.STDOUT, + timeout=10, + ) + .decode("utf-8") + .strip() + ) + # Parse output like "GPU-xxxx\nGPU-yyyy\n..." + uuids = [line.strip() for line in output.split("\n") if line.strip()] + for device_idx, uuid in enumerate(uuids): + if uuid == f"GPU-{device_uuid}": + return device_idx + # If not found, return -1 to indicate error + return -1 + except (subprocess.SubprocessError, FileNotFoundError): + warnings.warn( + f"'{nvidia_gpu_interface_path}' is not available and nvidia-smi failed. Multi-GPU support will be disabled. This is expected " + "on WSL2 where the NVIDIA proc interface is not mounted.", + stacklevel=2, + ) return -1