diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 02fd7e53082..21ba73bcd95 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,8 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.44.10" +version = "0.45.0" + # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index b4ac417adcf..dbd4a090bec 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ Changelog --------- +0.45.0 (2025-08-07) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.track_contact_points` to toggle tracking of contact + point locations between sensor bodies and filtered bodies. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorCfg.max_contact_data_per_prim` to configure the maximum + amount of contacts per sensor body. +* Added :attr:`~isaaclab.sensors.contact_sensor.ContactSensorData.contact_pos_w` data field for tracking contact point + locations. + + 0.44.12 (2025-08-12) ~~~~~~~~~~~~~~~~~~~ @@ -31,6 +45,7 @@ instantaneous term done count at reset. This let to inaccurate aggregation of te happeningduring the traing. Instead we log the episodic term done. + 0.44.9 (2025-07-30) ~~~~~~~~~~~~~~~~~~~ @@ -330,7 +345,6 @@ Changed Added ^^^^^ - * Added unit test for :func:`~isaaclab.utils.math.quat_inv`. Fixed diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py index f9679715936..ba2a019ef64 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor.py @@ -159,6 +159,11 @@ def reset(self, env_ids: Sequence[int] | None = None): self._data.last_air_time[env_ids] = 0.0 self._data.current_contact_time[env_ids] = 0.0 self._data.last_contact_time[env_ids] = 0.0 + # reset contact positions + if self.cfg.track_contact_points: + self._data.contact_pos_w[env_ids, :] = torch.nan + # buffer used during contact position aggregation + self._contact_position_aggregate_buffer[env_ids, :] = torch.nan def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: """Find bodies in the articulation based on the name keys. @@ -277,7 +282,9 @@ def _initialize_impl(self): # create a rigid prim view for the sensor self._body_physx_view = self._physics_sim_view.create_rigid_body_view(body_names_glob) self._contact_physx_view = self._physics_sim_view.create_rigid_contact_view( - body_names_glob, filter_patterns=filter_prim_paths_glob + body_names_glob, + filter_patterns=filter_prim_paths_glob, + max_contact_data_count=self.cfg.max_contact_data_count_per_prim * len(body_names) * self._num_envs, ) # resolve the true count of bodies self._num_bodies = self.body_physx_view.count // self._num_envs @@ -303,6 +310,19 @@ def _initialize_impl(self): if self.cfg.track_pose: self._data.pos_w = torch.zeros(self._num_envs, self._num_bodies, 3, device=self._device) self._data.quat_w = torch.zeros(self._num_envs, self._num_bodies, 4, device=self._device) + # -- position of contact points + if self.cfg.track_contact_points: + self._data.contact_pos_w = torch.full( + (self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3), + torch.nan, + device=self._device, + ) + # buffer used during contact position aggregation + self._contact_position_aggregate_buffer = torch.full( + (self._num_bodies * self._num_envs, self.contact_physx_view.filter_count, 3), + torch.nan, + device=self._device, + ) # -- air/contact time between contacts if self.cfg.track_air_time: self._data.last_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device) @@ -357,6 +377,35 @@ def _update_buffers_impl(self, env_ids: Sequence[int]): pose[..., 3:] = convert_quat(pose[..., 3:], to="wxyz") self._data.pos_w[env_ids], self._data.quat_w[env_ids] = pose.split([3, 4], dim=-1) + # obtain contact points + if self.cfg.track_contact_points: + _, buffer_contact_points, _, _, buffer_count, buffer_start_indices = ( + self.contact_physx_view.get_contact_data(dt=self._sim_physics_dt) + ) + # unpack the contact points: see RigidContactView.get_contact_data() documentation for details: + # https://docs.omniverse.nvidia.com/kit/docs/omni_physics/107.3/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.RigidContactView.get_net_contact_forces + # buffer_count: (N_envs * N_bodies, N_filters), buffer_contact_points: (N_envs * N_bodies, 3) + counts, starts = buffer_count.view(-1), buffer_start_indices.view(-1) + n_rows, total = counts.numel(), int(counts.sum()) + # default to NaN rows + agg = torch.full((n_rows, 3), float("nan"), device=self._device, dtype=buffer_contact_points.dtype) + if total > 0: + row_ids = torch.repeat_interleave(torch.arange(n_rows, device=self._device), counts) + total = row_ids.numel() + + block_starts = counts.cumsum(0) - counts + deltas = torch.arange(total, device=counts.device) - block_starts.repeat_interleave(counts) + flat_idx = starts[row_ids] + deltas + + pts = buffer_contact_points.index_select(0, flat_idx) + agg = agg.zero_().index_add_(0, row_ids, pts) / counts.clamp_min(1).unsqueeze(1) + agg[counts == 0] = float("nan") + + self._contact_position_aggregate_buffer[:] = agg.view(self._num_envs * self.num_bodies, -1, 3) + self._data.contact_pos_w[env_ids] = self._contact_position_aggregate_buffer.view( + self._num_envs, self._num_bodies, self.contact_physx_view.filter_count, 3 + )[env_ids] + # obtain the air time if self.cfg.track_air_time: # -- time elapsed since last update diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py index fcaa6bc1220..c51b09473bb 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_cfg.py @@ -20,6 +20,22 @@ class ContactSensorCfg(SensorBaseCfg): track_pose: bool = False """Whether to track the pose of the sensor's origin. Defaults to False.""" + track_contact_points: bool = False + """Whether to track the contact point locations. Defaults to False.""" + + max_contact_data_count_per_prim: int = 4 + """The maximum number of contacts across all batches of the sensor to keep track of. Default is 4. + + This parameter sets the total maximum counts of the simulation across all bodies and environments. The total number + of contacts allowed is max_contact_data_count_per_prim*num_envs*num_sensor_bodies. + + .. note:: + + If the environment is very contact rich it is suggested to increase this parameter to avoid out of bounds memory + errors and loss of contact data leading to inaccurate measurements. + + """ + track_air_time: bool = False """Whether to track the air/contact time of the bodies (time between contacts). Defaults to False.""" @@ -49,6 +65,7 @@ class ContactSensorCfg(SensorBaseCfg): single primitive in that environment. If the sensor primitive corresponds to multiple primitives, the filtering will not work as expected. Please check :class:`~isaaclab.sensors.contact_sensor.ContactSensor` for more details. + If track_contact_points is true, then filter_prim_paths_expr cannot be an empty list! """ visualizer_cfg: VisualizationMarkersCfg = CONTACT_SENSOR_MARKER_CFG.replace(prim_path="/Visuals/ContactSensor") diff --git a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py index 34063251c88..63edc8b127a 100644 --- a/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py +++ b/source/isaaclab/isaaclab/sensors/contact_sensor/contact_sensor_data.py @@ -23,6 +23,21 @@ class ContactSensorData: If the :attr:`ContactSensorCfg.track_pose` is False, then this quantity is None. """ + contact_pos_w: torch.Tensor | None = None + """Average of the positions of contact points between sensor body and filter prim in world frame. + + Shape is (N, B, M, 3), where N is the number of sensors, B is number of bodies in each sensor + and M is the number of filtered bodies. + + Collision pairs not in contact will result in nan. + + Note: + If the :attr:`ContactSensorCfg.track_contact_points` is False, then this quantity is None. + If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is an empty tensor. + If the :attr:`ContactSensorCfg.max_contact_data_per_prim` is not specified or less than 1, then this quantity + will not be calculated. + """ + quat_w: torch.Tensor | None = None """Orientation of the sensor origin in quaternion (w, x, y, z) in world frame. diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 5b3b13161b9..30d2c9be437 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -20,7 +20,7 @@ # add argparse arguments parser = argparse.ArgumentParser(description="Contact Sensor Test Script") -parser.add_argument("--num_robots", type=int, default=64, help="Number of robots to spawn.") +parser.add_argument("--num_robots", type=int, default=128, help="Number of robots to spawn.") # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) @@ -45,6 +45,7 @@ import isaaclab.sim as sim_utils from isaaclab.assets import Articulation from isaaclab.sensors.contact_sensor import ContactSensor, ContactSensorCfg +from isaaclab.utils.timer import Timer ## # Pre-defined configs @@ -63,9 +64,8 @@ def design_scene(): cfg = sim_utils.GroundPlaneCfg() cfg.func("/World/defaultGroundPlane", cfg) # Lights - cfg = sim_utils.SphereLightCfg() - cfg.func("/World/Light/GreySphere", cfg, translation=(4.5, 3.5, 10.0)) - cfg.func("/World/Light/WhiteSphere", cfg, translation=(-4.5, 3.5, 10.0)) + cfg = sim_utils.DomeLightCfg(intensity=2000) + cfg.func("/World/Light/DomeLight", cfg, translation=(-4.5, 3.5, 10.0)) """ @@ -103,7 +103,11 @@ def main(): robot = Articulation(cfg=robot_cfg) # Contact sensor contact_sensor_cfg = ContactSensorCfg( - prim_path="/World/envs/env_.*/Robot/.*_SHANK", track_air_time=True, debug_vis=not args_cli.headless + prim_path="/World/envs/env_.*/Robot/.*_FOOT", + track_air_time=True, + track_contact_points=True, + debug_vis=False, # not args_cli.headless, + filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"], ) contact_sensor = ContactSensor(cfg=contact_sensor_cfg) # filter collisions within each environment instance @@ -126,6 +130,7 @@ def main(): sim_dt = decimation * physics_dt sim_time = 0.0 count = 0 + dt = [] # Simulate physics while simulation_app.is_running(): # If simulation is stopped, then exit. @@ -136,14 +141,20 @@ def main(): sim.step(render=False) continue # reset - if count % 1000 == 0: + if count % 1000 == 0 and count != 0: # reset counters sim_time = 0.0 count = 0 + print("=" * 80) + print("avg dt real-time", sum(dt) / len(dt)) + print("=" * 80) + # reset dof state joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel robot.write_joint_state_to_sim(joint_pos, joint_vel) robot.reset() + dt = [] + # perform 4 steps for _ in range(decimation): # apply actions @@ -159,6 +170,10 @@ def main(): count += 1 # update the buffers if sim.is_playing(): + with Timer() as timer: + contact_sensor.update(sim_dt, force_recompute=True) + dt.append(timer.time_elapsed) + contact_sensor.update(sim_dt, force_recompute=True) if count % 100 == 0: print("Sim-time: ", sim_time) diff --git a/source/isaaclab/test/sensors/test_contact_sensor.py b/source/isaaclab/test/sensors/test_contact_sensor.py index bc281562fb6..3c1c6bdf86a 100644 --- a/source/isaaclab/test/sensors/test_contact_sensor.py +++ b/source/isaaclab/test/sensors/test_contact_sensor.py @@ -414,33 +414,63 @@ def _run_contact_sensor_test( """ for device in devices: for terrain in terrains: - with build_simulation_context(device=device, dt=sim_dt, add_lighting=True) as sim: - sim._app_control_on_stop_handle = None - scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) - scene_cfg.terrain = terrain - scene_cfg.shape = shape_cfg - scene_cfg.contact_sensor = ContactSensorCfg( - prim_path=shape_cfg.prim_path, - track_pose=True, - debug_vis=False, - update_period=0.0, - track_air_time=True, - history_length=3, - ) - scene = InteractiveScene(scene_cfg) - - # Check that contact processing is enabled - assert not carb_settings_iface.get("/physics/disableContactProcessing") - - # Play the simulator - sim.reset() - - _test_sensor_contact( - scene["shape"], scene["contact_sensor"], ContactTestMode.IN_CONTACT, sim, scene, sim_dt, durations - ) - _test_sensor_contact( - scene["shape"], scene["contact_sensor"], ContactTestMode.NON_CONTACT, sim, scene, sim_dt, durations - ) + for track_contact_points in [True, False]: + with build_simulation_context(device=device, dt=sim_dt, add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + + scene_cfg = ContactSensorSceneCfg(num_envs=1, env_spacing=1.0, lazy_sensor_update=False) + scene_cfg.terrain = terrain + scene_cfg.shape = shape_cfg + test_contact_position = False + if (type(shape_cfg.spawn) is sim_utils.SphereCfg) and (terrain.terrain_type == "plane"): + test_contact_position = True + elif track_contact_points: + continue + + if track_contact_points: + if terrain.terrain_type == "plane": + filter_prim_paths_expr = [terrain.prim_path + "/terrain/GroundPlane/CollisionPlane"] + elif terrain.terrain_type == "generator": + filter_prim_paths_expr = [terrain.prim_path + "/terrain/mesh"] + else: + filter_prim_paths_expr = [] + + scene_cfg.contact_sensor = ContactSensorCfg( + prim_path=shape_cfg.prim_path, + track_pose=True, + debug_vis=False, + update_period=0.0, + track_air_time=True, + history_length=3, + track_contact_points=track_contact_points, + filter_prim_paths_expr=filter_prim_paths_expr, + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulation + sim.reset() + + # Run contact time and air time tests. + _test_sensor_contact( + shape=scene["shape"], + sensor=scene["contact_sensor"], + mode=ContactTestMode.IN_CONTACT, + sim=sim, + scene=scene, + sim_dt=sim_dt, + durations=durations, + test_contact_position=test_contact_position, + ) + _test_sensor_contact( + shape=scene["shape"], + sensor=scene["contact_sensor"], + mode=ContactTestMode.NON_CONTACT, + sim=sim, + scene=scene, + sim_dt=sim_dt, + durations=durations, + test_contact_position=test_contact_position, + ) def _test_sensor_contact( @@ -451,6 +481,7 @@ def _test_sensor_contact( scene: InteractiveScene, sim_dt: float, durations: list[float], + test_contact_position: bool = False, ): """Test for the contact sensor. @@ -517,6 +548,8 @@ def _test_sensor_contact( expected_last_air_time=expected_last_test_contact_time, dt=duration + sim_dt, ) + if test_contact_position: + _test_contact_position(shape, sensor, mode) # switch the contact mode for 1 dt step before the next contact test begins. shape.write_root_pose_to_sim(root_pose=reset_pose) # perform simulation step @@ -527,6 +560,32 @@ def _test_sensor_contact( expected_last_reset_contact_time = 2 * sim_dt +def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: ContactTestMode) -> None: + """Test for the contact positions (only implemented for sphere and flat terrain) + checks that the contact position is radius distance away from the root of the object + Args: + shape: The contact prim used for the contact sensor test. + sensor: The sensor reporting data to be verified by the contact sensor test. + mode: The contact test mode: either contact with ground plane or air time. + """ + if sensor.cfg.track_contact_points: + # check shape of the contact_pos_w tensor + num_bodies = sensor.num_bodies + assert sensor._data.contact_pos_w.shape == (sensor.num_instances / num_bodies, num_bodies, 1, 3) + # check contact positions + if mode == ContactTestMode.IN_CONTACT: + contact_position = sensor._data.pos_w + torch.tensor( + [[0.0, 0.0, -shape.cfg.spawn.radius]], device=sensor._data.pos_w.device + ) + assert torch.all( + torch.abs(torch.norm(sensor._data.contact_pos_w - contact_position.unsqueeze(1), p=2, dim=-1)) < 1e-2 + ).item() + elif mode == ContactTestMode.NON_CONTACT: + assert torch.all(torch.isnan(sensor._data.contact_pos_w)).item() + else: + assert sensor._data.contact_pos_w is None + + def _check_prim_contact_state_times( sensor: ContactSensor, expected_air_time: float,