diff --git a/.gitignore b/.gitignore index 08d2e8dee5a..6c4b246577c 100644 --- a/.gitignore +++ b/.gitignore @@ -69,3 +69,5 @@ tests/ # Docker history .isaac-lab-docker-history + +**/tactile_record/* diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index fb5161181b1..89a63e5b52a 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -84,6 +84,7 @@ Guidelines for modifications: * Jingzhou Liu * Jinqi Wei * Johnson Sun +* Juana Du * Kaixi Bao * Kris Wilson * Kourosh Darvish diff --git a/docs/conf.py b/docs/conf.py index 00d7af5ae59..6c0dd9660a8 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -190,6 +190,8 @@ "nvidia.srl", "flatdict", "IPython", + "cv2", + "imageio", "ipywidgets", "mpl_toolkits", ] diff --git a/docs/source/_static/overview/sensors/tacsl_demo.jpg b/docs/source/_static/overview/sensors/tacsl_demo.jpg new file mode 100644 index 00000000000..a723d5ea8cb Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_demo.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_diagram.jpg b/docs/source/_static/overview/sensors/tacsl_diagram.jpg new file mode 100644 index 00000000000..323c9a77e8d Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_diagram.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg b/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg new file mode 100644 index 00000000000..eeca0295a2f Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_force_field_example.jpg differ diff --git a/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg b/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg new file mode 100644 index 00000000000..37e8dab491b Binary files /dev/null and b/docs/source/_static/overview/sensors/tacsl_taxim_example.jpg differ diff --git a/docs/source/api/lab/isaaclab.sensors.rst b/docs/source/api/lab/isaaclab.sensors.rst index c30ed948f09..a4120d37bd0 100644 --- a/docs/source/api/lab/isaaclab.sensors.rst +++ b/docs/source/api/lab/isaaclab.sensors.rst @@ -33,6 +33,9 @@ RayCasterCameraCfg Imu ImuCfg + VisuoTactileSensor + VisuoTactileSensorCfg + VisuoTactileSensorData Sensor Base ----------- @@ -166,3 +169,22 @@ Inertia Measurement Unit :inherited-members: :show-inheritance: :exclude-members: __init__, class_type + +Visuo-Tactile Sensor +-------------------- + +.. autoclass:: VisuoTactileSensor + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: VisuoTactileSensorData + :members: + :inherited-members: + :exclude-members: __init__ + +.. autoclass:: VisuoTactileSensorCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type diff --git a/docs/source/overview/core-concepts/sensors/index.rst b/docs/source/overview/core-concepts/sensors/index.rst index 31baaa9258b..d2c63f212b7 100644 --- a/docs/source/overview/core-concepts/sensors/index.rst +++ b/docs/source/overview/core-concepts/sensors/index.rst @@ -19,3 +19,4 @@ The following pages describe the available sensors in more detail: frame_transformer imu ray_caster + visuo_tactile_sensor diff --git a/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst b/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst new file mode 100644 index 00000000000..ae46ec47bed --- /dev/null +++ b/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst @@ -0,0 +1,201 @@ +.. _overview_sensors_tactile: + +.. currentmodule:: isaaclab + +Visuo-Tactile Sensor +==================== + + +The visuo-tactile sensor in Isaac Lab provides realistic tactile feedback through integration with TacSL (Tactile Sensor Learning) [Akinola2025]_. It is designed to simulate high-fidelity tactile interactions, generating both visual and force-based data that mirror real-world tactile sensors like GelSight devices. The sensor can provide tactile RGB images, force field distributions, and other intermediate tactile measurements essential for robotic manipulation tasks requiring fine tactile feedback. + + +.. figure:: ../../../_static/overview/sensors/tacsl_diagram.jpg + :align: center + :figwidth: 100% + :alt: Tactile sensor with RGB visualization and force fields + + +Configuration +~~~~~~~~~~~~~ + +Tactile sensors require specific configuration parameters to define their behavior and data collection properties. The sensor can be configured with various parameters including sensor resolution, force sensitivity, and output data types. + +.. code-block:: python + + from isaaclab.sensors.tacsl_sensor import VisuoTactileSensorCfg + from isaaclab_assets.sensors import GELSIGHT_R15_CFG + import isaaclab.sim as sim_utils + + # Tactile sensor configuration + tactile_sensor = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer/tactile_sensor", + ## Sensor configuration + render_cfg=GELSIGHT_R15_CFG, + enable_camera_tactile=True, + enable_force_field=True, + ## Elastomer configuration + num_tactile_rows=20, + num_tactile_cols=25, + tactile_margin=0.003, + ## Contact object configuration + contact_object_prim_path_expr="{ENV_REGEX_NS}/contact_object", + ## Force field physics parameters + tactile_kn=1.0, + tactile_mu=2.0, + tactile_kt=0.1, + ## Camera configuration + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/cam", + update_period=1 / 60, # 60 Hz + height=320, + width=240, + data_types=["distance_to_image_plane"], + spawn=None, # camera already spawned in USD file + ), + ) + +The configuration supports customization of: + +* **Render Configuration**: Specify the GelSight sensor rendering parameters using predefined configs + (e.g., ``GELSIGHT_R15_CFG``, ``GELSIGHT_MINI_CFG`` from ``isaaclab_assets.sensors``) +* **Tactile Modalities**: + * ``enable_camera_tactile`` - Enable tactile RGB imaging through camera sensors + * ``enable_force_field`` - Enable force field computation and visualization +* **Force Field Grid**: Set tactile grid dimensions (``num_tactile_rows``, ``num_tactile_cols``) and margins, which directly affects the spatial resolution of the computed force field +* **Contact Object Configuration**: Define properties of interacting objects using prim path expressions to locate objects with SDF collision meshes +* **Physics Parameters**: Control the sensor's force field computation: + * ``tactile_kn``, ``tactile_mu``, ``tactile_kt`` - Normal stiffness, friction coefficient, and tangential stiffness +* **Camera Settings**: Configure resolution, update rates, and data types, currently only ``distance_to_image_plane`` (alias for ``depth``) is supported. + ``spawn`` is set to ``None`` by default, which means that the camera is already spawned in the USD file. + If you want to spawn the camera yourself and set focal length, etc., you can set the spawn configuration to a valid spawn configuration. + +Configuration Requirements +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. important:: + The following requirements must be satisfied for proper sensor operation: + + **Camera Tactile Imaging** + If ``enable_camera_tactile=True``, a valid ``camera_cfg`` (TiledCameraCfg) must be provided with appropriate camera parameters. + + **Force Field Computation** + If ``enable_force_field=True``, the following parameters are required: + + * ``contact_object_prim_path_expr`` - Prim path expression to locate contact objects with SDF collision meshes + + **SDF Computation** + When force field computation is enabled, penalty-based normal and shear forces are computed using Signed Distance Field (SDF) queries. To achieve GPU acceleration: + + * Interacting objects should have SDF collision meshes + * An SDFView must be defined during initialization, therefore interacting objects should be specified before simulation. + + **Elastomer Configuration** + The sensor's ``prim_path`` must be configured as a child of the elastomer prim in the USD hierarchy. + The query points for the force field computation is computed from the surface of the elastomer mesh, which is searched for under the prim path of the elastomer. + + **Physics Materials** + The sensor uses physics materials to configure the compliant contact properties of the elastomer. + By default, physics material properties are pre-configured in the USD asset. However, you can override + these properties by specifying the following parameters in ``UsdFileWithPhysicsMaterialOnPrimsCfg`` when + spawning the robot: + + * ``compliant_contact_stiffness`` - Contact stiffness for the elastomer surface + * ``compliant_contact_damping`` - Contact damping for the elastomer surface + * ``apply_physics_material_prim_path`` - Prim path where physics material is applied (typically ``"elastomer"``) + + If any parameter is set to ``None``, the corresponding property from the USD asset will be retained. + + +Usage Example +~~~~~~~~~~~~~ + +To use the tactile sensor in a simulation environment, run the demo: + +.. code-block:: bash + + cd scripts/demos/sensors/tacsl + python tacsl_example.py --use_tactile_rgb --use_tactile_ff --tactile_compliance_stiffness 100.0 --tactile_compliant_damping 1.0 --contact_object_type nut --num_envs 16 --save_viz --enable_cameras + +Available command-line options include: + +* ``--use_tactile_rgb``: Enable camera-based tactile sensing +* ``--use_tactile_ff``: Enable force field tactile sensing +* ``--contact_object_type``: Specify the type of contact object (nut, cube, etc.) +* ``--num_envs``: Number of parallel environments +* ``--save_viz``: Save visualization outputs for analysis +* ``--tactile_compliance_stiffness``: Override compliant contact stiffness (default: use USD asset values) +* ``--tactile_compliant_damping``: Override compliant contact damping (default: use USD asset values) +* ``--tactile_kn``: Normal contact stiffness for force field computation +* ``--tactile_kt``: Tangential stiffness for shear forces +* ``--tactile_mu``: Friction coefficient for shear forces +* ``--debug_sdf_closest_pts``: Visualize closest SDF points for debugging +* ``--debug_tactile_sensor_pts``: Visualize tactile sensor points for debugging +* ``--trimesh_vis_tactile_points``: Enable trimesh-based visualization of tactile points + +For a complete list of available options: + +.. code-block:: bash + + python tacsl_example.py -h + +.. note:: + The demo examples are based on the Gelsight R1.5, which is a prototype sensor that is now discontinued. The same procedure can be adapted for other visuotactile sensors. + +.. figure:: ../../../_static/overview/sensors/tacsl_demo.jpg + :align: center + :figwidth: 100% + :alt: TacSL tactile sensor demo showing RGB tactile images and force field visualizations + +The tactile sensor supports multiple data modalities that provide comprehensive information about contact interactions: + + +Output Tactile Data +~~~~~~~~~~~~~~~~~~~ +**RGB Tactile Images** + Real-time generation of tactile RGB images as objects make contact with the sensor surface. These images show deformation patterns and contact geometry similar to gel-based tactile sensors [Si2022]_ + + +**Force Fields** + Detailed contact force field and pressure distributions across the sensor surface, including normal and shear components. + +.. list-table:: + :widths: 50 50 + :class: borderless + + * - .. figure:: ../../../_static/overview/sensors/tacsl_taxim_example.jpg + :align: center + :figwidth: 80% + :alt: Tactile output with RGB visualization + + - .. figure:: ../../../_static/overview/sensors/tacsl_force_field_example.jpg + :align: center + :figwidth: 80% + :alt: Tactile output with force field visualization + +Integration with Learning Frameworks +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The tactile sensor is designed to integrate seamlessly with reinforcement learning and imitation learning frameworks. The structured tensor outputs can be directly used as observations in learning algorithms: + +.. code-block:: python + + def get_tactile_observations(self): + """Extract tactile observations for learning.""" + tactile_data = self.scene["tactile_sensor"].data + + # tactile RGB image + tactile_rgb = tactile_data.tactile_rgb_image + + # force field + tactile_normal_force = tactile_data.tactile_normal_force + tactile_shear_force = tactile_data.tactile_shear_force + + return [tactile_rgb, tactile_normal_force, tactile_shear_force] + + + +References +~~~~~~~~~~ + +.. [Akinola2025] Akinola, I., Xu, J., Carius, J., Fox, D., & Narang, Y. (2025). TacSL: A library for visuotactile sensor simulation and learning. *IEEE Transactions on Robotics*. +.. [Si2022] Si, Z., & Yuan, W. (2022). Taxim: An example-based simulation model for GelSight tactile sensors. *IEEE Robotics and Automation Letters*, 7(2), 2361-2368. diff --git a/scripts/demos/sensors/tacsl/tacsl_example.py b/scripts/demos/sensors/tacsl/tacsl_example.py new file mode 100644 index 00000000000..51a68e0226d --- /dev/null +++ b/scripts/demos/sensors/tacsl/tacsl_example.py @@ -0,0 +1,386 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Example script demonstrating the TacSL tactile sensor implementation in IsaacLab. + +This script shows how to use the TactileSensor for both camera-based and force field +tactile sensing with the gelsight finger setup. + +.. code-block:: bash + + # Usage + python tacsl_example.py --use_tactile_rgb --use_tactile_ff --tactile_compliance_stiffness 100.0 --num_envs 16 --contact_object_type nut --save_viz --enable_cameras + +""" + +import argparse +import math +import numpy as np +import os +import torch + +import cv2 + +from isaaclab.app import AppLauncher +from isaaclab.utils.timer import Timer + +# Add argparse arguments +parser = argparse.ArgumentParser(description="TacSL tactile sensor example.") +parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.") +parser.add_argument("--tactile_kn", type=float, default=1.0, help="Tactile normal stiffness.") +parser.add_argument("--tactile_kt", type=float, default=0.1, help="Tactile tangential stiffness.") +parser.add_argument("--tactile_mu", type=float, default=2.0, help="Tactile friction coefficient.") +parser.add_argument( + "--tactile_compliance_stiffness", + type=float, + default=None, + help="Optional: Override compliant contact stiffness (default: use USD asset values)", +) +parser.add_argument( + "--tactile_compliant_damping", + type=float, + default=None, + help="Optional: Override compliant contact damping (default: use USD asset values)", +) +parser.add_argument("--save_viz", action="store_true", help="Visualize tactile data.") +parser.add_argument("--save_viz_dir", type=str, default="tactile_record", help="Directory to save tactile data.") +parser.add_argument("--use_tactile_rgb", action="store_true", help="Use tactile RGB sensor data collection.") +parser.add_argument("--use_tactile_ff", action="store_true", help="Use tactile force field sensor data collection.") +parser.add_argument("--debug_sdf_closest_pts", action="store_true", help="Visualize closest SDF points.") +parser.add_argument("--debug_tactile_sensor_pts", action="store_true", help="Visualize tactile sensor points.") +parser.add_argument("--trimesh_vis_tactile_points", action="store_true", help="Visualize tactile points using trimesh.") +parser.add_argument( + "--contact_object_type", + type=str, + default="nut", + choices=["none", "cube", "nut"], + help="Type of contact object to use.", +) + +# Append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# Parse the arguments +args_cli = parser.parse_args() + +# Launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +from isaaclab_assets.sensors import GELSIGHT_R15_CFG + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg + +# Import our TactileSensor +from isaaclab.sensors import TiledCameraCfg, VisuoTactileSensorCfg +from isaaclab.sensors.tacsl_sensor.gelsight_utils import visualize_tactile_shear_image +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + + +@configclass +class TactileSensorsSceneCfg(InteractiveSceneCfg): + """Design the scene with tactile sensors on the robot.""" + + # Ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # Lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Robot with tactile sensor + robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.UsdFileWithPhysicsMaterialOnPrimsCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + max_depenetration_velocity=5.0, + ), + compliant_contact_stiffness=args_cli.tactile_compliance_stiffness, + compliant_contact_damping=args_cli.tactile_compliant_damping, + apply_physics_material_prim_path="elastomer", + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + ), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.001, rest_offset=-0.0005), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(math.sqrt(2) / 2, -math.sqrt(2) / 2, 0.0, 0.0), # 90° rotation + joint_pos={}, + joint_vel={}, + ), + actuators={}, + ) + + # Camera configuration for tactile sensing + + # TacSL Tactile Sensor + tactile_sensor = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer/tactile_sensor", + history_length=0, + debug_vis=args_cli.debug_tactile_sensor_pts or args_cli.debug_sdf_closest_pts, + # Sensor configuration + render_cfg=GELSIGHT_R15_CFG, + enable_camera_tactile=args_cli.use_tactile_rgb, + enable_force_field=args_cli.use_tactile_ff, + # Elastomer configuration + num_tactile_rows=20, + num_tactile_cols=25, + tactile_margin=0.003, + # Contact object configuration + contact_object_prim_path_expr="{ENV_REGEX_NS}/contact_object", + # Force field physics parameters + tactile_kn=args_cli.tactile_kn, + tactile_mu=args_cli.tactile_mu, + tactile_kt=args_cli.tactile_kt, + # Camera configuration + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/cam", + update_period=1 / 60, # 60 Hz + height=GELSIGHT_R15_CFG.image_height, + width=GELSIGHT_R15_CFG.image_width, + data_types=["distance_to_image_plane"], + spawn=None, # the camera is already spawned in the scene, properties are set in the gelsight_r15_finger.usd file + ), + # Debug Visualization + trimesh_vis_tactile_points=args_cli.trimesh_vis_tactile_points, + visualize_sdf_closest_pts=args_cli.debug_sdf_closest_pts, + ) + + +@configclass +class CubeTactileSceneCfg(TactileSensorsSceneCfg): + """Scene with cube contact object.""" + + # Cube contact object + contact_object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/contact_object", + spawn=sim_utils.CuboidCfg( + size=(0.01, 0.01, 0.01), + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=0.00327211), + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=sim_utils.RigidBodyMaterialCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.1, 0.1)), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0 + 0.06776, 0.51), rot=(1.0, 0.0, 0.0, 0.0)), + ) + + +@configclass +class NutTactileSceneCfg(TactileSensorsSceneCfg): + """Scene with nut contact object.""" + + # Nut contact object + contact_object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/contact_object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_nut_m16.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + solver_position_iteration_count=12, + solver_velocity_iteration_count=1, + max_angular_velocity=180.0, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.1), + collision_props=sim_utils.CollisionPropertiesCfg(contact_offset=0.005, rest_offset=0), + articulation_props=sim_utils.ArticulationRootPropertiesCfg(articulation_enabled=False), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.0 + 0.06776, 0.498), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + +def mkdir_helper(dir_path): + tactile_img_folder = dir_path + os.makedirs(tactile_img_folder, exist_ok=True) + tactile_force_field_dir = os.path.join(tactile_img_folder, "tactile_force_field") + os.makedirs(tactile_force_field_dir, exist_ok=True) + tactile_rgb_image_dir = os.path.join(tactile_img_folder, "tactile_rgb_image") + os.makedirs(tactile_rgb_image_dir, exist_ok=True) + return tactile_force_field_dir, tactile_rgb_image_dir + + +def save_viz_helper(dir_path_list, count, tactile_data, num_envs, nrows, ncols): + # Only save the first 2 environments + + tactile_force_field_dir, tactile_rgb_image_dir = dir_path_list + + if tactile_data.tactile_shear_force is not None and tactile_data.tactile_normal_force is not None: + # visualize tactile forces + tactile_normal_force = tactile_data.tactile_normal_force.view((num_envs, nrows, ncols)) + tactile_shear_force = tactile_data.tactile_shear_force.view((num_envs, nrows, ncols, 2)) + + tactile_image = visualize_tactile_shear_image( + tactile_normal_force[0, :, :].detach().cpu().numpy(), tactile_shear_force[0, :, :].detach().cpu().numpy() + ) + + if tactile_normal_force.shape[0] > 1: + tactile_image_1 = visualize_tactile_shear_image( + tactile_normal_force[1, :, :].detach().cpu().numpy(), + tactile_shear_force[1, :, :].detach().cpu().numpy(), + ) + combined_image = np.vstack([tactile_image, tactile_image_1]) + cv2.imwrite(os.path.join(tactile_force_field_dir, f"{count}.png"), (combined_image * 255).astype(np.uint8)) + else: + cv2.imwrite(os.path.join(tactile_force_field_dir, f"{count}.png"), (tactile_image * 255).astype(np.uint8)) + + if tactile_data.tactile_rgb_image is not None: + tactile_rgb_data = tactile_data.tactile_rgb_image.cpu().numpy() + tactile_rgb_data = np.transpose(tactile_rgb_data, axes=(0, 2, 1, 3)) + tactile_rgb_data_first_2 = tactile_rgb_data[:2] if len(tactile_rgb_data) >= 2 else tactile_rgb_data + tactile_rgb_tiled = np.concatenate(tactile_rgb_data_first_2, axis=0) + # Convert to uint8 if not already + if tactile_rgb_tiled.dtype != np.uint8: + tactile_rgb_tiled = ( + (tactile_rgb_tiled * 255).astype(np.uint8) + if tactile_rgb_tiled.max() <= 1.0 + else tactile_rgb_tiled.astype(np.uint8) + ) + cv2.imwrite(os.path.join(tactile_rgb_image_dir, f"{count}.png"), tactile_rgb_tiled) + + +def run_simulator(sim, scene: InteractiveScene): + """Run the simulator.""" + # Define simulation stepping + sim_dt = sim.get_physics_dt() + sim_time = 0.0 + count = 0 + + # Assign different masses to contact objects in different environments + num_envs = scene.num_envs + + if args_cli.save_viz: + # Create output directories for tactile data + dir_path_list = mkdir_helper(args_cli.save_viz_dir) + + # Create constant downward force + force_tensor = torch.zeros(scene.num_envs, 1, 3, device=sim.device) + torque_tensor = torch.zeros(scene.num_envs, 1, 3, device=sim.device) + force_tensor[:, 0, 2] = -1.0 + + nrows = scene["tactile_sensor"].cfg.num_tactile_rows + ncols = scene["tactile_sensor"].cfg.num_tactile_cols + + physics_timer = Timer() + physics_total_time = 0.0 + physics_total_count = 0 + + scene.update(sim_dt) + + entity_list = ["robot"] + if "contact_object" in scene.keys(): + entity_list.append("contact_object") + + while simulation_app.is_running(): + + if count == 122: + print(scene["tactile_sensor"].get_timing_summary()) + # Reset robot and contact object positions + count = 0 + for entity in entity_list: + root_state = scene[entity].data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + scene[entity].write_root_state_to_sim(root_state) + + scene.reset() + print("[INFO]: Resetting robot and contact object state...") + + if "contact_object" in scene.keys(): + # rotation + if count > 20: + env_indices = torch.arange(scene.num_envs, device=sim.device) + odd_mask = env_indices % 2 == 1 + even_mask = env_indices % 2 == 0 + torque_tensor[odd_mask, 0, 2] = 10 # rotation for odd environments + torque_tensor[even_mask, 0, 2] = -10 # rotation for even environments + scene["contact_object"].set_external_force_and_torque(force_tensor, torque_tensor) + + # Step simulation + scene.write_data_to_sim() + physics_timer.start() + sim.step() + physics_timer.stop() + physics_total_time += physics_timer.total_run_time + physics_total_count += 1 + sim_time += sim_dt + count += 1 + scene.update(sim_dt) + + # Access tactile sensor data + tactile_data = scene["tactile_sensor"].data + + if args_cli.save_viz: + save_viz_helper(dir_path_list, count, tactile_data, num_envs, nrows, ncols) + + # Get timing summary from sensor and add physics timing + timing_summary = scene["tactile_sensor"].get_timing_summary() + + # Add physics timing to the summary + physics_avg = physics_total_time / (physics_total_count * scene.num_envs) if physics_total_count > 0 else 0.0 + timing_summary["physics_total"] = physics_total_time + timing_summary["physics_average"] = physics_avg + timing_summary["physics_fps"] = 1 / physics_avg if physics_avg > 0 else 0.0 + + print(timing_summary) + + +def main(): + """Main function.""" + # Initialize simulation + sim_cfg = sim_utils.SimulationCfg( + dt=0.005, + device=args_cli.device, + physx=sim_utils.PhysxCfg( + gpu_collision_stack_size=2**30, # Prevent collisionStackSize buffer overflow in contact-rich environments. + ), + ) + sim = sim_utils.SimulationContext(sim_cfg) + + # Set main camera + sim.set_camera_view(eye=[1.5, 1.5, 1.5], target=[0.0, 0.0, 0.0]) + + # Create scene based on contact object type + if args_cli.contact_object_type == "cube": + scene_cfg = CubeTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + # disabled force field for cube contact object because a SDF collision mesh cannot be created for the Shape Prims + scene_cfg.tactile_sensor.enable_force_field = False + elif args_cli.contact_object_type == "nut": + scene_cfg = NutTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + elif args_cli.contact_object_type == "none": + scene_cfg = TactileSensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + scene_cfg.tactile_sensor.contact_object_prim_path_expr = None + # this flag is to visualize the tactile sensor points + scene_cfg.tactile_sensor.debug_vis = True + + scene = InteractiveScene(scene_cfg) + + # Initialize simulation + sim.reset() + print("[INFO]: Setup complete...") + + # Get initial render + scene["tactile_sensor"].get_initial_render() + # Run simulation + run_simulator(sim, scene) + + +if __name__ == "__main__": + # Run the main function + main() + # Close sim app + simulation_app.close() diff --git a/source/isaaclab/isaaclab/markers/config/__init__.py b/source/isaaclab/isaaclab/markers/config/__init__.py index 27f83022d31..e57135b2b14 100644 --- a/source/isaaclab/isaaclab/markers/config/__init__.py +++ b/source/isaaclab/isaaclab/markers/config/__init__.py @@ -47,6 +47,15 @@ ) """Configuration for the deformable object's kinematic target marker.""" +VISUO_TACTILE_SENSOR_MARKER_CFG = VisualizationMarkersCfg( + markers={ + "debug_pts": sim_utils.SphereCfg( + radius=0.0002, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ), + }, +) +"""Configuration for the visuo-tactile sensor marker.""" ## # Frames. diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 6772f77bba7..d5f0e3ea325 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -29,7 +29,7 @@ SurfaceGripper, SurfaceGripperCfg, ) -from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg +from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg, VisuoTactileSensorCfg from isaaclab.sim import SimulationContext from isaaclab.sim.utils import get_current_stage_id from isaaclab.terrains import TerrainImporter, TerrainImporterCfg @@ -770,6 +770,18 @@ def _add_entities_from_cfg(self): for filter_prim_path in asset_cfg.filter_prim_paths_expr: updated_filter_prim_paths_expr.append(filter_prim_path.format(ENV_REGEX_NS=self.env_regex_ns)) asset_cfg.filter_prim_paths_expr = updated_filter_prim_paths_expr + elif isinstance(asset_cfg, VisuoTactileSensorCfg): + if hasattr(asset_cfg, "camera_cfg") and asset_cfg.camera_cfg is not None: + asset_cfg.camera_cfg.prim_path = asset_cfg.camera_cfg.prim_path.format( + ENV_REGEX_NS=self.env_regex_ns + ) + if ( + hasattr(asset_cfg, "contact_object_prim_path_expr") + and asset_cfg.contact_object_prim_path_expr is not None + ): + asset_cfg.contact_object_prim_path_expr = asset_cfg.contact_object_prim_path_expr.format( + ENV_REGEX_NS=self.env_regex_ns + ) self._sensors[asset_name] = asset_cfg.class_type(asset_cfg) elif isinstance(asset_cfg, AssetBaseCfg): diff --git a/source/isaaclab/isaaclab/sensors/__init__.py b/source/isaaclab/isaaclab/sensors/__init__.py index 82340483d62..c12862554bf 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sensors/__init__.py @@ -32,6 +32,8 @@ +---------------------+---------------------------+---------------------------------------------------------------+ | Imu | /World/robot/base | Leaf exists and is a physics body (Rigid Body) | +---------------------+---------------------------+---------------------------------------------------------------+ +| Visuo-Tactile Sensor| /World/robot/base | Leaf exists and is a physics body (Rigid Body) | ++---------------------+---------------------------+---------------------------------------------------------------+ """ @@ -42,3 +44,4 @@ from .ray_caster import * # noqa: F401, F403 from .sensor_base import SensorBase # noqa: F401 from .sensor_base_cfg import SensorBaseCfg # noqa: F401 +from .tacsl_sensor import * # noqa: F401, F403 diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/__init__.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/__init__.py new file mode 100644 index 00000000000..a17cce52c42 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""TacSL Tactile Sensor implementation for IsaacLab.""" + +from .visuotactile_sensor import VisuoTactileSensor +from .visuotactile_sensor_cfg import GelSightRenderCfg, VisuoTactileSensorCfg +from .visuotactile_sensor_data import VisuoTactileSensorData diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/bg.jpg b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/bg.jpg new file mode 100644 index 00000000000..a86dd0009ee Binary files /dev/null and b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/bg.jpg differ diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/polycalib.npz b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/polycalib.npz new file mode 100644 index 00000000000..e004a1b21ac Binary files /dev/null and b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_r15_data/polycalib.npz differ diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_utils.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_utils.py new file mode 100644 index 00000000000..e00b84e41aa --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_utils.py @@ -0,0 +1,297 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import os +import scipy +import torch +from typing import TYPE_CHECKING + +import cv2 +import omni.log + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +if TYPE_CHECKING: + from .visuotactile_sensor_cfg import GelSightRenderCfg + + +def get_gelsight_render_data(base_data_path: str | None, data_dir: str, file_name: str) -> str | None: + """Gets the path for the GelSight render data file. + + If using a custom base path, the file is used directly from that location. + If using the default Nucleus path, the file is downloaded and cached locally. + + Args: + base_data_path: Base path for the sensor data. If None, defaults to + Isaac Lab Nucleus TacSL directory (will download and cache). + data_dir: The data directory name containing the render data. + file_name: The specific file name to retrieve. + + Returns: + The local path to the file, or None if unavailable. + """ + if base_data_path is not None: + # Custom path provided - use it directly without copying + file_path = os.path.join(base_data_path, data_dir, file_name) + if os.path.exists(file_path): + omni.log.info(f"Using custom GelSight render data: {file_path}") + return file_path + else: + raise FileNotFoundError(f"Custom GelSight render data not found: {file_path}") + else: + # Default to Isaac Lab Nucleus directory - download and cache + nucleus_path = os.path.join(ISAACLAB_NUCLEUS_DIR, "TacSL", data_dir, file_name) + cache_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), data_dir) + os.makedirs(cache_dir, exist_ok=True) + cache_path = os.path.join(cache_dir, file_name) + + if not os.path.exists(cache_path): + omni.log.info(f"Downloading GelSight render data from Nucleus: {nucleus_path}") + cache_path = retrieve_file_path(nucleus_path, cache_dir) + else: + omni.log.info(f"Using cached GelSight render data: {cache_path}") + return cache_path + + +def visualize_tactile_shear_image( + tactile_normal_force: np.ndarray, + tactile_shear_force: np.ndarray, + normal_force_threshold: float = 0.00008, + shear_force_threshold: float = 0.0005, + resolution: int = 30, +) -> np.ndarray: + """Visualize the tactile shear field. + + Args: + tactile_normal_force: Array of tactile normal forces. + tactile_shear_force: Array of tactile shear forces. + normal_force_threshold: Threshold for normal force visualization. Defaults to 0.00008. + shear_force_threshold: Threshold for shear force visualization. Defaults to 0.0005. + resolution: Resolution for the visualization. Defaults to 30. + + Returns: + Image visualizing the tactile shear forces. + """ + nrows = tactile_normal_force.shape[0] + ncols = tactile_normal_force.shape[1] + + imgs_tactile = np.zeros((nrows * resolution, ncols * resolution, 3), dtype=float) + + for row in range(nrows): + for col in range(ncols): + loc0_x = row * resolution + resolution // 2 + loc0_y = col * resolution + resolution // 2 + loc1_x = loc0_x + tactile_shear_force[row, col][0] / shear_force_threshold * resolution + loc1_y = loc0_y + tactile_shear_force[row, col][1] / shear_force_threshold * resolution + color = ( + 0.0, + max(0.0, 1.0 - tactile_normal_force[row][col] / normal_force_threshold), + min(1.0, tactile_normal_force[row][col] / normal_force_threshold), + ) + + cv2.arrowedLine( + imgs_tactile, (int(loc0_y), int(loc0_x)), (int(loc1_y), int(loc1_x)), color, 6, tipLength=0.4 + ) + + return imgs_tactile + + +def visualize_penetration_depth( + penetration_depth_img: np.ndarray, resolution: int = 5, depth_multiplier: float = 300.0 +) -> np.ndarray: + """Visualize the penetration depth. + + Args: + penetration_depth_img: Image of penetration depth. + resolution: Resolution for the upsampling. Defaults to 5. + depth_multiplier: Multiplier for the depth values. Defaults to 300.0. + + Returns: + Upsampled image visualizing the penetration depth. + """ + # penetration_depth_img_upsampled = penetration_depth.repeat(resolution, 0).repeat(resolution, 1) + penetration_depth_img_upsampled = np.kron(penetration_depth_img, np.ones((resolution, resolution))) + penetration_depth_img_upsampled = np.clip(penetration_depth_img_upsampled, 0.0, 1.0) * depth_multiplier + return penetration_depth_img_upsampled + + +class GelsightRender: + """Class to handle GelSight rendering using the Taxim example-based approach. + + Reference: https://arxiv.org/abs/2109.04027 + """ + + def __init__(self, cfg: GelSightRenderCfg, device: str | torch.device): + """Initialize the GelSight renderer. + + Args: + cfg: Configuration object for the GelSight sensor. + device: Device to use ('cpu' or 'cuda'). + + Raises: + ValueError: If mm_per_pixel is zero or negative. + FileNotFoundError: If render data files cannot be retrieved. + """ + self.cfg = cfg + self.device = device + + # Validate configuration parameters + eps = 1e-9 + if self.cfg.mm_per_pixel < eps: + raise ValueError(f"mm_per_pixel must be positive (>= {eps}), got {self.cfg.mm_per_pixel}") + + # Retrieve render data files using the configured base path + bg_path = get_gelsight_render_data( + self.cfg.base_data_path, self.cfg.sensor_data_dir_name, self.cfg.background_path + ) + calib_path = get_gelsight_render_data( + self.cfg.base_data_path, self.cfg.sensor_data_dir_name, self.cfg.calib_path + ) + + if bg_path is None or calib_path is None: + raise FileNotFoundError( + "Failed to retrieve GelSight render data files. " + f"Base path: {self.cfg.base_data_path or 'default (Isaac Lab Nucleus)'}, " + f"Data dir: {self.cfg.sensor_data_dir_name}" + ) + + self.background = cv2.cvtColor(cv2.imread(bg_path), cv2.COLOR_BGR2RGB) + + # Load calibration data directly + calib_data = np.load(calib_path) + calib_grad_r = calib_data["grad_r"] + calib_grad_g = calib_data["grad_g"] + calib_grad_b = calib_data["grad_b"] + + image_height = self.cfg.image_height + image_width = self.cfg.image_width + num_bins = self.cfg.num_bins + [xx, yy] = np.meshgrid(range(image_width), range(image_height)) + xf = xx.flatten() + yf = yy.flatten() + self.A = np.array([xf * xf, yf * yf, xf * yf, xf, yf, np.ones(image_height * image_width)]).T + + binm = num_bins - 1 + self.x_binr = 0.5 * np.pi / binm # x [0,pi/2] + self.y_binr = 2 * np.pi / binm # y [-pi, pi] + + kernel = self._get_filtering_kernel(kernel_sz=5) + self.kernel = torch.tensor(kernel, dtype=torch.float, device=self.device) + + self.calib_data_grad_r = torch.tensor(calib_grad_r, device=self.device) + self.calib_data_grad_g = torch.tensor(calib_grad_g, device=self.device) + self.calib_data_grad_b = torch.tensor(calib_grad_b, device=self.device) + + self.A_tensor = torch.tensor(self.A.reshape(image_height, image_width, 6), device=self.device).unsqueeze(0) + self.background_tensor = torch.tensor(self.background, device=self.device) + + # Pre-allocate buffer for RGB output (will be resized if needed) + self._sim_img_rgb_buffer = torch.empty((1, image_height, image_width, 3), device=self.device) + + omni.log.info("Gelsight initialization done!") + + def render(self, heightMap: torch.Tensor) -> torch.Tensor: + """Render the height map using the GelSight sensor (tensorized version). + + Args: + heightMap: Input height map tensor. + + Returns: + Rendered image tensor. + """ + height_map = heightMap.clone() + height_map[torch.abs(height_map) < 1e-6] = 0 # remove minor artifact + height_map = height_map * -1000.0 + height_map /= self.cfg.mm_per_pixel + + height_map = self._gaussian_filtering(height_map.unsqueeze(-1), self.kernel).squeeze(-1) + + grad_mag, grad_dir, _ = self._generate_normals(height_map) + + idx_x = torch.floor(grad_mag / self.x_binr).long() + idx_y = torch.floor((grad_dir + np.pi) / self.y_binr).long() + + # Clamp indices to valid range to prevent out-of-bounds errors + max_idx = self.cfg.num_bins - 1 + idx_x = torch.clamp(idx_x, 0, max_idx) + idx_y = torch.clamp(idx_y, 0, max_idx) + + params_r = self.calib_data_grad_r[idx_x, idx_y, :] + params_g = self.calib_data_grad_g[idx_x, idx_y, :] + params_b = self.calib_data_grad_b[idx_x, idx_y, :] + + # Reuse pre-allocated buffer, resize if batch size changed + target_shape = (*idx_x.shape, 3) + if self._sim_img_rgb_buffer.shape != target_shape: + self._sim_img_rgb_buffer = torch.empty(target_shape, device=self.device) + sim_img_rgb = self._sim_img_rgb_buffer + + sim_img_rgb[..., 0] = torch.sum(self.A_tensor * params_r, dim=-1) # R + sim_img_rgb[..., 1] = torch.sum(self.A_tensor * params_g, dim=-1) # G + sim_img_rgb[..., 2] = torch.sum(self.A_tensor * params_b, dim=-1) # B + + # write tactile image + sim_img = sim_img_rgb + self.background_tensor # /255.0 + sim_img = torch.clip(sim_img, 0, 255, out=sim_img).to(torch.uint8) + return sim_img + + def _generate_normals(self, img: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor, None]: + """Generate the gradient magnitude and direction of the height map. + + Args: + img: Input height map tensor. + + Returns: + Tuple containing gradient magnitude tensor, gradient direction tensor, and None. + """ + img_grad = torch.gradient(img, dim=(1, 2)) + dzdx, dzdy = img_grad + + grad_mag_orig = torch.sqrt(dzdx**2 + dzdy**2) + grad_mag = torch.arctan(grad_mag_orig) # seems that arctan is used as a squashing function + grad_dir = torch.arctan2(dzdx, dzdy) + grad_dir[grad_mag_orig == 0] = 0 + + # handle edges + grad_mag = torch.nn.functional.pad(grad_mag[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + grad_dir = torch.nn.functional.pad(grad_dir[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + + return grad_mag, grad_dir, None + + def _get_filtering_kernel(self, kernel_sz: int = 5) -> np.ndarray: + """Create a Gaussian filtering kernel. + + For kernel derivation, see https://cecas.clemson.edu/~stb/ece847/internal/cvbook/ch03_filtering.pdf + + Args: + kernel_sz: Size of the kernel. Defaults to 5. + + Returns: + Filtering kernel. + """ + filter_1D = scipy.special.binom(kernel_sz - 1, np.arange(kernel_sz)) + filter_1D /= filter_1D.sum() + filter_1D = filter_1D[..., None] + + kernel = filter_1D @ filter_1D.T + return kernel + + def _gaussian_filtering(self, img: torch.Tensor, kernel: torch.Tensor) -> torch.Tensor: + """Apply Gaussian filtering to the input image tensor. + + Args: + img: Input image tensor. + kernel: Filtering kernel tensor. + + Returns: + Filtered image tensor. + """ + img_output = torch.nn.functional.conv2d( + img.permute(0, 3, 1, 2), kernel.unsqueeze(0).unsqueeze(0), stride=1, padding="same" + ).permute(0, 2, 3, 1) + return img_output diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor.py new file mode 100644 index 00000000000..8915f07c4b8 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor.py @@ -0,0 +1,965 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import itertools +import numpy as np +import torch +from collections.abc import Sequence +from scipy.spatial.transform import Rotation as R +from typing import TYPE_CHECKING, Any + +import isaacsim.core.utils.torch as torch_utils +import omni.log +from isaacsim.core.prims import SdfShapePrim +from isaacsim.core.simulation_manager import SimulationManager +from pxr import Usd, UsdGeom, UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.markers import VisualizationMarkers +from isaaclab.sensors.camera import Camera, TiledCamera +from isaaclab.sensors.sensor_base import SensorBase +from isaaclab.sensors.tacsl_sensor.gelsight_utils import GelsightRender +from isaaclab.sensors.tacsl_sensor.visuotactile_sensor_data import VisuoTactileSensorData +from isaaclab.utils.timer import Timer + +if TYPE_CHECKING: + from .visuotactile_sensor_cfg import VisuoTactileSensorCfg + +import trimesh + + +class VisuoTactileSensor(SensorBase): + """A tactile sensor for both camera-based and force field tactile sensing. + + This sensor provides: + 1. Camera-based tactile sensing: depth images from tactile surface + 2. Force field tactile sensing: Penalty-based normal and shear forces using SDF queries + + The sensor can be configured to use either or both sensing modalities. + + **Computation Pipeline:** + Camera-based sensing computes depth differences from a nominal (no-contact) baseline and + processes them through the tac-sl GelSight renderer to produce realistic tactile images. + + Force field sensing queries Signed Distance Fields (SDF) to compute penetration depths, + then applies penalty-based spring-damper models (F_n = k_n * depth, F_t = min(k_t * |v_t|, mu * F_n)) + to compute normal and shear forces at discrete tactile points. + + **Example Usage:** + For a complete working example, see: ``scripts/demos/sensors/tacsl/tacsl_example.py`` + + **Current Limitations:** + - SDF collision meshes must be pre-computed and objects specified before simulation starts + - Force field computation requires specific rigid body and mesh configurations + - No support for dynamic addition/removal of interacting objects during runtime + + Configuration Requirements: + The following requirements must be satisfied for proper sensor operation: + + **Camera Tactile Imaging** + If ``enable_camera_tactile=True``, a valid ``camera_cfg`` (TiledCameraCfg) must be + provided with appropriate camera parameters. + + **Force Field Computation** + If ``enable_force_field=True``, the following parameters are required: + + * ``contact_object_prim_path_expr`` - Prim path expression to find the contact object prim + + **SDF Computation** + When force field computation is enabled, penalty-based normal and shear forces are + computed using Signed Distance Field (SDF) queries. To achieve GPU acceleration: + + * Interacting objects should have pre-computed SDF collision meshes + * An SDFView must be defined during initialization, therefore interacting objects + should be specified before simulation. + + """ + + cfg: VisuoTactileSensorCfg + """The configuration parameters.""" + + def __init__(self, cfg: VisuoTactileSensorCfg): + """Initializes the tactile sensor object. + + Args: + cfg: The configuration parameters. + """ + + # Create empty variables for storing output data + self._data: VisuoTactileSensorData = VisuoTactileSensorData() + + # Camera-based tactile sensing + self._camera_sensor: Camera | TiledCamera | None = None + self._nominal_tactile: dict | None = None + + # Force field tactile sensing + self._tactile_pos_local: torch.Tensor | None = None + self._tactile_quat_local: torch.Tensor | None = None + self._sdf_object: Any | None = None + + # Physics views + self._physics_sim_view = None + self._elastomer_body_view = None + self._elastomer_tip_view = None + self._contact_object_body_view = None + + # Visualization + self._tactile_visualizer: VisualizationMarkers | None = None + + # Timing tracking attributes + self._camera_timing_total: float = 0.0 + self._force_field_timing_total: float = 0.0 + self._timing_call_count: int = 0 + self._camera_timer: Timer = Timer() + self._force_field_timer: Timer = Timer() + + # Tactile points count + self.num_tactile_points: int = 0 + + # Now call parent class constructor + super().__init__(cfg) + + def __del__(self): + """Unsubscribes from callbacks and detach from the replicator registry.""" + if self._camera_sensor is not None: + self._camera_sensor.__del__() + # unsubscribe from callbacks + super().__del__() + + def __str__(self) -> str: + """Returns: A string containing information about the instance.""" + return ( + f"Tactile sensor @ '{self.cfg.prim_path}': \n" + f"\trender config : {self.cfg.render_cfg.base_data_path}/{self.cfg.render_cfg.sensor_data_dir_name}\n" + f"\tupdate period (s) : {self.cfg.update_period}\n" + f"\tcamera enabled : {self.cfg.enable_camera_tactile}\n" + f"\tforce field enabled: {self.cfg.enable_force_field}\n" + f"\tnum instances : {self.num_instances}\n" + ) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self._num_envs + + @property + def data(self) -> VisuoTactileSensorData: + # Update sensors if needed + self._update_outdated_buffers() + # Return the data + return self._data + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the sensor internals.""" + # reset the timestamps + super().reset(env_ids) + self.reset_timing_statistics() + + # Reset camera sensor if enabled + if self._camera_sensor is not None: + self._camera_sensor.reset(env_ids) + + """ + Implementation + """ + + def _initialize_impl(self): + """Initializes the sensor-related handles and internal buffers.""" + super()._initialize_impl() + + # Obtain global simulation view + self._physics_sim_view = SimulationManager.get_physics_sim_view() + + # Initialize camera-based tactile sensing + if self.cfg.enable_camera_tactile: + self._initialize_camera_tactile() + + # Initialize force field tactile sensing + if self.cfg.enable_force_field: + self._initialize_force_field() + + # Initialize visualization + if self.cfg.debug_vis: + self._initialize_visualization() + + def get_initial_render(self): + """Get the initial tactile sensor render for baseline comparison. + + This method captures the initial state of the tactile sensor when no contact + is occurring. This baseline is used for computing relative changes during + tactile interactions. + + Returns: + dict | None: Dictionary containing initial render data with sensor output keys + and corresponding tensor values. Returns None if camera tactile + sensing is disabled. + + Raises: + RuntimeError: If camera sensor is not initialized or initial render fails. + """ + if not self.cfg.enable_camera_tactile or self._camera_sensor is None: + return None + if not self._camera_sensor.is_initialized: + raise RuntimeError("Camera sensor is not initialized") + + self._camera_sensor.update(dt=0.0) + + # get the initial render + initial_render = self._camera_sensor.data.output + if initial_render is None: + raise RuntimeError("Initial render is None") + + # Store the initial nominal tactile data + self._nominal_tactile = dict() + for key, value in initial_render.items(): + self._nominal_tactile[key] = value.clone() + + return self._nominal_tactile + + def _initialize_camera_tactile(self): + """Initialize camera-based tactile sensing.""" + if self.cfg.camera_cfg is None: + raise ValueError("Camera configuration is None. Please provide a valid camera configuration.") + # check image size is consistent with the render config + if ( + self.cfg.camera_cfg.height != self.cfg.render_cfg.image_height + or self.cfg.camera_cfg.width != self.cfg.render_cfg.image_width + ): + raise ValueError( + "Camera configuration image size is not consistent with the render config. Camera size:" + f" {self.cfg.camera_cfg.height}x{self.cfg.camera_cfg.width}, Render config:" + f" {self.cfg.render_cfg.image_height}x{self.cfg.render_cfg.image_width}" + ) + # check data types + if not all(data_type in ["distance_to_image_plane", "depth"] for data_type in self.cfg.camera_cfg.data_types): + raise ValueError( + f"Camera configuration data types are not supported. Data types: {self.cfg.camera_cfg.data_types}" + ) + + # gelsightRender + self._tactile_rgb_render = GelsightRender(self.cfg.render_cfg, device=self.device) + + # Create camera sensor + self._camera_sensor = TiledCamera(self.cfg.camera_cfg) + + # Initialize camera + if not self._camera_sensor.is_initialized: + self._camera_sensor._initialize_impl() + self._camera_sensor._is_initialized = True + + # Initialize camera buffers + self._data.tactile_rgb_image = torch.zeros( + (self._num_envs, self.cfg.camera_cfg.height, self.cfg.camera_cfg.width, 3), device=self._device + ) + self._data.tactile_camera_depth = torch.zeros( + (self._num_envs, self.cfg.camera_cfg.height, self.cfg.camera_cfg.width, 1), device=self._device + ) + + omni.log.info("Camera-based tactile sensing initialized.") + + def _initialize_force_field(self): + """Initialize force field tactile sensing components. + + This method sets up all components required for force field based tactile sensing: + 1. Creates PhysX views for elastomer and contact object rigid bodies + 2. Generates tactile sensing points on the elastomer surface using mesh geometry + 3. Initializes SDF (Signed Distance Field) for collision detection + 4. Creates data buffers for storing force field measurements + + The tactile points are generated by ray-casting onto the elastomer mesh surface + to create a grid of sensing points that will be used for force computation. + + """ + + # Generate tactile points on elastomer surface + self._generate_tactile_points( + num_divs=[self.cfg.num_tactile_rows, self.cfg.num_tactile_cols], + margin=getattr(self.cfg, "tactile_margin", 0.003), + visualize=self.cfg.trimesh_vis_tactile_points, + ) + + self._create_physx_views() + + # Initialize force field data buffers + self._initialize_force_field_buffers() + omni.log.info("Force field tactile sensing initialized.") + + def _create_physx_views(self) -> bool: + """Create PhysX views for contact object and elastomer bodies. + + This method sets up the necessary PhysX views for force field computation: + 1. Finds and validates the object prim and its collision mesh + 2. Creates SDF view for collision detection + 3. Creates rigid body views for both object and elastomer + + Returns: + True if PhysX views were successfully created, False otherwise. + + """ + if self.cfg.contact_object_prim_path_expr is None: + return False + + contact_object_mesh, contact_object_rigid_body = self._find_contact_object_components() + # Create SDF view for collision detection + num_query_points = self.cfg.num_tactile_rows * self.cfg.num_tactile_cols + mesh_path_pattern = contact_object_mesh.GetPath().pathString.replace("env_0", "env_*") + self._contact_object_sdf_view = SdfShapePrim( + prim_paths_expr=mesh_path_pattern, + name="contact_object_sdf_view", + num_query_points=num_query_points, + prepare_sdf_schemas=False, + ) + self._contact_object_sdf_view.initialize(physics_sim_view=self._physics_sim_view) + + # Create rigid body views for contact object and elastomer + body_path_pattern = contact_object_rigid_body.GetPath().pathString.replace("env_0", "env_*") + self._contact_object_body_view = self._physics_sim_view.create_rigid_body_view([body_path_pattern]) + elastomer_pattern = self._parent_prims[0].GetPath().pathString.replace("env_0", "env_*") + self._elastomer_body_view = self._physics_sim_view.create_rigid_body_view([elastomer_pattern]) + return True + + def _find_contact_object_components(self) -> tuple[Any, Any]: + """Find and validate contact object SDF mesh and its parent rigid body. + + This method searches for the contact object prim using the configured filter pattern, + then locates the first SDF collision mesh within that prim hierarchy and + identifies its parent rigid body for physics simulation. + + Returns: + Tuple of (contact_object_mesh, contact_object_rigid_body) + Returns None if contact object components are not found. + + Note: + Only SDF meshes are supported for optimal force field computation performance. + If no SDF mesh is found, the method will log a warning and return None. + """ + # Find the contact object prim using the configured pattern + contact_object_prim = sim_utils.find_first_matching_prim(self.cfg.contact_object_prim_path_expr) + if contact_object_prim is None: + raise RuntimeError( + f"No contact object prim found matching pattern: {self.cfg.contact_object_prim_path_expr}" + ) + + def is_sdf_mesh(prim: Usd.Prim) -> bool: + """Check if a mesh prim is configured for SDF approximation.""" + return ( + prim.HasAPI(UsdPhysics.MeshCollisionAPI) + and UsdPhysics.MeshCollisionAPI(prim).GetApproximationAttr().Get() == "sdf" + ) + + # Find the SDF mesh within the contact object + contact_object_mesh = sim_utils.get_first_matching_child_prim( + contact_object_prim.GetPath(), predicate=is_sdf_mesh + ) + if contact_object_mesh is None: + raise RuntimeError( + f"No SDF mesh found under contact object at path: {contact_object_prim.GetPath().pathString}" + ) + + def find_parent_rigid_body(prim: Usd.Prim) -> Usd.Prim | None: + """Find the first parent prim with RigidBodyAPI.""" + current_prim = prim + while current_prim and current_prim.IsValid(): + if current_prim.HasAPI(UsdPhysics.RigidBodyAPI): + return current_prim + current_prim = current_prim.GetParent() + if current_prim.GetPath() == "/": + break + return None + + # Find the rigid body parent of the SDF mesh + contact_object_rigid_body = find_parent_rigid_body(contact_object_mesh) + if contact_object_rigid_body is None: + raise RuntimeError( + f"No contact object rigid body found for mesh at path: {contact_object_mesh.GetPath().pathString}" + ) + + return contact_object_mesh, contact_object_rigid_body + + def _generate_tactile_points(self, num_divs: list, margin: float, visualize: bool): + """Generate tactile sensing points from elastomer mesh geometry. + + This method creates a grid of tactile sensing points on the elastomer surface + by ray-casting onto the mesh geometry. Visual meshes are used for smoother point sampling. + + Args: + num_divs: Number of divisions [rows, cols] for the tactile grid. + margin: Margin distance from mesh edges in meters. + visualize: Whether to show the generated points in trimesh visualization. + + """ + + # Get the elastomer prim path + elastomer_prim_path = self._parent_prims[0].GetPath().pathString + + def is_visual_mesh(prim) -> bool: + """Check if a mesh prim has visual properties (visual mesh, not collision mesh).""" + return prim.IsA(UsdGeom.Mesh) and not prim.HasAPI(UsdPhysics.CollisionAPI) + + elastomer_mesh_prim = sim_utils.get_first_matching_child_prim(elastomer_prim_path, predicate=is_visual_mesh) + if elastomer_mesh_prim is None: + raise RuntimeError(f"No visual mesh found under elastomer at path: {elastomer_prim_path}") + + omni.log.info(f"Generating tactile points from USD mesh: {elastomer_mesh_prim.GetPath().pathString}") + + # Extract mesh data + usd_mesh = UsdGeom.Mesh(elastomer_mesh_prim) + points = np.asarray(usd_mesh.GetPointsAttr().Get()) + face_indices = np.asarray(usd_mesh.GetFaceVertexIndicesAttr().Get()) + + # Simple triangulation + faces = face_indices.reshape(-1, 3) + + # Create bounds + mesh_bounds = np.array([points.min(axis=0), points.max(axis=0)]) + + # Create trimesh object + mesh = trimesh.Trimesh(vertices=points, faces=faces) + + # Generate grid on elastomer + elastomer_dims = np.diff(mesh_bounds, axis=0).squeeze() + slim_axis = np.argmin(elastomer_dims) # Determine flat axis of elastomer + + # Determine tip direction using dome geometry + # For dome-shaped elastomers, the center of mass is shifted toward the dome (contact) side + mesh_center_of_mass = mesh.center_mass[slim_axis] + bounding_box_center = (mesh_bounds[0, slim_axis] + mesh_bounds[1, slim_axis]) / 2.0 + + tip_direction_sign = 1.0 if mesh_center_of_mass > bounding_box_center else -1.0 + + # Determine gap between adjacent tactile points + axis_idxs = list(range(3)) + axis_idxs.remove(int(slim_axis)) # Remove slim idx + div_sz = (elastomer_dims[axis_idxs] - margin * 2.0) / (np.array(num_divs) + 1) + tactile_points_dx = min(div_sz) + + # Sample points on the flat plane + planar_grid_points = [] + center = (mesh_bounds[0] + mesh_bounds[1]) / 2.0 + idx = 0 + for axis_i in range(3): + if axis_i == slim_axis: + # On the slim axis, place a point far away so ray is pointing at the elastomer tip + planar_grid_points.append([tip_direction_sign]) + else: + axis_grid_points = np.linspace( + center[axis_i] - tactile_points_dx * (num_divs[idx] + 1.0) / 2.0, + center[axis_i] + tactile_points_dx * (num_divs[idx] + 1.0) / 2.0, + num_divs[idx] + 2, + ) + planar_grid_points.append(axis_grid_points[1:-1]) # Leave out the extreme corners + idx += 1 + + grid_corners = itertools.product(planar_grid_points[0], planar_grid_points[1], planar_grid_points[2]) + grid_corners = np.array(list(grid_corners)) + + # Project ray in positive y direction on the mesh + mesh_data = trimesh.ray.ray_triangle.RayMeshIntersector(mesh) + ray_dir = np.array([0, 0, 0]) + ray_dir[slim_axis] = -tip_direction_sign # Ray points towards elastomer (opposite of tip direction) + + # Handle the ray intersection result + index_tri, index_ray, locations = mesh_data.intersects_id( + grid_corners, np.tile([ray_dir], (grid_corners.shape[0], 1)), return_locations=True, multiple_hits=False + ) + + if visualize: + query_pointcloud = trimesh.PointCloud(locations, colors=(0.0, 0.0, 1.0)) + trimesh.Scene([mesh, query_pointcloud]).show() + + # Sort and store tactile points + tactile_points = locations[index_ray.argsort()] + # in the frame of the elastomer + self._tactile_pos_local = torch.tensor(tactile_points, dtype=torch.float32, device=self._device) + self.num_tactile_points = self._tactile_pos_local.shape[0] + if self.num_tactile_points != self.cfg.num_tactile_rows * self.cfg.num_tactile_cols: + raise RuntimeError( + f"Number of tactile points does not match expected: {self.num_tactile_points} !=" + f" {self.cfg.num_tactile_rows * self.cfg.num_tactile_cols}" + ) + + # Assume tactile frame rotation are all the same + rotation = (0, 0, -np.pi) + rotation = R.from_euler("xyz", rotation).as_quat() + self._tactile_quat_local = ( + torch.tensor(rotation, dtype=torch.float32, device=self._device).unsqueeze(0).repeat(len(tactile_points), 1) + ) + + omni.log.info(f"Generated {len(tactile_points)} tactile points from USD mesh using ray casting") + + def _initialize_force_field_buffers(self): + """Initialize data buffers for force field sensing.""" + num_pts = self.num_tactile_points + + # Initialize force field data tensors + self._data.tactile_points_pos_w = torch.zeros((self._num_envs, num_pts, 3), device=self._device) + self._data.tactile_points_quat_w = torch.zeros((self._num_envs, num_pts, 4), device=self._device) + self._data.penetration_depth = torch.zeros((self._num_envs, num_pts), device=self._device) + self._data.tactile_normal_force = torch.zeros((self._num_envs, num_pts), device=self._device) + self._data.tactile_shear_force = torch.zeros((self._num_envs, num_pts, 2), device=self._device) + # Pre-compute expanded tactile point tensors to avoid repeated unsqueeze/expand operations + self._tactile_pos_expanded = self._tactile_pos_local.unsqueeze(0).expand(self._num_envs, -1, -1) + self._tactile_quat_expanded = self._tactile_quat_local.unsqueeze(0).expand(self._num_envs, -1, -1) + + def _initialize_visualization(self): + """Initialize visualization markers for tactile points.""" + if self.cfg.visualizer_cfg is not None: + self._visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Fills the buffers of the sensor data. + + This method updates both camera-based and force field tactile sensing data + for the specified environments. It also tracks timing statistics for + performance monitoring. + + Args: + env_ids: Sequence of environment indices to update. If length equals + total number of environments, all environments are updated. + + Note: + The first two timing measurements are excluded from statistics to + account for initialization overhead. + """ + # Convert to proper indices for internal methods + if len(env_ids) == self._num_envs: + internal_env_ids = slice(None) + else: + internal_env_ids = env_ids + + # Increment call counter for timing tracking + self._timing_call_count += 1 + + # Update camera-based tactile data + if self.cfg.enable_camera_tactile and self._camera_sensor is not None: + self._camera_timer.start() + self._update_camera_tactile(internal_env_ids) + if self._timing_call_count > 2: + self._camera_timing_total += self._camera_timer.time_elapsed + self._camera_timer.stop() + + # Update force field tactile data + if self.cfg.enable_force_field and self._tactile_pos_local is not None: + self._force_field_timer.start() + self._update_force_field(internal_env_ids) + if self._timing_call_count > 2: + self._force_field_timing_total += self._force_field_timer.time_elapsed + self._force_field_timer.stop() + + def _update_camera_tactile(self, env_ids: Sequence[int] | slice): + """Update camera-based tactile sensing data. + + This method updates the camera sensor and processes the depth information + to compute tactile measurements. It computes the difference from the nominal + (no-contact) state and renders it using the GelSight tactile renderer. + + Args: + env_ids: Environment indices or slice to update. Can be a sequence of + integers or a slice object for batch processing. + """ + if self._nominal_tactile is None: + raise RuntimeError("Nominal tactile is not set. Please call get_initial_render() first.") + # Update camera sensor + self._camera_sensor.update(self._sim_physics_dt) + + # Get camera data + camera_data = self._camera_sensor.data + + # Check for either distance_to_image_plane or depth (they are equivalent) + depth_key = None + if "distance_to_image_plane" in camera_data.output: + depth_key = "distance_to_image_plane" + elif "depth" in camera_data.output: + depth_key = "depth" + + if depth_key is not None: + self._data.tactile_camera_depth[env_ids] = camera_data.output[depth_key][env_ids].clone() + diff = self._nominal_tactile[depth_key][env_ids] - self._data.tactile_camera_depth[env_ids] + self._data.tactile_rgb_image[env_ids] = self._tactile_rgb_render.render(diff.squeeze(-1)) + + ######################################################################################### + # Force field tactile sensing + ######################################################################################### + + def _update_force_field(self, env_ids: Sequence[int] | slice): + """Update force field tactile sensing data. + + This method computes penalty-based tactile forces using Signed Distance Field (SDF) + queries. It transforms tactile points to contact object local coordinates, queries the SDF of the + contact object for collision detection, and computes normal and shear forces based on + penetration depth and relative velocities. + + Args: + env_ids: Environment indices or slice to update. Can be a sequence of + integers or a slice object for batch processing. + + Note: + Requires both elastomer and contact object body views to be initialized. Returns + early if tactile points or body views are not available. + """ + if self._elastomer_body_view is None : + return + + # Step 1: Get elastomer pose and precompute pose components + elastomer_pos_w, elastomer_quat_w = self._elastomer_body_view.get_transforms().split([3, 4], dim=-1) + elastomer_quat_w = math_utils.convert_quat(elastomer_quat_w, to="wxyz") + + # Transform tactile points to world coordinates, used for visualization + self._transform_tactile_points_to_world(elastomer_pos_w, elastomer_quat_w) + + if not self._is_contact_object_available(): + return + + # Step 2: Transform tactile points to contact object local frame for SDF queries + contact_object_pos_w, contact_object_quat_w = self._contact_object_body_view.get_transforms().split( + [3, 4], dim=-1 + ) + contact_object_quat_w = math_utils.convert_quat(contact_object_quat_w, to="wxyz") + + world_tactile_points = self._data.tactile_points_pos_w + points_contact_object_local, contact_object_quat_inv = self._transform_points_to_contact_object_local( + world_tactile_points, contact_object_pos_w, contact_object_quat_w + ) + + # Step 3: Query SDF for collision detection + sdf_values_and_gradients = self._contact_object_sdf_view.get_sdf_and_gradients(points_contact_object_local) + sdf_values = sdf_values_and_gradients[..., -1] # Last component is SDF value + sdf_gradients = sdf_values_and_gradients[..., :-1] # First 3 components are gradients + + # Step 4: Compute tactile forces from SDF data + self._compute_tactile_forces_from_sdf( + points_contact_object_local, + sdf_values, + sdf_gradients, + contact_object_pos_w, + contact_object_quat_w, + elastomer_quat_w, + env_ids, + ) + + def _is_contact_object_available(self) -> bool: + """Check if contact object components are properly initialized for force computation. + + Returns: + True if all contact object components are available, False otherwise. + """ + return self._contact_object_body_view is not None and hasattr(self, "_contact_object_sdf_view") + + def _transform_tactile_points_to_world(self, pos_w: torch.Tensor, quat_w: torch.Tensor): + """Transform tactile points from local to world coordinates. + + Args: + pos_w: Elastomer positions in world frame. Shape: (num_envs, 3) + quat_w: Elastomer quaternions in world frame. Shape: (num_envs, 4) + """ + num_pts = self.num_tactile_points + + quat_expanded = quat_w.unsqueeze(1).expand(-1, num_pts, -1) + pos_expanded = pos_w.unsqueeze(1).expand(-1, num_pts, -1) + + # Apply transformation + tactile_pos_w = math_utils.quat_apply(quat_expanded, self._tactile_pos_expanded) + pos_expanded + tactile_quat_w = math_utils.quat_mul(quat_expanded, self._tactile_quat_expanded) + + # Store in data + self._data.tactile_points_pos_w = tactile_pos_w + self._data.tactile_points_quat_w = tactile_quat_w + + def _transform_points_to_contact_object_local( + self, world_points: torch.Tensor, contact_object_pos_w: torch.Tensor, contact_object_quat_w: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor]: + """Optimized version: Transform world coordinates to contact object local frame. + + Args: + world_points: Points in world coordinates. Shape: (num_envs, num_points, 3) + contact_object_pos_w: Contact object positions in world frame. Shape: (num_envs, 3) + contact_object_quat_w: Contact object quaternions in world frame. Shape: (num_envs, 4) + + Returns: + Points in contact object local coordinates and inverse quaternions + """ + # Get inverse transformation (per environment) + # wxyz in torch + contact_object_quat_inv, contact_object_pos_inv = torch_utils.tf_inverse( + contact_object_quat_w, contact_object_pos_w + ) + num_pts = self.num_tactile_points + + contact_object_quat_expanded = contact_object_quat_inv.unsqueeze(1).expand(-1, num_pts, 4) + contact_object_pos_expanded = contact_object_pos_inv.unsqueeze(1).expand(-1, num_pts, 3) + + # Apply transformation + points_sdf = torch_utils.tf_apply(contact_object_quat_expanded, contact_object_pos_expanded, world_points) + + return points_sdf, contact_object_quat_inv + + def _get_tactile_points_velocities( + self, linvel_world: torch.Tensor, angvel_world: torch.Tensor, quat_world: torch.Tensor + ) -> torch.Tensor: + """Optimized version: Compute tactile point velocities from precomputed velocities. + + Args: + linvel_world: Elastomer linear velocities. Shape: (num_envs, 3) + angvel_world: Elastomer angular velocities. Shape: (num_envs, 3) + quat_world: Elastomer quaternions. Shape: (num_envs, 4) + + Returns: + Tactile point velocities in world frame. Shape: (num_envs, num_points, 3) + """ + num_pts = self.num_tactile_points + + # Pre-expand all required tensors once + quat_expanded = quat_world.unsqueeze(1).expand(-1, num_pts, 4) + tactile_pos_expanded = self._tactile_pos_expanded + + # Transform local positions to world frame relative vectors + tactile_pos_world_relative = math_utils.quat_apply(quat_expanded, tactile_pos_expanded) + + # Compute velocity due to angular motion: ω × r + angvel_expanded = angvel_world.unsqueeze(1).expand(-1, num_pts, 3) + angular_velocity_contribution = torch.cross(angvel_expanded, tactile_pos_world_relative, dim=-1) + + # Add linear velocity contribution + linvel_expanded = linvel_world.unsqueeze(1).expand(-1, num_pts, 3) + tactile_velocity_world = angular_velocity_contribution + linvel_expanded + + return tactile_velocity_world + + def _compute_tactile_forces_from_sdf( + self, + points_contact_object_local: torch.Tensor, + sdf_values: torch.Tensor, + sdf_gradients: torch.Tensor, + contact_object_pos_w: torch.Tensor, + contact_object_quat_w: torch.Tensor, + elastomer_quat_w: torch.Tensor, + env_ids: Sequence[int] | slice, + ) -> None: + """Optimized version: Compute tactile forces from SDF values using precomputed parameters. + + This method now operates directly on the pre-allocated data tensors to avoid + unnecessary memory allocation and copying. + + Args: + points_contact_object_local: Points in contact object local frame + sdf_values: SDF values (negative means penetration) + sdf_gradients: SDF gradients (surface normals) + contact_object_pos_w: Contact object positions in world frame + contact_object_quat_w: Contact object quaternions in world frame + elastomer_quat_w: Elastomer quaternions + env_ids: Environment indices being updated + + """ + depth = self._data.penetration_depth[env_ids] + tactile_normal_force = self._data.tactile_normal_force[env_ids] + tactile_shear_force = self._data.tactile_shear_force[env_ids] + + # Clear the output tensors + tactile_normal_force.zero_() + tactile_shear_force.zero_() + depth.zero_() + + # Convert SDF values to penetration depth (positive for penetration) + depth[:] = torch.clamp(-sdf_values[env_ids], min=0.0) # Negative SDF means inside (penetrating) + + # Get collision mask for points that are penetrating + collision_mask = depth > 0.0 + + # Use pre-allocated tensors instead of creating new ones + num_pts = self.num_tactile_points + + if collision_mask.any() or self.cfg.visualize_sdf_closest_pts: + + # Get contact object and elastomer velocities + contact_object_velocities = self._contact_object_body_view.get_velocities() + contact_object_linvel_w = contact_object_velocities[env_ids, :3] + contact_object_angvel_w = contact_object_velocities[env_ids, 3:] + + elastomer_velocities = self._elastomer_body_view.get_velocities() + elastomer_linvel_w = elastomer_velocities[env_ids, :3] + elastomer_angvel_w = elastomer_velocities[env_ids, 3:] + + # Normalize gradients to get surface normals in local frame + normals_local = torch.nn.functional.normalize(sdf_gradients[env_ids], dim=-1) + + # Transform normals to world frame (rotate by contact object orientation) - use precomputed quaternions + contact_object_quat_expanded = contact_object_quat_w[env_ids].unsqueeze(1).expand(-1, num_pts, 4) + + # Apply quaternion transformation + normals_world = math_utils.quat_apply(contact_object_quat_expanded, normals_local) + + # Compute normal contact force: F_n = k_n * depth + fc_norm = self.cfg.tactile_kn * depth + fc_world = fc_norm.unsqueeze(-1) * normals_world + + # Get tactile point velocities using precomputed velocities + tactile_velocity_world = self._get_tactile_points_velocities( + elastomer_linvel_w, elastomer_angvel_w, elastomer_quat_w[env_ids] + ) + + # Use precomputed contact object velocities + closest_points_sdf = points_contact_object_local[env_ids] + depth.unsqueeze(-1) * normals_local + + if self.cfg.visualize_sdf_closest_pts: + debug_closest_points_sdf = ( + points_contact_object_local[env_ids] - sdf_values[env_ids].unsqueeze(-1) * normals_local + ) + self.debug_closest_points_wolrd = math_utils.quat_apply( + contact_object_quat_expanded, debug_closest_points_sdf + ) + contact_object_pos_w[env_ids].unsqueeze(1).expand(-1, num_pts, 3) + + contact_object_linvel_expanded = contact_object_linvel_w.unsqueeze(1).expand(-1, num_pts, 3) + contact_object_angvel_expanded = contact_object_angvel_w.unsqueeze(1).expand(-1, num_pts, 3) + closest_points_vel_world = ( + torch.linalg.cross( + contact_object_angvel_expanded, + math_utils.quat_apply(contact_object_quat_expanded, closest_points_sdf), + ) + + contact_object_linvel_expanded + ) + + # Compute relative velocity at contact points + relative_velocity_world = tactile_velocity_world - closest_points_vel_world + + # Compute tangential velocity (perpendicular to normal) + vt_world = relative_velocity_world - normals_world * torch.sum( + normals_world * relative_velocity_world, dim=-1, keepdim=True + ) + vt_norm = torch.norm(vt_world, dim=-1) + + # Compute friction force: F_t = min(k_t * |v_t|, mu * F_n) + ft_static_norm = self.cfg.tactile_kt * vt_norm + ft_dynamic_norm = self.cfg.tactile_mu * fc_norm + ft_norm = torch.minimum(ft_static_norm, ft_dynamic_norm) + + # Apply friction force opposite to tangential velocity + ft_world = -ft_norm.unsqueeze(-1) * vt_world / (vt_norm.unsqueeze(-1).clamp(min=1e-9)) + + # Total tactile force in world frame + tactile_force_world = fc_world + ft_world + + # Transform forces to tactile frame + tactile_force_tactile = math_utils.quat_apply_inverse( + self._data.tactile_points_quat_w[env_ids], tactile_force_world + ) + + # Extract normal and shear components + # Assume tactile frame has Z as normal direction + tactile_normal_force[:] = tactile_force_tactile[..., 2] # Z component + tactile_shear_force[:] = tactile_force_tactile[..., :2] # X,Y components + + ######################################################################################### + # Timing statistics + ######################################################################################### + + def get_timing_summary(self) -> dict: + """Get current timing statistics as a dictionary. + + Returns: + Dictionary containing timing statistics. + """ + if self._timing_call_count <= 0: + return { + "call_count": 0, + "camera_total": 0.0, + "camera_average": 0.0, + "force_field_total": 0.0, + "force_field_average": 0.0, + "combined_average": 0.0, + } + + # skip the first two calls for calculation (without mutating state) + adjusted_call_count = max(0, self._timing_call_count - 2) + num_frames = adjusted_call_count * self._num_envs + force_field_avg = self._force_field_timing_total / num_frames if self._force_field_timing_total > 0 else 0.0 + camera_avg = self._camera_timing_total / num_frames if self._camera_timing_total > 0 else 0.0 + + return { + "call_count": adjusted_call_count, + "camera_total": self._camera_timing_total, + "camera_average": camera_avg, + "force_field_total": self._force_field_timing_total, + "force_field_average": force_field_avg, + "combined_average": camera_avg + force_field_avg, + "num_envs": self._num_envs, + "num_frames": num_frames, + "camera_fps": 1 / camera_avg if camera_avg > 0 else 0.0, + "force_field_fps": 1 / force_field_avg if force_field_avg > 0 else 0.0, + "total_fps": 1 / (camera_avg + force_field_avg) if (camera_avg + force_field_avg) > 0 else 0.0, + } + + def reset_timing_statistics(self): + """Reset all timing statistics to zero.""" + self._camera_timing_total = 0.0 + self._force_field_timing_total = 0.0 + self._timing_call_count = 0 + + ######################################################################################### + # Debug visualization + ######################################################################################### + + def _set_debug_vis_impl(self, debug_vis: bool): + """Set debug visualization into visualization objects.""" + # set visibility of markers + # note: parent only deals with callbacks. not their visibility + if debug_vis: + # create markers if necessary for the first time + if self._tactile_visualizer is None: + self._tactile_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + # set their visibility to true + self._tactile_visualizer.set_visibility(True) + else: + if self._tactile_visualizer is not None: + self._tactile_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + """Callback for debug visualization of tactile sensor data. + + This method is called during each simulation step when debug visualization is enabled. + It visualizes tactile sensing points as 3D markers in the simulation viewport to help + with debugging and understanding sensor behavior. + + The method handles two visualization modes: + 1. **Standard mode**: Visualizes ``tactile_points_pos_w`` - the world positions of + tactile sensing points on the sensor surface + 2. **SDF debug mode**: When ``cfg.visualize_sdf_closest_pts`` is True, visualizes + ``debug_closest_points_wolrd`` - the closest surface points computed during + SDF-based force calculations + """ + # Safety check - return if not properly initialized + if not hasattr(self, "_tactile_visualizer") or self._tactile_visualizer is None: + return + vis_points = None + + if self.cfg.visualize_sdf_closest_pts and hasattr(self, "debug_closest_points_wolrd"): + vis_points = self.debug_closest_points_wolrd + else: + vis_points = self._data.tactile_points_pos_w + + if vis_points is None: + return + + if vis_points.numel() == 0: + return + + viz_points = vis_points.view(-1, 3) # Shape: (num_envs * num_points, 3) + + indices = torch.zeros(viz_points.shape[0], dtype=torch.long, device=self._device) + + marker_scales = torch.ones(viz_points.shape[0], 3, device=self._device) + + # Visualize tactile points + self._tactile_visualizer.visualize(translations=viz_points, marker_indices=indices, scales=marker_scales) diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_cfg.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_cfg.py new file mode 100644 index 00000000000..916eefded51 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_cfg.py @@ -0,0 +1,185 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +# needed to import for allowing type-hinting: torch.Tensor | None +from __future__ import annotations + +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import VISUO_TACTILE_SENSOR_MARKER_CFG +from isaaclab.utils import configclass + +from ..camera.tiled_camera_cfg import TiledCameraCfg +from ..sensor_base_cfg import SensorBaseCfg +from .visuotactile_sensor import VisuoTactileSensor + +## +# GelSight Render Configuration +## + + +@configclass +class GelSightRenderCfg: + """Configuration for GelSight sensor rendering parameters. + + This configuration defines the rendering parameters for example-based tactile image synthesis + using the Taxim approach. + + Reference: + Si, Z., & Yuan, W. (2022). Taxim: An example-based simulation model for GelSight + tactile sensors. IEEE Robotics and Automation Letters, 7(2), 2361-2368. + https://arxiv.org/abs/2109.04027 + + Data Directory Structure: + The sensor data should be organized in the following structure:: + + base_data_path/ + └── sensor_data_dir_name/ + ├── bg.jpg # Background image (required) + ├── polycalib.npz # Polynomial calibration data (required) + └── real_bg.npy # Real background data (optional) + + Path Resolution: + - If ``base_data_path`` is ``None`` (default): Downloads from Isaac Lab Nucleus at + ``{ISAACLAB_NUCLEUS_DIR}/TacSL/{sensor_data_dir_name}/{filename}`` + - If ``base_data_path`` is provided: Uses custom path at + ``{base_data_path}/{sensor_data_dir_name}/{filename}`` + + Example: + Using predefined sensor configuration:: + + from isaaclab_assets.sensors import GELSIGHT_R15_CFG + sensor_cfg = VisuoTactileSensorCfg(render_cfg=GELSIGHT_R15_CFG) + + Using custom sensor data:: + + custom_cfg = GelSightRenderCfg( + base_data_path="/path/to/my/sensors", + sensor_data_dir_name="my_custom_sensor", + image_height=480, + image_width=640, + mm_per_pixel=0.05, + ) + """ + + base_data_path: str | None = None + """Base path to the directory containing sensor calibration data. + + If ``None`` (default), downloads and caches data from Isaac Lab Nucleus directory. + If a custom path is provided, uses the data directly from that location without copying. + """ + + sensor_data_dir_name: str = "gelsight_r15_data" + """Directory name containing the sensor calibration and background data.""" + + background_path: str = "bg.jpg" + """Filename of the background image within the data directory.""" + + calib_path: str = "polycalib.npz" + """Filename of the polynomial calibration data within the data directory.""" + + real_background: str = "real_bg.npy" + """Filename of the real background data within the data directory.""" + + image_height: int = 320 + """Height of the tactile image in pixels.""" + + image_width: int = 240 + """Width of the tactile image in pixels.""" + + num_bins: int = 120 + """Number of bins for gradient magnitude and direction quantization.""" + + mm_per_pixel: float = 0.0877 + """Millimeters per pixel conversion factor for the tactile sensor.""" + + +## +# Visuo-Tactile Sensor Configuration +## + + +@configclass +class VisuoTactileSensorCfg(SensorBaseCfg): + """Configuration for the visuo-tactile sensor. + + This sensor provides both camera-based tactile sensing and force field tactile sensing. + It can capture tactile RGB/depth images and compute penalty-based contact forces. + """ + + class_type: type = VisuoTactileSensor + + # Sensor type and capabilities + render_cfg: GelSightRenderCfg = GelSightRenderCfg() + """Configuration for GelSight sensor rendering. + + This defines the rendering parameters for converting depth maps to realistic tactile images. + Defaults to GelSight R1.5 parameters. Use predefined configs like GELSIGHT_R15_CFG or + GELSIGHT_MINI_CFG from isaaclab_assets.sensors for standard sensor models. + """ + + enable_camera_tactile: bool = True + """Whether to enable camera-based tactile sensing.""" + + enable_force_field: bool = True + """Whether to enable force field tactile sensing.""" + + # Force field configuration + num_tactile_rows: int = 20 + """Number of rows of tactile points for force field sensing.""" + + num_tactile_cols: int = 25 + """Number of columns of tactile points for force field sensing.""" + + tactile_margin: float = 0.003 + """Margin for tactile point generation (in meters).""" + + contact_object_prim_path_expr: str | None = None + """Prim path expression to find the contact object for force field computation. + + This specifies the object that will make contact with the tactile sensor. The sensor will automatically + find the SDF collision mesh within this object for optimal force field computation. + + .. note:: + The expression can contain the environment namespace regex ``{ENV_REGEX_NS}`` which + will be replaced with the environment namespace. + + Example: ``{ENV_REGEX_NS}/ContactObject`` will be replaced with ``/World/envs/env_.*/ContactObject``. + + .. attention:: + For force field computation to work properly, the contact object must have an SDF collision mesh. + The sensor will search for the first SDF mesh within the specified prim hierarchy. + """ + + # Force field physics parameters + tactile_kn: float = 1.0 + """Normal contact stiffness for penalty-based force computation.""" + + tactile_mu: float = 2.0 + """Friction coefficient for shear forces.""" + + tactile_kt: float = 0.1 + """Tangential stiffness for shear forces.""" + + camera_cfg: TiledCameraCfg | None = None + """Camera configuration for tactile RGB/depth sensing. + + If None, camera-based sensing will be disabled even if enable_camera_tactile is True. + """ + + # Visualization + visualizer_cfg: VisualizationMarkersCfg = VISUO_TACTILE_SENSOR_MARKER_CFG.replace( + prim_path="/Visuals/TactileSensor" + ) + """The configuration object for the visualization markers. + + .. note:: + This attribute is only used when debug visualization is enabled. + """ + + trimesh_vis_tactile_points: bool = False + """Whether to visualize tactile points for debugging using trimesh.""" + visualize_sdf_closest_pts: bool = False + """Whether to visualize SDF closest points for debugging.""" diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_data.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_data.py new file mode 100644 index 00000000000..15e083eb680 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_data.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from __future__ import annotations + +import torch +from dataclasses import dataclass + + +@dataclass +class VisuoTactileSensorData: + """Data container for the visuo-tactile sensor. + + This class contains the tactile sensor data that includes: + - Camera-based tactile sensing (RGB and depth images) + - Force field tactile sensing (normal and shear forces) + - Tactile point positions and contact information + """ + + # Camera-based tactile data + tactile_camera_depth: torch.Tensor | None = None + """Tactile depth images. Shape: (num_instances, height, width).""" + + tactile_rgb_image: torch.Tensor | None = None + """Tactile RGB images rendered using the Taxim approach (https://arxiv.org/abs/2109.04027). Shape: (num_instances, height, width, 3).""" + + # Force field tactile data + tactile_points_pos_w: torch.Tensor | None = None + """Positions of tactile points in world frame. Shape: (num_instances, num_tactile_points, 3).""" + + tactile_points_quat_w: torch.Tensor | None = None + """Orientations of tactile points in world frame. Shape: (num_instances, num_tactile_points, 4).""" + + penetration_depth: torch.Tensor | None = None + """Penetration depth at each tactile point. Shape: (num_instances, num_tactile_points).""" + + tactile_normal_force: torch.Tensor | None = None + """Normal forces at each tactile point in sensor frame. Shape: (num_instances, num_tactile_points).""" + + tactile_shear_force: torch.Tensor | None = None + """Shear forces at each tactile point in sensor frame. Shape: (num_instances, num_tactile_points, 2).""" diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py index 0bfda4d270c..5f24df083e8 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/__init__.py @@ -13,5 +13,10 @@ """ -from .from_files import spawn_from_urdf, spawn_from_usd, spawn_ground_plane -from .from_files_cfg import GroundPlaneCfg, UrdfFileCfg, UsdFileCfg +from .from_files import ( + spawn_from_urdf, + spawn_from_usd, + spawn_from_usd_with_physics_material_on_prim, + spawn_ground_plane, +) +from .from_files_cfg import GroundPlaneCfg, UrdfFileCfg, UsdFileCfg, UsdFileWithPhysicsMaterialOnPrimsCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index 04592a4066d..55d42c7b790 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -21,6 +21,7 @@ from isaacsim.core.utils.stage import get_current_stage from isaaclab.sim import converters, schemas +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.sim.utils import ( bind_physics_material, bind_visual_material, @@ -332,3 +333,49 @@ def _spawn_from_usd_file( # return the prim return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_from_usd_with_physics_material_on_prim( + prim_path: str, + cfg: from_files_cfg.UsdFileWithPhysicsMaterialOnPrimsCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Spawn an asset from a USD file and override the settings with the given config.""" + + prim = _spawn_from_usd_file(prim_path, cfg.usd_path, cfg, translation, orientation) + stiff = cfg.compliant_contact_stiffness + damp = cfg.compliant_contact_damping + if cfg.apply_physics_material_prim_path is None: + omni.log.warn("No physics material prim path specified. Skipping physics material application.") + return prim + + if not cfg.apply_physics_material_prim_path.startswith("/"): + rigid_body_prim_path = f"{prim_path}/{cfg.apply_physics_material_prim_path}" + else: + rigid_body_prim_path = cfg.apply_physics_material_prim_path + + material_path = f"{rigid_body_prim_path}/compliant_material" + if stiff is not None or damp is not None: + material_kwargs = {} + if stiff is not None: + material_kwargs["compliant_contact_stiffness"] = stiff + if damp is not None: + material_kwargs["compliant_contact_damping"] = damp + material_cfg = RigidBodyMaterialCfg(**material_kwargs) + + # spawn physics material + material_cfg.func(material_path, material_cfg) + + bind_physics_material( + rigid_body_prim_path, + material_path, + ) + omni.log.info( + f"Applied physics material to prim: {rigid_body_prim_path} with compliance stiffness: {stiff} and" + f" compliance damping: {damp}." + ) + + return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py index f2914fa5043..29c56fa1a06 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py @@ -132,9 +132,22 @@ class UrdfFileCfg(FileCfg, converters.UrdfConverterCfg): func: Callable = from_files.spawn_from_urdf -""" -Spawning ground plane. -""" +@configclass +class UsdFileWithPhysicsMaterialOnPrimsCfg(UsdFileCfg): + """USD file to spawn asset from with physics material. + + It uses the :meth:`spawn_from_usd_with_physics_material` function to spawn the USD file and apply the physics material. + """ + + func: Callable = from_files.spawn_from_usd_with_physics_material_on_prim + compliant_contact_stiffness: float | None = None + compliant_contact_damping: float | None = None + apply_physics_material_prim_path: str | None = None + """Path to the prim to apply the physics material to. + + If the path is relative, then it will be relative to the prim's path. + If None, then the physics material will not be applied. + """ @configclass diff --git a/source/isaaclab/test/sensors/test_gelsight_utils.py b/source/isaaclab/test/sensors/test_gelsight_utils.py new file mode 100644 index 00000000000..26bcaf6844d --- /dev/null +++ b/source/isaaclab/test/sensors/test_gelsight_utils.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for GelSight utility functions - primarily focused on get_gelsight_render_data.""" +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +import numpy as np +import os +import tempfile +import torch + +import pytest + +from isaaclab.sensors.tacsl_sensor.gelsight_utils import GelsightRender, get_gelsight_render_data +from isaaclab.sensors.tacsl_sensor.visuotactile_sensor_cfg import GelSightRenderCfg + + +def test_get_gelsight_render_data_custom_path_missing_file(): + """Test retrieving data from custom path when file doesn't exist.""" + with pytest.raises(FileNotFoundError) as e: + get_gelsight_render_data("non_existent_path", "gelsight_r15_data", "bg.jpg") + assert "Custom GelSight render data not found" in str(e.value) + + +def test_get_gelsight_render_data_nucleus_path_default(): + """Test retrieving data from Nucleus (default path).""" + data_dir = "gelsight_r15_data" + file_name = "bg.jpg" + + # This will either download from Nucleus or use cached data + result_path = get_gelsight_render_data(None, data_dir, file_name) + + # Result should be a valid path or None if Nucleus is unavailable + if result_path is not None: + assert os.path.exists(result_path) + assert file_name in result_path + assert data_dir in result_path + + +def test_get_gelsight_render_data_path_construction_custom(): + """Test correct path construction for custom paths.""" + with tempfile.TemporaryDirectory() as tmpdir: + data_dir = "sensor_data" + file_name = "test.jpg" + + # Create the expected structure + full_dir = os.path.join(tmpdir, data_dir) + os.makedirs(full_dir, exist_ok=True) + + expected_path = os.path.join(tmpdir, data_dir, file_name) + with open(expected_path, "w") as f: + f.write("test") + + result_path = get_gelsight_render_data(tmpdir, data_dir, file_name) + assert result_path == expected_path + + +def test_get_gelsight_render_data_real_cached_files(): + """Test with real cached GelSight data files that should exist.""" + data_dir = "gelsight_r15_data" + + # Test background image + bg_path = get_gelsight_render_data(None, data_dir, "bg.jpg") + if bg_path is not None: + assert os.path.exists(bg_path) + assert bg_path.endswith("bg.jpg") + # Verify it's actually an image file by checking file size + assert os.path.getsize(bg_path) > 0 + + # Test calibration file + calib_path = get_gelsight_render_data(None, data_dir, "polycalib.npz") + if calib_path is not None: + assert os.path.exists(calib_path) + assert calib_path.endswith("polycalib.npz") + # Verify it's actually a numpy file by checking file size + assert os.path.getsize(calib_path) > 0 + + # Try to load the calibration data to verify it's valid + try: + + calib_data = np.load(calib_path) + # Check that expected keys exist + expected_keys = ["grad_r", "grad_g", "grad_b"] + for key in expected_keys: + assert key in calib_data.files, f"Missing key '{key}' in calibration data" + except Exception as e: + pytest.fail(f"Failed to load calibration data: {e}") + + +@pytest.fixture +def gelsight_render_setup(): + """Fixture to set up GelsightRender for testing.""" + # Use default GelSight R1.5 configuration + cfg = GelSightRenderCfg() + device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") + + # Create render instance + try: + render = GelsightRender(cfg, device=device) + yield render, device + except Exception as e: + # If initialization fails (e.g., missing data files), skip tests + pytest.skip(f"GelsightRender initialization failed: {e}") + + +def test_gelsight_render_initialization(gelsight_render_setup): + """Test GelsightRender initialization.""" + render, device = gelsight_render_setup + + # Check that render object was created + assert render is not None + assert render.device == device diff --git a/source/isaaclab/test/sensors/test_visuotactile_sensor.py b/source/isaaclab/test/sensors/test_visuotactile_sensor.py new file mode 100644 index 00000000000..230d3270ea1 --- /dev/null +++ b/source/isaaclab/test/sensors/test_visuotactile_sensor.py @@ -0,0 +1,391 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True, enable_cameras=True).app + +"""Rest everything follows.""" + +import math +import torch + +import isaacsim.core.utils.stage as stage_utils +import omni.replicator.core as rep +import pytest + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation, RigidObject, RigidObjectCfg +from isaaclab.sensors.camera import TiledCameraCfg +from isaaclab.sensors.tacsl_sensor import VisuoTactileSensor, VisuoTactileSensorCfg +from isaaclab.terrains.trimesh.utils import make_plane +from isaaclab.terrains.utils import create_prim_from_mesh +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +# Sample sensor poses + + +def get_sensor_cfg_by_type(sensor_type: str) -> VisuoTactileSensorCfg: + """Return a sensor configuration based on the input type. + + Args: + sensor_type: Type of sensor configuration. Options: "minimum_config", "tactile_cam", "nut_rgb_ff". + + Returns: + VisuoTactileSensorCfg: The sensor configuration for the specified type. + + Raises: + ValueError: If the sensor_type is not supported. + """ + + if sensor_type == "minimum_config": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/sensor_minimum_config", + enable_camera_tactile=False, + enable_force_field=False, + ) + elif sensor_type == "tactile_cam": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/tactile_cam", + enable_force_field=False, + camera_cfg=TiledCameraCfg( + height=320, + width=240, + prim_path="/World/Robot/elastomer_tip/cam", + update_period=0, + data_types=["distance_to_image_plane"], + spawn=None, + ), + ) + + elif sensor_type == "nut_rgb_ff": + return VisuoTactileSensorCfg( + prim_path="/World/Robot/elastomer/sensor_nut", + update_period=0, + debug_vis=False, + enable_camera_tactile=True, + enable_force_field=True, + camera_cfg=TiledCameraCfg( + height=320, + width=240, + prim_path="/World/Robot/elastomer_tip/cam", + update_period=0, + data_types=["distance_to_image_plane"], + spawn=None, + ), + num_tactile_rows=5, + num_tactile_cols=10, + contact_object_prim_path_expr="/World/Nut", + ) + + else: + raise ValueError( + f"Unsupported sensor type: {sensor_type}. Supported types: 'minimum_config', 'tactile_cam', 'nut_rgb_ff'" + ) + + +def setup(sensor_type: str = "cube"): + """Create a new stage and setup simulation environment with robot, objects, and sensor. + + Args: + sensor_type: Type of sensor configuration. Options: "minimum_config", "tactile_cam", "nut_rgb_ff". + + Returns: + Tuple containing simulation context, sensor config, timestep, robot config, cube config, and nut config. + """ + # Create a new stage + stage_utils.create_new_stage() + + # Simulation time-step + dt = 0.01 + + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=dt) + sim = sim_utils.SimulationContext(sim_cfg) + + # Ground-plane + mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) + create_prim_from_mesh("/World/defaultGroundPlane", mesh) + + # gelsightr15 filter + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + # robot + from isaaclab.assets import ArticulationCfg + + robot_cfg = ArticulationCfg( + prim_path="/World/Robot", + spawn=sim_utils.UsdFileWithPhysicsMaterialOnPrimsCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=10.0, + compliant_contact_damping=1.0, + apply_physics_material_prim_path="elastomer", + ), + actuators={}, + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(math.sqrt(2) / 2, -math.sqrt(2) / 2, 0.0, 0.0), # 90° rotation + joint_pos={}, + joint_vel={}, + ), + ) + # Cube + cube_cfg = RigidObjectCfg( + prim_path="/World/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + ) + # Nut + nut_cfg = RigidObjectCfg( + prim_path="/World/Nut", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Factory/factory_nut_m16.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False), + articulation_props=sim_utils.ArticulationRootPropertiesCfg(articulation_enabled=False), + mass_props=sim_utils.MassPropertiesCfg(mass=0.1), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.0 + 0.06776, 0.52), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + # Get the requested sensor configuration using the factory function + sensor_cfg = get_sensor_cfg_by_type(sensor_type) + + # load stage + stage_utils.update_stage() + return sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg + + +def teardown(sim): + """Teardown simulation environment.""" + # close all the opened viewport from before. + rep.vp_manager.destroy_hydra_textures("Replicator") + # stop simulation + # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( + sim._timeline.stop() + # clear the stage + sim.clear_all_callbacks() + sim.clear_instance() + + +@pytest.mark.isaacsim_ci +def test_sensor_minimum_config(): + """Test sensor with minimal configuration (no camera, no force field).""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("minimum_config") + try: + _ = Articulation(cfg=robot_cfg) + sensor_minimum = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + # Simulate physics + for _ in range(10): + sim.step() + sensor_minimum.update(dt) + + # check data should be None, since both camera and force field are disabled + assert sensor_minimum.data.tactile_camera_depth is None + assert sensor_minimum.data.tactile_rgb_image is None + assert sensor_minimum.data.tactile_points_pos_w is None + assert sensor_minimum.data.tactile_points_quat_w is None + assert sensor_minimum.data.penetration_depth is None + assert sensor_minimum.data.tactile_normal_force is None + assert sensor_minimum.data.tactile_shear_force is None + + # Check reset functionality + sensor_minimum.reset() + + for i in range(10): + sim.step() + sensor_minimum.update(dt) + sensor_minimum.reset(env_ids=[0]) + + finally: + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_size_false(): + """Test sensor initialization fails with incorrect camera image size.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + sensor_cfg.camera_cfg.height = 80 + _ = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(ValueError) as excinfo: + sim.reset() + assert "Camera configuration image size is not consistent with the render config" in str(excinfo.value) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_type_false(): + """Test sensor initialization fails with unsupported camera data types.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + sensor_cfg.camera_cfg.data_types = ["rgb"] + _ = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(ValueError) as excinfo: + sim.reset() + assert "Camera configuration data types are not supported" in str(excinfo.value) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_set(): + """Test sensor with camera configuration using existing camera prim.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + assert sensor.is_initialized + assert sensor.data.tactile_camera_depth.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w is None + + sensor.reset() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + sensor.reset(env_ids=[0]) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_set_wrong_prim(): + """Test sensor initialization fails with invalid camera prim path.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + sensor_cfg.camera_cfg.prim_path = "/World/Robot/elastomer_tip/cam_wrong" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + assert "Could not find prim with path" in str(excinfo.value) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_cam_new_spawn(): + """Test sensor with camera configuration that spawns a new camera.""" + sim, sensor_cfg, dt, robot_cfg, object_cfg, nut_cfg = setup("tactile_cam") + sensor_cfg.camera_cfg.prim_path = "/World/Robot/elastomer_tip/cam_new" + sensor_cfg.camera_cfg.spawn = sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.01, 1.0e5) + ) + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + assert sensor.is_initialized + assert sensor.data.tactile_camera_depth.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w is None + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_rgb_forcefield(): + """Test sensor with both camera and force field enabled, detecting contact forces.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup("nut_rgb_ff") + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + nut = RigidObject(cfg=nut_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + nut.update(dt) + # check str + print(sensor) + assert sensor.is_initialized + assert sensor.data.tactile_camera_depth.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w.shape == (1, 50, 3) + assert sensor.data.penetration_depth.shape == (1, 50) + assert sensor.data.tactile_normal_force.shape == (1, 50) + assert sensor.data.tactile_shear_force.shape == (1, 50, 2) + assert sensor._is_contact_object_available() + sum_depth = torch.sum(sensor.data.penetration_depth) # 0.020887471735477448 + normal_force_sum = torch.sum(sensor.data.tactile_normal_force) + shear_force_sum = torch.sum(sensor.data.tactile_shear_force) + assert normal_force_sum > 0.0 + assert sum_depth > 0.0 + assert shear_force_sum > 0.0 + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_no_contact_object(): + """Test sensor with force field but no contact object specified.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, nut_cfg = setup("nut_rgb_ff") + sensor_cfg.contact_object_prim_path_expr = None + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + nut = RigidObject(cfg=nut_cfg) + sim.reset() + sensor.get_initial_render() + for _ in range(10): + sim.step() + sensor.update(dt) + robot.update(dt) + nut.update(dt) + assert sensor.is_initialized + assert sensor.data.tactile_camera_depth.shape == (1, 320, 240, 1) + assert sensor.data.tactile_rgb_image.shape == (1, 320, 240, 3) + assert sensor.data.tactile_points_pos_w.shape == (1, 50, 3) + assert not sensor._is_contact_object_available() + # check no forces are detected + assert torch.all(torch.abs(sensor.data.penetration_depth) < 1e-9) + assert torch.all(torch.abs(sensor.data.tactile_normal_force) < 1e-9) + assert torch.all(torch.abs(sensor.data.tactile_shear_force) < 1e-9) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_force_field_contact_object_not_found(): + """Test sensor initialization fails when contact object prim path is not found.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, NutCfg = setup("nut_rgb_ff") + + sensor_cfg.enable_camera_tactile = False + sensor_cfg.contact_object_prim_path_expr = "/World/Nut/wrong_prim" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + assert "No contact object prim found matching pattern" in str(excinfo.value) + teardown(sim) + + +@pytest.mark.isaacsim_ci +def test_sensor_force_field_contact_object_no_sdf(): + """Test sensor initialization fails when contact object has no SDF mesh.""" + sim, sensor_cfg, dt, robot_cfg, cube_cfg, NutCfg = setup("nut_rgb_ff") + sensor_cfg.enable_camera_tactile = False + sensor_cfg.contact_object_prim_path_expr = "/World/Cube" + robot = Articulation(cfg=robot_cfg) + sensor = VisuoTactileSensor(cfg=sensor_cfg) + cube = RigidObject(cfg=cube_cfg) + with pytest.raises(RuntimeError) as excinfo: + sim.reset() + assert "No SDF mesh found under contact object at path" in str(excinfo.value) + teardown(sim) diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 59b2741e4ee..7701e9ba7bb 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -96,3 +96,66 @@ def test_spawn_ground_plane(sim): assert prim.IsValid() assert prim_utils.is_prim_path_valid("/World/ground_plane") assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_with_physics_material_on_prim(sim): + """Test loading prim from USD file with physics material applied to specific prim.""" + # Spawn gelsight finger with physics material on specific prim + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # Create spawn configuration + spawn_cfg = sim_utils.UsdFileWithPhysicsMaterialOnPrimsCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=1000.0, + compliant_contact_damping=100.0, + apply_physics_material_prim_path="elastomer", + ) + + # Spawn the prim + prim = spawn_cfg.func("/World/Robot", spawn_cfg) + + # Check validity + assert prim.IsValid() + assert prim_utils.is_prim_path_valid("/World/Robot") + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + material_prim_path = "/World/Robot/elastomer/compliant_material" + # Check that the physics material was applied to the specified prim + assert prim_utils.is_prim_path_valid(material_prim_path) + + # Check properties + material_prim = prim_utils.get_prim_at_path(material_prim_path) + assert material_prim.IsValid() + assert material_prim.GetAttribute("physxMaterial:compliantContactStiffness").Get() == 1000.0 + assert material_prim.GetAttribute("physxMaterial:compliantContactDamping").Get() == 100.0 + + +@pytest.mark.isaacsim_ci +def test_spawn_usd_with_physics_material_no_prim_path(sim): + """Test loading prim from USD file with physics material but no prim path specified.""" + # Spawn gelsight finger without specifying prim path for physics material + usd_file_path = f"{ISAACLAB_NUCLEUS_DIR}/TacSL/gelsight_r15_finger/gelsight_r15_finger.usd" + + # Create spawn configuration without physics material prim path + spawn_cfg = sim_utils.UsdFileWithPhysicsMaterialOnPrimsCfg( + usd_path=usd_file_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=True), + compliant_contact_stiffness=1000.0, + compliant_contact_damping=100.0, + apply_physics_material_prim_path=None, + ) + + # Spawn the prim + prim = spawn_cfg.func("/World/Robot", spawn_cfg) + + # Check validity - should still spawn successfully but without physics material + assert prim.IsValid() + assert prim_utils.is_prim_path_valid("/World/Robot") + assert prim.GetPrimTypeInfo().GetTypeName() == "Xform" + + material_prim_path = "/World/Robot/elastomer/compliant_material" + material_prim = prim_utils.get_prim_at_path(material_prim_path) + assert material_prim is not None + assert not material_prim.IsValid() diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py b/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py index 67613a81900..6543b45bf60 100644 --- a/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/sensors/__init__.py @@ -7,4 +7,5 @@ # Configuration for different assets. ## +from .gelsight import * from .velodyne import * diff --git a/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py b/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py new file mode 100644 index 00000000000..b3861ffe805 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/sensors/gelsight.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Predefined configurations for GelSight tactile sensors.""" + +from isaaclab.sensors.tacsl_sensor.visuotactile_sensor_cfg import GelSightRenderCfg + +## +# Predefined Configurations +## + +GELSIGHT_R15_CFG = GelSightRenderCfg( + sensor_data_dir_name="gelsight_r15_data", + background_path="bg.jpg", + calib_path="polycalib.npz", + real_background="real_bg.npy", + image_height=320, + image_width=240, + num_bins=120, + mm_per_pixel=0.0877, +) +"""Configuration for GelSight R1.5 sensor rendering parameters. + +The GelSight R1.5 is a high-resolution tactile sensor with a 320x240 pixel tactile image. +It uses a pixel-to-millimeter ratio of 0.0877 mm/pixel. + +Reference: https://www.gelsight.com/gelsightinc-products/ +""" + +GELSIGHT_MINI_CFG = GelSightRenderCfg( + sensor_data_dir_name="gs_mini_data", + background_path="bg.jpg", + calib_path="polycalib.npz", + real_background="real_bg.npy", + image_height=240, + image_width=320, + num_bins=120, + mm_per_pixel=0.065, +) +"""Configuration for GelSight Mini sensor rendering parameters. + +The GelSight Mini is a compact tactile sensor with a 240x320 pixel tactile image. +It uses a pixel-to-millimeter ratio of 0.065 mm/pixel, providing higher spatial resolution +than the R1.5 model. + +Reference: https://www.gelsight.com/gelsightinc-products/ +"""