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 ed704177acd..fd07994d588 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 e63039a9c6e..2f07206b3ca 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..b10322d2d4c --- /dev/null +++ b/docs/source/overview/core-concepts/sensors/visuo_tactile_sensor.rst @@ -0,0 +1,188 @@ +.. _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 + + tactile_sensor = VisuoTactileSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/tactile_sensor", + ## Sensor configuration + sensor_type="gelsight_r15", + enable_camera_tactile=True, + enable_force_field=True, + ## Elastomer configuration + elastomer_rigid_body="elastomer", + elastomer_tactile_mesh="elastomer/visuals", + elastomer_tip_link_name="elastomer_tip", + # Force field configuration + num_tactile_rows=20, + num_tactile_cols=25, + tactile_margin=0.003, + ## Indenter configuration (will be set based on indenter type) + indenter_rigid_body="indenter", + indenter_sdf_mesh="factory_nut_loose/collisions", + ## Force field physics parameters + tactile_kn=1.0, + tactile_mu=2.0, + tactile_kt=0.1, + ## Compliant dynamics + compliance_stiffness=150.0, + compliant_damping=1.0, + ## Camera configuration + camera_cfg=TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/elastomer_tip/tactile_cam", + update_period=1 / 60, # 60 Hz + height=320, + width=240, + 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 + ), + + ) + +The configuration supports customization of: + +* **Sensor Type**: Specify the tactile sensor model (e.g., ``"gelsight_r15"``) +* **Tactile Modalities**: + * ``enable_camera_tactile`` - Enable tactile RGB imaging through camera sensors + * ``enable_force_field`` - Enable force field computation and visualization +* **Elastomer Properties**: Configure elastomer links and tip components that define the sensor's deformable surface +* **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 +* **Indenter Configuration**: Define properties of interacting objects including rigid body name, and collision mesh +* **Physics Parameters**: Control the sensor's physical behavior: + * ``tactile_kn``, ``tactile_mu``, ``tactile_kt`` - Normal, friction, and tangential stiffness + * ``compliance_stiffness``, ``compliant_damping`` - Compliant dynamics parameters +* **Camera Settings**: Configure resolution, focal length, update rates, and 6-DOF positioning relative to the sensor + +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: + + * ``indenter_rigid_body`` - Specific rigid body within the actor + * ``indenter_sdf_mesh`` - Collision mesh for SDF computation + + **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** + Elastomer properties (``elastomer_rigid_body``, ``elastomer_tip_link_name``) must match the robot model where the sensor is attached. + + + +Usage Example +~~~~~~~~~~~~~ + +To use the tactile sensor in a simulation environment, run the deom: + +.. code-block:: bash + + cd scripts/demos/sensors/tacsl + python tacsl_example.py --enable_cameras --indenter nut --num_envs 16 --use_tactile_taxim --use_tactile_ff --save_viz + +Available command-line options include: + +* ``--enable_cameras``: Enable camera rendering for visualization +* ``--indenter``: Specify the type of indenter object (nut, cube, etc.) +* ``--num_envs``: Number of parallel environments +* ``--use_tactile_taxim``: Enable RGB tactile imaging +* ``--use_tactile_ff``: Enable force field computation +* ``--save_viz``: Save visualization outputs for analysis + +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.taxim_tactile + + # 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..be70935c01c --- /dev/null +++ b/scripts/demos/sensors/tacsl/tacsl_example.py @@ -0,0 +1,388 @@ +# 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 --enable_cameras --num_envs 16 --indenter_type nut --save_viz --use_tactile_taxim --use_tactile_ff + +""" + +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=150.0, help="Tactile compliance stiffness.") +parser.add_argument("--tactile_compliant_damping", type=float, default=1.0, help="Tactile compliant damping.") +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_taxim", action="store_true", help="Use tactile taxim 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( + "--indenter_type", type=str, default="nut", choices=["none", "cube", "nut"], help="Type of indenter 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.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.markers.visualization_markers import VisualizationMarkersCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg + +# Import our TactileSensor +from isaaclab.sensors import TiledCameraCfg, VisuoTactileSensorCfg +from isaaclab.sensors.tacsl_sensor.visuotactile_viz_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.UsdFileCfg( + 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, + ), + 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/tactile_sensor", + history_length=0, + debug_vis=args_cli.debug_tactile_sensor_pts or args_cli.debug_sdf_closest_pts, + # Sensor configuration + sensor_type="gelsight_r15", + enable_camera_tactile=args_cli.use_tactile_taxim, + enable_force_field=args_cli.use_tactile_ff, + # Elastomer configuration + elastomer_rigid_body="elastomer", + elastomer_tactile_mesh="elastomer/visuals", + elastomer_tip_link_name="elastomer_tip", + # Force field configuration + num_tactile_rows=20, + num_tactile_cols=25, + tactile_margin=0.003, + # Indenter configuration (will be set based on indenter type) + indenter_rigid_body=None, # Will be updated based on indenter type + indenter_sdf_mesh=None, # Will be updated based on indenter type + # Force field physics parameters + tactile_kn=args_cli.tactile_kn, + tactile_mu=args_cli.tactile_mu, + tactile_kt=args_cli.tactile_kt, + # Compliant dynamics + compliance_stiffness=args_cli.tactile_compliance_stiffness, + compliant_damping=args_cli.tactile_compliant_damping, + # 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, # 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, + visualizer_cfg=VisualizationMarkersCfg( + prim_path="/Visuals/TactileSensorDebugPts", + markers={ + "debug_pts": sim_utils.SphereCfg( + radius=0.0002, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)), + ), + }, + ), + ) + + +@configclass +class CubeTactileSceneCfg(TactileSensorsSceneCfg): + """Scene with cube indenter.""" + + # Cube indenter + indenter = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/indenter", + 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 indenter.""" + + # Nut indenter + indenter = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/indenter", + 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_taxim_dir = os.path.join(tactile_img_folder, "tactile_taxim") + os.makedirs(tactile_taxim_dir, exist_ok=True) + return tactile_force_field_dir, tactile_taxim_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_taxim_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) + else: + cv2.imwrite(os.path.join(tactile_force_field_dir, f"{count}.png"), tactile_image * 255) + + if tactile_data.taxim_tactile is not None: + taxim_data = tactile_data.taxim_tactile.cpu().numpy() + taxim_data = np.transpose(taxim_data, axes=(0, 2, 1, 3)) + taxim_data_first_2 = taxim_data[:2] if len(taxim_data) >= 2 else taxim_data + taxim_tiled = np.concatenate(taxim_data_first_2, axis=0) + cv2.imwrite(os.path.join(tactile_taxim_dir, f"{count}.png"), taxim_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 indenters 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 "indenter" in scene.keys(): + entity_list.append("indenter") + + while simulation_app.is_running(): + + if count == 122: + print(scene["tactile_sensor"].get_timing_summary()) + # Reset robot and indenter 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 indenter state...") + + if "indenter" 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["indenter"].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, # Important to 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 indenter type + if args_cli.indenter_type == "cube": + scene_cfg = CubeTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + # disabled force field for cube indenter because a SDF collision mesh cannot be created for the Shape Prims + scene_cfg.tactile_sensor.enable_force_field = False + # Update tactile sensor configuration for cube + scene_cfg.tactile_sensor.indenter_rigid_body = "indenter" + scene_cfg.tactile_sensor.indenter_sdf_mesh = None + elif args_cli.indenter_type == "nut": + scene_cfg = NutTactileSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + # Update tactile sensor configuration for nut + scene_cfg.tactile_sensor.indenter_rigid_body = "indenter/factory_nut_loose" + scene_cfg.tactile_sensor.indenter_sdf_mesh = "indenter/factory_nut_loose/collisions" + elif args_cli.indenter_type == "none": + scene_cfg = TactileSensorsSceneCfg(num_envs=args_cli.num_envs, env_spacing=0.2) + # this flag is to visualize the tactile sensor points + scene_cfg.tactile_sensor.debug_vis = True + + scene = InteractiveScene(scene_cfg) + + # Setup compliant materials (required after scene initialization, can be skipped if materials are pre-configured and not needed to be changed) + scene["tactile_sensor"].setup_compliant_materials() + + # Initialize simulation + sim.reset() + print("[INFO]: Setup complete...") + + # Juana: this should be manually called before running any simulation ? + 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/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 15739c33ad7..742a109e5c9 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -30,7 +30,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 @@ -768,6 +768,11 @@ 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 + ) 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..b21d00cff7e --- /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 VisuoTactileSensorCfg +from .visuotactile_sensor_data import VisuoTactileSensorData diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_render.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_render.py new file mode 100644 index 00000000000..339053ee19b --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/gelsight_render.py @@ -0,0 +1,234 @@ +# 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 + +import numpy as np +import os +import scipy +import torch + +import cv2 + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +conf_r15 = { + "data_dir": "gelsight_r15_data", + "background_path": "bg.jpg", + "calib_path": "polycalib.npz", + "real_bg": "real_bg.npy", + "h": 320, + "w": 240, + "numBins": 120, + "pixmm": 0.0877, +} +conf_gs_mini = { + "data_dir": "gs_mini_data", + "background_path": "bg.jpg", + "calib_path": "polycalib.npz", + "real_bg": "real_bg.npy", + "h": 240, + "w": 320, + "numBins": 120, + "pixmm": 0.065, +} +conf_options = { + "gelsight_r15": conf_r15, + "gs_mini": conf_gs_mini, +} + + +def generate_normals_tensor(img_tensor): + """ + Generate the gradient magnitude and direction of the height map using tensors. + + Parameters: + img_tensor (torch.Tensor): Input height map tensor. + + Returns: + tuple: Gradient magnitude tensor, gradient direction tensor, and None. + """ + img_grad_tensor = torch.gradient(img_tensor, dim=(1, 2)) + dzdx, dzdy = img_grad_tensor + + grad_mag_orig_tensor = torch.sqrt(dzdx**2 + dzdy**2) + grad_mag_tensor = torch.arctan(grad_mag_orig_tensor) # seems that arctan is used as a squashing function + grad_dir_tensor = torch.arctan2(dzdx, dzdy) + grad_dir_tensor[grad_mag_orig_tensor == 0] = 0 + + # handle edges + grad_mag_tensor = torch.nn.functional.pad(grad_mag_tensor[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + grad_dir_tensor = torch.nn.functional.pad(grad_dir_tensor[:, 1:-1, 1:-1], pad=(1, 1, 1, 1)) + + return grad_mag_tensor, grad_dir_tensor, None + + +def get_filtering_kernel(kernel_sz=5): + """ + Create a Gaussian filtering kernel. + # For kernel derivation, see https://cecas.clemson.edu/~stb/ece847/internal/cvbook/ch03_filtering.pdf + + Parameters: + kernel_sz (int): Size of the kernel. + + Returns: + numpy.ndarray: 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(img_tensor, kernel_tensor): + """ + Apply Gaussian filtering to the input image tensor. + + Parameters: + img_tensor (torch.Tensor): Input image tensor. + kernel_tensor (torch.Tensor): Filtering kernel tensor. + + Returns: + torch.Tensor: Filtered image tensor. + """ + img_tensor_output = torch.nn.functional.conv2d( + img_tensor.permute(0, 3, 1, 2), kernel_tensor.unsqueeze(0).unsqueeze(0), stride=1, padding="same" + ).permute(0, 2, 3, 1) + return img_tensor_output + + +def get_gs_render_data(data_dir: str, file_name: str) -> str | None: + """Gets the path for the GelSight render data file. + + If the data file is not cached locally then the file is downloaded from + the Isaac Lab Nucleus directory. The cached path is then returned. + + Args: + data_dir: The data directory name containing the render data. + file_name: The specific file name to retrieve. + + Returns: + The local path to the downloaded/cached file, or None if unavailable. + """ + ov_path = os.path.join(ISAACLAB_NUCLEUS_DIR, "TacSL", data_dir, file_name) + download_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), data_dir) + os.makedirs(download_dir, exist_ok=True) + download_path = os.path.join(download_dir, file_name) + + if not os.path.exists(download_path): + print(f"Fetching gs render data : {ov_path}") + try: + download_path = retrieve_file_path(ov_path, download_dir) + except Exception: + print("A gs render data is currently unavailable for this task.") + return None + else: + print("Using pre-fetched gs render data: ", download_path) + return download_path + + +class CalibData: + """ + Class to handle calibration data. + """ + + def __init__(self, dataPath): + """ + Initialize the calibration data. + + Parameters: + dataPath (str): Path to the calibration data file. + """ + self.dataPath = dataPath + data = np.load(dataPath) + + self.numBins = data["bins"] + self.grad_r = data["grad_r"] + self.grad_g = data["grad_g"] + self.grad_b = data["grad_b"] + + +class gelsightRender: + """ + Class to handle GelSight rendering using the Taxim example-based approach. + Ref: https://arxiv.org/abs/2109.04027 + """ + + def __init__(self, sensor_name, device): + """ + Initialize the GelSight renderer. + + Parameters: + sensor_name (str): Name of the sensor. + device (str): Device to use ('cpu' or 'cuda'). + """ + + self.sensor_name = sensor_name + self.device = device + self.conf = conf_options[self.sensor_name] + + bg_path = get_gs_render_data(self.conf["data_dir"], self.conf["background_path"]) + calib_path = get_gs_render_data(self.conf["data_dir"], self.conf["calib_path"]) + + self.background = cv2.cvtColor(cv2.imread(bg_path), cv2.COLOR_BGR2RGB) + + self.calib_data = CalibData(calib_path) + h, w = self.conf["h"], self.conf["w"] + bins = self.conf["numBins"] + [xx, yy] = np.meshgrid(range(w), range(h)) + xf = xx.flatten() + yf = yy.flatten() + self.A = np.array([xf * xf, yf * yf, xf * yf, xf, yf, np.ones(h * w)]).T + + binm = 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 = get_filtering_kernel(kernel_sz=5) + self.kernel = torch.tensor(kernel, dtype=torch.float, device=self.device) + + self.calib_data_grad_r_tensor = torch.tensor(self.calib_data.grad_r, device=self.device) + self.calib_data_grad_g_tensor = torch.tensor(self.calib_data.grad_g, device=self.device) + self.calib_data_grad_b_tensor = torch.tensor(self.calib_data.grad_b, device=self.device) + + self.A_tensor = torch.tensor(self.A.reshape(h, w, 6), device=self.device).unsqueeze(0) + self.background_tensor = torch.tensor(self.background, device=self.device) + print("Gelsight initialization done!") + + def render_tensorized(self, heightMap): + """ + Render the height map using the GelSight sensor (tensorized version). + + Parameters: + heightMap (torch.Tensor): Input height map tensor. + + Returns: + torch.Tensor: 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.conf["pixmm"] + + height_map = gaussian_filtering(height_map.unsqueeze(-1), self.kernel).squeeze(-1) + + grad_mag_tensor, grad_dir_tensor, _ = generate_normals_tensor(height_map) + + idx_x_tensor = torch.floor(grad_mag_tensor / self.x_binr).long() + idx_y_tensor = torch.floor((grad_dir_tensor + np.pi) / self.y_binr).long() + + params_r_tensor = self.calib_data_grad_r_tensor[idx_x_tensor, idx_y_tensor, :] + params_g_tensor = self.calib_data_grad_g_tensor[idx_x_tensor, idx_y_tensor, :] + params_b_tensor = self.calib_data_grad_b_tensor[idx_x_tensor, idx_y_tensor, :] + + sim_img_rgb_tensor = torch.zeros((*idx_x_tensor.shape, 3), device=self.device) # TODO: move to init + sim_img_rgb_tensor[..., 0] = torch.sum(self.A_tensor * params_r_tensor, axis=-1) # R + sim_img_rgb_tensor[..., 1] = torch.sum(self.A_tensor * params_g_tensor, axis=-1) # G + sim_img_rgb_tensor[..., 2] = torch.sum(self.A_tensor * params_b_tensor, axis=-1) # B + + # write tactile image + sim_img = sim_img_rgb_tensor + self.background_tensor # /255.0 + sim_img = torch.clip(sim_img, 0, 255, out=sim_img).to(torch.uint8) + return sim_img 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..69917abecbd --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor.py @@ -0,0 +1,990 @@ +# 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 typing import TYPE_CHECKING, Any + +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils +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 omni.physx.scripts import physicsUtils + +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_render import gelsightRender +from isaaclab.sensors.tacsl_sensor.visuotactile_sensor_data import VisuoTactileSensorData +from isaaclab.sim.spawners.materials.physics_materials import spawn_rigid_body_material +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg +from isaaclab.utils.timer import Timer + +if TYPE_CHECKING: + from .visuotactile_sensor_cfg import VisuoTactileSensorCfg + +# Try importing optional dependencies +try: + import trimesh + + TRIMESH_AVAILABLE = True +except ImportError: + TRIMESH_AVAILABLE = False + trimesh = None + + +from scipy.spatial.transform import Rotation as R + + +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. + It follows the same initialization and update patterns as other IsaacLab sensors. + + 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: + + * ``indenter_rigid_body`` - Specific rigid body within the actor + * ``indenter_sdf_mesh`` - Collision mesh for SDF computation + + **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. + + **Elastomer Configuration** + Elastomer properties (``elastomer_rigid_body``, ``elastomer_tip_link_name``) must + match the robot model where the sensor is attached. + """ + + cfg: VisuoTactileSensorCfg + """The configuration parameters.""" + + def __init__(self, cfg: VisuoTactileSensorCfg): + """Initializes the tactile sensor object. + + Args: + cfg: The configuration parameters. + """ + # Initialize instance variables before calling super().__init__ + # This is needed because super().__init__ may call _set_debug_vis_impl + + # 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 + self._indenter_mesh: Any | None = None + self._indenter_mesh_local_tf: tuple[torch.Tensor, torch.Tensor] | None = None + + # Physics views + self._physics_sim_view = None + self._elastomer_body_view = None + self._elastomer_tip_view = None + self._indenter_body_view = None + + # Internal state + self._elastomer_link_id: int | None = None + self._indenter_link_id: int | None = 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"\tsensor type : {self.cfg.sensor_type}\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 and self.cfg.camera_cfg is not None: + 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 setup_compliant_materials(self): + """Setup compliant contact materials for the elastomer. + + This method configures the elastomer collision geometry with compliant contact + materials that provide softer, more realistic contact behavior. Only applies + materials if compliant contact is enabled in the configuration. + + Note: + This method should be called after sensor initialization and before + simulation starts to ensure proper contact behavior. + """ + # Get the current stage + stage = stage_utils.get_current_stage() + + # Create material configuration + print("set up compliant contact materials", self.cfg.elastomer_collision_path) + material_cfg = RigidBodyMaterialCfg( + compliant_contact_stiffness=self.cfg.compliance_stiffness, + compliant_contact_damping=self.cfg.compliant_damping, + ) + parent_prims = sim_utils.find_matching_prims(self.cfg.prim_path.rsplit("/", 1)[0]) + self._num_envs = len(parent_prims) + assert self._num_envs > 0, "No environments found" + + # Apply material to each environment + for env_id in range(self._num_envs): + # Construct the prim path for elastomer collision geometry + # Get the environment path from parent prims + if len(parent_prims) > env_id: + # Use the specific environment's parent prim + env_prim_path = parent_prims[env_id].GetPath().pathString + + # Construct full path to elastomer collision + elastomer_collision_path = ( + f"{env_prim_path}/{self.cfg.elastomer_rigid_body}/{self.cfg.elastomer_collision_path}" + ) + # Spawn the rigid body material + mat_path = spawn_rigid_body_material(elastomer_collision_path, material_cfg) + + # Get the body prim and apply the physics material + body_prim = prim_utils.get_prim_parent(mat_path) + physicsUtils.add_physics_material_to_prim(stage, body_prim, elastomer_collision_path) + + omni.log.warn(f"Applied compliant contact materials to {self._num_envs} environments.") + + 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: + AssertionError: If camera sensor is not initialized or initial render fails. + """ + if not self.cfg.enable_camera_tactile: + return None + if not self._camera_sensor.is_initialized: + assert self._camera_sensor.is_initialized, "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: + assert False, "Initial render is None" + + if self._nominal_tactile is not None: + assert False, "Nominal tactile is not None" + + self._nominal_tactile = dict() + for key, value in initial_render.items(): + print(f"key: {key}, value: {value.shape}") + 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: + omni.log.warn("Camera configuration is None. Disabling camera-based tactile sensing.") + return + + # gelsightRender + self.taxim_gelsight = gelsightRender(self.cfg.sensor_type, 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.taxim_tactile = torch.zeros( + (self._num_envs, self.cfg.camera_cfg.height, self.cfg.camera_cfg.width, 3), 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. Finds and stores body handles for elastomer and indenter + 2. Generates tactile sensing points on the elastomer surface + 3. Initializes SDF (Signed Distance Field) for collision detection + 4. Creates data buffers for storing force field measurements + + Raises: + AssertionError: If tactile point generation fails or point count mismatch. + """ + + # Find parent prims and get body handles + self._find_body_handles() + + # Generate tactile points on elastomer surface + b_success = 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, + ) + assert ( + b_success and self.num_tactile_points == self.cfg.num_tactile_rows * self.cfg.num_tactile_cols + ), "Failed to generate tactile points" + + # Initialize SDF for collision detection + self._initialize_sdf() + + # Initialize force field data buffers + self._initialize_force_field_buffers() + + def _find_body_handles(self): + """Find and store body handles for elastomer and indenter components. + + This method locates the relevant rigid bodies in the simulation and creates + physics views for them. These views are used for accessing pose and velocity + information during force field tactile sensing. + + Creates body views for: + - Elastomer main body (base of the tactile sensor) + - Elastomer tip (contact surface) + - Indenter (object making contact, if configured) + + Note: + Uses pattern matching to handle multiple environments by replacing + "env_0" with "env_*" in prim paths. + """ + # Find elastomer and indenter links + template_prim_path = self._parent_prims[0].GetPath().pathString + + # Create body views for elastomer and indenter + elastomer_pattern = f"{template_prim_path}/{self.cfg.elastomer_rigid_body}" + body_names_regex = [elastomer_pattern.replace("env_0", "env_*")] + self._elastomer_body_view = self._physics_sim_view.create_rigid_body_view(body_names_regex) + elastomer_tip_pattern = f"{template_prim_path}/{self.cfg.elastomer_tip_link_name}" + body_names_regex = [elastomer_tip_pattern.replace("env_0", "env_*")] + self._elastomer_tip_body_view = self._physics_sim_view.create_rigid_body_view(body_names_regex) + + # For force field sensing, we need indenter information + if hasattr(self.cfg, "indenter_rigid_body") and self.cfg.indenter_rigid_body is not None: + # Get environment path by going up from the template_prim_path + # template_prim_path is like "/World/envs/env_0/Robot", we want "/World/envs/env_0" + env_path = "/".join(template_prim_path.split("/")[:-1]) + indenter_pattern = f"{env_path}/{self.cfg.indenter_rigid_body}" + self._indenter_body_view = self._physics_sim_view.create_rigid_body_view( + indenter_pattern.replace("env_0", "env_*") + ) + print("create indenter body view: ", self.cfg.indenter_rigid_body) + + def _generate_tactile_points(self, num_divs: list, margin: float, visualize: bool) -> bool: + """Try to generate tactile points from USD mesh data.""" + from pxr import UsdGeom + + # Check if required dependencies are available + if not TRIMESH_AVAILABLE: + print("Trimesh not available, please install trimesh") + return False + + # Get the elastomer prim path + template_prim_path = self._parent_prims[0].GetPath().pathString + elastomer_prim_path = f"{template_prim_path}/{self.cfg.elastomer_tactile_mesh}" + print("generate tactile points from USD mesh: elastomer_prim_path: ", elastomer_prim_path) + + # Find mesh prim + mesh_prim = sim_utils.get_first_matching_child_prim( + elastomer_prim_path, lambda prim: prim.GetTypeName() == "Mesh" + ) + + if mesh_prim is None or not mesh_prim.IsValid(): + return False + + # Extract mesh data + usd_mesh = UsdGeom.Mesh(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 + + # Get elastomer to tip transform + elastomer_to_tip_link_pos = self._get_elastomer_to_tip_transform() + + # 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([np.sign(elastomer_to_tip_link_pos[0][slim_axis].item())]) + 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] = -np.sign(elastomer_to_tip_link_pos[0][slim_axis].item()) # Ray point towards elastomer + + # Handle the ray intersection result + result = mesh_data.intersects_id( + grid_corners, np.tile([ray_dir], (grid_corners.shape[0], 1)), return_locations=True, multiple_hits=False + ) + + # Extract results based on what was returned + if len(result) == 3: + index_tri, index_ray, locations = result + else: + index_tri, index_ray = result + locations = None + + if visualize: + query_pointcloud = trimesh.PointCloud(locations, colors=(0.0, 0.0, 1.0)) + trimesh.Scene([mesh, query_pointcloud]).show() + + # Check if we got the expected number of points + if len(index_ray) != len(grid_corners): + raise ValueError("Fewer number of tactile points than expected") + + # 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] + + # Assume tactile frame rotation are all the same + rotation = (0, 0, -np.pi) # NOTE: assume tactile frame rotation are all the same + 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) + ) + + print(f"Generated {len(tactile_points)} tactile points from USD mesh using ray casting") + return True + + def _get_elastomer_to_tip_transform(self): + """Get the transformation from the elastomer to the tip. + + Returns: + Position of the elastomer tip in the elastomer local frame. + """ + # Get elastomer and tip body handles using IsaacLab's body view system + if self._elastomer_body_view is None or self._elastomer_tip_body_view is None: + raise RuntimeError("Elastomer body view not initialized") + + # Get poses directly from the dedicated body views + elastomer_pose = self._elastomer_body_view.get_transforms() + elastomer_tip_pose = self._elastomer_tip_body_view.get_transforms() + elastomer_pose[..., 3:] = math_utils.convert_quat(elastomer_pose[..., 3:], to="wxyz") + elastomer_tip_pose[..., 3:] = math_utils.convert_quat(elastomer_tip_pose[..., 3:], to="wxyz") + + # Extract positions and quaternions from the first environment + elastomer_pos = elastomer_pose[0, :3] # (3,) + elastomer_quat = elastomer_pose[0, 3:] # (4,) + tip_pos = elastomer_tip_pose[0, :3] # (3,) + + # Compute relative transform from elastomer to tip + # Position: transform tip position to elastomer local frame + relative_pos_world = tip_pos - elastomer_pos + tip_pos_local = math_utils.quat_apply_inverse(elastomer_quat, relative_pos_world.unsqueeze(0)) + + return tip_pos_local + + def _initialize_sdf(self): + """Initialize SDF for collision detection.""" + if self._indenter_body_view is None: + return + # Create SDF Shape View of indenter for querying. + prim_paths_expr = f"/World/envs/env_.*/{self.cfg.indenter_sdf_mesh}" + num_query_points = self.cfg.num_tactile_rows * self.cfg.num_tactile_cols + prepare_sdf_schemas = False # indenter should already have an SDF collision mesh + + # Create SDF Shape View of indenter for querying. + omni.log.info(f"create SDF Shape View of indenter for querying: {prim_paths_expr}") + self._indenter_sdf_view = SdfShapePrim( + prim_paths_expr=prim_paths_expr, + name="indenter_sdf_view", + num_query_points=num_query_points, + prepare_sdf_schemas=prepare_sdf_schemas, + ) + self._indenter_sdf_view.initialize(physics_sim_view=self._physics_sim_view) + + 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. + """ + # Update camera sensor + self._camera_sensor.update(self._sim_physics_dt) + + # Get camera data + camera_data = self._camera_sensor.data + + if "distance_to_image_plane" in camera_data.output: + self._data.tactile_camera_depth = camera_data.output["distance_to_image_plane"][env_ids].clone() + diff = self._nominal_tactile["distance_to_image_plane"][env_ids] - self._data.tactile_camera_depth + self._data.taxim_tactile[env_ids] = self.taxim_gelsight.render_tensorized(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 world coordinates, queries the SDF of the + indenter 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 indenter body views to be initialized. Returns + early if tactile points or body views are not available. + """ + if self._elastomer_body_view is None or self._tactile_pos_local is None: + return + + # 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 + self._transform_tactile_points_to_world(elastomer_pos_w, elastomer_quat_w) + + # Compute penalty-based tactile forces using SDF + if ( + self._indenter_body_view is not None + and self._data.tactile_points_pos_w is not None + and hasattr(self, "_indenter_sdf_view") + ): + + # Get indenter poses and precompute components + indenter_pos_w, indenter_quat_w = self._indenter_body_view.get_transforms().split([3, 4], dim=-1) + indenter_quat_w = math_utils.convert_quat(indenter_quat_w, to="wxyz") + + # Get tactile points in world coordinates + world_tactile_points = self._data.tactile_points_pos_w + + # Transform tactile points to indenter local frame for SDF queries + points_indenter_local, indenter_quat_inv = self._transform_points_to_indenter_local( + world_tactile_points, indenter_pos_w, indenter_quat_w + ) + + # Query SDF for collision detection + sdf_values_and_gradients = self._indenter_sdf_view.get_sdf_and_gradients(points_indenter_local) + + # Extract SDF values (penetration depth) and gradients (normals) + sdf_values = sdf_values_and_gradients[..., -1] # Last component is SDF value + sdf_gradients = sdf_values_and_gradients[..., :-1] # First 3 components are gradients + + # Compute tactile forces using optimized version with precomputed values + self._compute_tactile_forces_from_sdf( + points_indenter_local, + sdf_values, + sdf_gradients, + indenter_pos_w, + indenter_quat_w, + elastomer_quat_w, + env_ids, + ) + + 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_indenter_local( + self, world_points: torch.Tensor, indenter_pos_w: torch.Tensor, indenter_quat_w: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor]: + """Optimized version: Transform world coordinates to indenter local frame. + + Args: + world_points: Points in world coordinates. Shape: (num_envs, num_points, 3) + indenter_pos_w: Indenter positions in world frame. Shape: (num_envs, 3) + indenter_quat_w: Indenter quaternions in world frame. Shape: (num_envs, 4) + + Returns: + Points in indenter local coordinates and inverse quaternions + """ + # Get inverse transformation (per environment) + # wxyz in torch + indenter_quat_inv, indenter_pos_inv = torch_utils.tf_inverse(indenter_quat_w, indenter_pos_w) + num_pts = self.num_tactile_points + + indenter_quat_expanded = indenter_quat_inv.unsqueeze(1).expand(-1, num_pts, 4) + indenter_pos_expanded = indenter_pos_inv.unsqueeze(1).expand(-1, num_pts, 3) + + # Apply transformation + points_sdf = torch_utils.tf_apply(indenter_quat_expanded, indenter_pos_expanded, world_points) + + return points_sdf, indenter_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_indenter_local: torch.Tensor, + sdf_values: torch.Tensor, + sdf_gradients: torch.Tensor, + indenter_pos_w: torch.Tensor, + indenter_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_indenter_local: Points in indenter local frame + sdf_values: SDF values (negative means penetration) + sdf_gradients: SDF gradients (surface normals) + indenter_pos_w: Indenter positions in world frame + indenter_quat_w: Indenter 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 indenter and elastomer velocities + indenter_velocities = self._indenter_body_view.get_velocities() + indenter_linvel_w = indenter_velocities[env_ids, :3] + indenter_angvel_w = indenter_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 indenter orientation) - use precomputed quaternions + indenter_quat_expanded = indenter_quat_w[env_ids].unsqueeze(1).expand(-1, num_pts, 4) + + # Apply quaternion transformation + normals_world = math_utils.quat_apply(indenter_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 indenter velocities + closest_points_sdf = points_indenter_local[env_ids] + depth.unsqueeze(-1) * normals_local + + if self.cfg.visualize_sdf_closest_pts: + debug_closest_points_sdf = ( + points_indenter_local[env_ids] - sdf_values[env_ids].unsqueeze(-1) * normals_local + ) + self.debug_closest_points_wolrd = math_utils.quat_apply( + indenter_quat_expanded, debug_closest_points_sdf + ) + indenter_pos_w[env_ids].unsqueeze(1).expand(-1, num_pts, 3) + + indenter_linvel_expanded = indenter_linvel_w.unsqueeze(1).expand(-1, num_pts, 3) + indenter_angvel_expanded = indenter_angvel_w.unsqueeze(1).expand(-1, num_pts, 3) + closest_points_vel_world = ( + torch.linalg.cross( + indenter_angvel_expanded, math_utils.quat_apply(indenter_quat_expanded, closest_points_sdf) + ) + + indenter_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 + self._timing_call_count -= 2 + num_frames = self._timing_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": self._timing_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..ccdf16a3b08 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_sensor_cfg.py @@ -0,0 +1,103 @@ +# 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.utils import configclass + +from ..camera.tiled_camera_cfg import TiledCameraCfg +from ..sensor_base_cfg import SensorBaseCfg +from .visuotactile_sensor import VisuoTactileSensor + + +@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 + sensor_type: str = "gelsight_r15" + """Type of tactile sensor. Options: 'gelsight_r15', 'gs_mini'.""" + + enable_camera_tactile: bool = True + """Whether to enable camera-based tactile sensing.""" + + enable_force_field: bool = True + """Whether to enable force field tactile sensing.""" + + # Elastomer configuration + elastomer_rigid_body: str = "elastomer" + """Prim path of the elastomer rigid body for tactile sensing.""" + + elastomer_tactile_mesh: str = "elastomer/visual" + """Prim path of the elastomer mesh for tactile point generation.""" + + elastomer_tip_link_name: str = "elastomer_tip" + """Prim path of the elastomer tip link.""" + + # 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).""" + + # Indenter configuration for force field sensing + indenter_rigid_body: str | None = None + """Prim path of the indenter rigid body for SDF-based collision detection.""" + + indenter_sdf_mesh: str | None = None + """Prim path of the indenter SDF mesh for SDF-based collision detection.""" + + # 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.""" + + # Compliant dynamics configuration + compliance_stiffness: float = 1.0 + """Compliance stiffness for elastomer dynamics.""" + + compliant_damping: float = 0.1 + """Compliant damping for elastomer dynamics.""" + + elastomer_collision_path: str = "collisions/compliant_contact" + """Path to the elastomer collision geometry.""" + + # Camera configuration (optional, for camera-based tactile sensing) + 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 = VisualizationMarkersCfg(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..4f1a027b368 --- /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).""" + + taxim_tactile: torch.Tensor | None = None + """Nominal (reference) tactile images with no contact. 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. Shape: (num_instances, num_tactile_points).""" + + tactile_shear_force: torch.Tensor | None = None + """Shear forces at each tactile point. Shape: (num_instances, num_tactile_points, 2).""" diff --git a/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_viz_utils.py b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_viz_utils.py new file mode 100644 index 00000000000..44c88af4979 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/tacsl_sensor/visuotactile_viz_utils.py @@ -0,0 +1,72 @@ +# 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 + +import numpy as np + +import cv2 + + +def visualize_tactile_shear_image( + tactile_normal_force, + tactile_shear_force, + normal_force_threshold=0.00008, + shear_force_threshold=0.0005, + resolution=30, +): + """ + Visualize the tactile shear field. + + Args: + tactile_normal_force (np.ndarray): Array of tactile normal forces. + tactile_shear_force (np.ndarray): Array of tactile shear forces. + normal_force_threshold (float): Threshold for normal force visualization. + shear_force_threshold (float): Threshold for shear force visualization. + resolution (int): Resolution for the visualization. + + Returns: + np.ndarray: 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) + + # print('(min, max) tactile normal force: ', np.min(tactile_normal_force), np.max(tactile_normal_force)) + 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, resolution=5, depth_multiplier=300.0): + """ + Visualize the penetration depth. + + Args: + penetration_depth_img (np.ndarray): Image of penetration depth. + resolution (int): Resolution for the upsampling. + depth_multiplier (float): Multiplier for the depth values. + + Returns: + np.ndarray: Upsampled image visualizing the penetration depth. + """ + # penetration_depth_img_upsampled = penetration_depth.repeat(resolution, 0).repeat(resolution, 1) + print("penetration_depth_img: ", np.max(penetration_depth_img)) + 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 diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index c78f9817245..415b2e7dad0 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -34,6 +34,7 @@ "transformers", "einops", # needed for transformers, doesn't always auto-install "warp-lang", + "opencv-python", # make sure this is consistent with isaac sim version "pillow==11.2.1", # livestream