Commit 14a3a7af authored by Antoine RICHARD's avatar Antoine RICHARD Committed by Kelly Guo

Adds support and example for SurfaceGrippers (#464)

# Description

Adds support for SurfaceGrippers inside IsaacLab


## Type of change

- New feature (non-breaking change which adds functionality)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [ ] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
parent 0485611c
...@@ -192,6 +192,7 @@ enabled=true # Enable this for DLSS ...@@ -192,6 +192,7 @@ enabled=true # Enable this for DLSS
"isaacsim.core.cloner" = {} "isaacsim.core.cloner" = {}
"isaacsim.core.utils" = {} "isaacsim.core.utils" = {}
"isaacsim.core.version" = {} "isaacsim.core.version" = {}
"isaacsim.robot.surface_gripper" = {}
######################## ########################
# Isaac Lab Extensions # # Isaac Lab Extensions #
......
.. _tutorial-interact-surface-gripper:
Interacting with a surface gripper
==================================
.. currentmodule:: isaaclab
This tutorial shows how to interact with an articulated robot with a surface gripper attached to its end-effector in
the simulation. It is a continuation of the :ref:`tutorial-interact-articulation` tutorial, where we learned how to
interact with an articulated robot. Note that as of IsaacSim 5.0 the surface gripper are only supported on the cpu
backend.
The Code
~~~~~~~~
The tutorial corresponds to the ``run_surface_gripper.py`` script in the ``scripts/tutorials/01_assets``
directory.
.. dropdown:: Code for run_surface_gripper.py
:icon: code
.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py
:language: python
:emphasize-lines: 61-85, 124-125, 128-142, 147-150
:linenos:
The Code Explained
~~~~~~~~~~~~~~~~~~
Designing the scene
-------------------
Similarly to the previous tutorial, we populate the scene with a ground plane and a distant light. Then, we spawn
an articulation from its USD file. This time a pick-and-place robot is spawned. The pick-and-place robot is a simple
robot with 3 driven axes, its gantry allows it to move along the x and y axes, as well as up and down along the z-axis.
Furthermore, the robot end-effector is outfitted with a surface gripper.
The USD file for the pick-and-place robot contains the robot's geometry, joints, and other physical properties
as well as the surface gripper. Before implementing a similar gripper on your own robot, we recommend to
check out the USD file for the gripper found on Isaaclab's Nucleus.
For the pick-and-place robot, we use its pre-defined configuration object, you can find out more about it in the
:ref:`how-to-write-articulation-config` tutorial. For the surface gripper, we also need to create a configuration
object. This is done by instantiating a :class:`assets.SurfaceGripperCfg` object and passing it the relevant
parameters.
The available parameters are:
- ``max_grip_distance``: The maximum distance at which the gripper can grasp an object.
- ``shear_force_limit``: The maximum force the gripper can exert in the direction perpendicular to the gripper's axis.
- ``coaxial_force_limit``: The maximum force the gripper can exert in the direction of the gripper's axis.
- ``retry_interval``: The time the gripper will stay in a grasping state.
As seen in the previous tutorial, we can spawn the articulation into the scene in a similar fashion by creating
an instance of the :class:`assets.Articulation` class by passing the configuration object to its constructor. The same
principle applies to the surface gripper. By passing the configuration object to the :class:`assets.SurfaceGripper`
constructor, the surface gripper is created and can be added to the scene. In practice, the object will only be
initialized when the play button is pressed.
.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py
:language: python
:start-at: # Create separate groups called "Origin1", "Origin2"
:end-at: surface_gripper = SurfaceGripper(cfg=surface_gripper_cfg)
Running the simulation loop
---------------------------
Continuing from the previous tutorial, we reset the simulation at regular intervals, set commands to the articulation,
step the simulation, and update the articulation's internal buffers.
Resetting the simulation
""""""""""""""""""""""""
To reset the surface gripper, we only need to call the :meth:`SurfaceGripper.reset` method which will reset the
internal buffers and caches.
.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py
:language: python
:start-at: # Opens the gripper and makes sure the gripper is in the open state
:end-at: surface_gripper.reset()
Stepping the simulation
"""""""""""""""""""""""
Applying commands to the surface gripper involves two steps:
1. *Setting the desired commands*: This sets the desired gripper commands (Open, Close, or Idle).
2. *Writing the data to the simulation*: Based on the surface gripper's configuration, this step handles writes the
converted values to the PhysX buffer.
In this tutorial, we use a random command to set the gripper's command. The gripper behavior is as follows:
- -1 < command < -0.3 --> Gripper is Opening
- -0.3 < command < 0.3 --> Gripper is Idle
- 0.3 < command < 1 --> Gripper is Closing
At every step, we randomly sample commands and set them to the gripper by calling the
:meth:`SurfaceGripper.set_grippers_command` method. After setting the commands, we call the
:meth:`SurfaceGripper.write_data_to_sim` method to write the data to the PhysX buffer. Finally, we step
the simulation.
.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py
:language: python
:start-at: # Sample a random command between -1 and 1.
:end-at: surface_gripper.write_data_to_sim()
Updating the state
""""""""""""""""""
To know the current state of the surface gripper, we can query the :meth:`assets.SurfaceGripper.state` property.
This property returns a tensor of size ``[num_envs]`` where each element is either ``-1``, ``0``, or ``1``
corresponding to the gripper state. This property is updated every time the :meth:`assets.SurfaceGripper.update` method
is called.
- ``-1`` --> Gripper is Open
- ``0`` --> Gripper is Closing
- ``1`` --> Gripper is Closed
.. literalinclude:: ../../../../scripts/tutorials/01_assets/run_surface_gripper.py
:language: python
:start-at: # Read the gripper state from the simulation
:end-at: surface_gripper_state = surface_gripper.state
The Code Execution
~~~~~~~~~~~~~~~~~~
To run the code and see the results, let's run the script from the terminal:
.. code-block:: bash
./isaaclab.sh -p scripts/tutorials/01_assets/run_surface_gripper.py --device cpu
This command should open a stage with a ground plane, lights, and two pick-and-place robots.
In the terminal, you should see the gripper state and the command being printed.
To stop the simulation, you can either close the window, or press ``Ctrl+C`` in the terminal.
.. figure:: ../../_static/tutorials/tutorial_run_surface_gripper.jpg
:align: center
:figwidth: 100%
:alt: result of run_surface_gripper.py
In this tutorial, we learned how to create and interact with a surface gripper. We saw how to set commands and
query the gripper state. We also saw how to update its buffers to read the latest state from the simulation.
In addition to this tutorial, we also provide a few other scripts that spawn different robots. These are included
in the ``scripts/demos`` directory. You can run these scripts as:
.. code-block:: bash
# Spawn many pick-and-place robots and perform a pick-and-place task
./isaaclab.sh -p scripts/demos/pick_and_place.py
Note that in practice, the users would be expected to register their :class:`assets.SurfaceGripper` instances inside
a :class:`isaaclab.InteractiveScene` object, which will automatically handle the calls to the
:meth:`assets.SurfaceGripper.write_data_to_sim` and :meth:`assets.SurfaceGripper.update` methods.
.. code-block:: python
# Create a scene
scene = InteractiveScene()
# Register the surface gripper
scene.surface_grippers["gripper"] = surface_gripper
...@@ -47,6 +47,7 @@ class and its derivatives such as :class:`~isaaclab.assets.RigidObject`, ...@@ -47,6 +47,7 @@ class and its derivatives such as :class:`~isaaclab.assets.RigidObject`,
01_assets/run_rigid_object 01_assets/run_rigid_object
01_assets/run_articulation 01_assets/run_articulation
01_assets/run_deformable_object 01_assets/run_deformable_object
01_assets/run_surface_gripper
Creating a Scene Creating a Scene
---------------- ----------------
......
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import argparse
from isaaclab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Keyboard control for Isaac Lab Pick and Place.")
# 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
import torch
from collections.abc import Sequence
import carb
import omni
from isaaclab_assets.robots.pick_and_place import PICK_AND_PLACE_CFG
import isaaclab.sim as sim_utils
from isaaclab.assets import (
Articulation,
ArticulationCfg,
RigidObject,
RigidObjectCfg,
SurfaceGripper,
SurfaceGripperCfg,
)
from isaaclab.envs import DirectRLEnv, DirectRLEnvCfg
from isaaclab.markers import SPHERE_MARKER_CFG, VisualizationMarkers
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sim import SimulationCfg
from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane
from isaaclab.utils import configclass
from isaaclab.utils.math import sample_uniform
@configclass
class PickAndPlaceEnvCfg(DirectRLEnvCfg):
"""Example configuration for a PickAndPlace robot using suction-cups.
This example follows what would be typically done in a DirectRL pipeline.
"""
# env
decimation = 4
episode_length_s = 240.0
action_space = 4
observation_space = 6
state_space = 0
device = "cpu"
# Simulation cfg. Note that we are forcing the simulation to run on CPU.
# This is because the surface gripper API is only supported on CPU backend for now.
sim: SimulationCfg = SimulationCfg(dt=1 / 60, render_interval=decimation, device="cpu")
debug_vis = True
# robot
robot_cfg: ArticulationCfg = PICK_AND_PLACE_CFG.replace(prim_path="/World/envs/env_.*/Robot")
x_dof_name = "x_axis"
y_dof_name = "y_axis"
z_dof_name = "z_axis"
# We add a cube to pick-up
cube_cfg: RigidObjectCfg = RigidObjectCfg(
prim_path="/World/envs/env_.*/Robot/Cube",
spawn=sim_utils.CuboidCfg(
size=(0.4, 0.4, 0.4),
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
collision_props=sim_utils.CollisionPropertiesCfg(),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 0.0, 0.8)),
),
init_state=RigidObjectCfg.InitialStateCfg(),
)
# Surface Gripper, the prim_expr need to point to a unique surface gripper per environment.
gripper = SurfaceGripperCfg(
prim_expr="/World/envs/env_.*/Robot/picker_head/SurfaceGripper",
max_grip_distance=0.1,
shear_force_limit=500.0,
coaxial_force_limit=500.0,
retry_interval=0.2,
)
# scene
scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=1, env_spacing=12.0, replicate_physics=True)
# reset logic
# Initial position of the robot
initial_x_pos_range = [-2.0, 2.0]
initial_y_pos_range = [-2.0, 2.0]
initial_z_pos_range = [0.0, 0.5]
# Initial position of the cube
initial_object_x_pos_range = [-2.0, 2.0]
initial_object_y_pos_range = [-2.0, -0.5]
initial_object_z_pos = 0.2
# Target position of the cube
target_x_pos_range = [-2.0, 2.0]
target_y_pos_range = [2.0, 0.5]
target_z_pos = 0.2
class PickAndPlaceEnv(DirectRLEnv):
"""Example environment for a PickAndPlace robot using suction-cups.
This example follows what would be typically done in a DirectRL pipeline.
Here we substitute the policy by keyboard inputs.
"""
cfg: PickAndPlaceEnvCfg
def __init__(self, cfg: PickAndPlaceEnvCfg, render_mode: str | None = None, **kwargs):
super().__init__(cfg, render_mode, **kwargs)
# Indices used to control the different axes of the gantry
self._x_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.x_dof_name)
self._y_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.y_dof_name)
self._z_dof_idx, _ = self.pick_and_place.find_joints(self.cfg.z_dof_name)
# joints info
self.joint_pos = self.pick_and_place.data.joint_pos
self.joint_vel = self.pick_and_place.data.joint_vel
# Buffers
self.go_to_cube = False
self.go_to_target = False
self.target_pos = torch.zeros((self.num_envs, 3), device=self.device, dtype=torch.float32)
self.instant_controls = torch.zeros((self.num_envs, 3), device=self.device, dtype=torch.float32)
self.permanent_controls = torch.zeros((self.num_envs, 1), device=self.device, dtype=torch.float32)
# Visual marker for the target
self.set_debug_vis(self.cfg.debug_vis)
# Sets up the keyboard callback and settings
self.set_up_keyboard()
def set_up_keyboard(self):
"""Sets up interface for keyboard input and registers the desired keys for control."""
# Acquire keyboard interface
self._input = carb.input.acquire_input_interface()
self._keyboard = omni.appwindow.get_default_app_window().get_keyboard()
self._sub_keyboard = self._input.subscribe_to_keyboard_events(self._keyboard, self._on_keyboard_event)
# Open / Close / Idle commands for gripper
self._instant_key_controls = {
"Q": torch.tensor([0, 0, -1]),
"E": torch.tensor([0, 0, 1]),
"ZEROS": torch.tensor([0, 0, 0]),
}
# Move up or down
self._permanent_key_controls = {
"W": torch.tensor([-200.0], device=self.device),
"S": torch.tensor([100.0], device=self.device),
}
# Aiming manually is painful we can automate this.
self._auto_aim_cube = "A"
self._auto_aim_target = "D"
# Task description:
print("Keyboard set up!")
print("The simulation is ready for you to try it out!")
print("Your goal is pick up the purple cube and to drop it on the red sphere!")
print("Use the following controls to interact with the simulation:")
print("Press the 'A' key to have the gripper track the cube position.")
print("Press the 'D' key to have the gripper track the target position")
print("Press the 'W' or 'S' keys to move the gantry UP or DOWN respectively")
print("Press 'Q' or 'E' to OPEN or CLOSE the gripper respectively")
def _on_keyboard_event(self, event):
"""Checks for a keyboard event and assign the corresponding command control depending on key pressed."""
if event.type == carb.input.KeyboardEventType.KEY_PRESS:
# Logic on key press
if event.input.name == self._auto_aim_target:
self.go_to_target = True
self.go_to_cube = False
if event.input.name == self._auto_aim_cube:
self.go_to_cube = True
self.go_to_target = False
if event.input.name in self._instant_key_controls:
self.go_to_cube = False
self.go_to_target = False
self.instant_controls[0] = self._instant_key_controls[event.input.name]
if event.input.name in self._permanent_key_controls:
self.go_to_cube = False
self.go_to_target = False
self.permanent_controls[0] = self._permanent_key_controls[event.input.name]
# On key release, the robot stops moving
elif event.type == carb.input.KeyboardEventType.KEY_RELEASE:
self.go_to_cube = False
self.go_to_target = False
self.instant_controls[0] = self._instant_key_controls["ZEROS"]
def _setup_scene(self):
self.pick_and_place = Articulation(self.cfg.robot_cfg)
self.cube = RigidObject(self.cfg.cube_cfg)
self.gripper = SurfaceGripper(self.cfg.gripper)
# add ground plane
spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg())
# clone and replicate
self.scene.clone_environments(copy_from_source=False)
# add articulation to scene
self.scene.articulations["pick_and_place"] = self.pick_and_place
self.scene.rigid_objects["cube"] = self.cube
self.scene.surface_grippers["gripper"] = self.gripper
# add lights
light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75))
light_cfg.func("/World/Light", light_cfg)
def _pre_physics_step(self, actions: torch.Tensor) -> None:
# Store the actions
self.actions = actions.clone()
def _apply_action(self) -> None:
# We use the keyboard outputs as an action.
if self.go_to_cube:
# Effort based proportional controller to track the cube position
head_pos_x = self.pick_and_place.data.joint_pos[:, self._x_dof_idx[0]]
head_pos_y = self.pick_and_place.data.joint_pos[:, self._y_dof_idx[0]]
cube_pos_x = self.cube.data.root_pos_w[:, 0] - self.scene.env_origins[:, 0]
cube_pos_y = self.cube.data.root_pos_w[:, 1] - self.scene.env_origins[:, 1]
d_cube_robot_x = cube_pos_x - head_pos_x
d_cube_robot_y = cube_pos_y - head_pos_y
self.instant_controls[0] = torch.tensor(
[d_cube_robot_x * 5.0, d_cube_robot_y * 5.0, 0.0], device=self.device
)
elif self.go_to_target:
# Effort based proportional controller to track the target position
head_pos_x = self.pick_and_place.data.joint_pos[:, self._x_dof_idx[0]]
head_pos_y = self.pick_and_place.data.joint_pos[:, self._y_dof_idx[0]]
target_pos_x = self.target_pos[:, 0]
target_pos_y = self.target_pos[:, 1]
d_target_robot_x = target_pos_x - head_pos_x
d_target_robot_y = target_pos_y - head_pos_y
self.instant_controls[0] = torch.tensor(
[d_target_robot_x * 5.0, d_target_robot_y * 5.0, 0.0], device=self.device
)
# Set the joint effort targets for the picker
self.pick_and_place.set_joint_effort_target(
self.instant_controls[:, 0].unsqueeze(dim=1), joint_ids=self._x_dof_idx
)
self.pick_and_place.set_joint_effort_target(
self.instant_controls[:, 1].unsqueeze(dim=1), joint_ids=self._y_dof_idx
)
self.pick_and_place.set_joint_effort_target(
self.permanent_controls[:, 0].unsqueeze(dim=1), joint_ids=self._z_dof_idx
)
# Set the gripper command
self.gripper.set_grippers_command(self.instant_controls[:, 2].unsqueeze(dim=1))
def _get_observations(self) -> dict:
# Get the observations
gripper_state = self.gripper.state.clone()
obs = torch.cat(
(
self.joint_pos[:, self._x_dof_idx[0]].unsqueeze(dim=1),
self.joint_vel[:, self._x_dof_idx[0]].unsqueeze(dim=1),
self.joint_pos[:, self._y_dof_idx[0]].unsqueeze(dim=1),
self.joint_vel[:, self._y_dof_idx[0]].unsqueeze(dim=1),
self.joint_pos[:, self._z_dof_idx[0]].unsqueeze(dim=1),
self.joint_vel[:, self._z_dof_idx[0]].unsqueeze(dim=1),
self.target_pos[:, 0].unsqueeze(dim=1),
self.target_pos[:, 1].unsqueeze(dim=1),
gripper_state.unsqueeze(dim=1),
),
dim=-1,
)
observations = {"policy": obs}
return observations
def _get_rewards(self) -> torch.Tensor:
return torch.zeros_like(self.reset_terminated, dtype=torch.float32)
def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]:
# Dones
self.joint_pos = self.pick_and_place.data.joint_pos
self.joint_vel = self.pick_and_place.data.joint_vel
# Check for time out
time_out = self.episode_length_buf >= self.max_episode_length - 1
# Check if the cube reached the target
cube_to_target_x_dist = self.cube.data.root_pos_w[:, 0] - self.target_pos[:, 0] - self.scene.env_origins[:, 0]
cube_to_target_y_dist = self.cube.data.root_pos_w[:, 1] - self.target_pos[:, 1] - self.scene.env_origins[:, 1]
cube_to_target_z_dist = self.cube.data.root_pos_w[:, 2] - self.target_pos[:, 2] - self.scene.env_origins[:, 2]
cube_to_target_distance = torch.norm(
torch.stack((cube_to_target_x_dist, cube_to_target_y_dist, cube_to_target_z_dist), dim=1), dim=1
)
self.target_reached = cube_to_target_distance < 0.3
# Check if the cube is out of bounds (that is outside of the picking area)
cube_to_origin_xy_diff = self.cube.data.root_pos_w[:, :2] - self.scene.env_origins[:, :2]
cube_to_origin_x_dist = torch.abs(cube_to_origin_xy_diff[:, 0])
cube_to_origin_y_dist = torch.abs(cube_to_origin_xy_diff[:, 1])
self.cube_out_of_bounds = (cube_to_origin_x_dist > 2.5) | (cube_to_origin_y_dist > 2.5)
time_out = time_out | self.target_reached
return self.cube_out_of_bounds, time_out
def _reset_idx(self, env_ids: Sequence[int] | None):
if env_ids is None:
env_ids = self.pick_and_place._ALL_INDICES
# Reset the environment, this must be done first! As it releases the objects held by the grippers.
# (And that's an operation that should be done before the gripper or the gripped objects are moved)
super()._reset_idx(env_ids)
num_resets = len(env_ids)
# Set a target position for the cube
self.target_pos[env_ids, 0] = sample_uniform(
self.cfg.target_x_pos_range[0],
self.cfg.target_x_pos_range[1],
num_resets,
self.device,
)
self.target_pos[env_ids, 1] = sample_uniform(
self.cfg.target_y_pos_range[0],
self.cfg.target_y_pos_range[1],
num_resets,
self.device,
)
self.target_pos[env_ids, 2] = self.cfg.target_z_pos
# Set the initial position of the cube
cube_pos = self.cube.data.default_root_state[env_ids, :7]
cube_pos[:, 0] = sample_uniform(
self.cfg.initial_object_x_pos_range[0],
self.cfg.initial_object_x_pos_range[1],
cube_pos[:, 0].shape,
self.device,
)
cube_pos[:, 1] = sample_uniform(
self.cfg.initial_object_y_pos_range[0],
self.cfg.initial_object_y_pos_range[1],
cube_pos[:, 1].shape,
self.device,
)
cube_pos[:, 2] = self.cfg.initial_object_z_pos
cube_pos[:, :3] += self.scene.env_origins[env_ids]
self.cube.write_root_pose_to_sim(cube_pos, env_ids)
# Set the initial position of the robot
joint_pos = self.pick_and_place.data.default_joint_pos[env_ids]
joint_pos[:, self._x_dof_idx] += sample_uniform(
self.cfg.initial_x_pos_range[0],
self.cfg.initial_x_pos_range[1],
joint_pos[:, self._x_dof_idx].shape,
self.device,
)
joint_pos[:, self._y_dof_idx] += sample_uniform(
self.cfg.initial_y_pos_range[0],
self.cfg.initial_y_pos_range[1],
joint_pos[:, self._y_dof_idx].shape,
self.device,
)
joint_pos[:, self._z_dof_idx] += sample_uniform(
self.cfg.initial_z_pos_range[0],
self.cfg.initial_z_pos_range[1],
joint_pos[:, self._z_dof_idx].shape,
self.device,
)
joint_vel = self.pick_and_place.data.default_joint_vel[env_ids]
self.joint_pos[env_ids] = joint_pos
self.joint_vel[env_ids] = joint_vel
self.pick_and_place.write_joint_state_to_sim(joint_pos, joint_vel, None, env_ids)
def _set_debug_vis_impl(self, debug_vis: bool):
# create markers if necessary for the first tome
if debug_vis:
if not hasattr(self, "goal_pos_visualizer"):
marker_cfg = SPHERE_MARKER_CFG.copy()
marker_cfg.markers["sphere"].radius = 0.25
# -- goal pose
marker_cfg.prim_path = "/Visuals/Command/goal_position"
self.goal_pos_visualizer = VisualizationMarkers(marker_cfg)
# set their visibility to true
self.goal_pos_visualizer.set_visibility(True)
else:
if hasattr(self, "goal_pos_visualizer"):
self.goal_pos_visualizer.set_visibility(False)
def _debug_vis_callback(self, event):
# update the markers
self.goal_pos_visualizer.visualize(self.target_pos + self.scene.env_origins)
def main():
"""Main function."""
# create environment
pick_and_place = PickAndPlaceEnv(PickAndPlaceEnvCfg())
obs, _ = pick_and_place.reset()
while simulation_app.is_running():
# check for selected robots
with torch.inference_mode():
actions = torch.zeros((pick_and_place.num_envs, 4), device=pick_and_place.device, dtype=torch.float32)
pick_and_place.step(actions)
if __name__ == "__main__":
main()
simulation_app.close()
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""This script demonstrates how to spawn a pick-and-place robot equipped with a surface gripper and interact with it.
.. code-block:: bash
# Usage
./isaaclab.sh -p scripts/tutorials/01_assets/run_surface_gripper.py --device=cpu
When running this script make sure the --device flag is set to cpu. This is because the surface gripper is
currently only supported on the CPU.
"""
"""Launch Isaac Sim Simulator first."""
import argparse
from isaaclab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Tutorial on spawning and interacting with a Surface Gripper.")
# 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 torch
import isaacsim.core.utils.prims as prim_utils
import isaacsim.core.utils.stage as stage_utils
import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation, SurfaceGripper, SurfaceGripperCfg
from isaaclab.sim import SimulationContext
from isaaclab.sim.utils import attach_stage_to_usd_context
##
# Pre-defined configs
##
from isaaclab_assets import PICK_AND_PLACE_CFG # isort:skip
def design_scene():
"""Designs the scene."""
# Ground-plane
cfg = sim_utils.GroundPlaneCfg()
cfg.func("/World/defaultGroundPlane", cfg)
# Lights
cfg = sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
cfg.func("/World/Light", cfg)
# Create separate groups called "Origin1", "Origin2"
# Each group will have a robot in it
origins = [[2.75, 0.0, 0.0], [-2.75, 0.0, 0.0]]
# Origin 1
prim_utils.create_prim("/World/Origin1", "Xform", translation=origins[0])
# Origin 2
prim_utils.create_prim("/World/Origin2", "Xform", translation=origins[1])
# Articulation: First we define the robot config
pick_and_place_robot_cfg = PICK_AND_PLACE_CFG.copy()
pick_and_place_robot_cfg.prim_path = "/World/Origin.*/Robot"
pick_and_place_robot = Articulation(cfg=pick_and_place_robot_cfg)
# Surface Gripper: Next we define the surface gripper config
surface_gripper_cfg = SurfaceGripperCfg()
# We need to tell the View which prim to use for the surface gripper
surface_gripper_cfg.prim_expr = "/World/Origin.*/Robot/picker_head/SurfaceGripper"
# We can then set different parameters for the surface gripper, note that if these parameters are not set,
# the View will try to read them from the prim.
surface_gripper_cfg.max_grip_distance = 0.1 # [m] (Maximum distance at which the gripper can grasp an object)
surface_gripper_cfg.shear_force_limit = 500.0 # [N] (Force limit in the direction perpendicular direction)
surface_gripper_cfg.coaxial_force_limit = 500.0 # [N] (Force limit in the direction of the gripper's axis)
surface_gripper_cfg.retry_interval = 0.1 # seconds (Time the gripper will stay in a grasping state)
# We can now spawn the surface gripper
surface_gripper = SurfaceGripper(cfg=surface_gripper_cfg)
# return the scene information
scene_entities = {"pick_and_place_robot": pick_and_place_robot, "surface_gripper": surface_gripper}
return scene_entities, origins
def run_simulator(
sim: sim_utils.SimulationContext, entities: dict[str, Articulation | SurfaceGripper], origins: torch.Tensor
):
"""Runs the simulation loop."""
# Extract scene entities
robot: Articulation = entities["pick_and_place_robot"]
surface_gripper: SurfaceGripper = entities["surface_gripper"]
# Define simulation stepping
sim_dt = sim.get_physics_dt()
count = 0
# Simulation loop
while simulation_app.is_running():
# Reset
if count % 500 == 0:
# reset counter
count = 0
# reset the scene entities
# root state
# we offset the root state by the origin since the states are written in simulation world frame
# if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world
root_state = robot.data.default_root_state.clone()
root_state[:, :3] += origins
robot.write_root_pose_to_sim(root_state[:, :7])
robot.write_root_velocity_to_sim(root_state[:, 7:])
# set joint positions with some noise
joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone()
joint_pos += torch.rand_like(joint_pos) * 0.1
robot.write_joint_state_to_sim(joint_pos, joint_vel)
# clear internal buffers
robot.reset()
print("[INFO]: Resetting robot state...")
# Opens the gripper and makes sure the gripper is in the open state
surface_gripper.reset()
print("[INFO]: Resetting gripper state...")
# Sample a random command between -1 and 1.
gripper_commands = torch.rand(surface_gripper.num_instances) * 2.0 - 1.0
# The gripper behavior is as follows:
# -1 < command < -0.3 --> Gripper is Opening
# -0.3 < command < 0.3 --> Gripper is Idle
# 0.3 < command < 1 --> Gripper is Closing
print(f"[INFO]: Gripper commands: {gripper_commands}")
mapped_commands = [
"Opening" if command < -0.3 else "Closing" if command > 0.3 else "Idle" for command in gripper_commands
]
print(f"[INFO]: Mapped commands: {mapped_commands}")
# Set the gripper command
surface_gripper.set_grippers_command(gripper_commands)
# Write data to sim
surface_gripper.write_data_to_sim()
# Perform step
sim.step()
# Increment counter
count += 1
# Read the gripper state from the simulation
surface_gripper.update(sim_dt)
# Read the gripper state from the buffer
surface_gripper_state = surface_gripper.state
# The gripper state is a list of integers that can be mapped to the following:
# -1 --> Open
# 0 --> Closing
# 1 --> Closed
# Print the gripper state
print(f"[INFO]: Gripper state: {surface_gripper_state}")
mapped_commands = [
"Open" if state == -1 else "Closing" if state == 0 else "Closed" for state in surface_gripper_state.tolist()
]
print(f"[INFO]: Mapped commands: {mapped_commands}")
def main():
"""Main function."""
# Load kit helper
sim_cfg = sim_utils.SimulationCfg(device=args_cli.device)
sim = SimulationContext(sim_cfg)
# Set main camera
sim.set_camera_view([2.75, 7.5, 10.0], [2.75, 0.0, 0.0])
# Design scene
# Create scene with stage in memory and then attach to USD context
with stage_utils.use_stage(sim.get_initial_stage()):
scene_entities, scene_origins = design_scene()
attach_stage_to_usd_context()
scene_origins = torch.tensor(scene_origins, device=sim.device)
# Play the simulator
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# Run the simulator
run_simulator(sim, scene_entities, scene_origins)
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()
...@@ -44,3 +44,4 @@ from .asset_base_cfg import AssetBaseCfg ...@@ -44,3 +44,4 @@ from .asset_base_cfg import AssetBaseCfg
from .deformable_object import DeformableObject, DeformableObjectCfg, DeformableObjectData from .deformable_object import DeformableObject, DeformableObjectCfg, DeformableObjectData
from .rigid_object import RigidObject, RigidObjectCfg, RigidObjectData from .rigid_object import RigidObject, RigidObjectCfg, RigidObjectData
from .rigid_object_collection import RigidObjectCollection, RigidObjectCollectionCfg, RigidObjectCollectionData from .rigid_object_collection import RigidObjectCollection, RigidObjectCollectionCfg, RigidObjectCollectionData
from .surface_gripper import SurfaceGripper, SurfaceGripperCfg
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Sub-module for surface_gripper assets."""
from .surface_gripper import SurfaceGripper
from .surface_gripper_cfg import SurfaceGripperCfg
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import torch
import warnings
import weakref
import omni.timeline
from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager
from isaacsim.core.version import get_version
from isaacsim.robot.surface_gripper import GripperView
import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBase
from .surface_gripper_cfg import SurfaceGripperCfg
class SurfaceGripper(AssetBase):
"""A surface gripper actuator class.
Surface grippers are actuators capable of grasping objects when in close proximity with them.
Each surface gripper in the collection must be a `Isaac Sim SurfaceGripper` primitive.
On playing the simulation, the physics engine will automatically register the surface grippers into a
SurfaceGripperView object. This object can be accessed using the :attr:`gripper_view` attribute.
To interact with the surface grippers, the user can use the :attr:`state` to get the current state of the grippers,
:attr:`command` to get the current command sent to the grippers, and :func:`update_gripper_properties` to update the
properties of the grippers at runtime. Finally, the :func:`set_grippers_command` function should be used to set the
desired command for the grippers.
Note:
The :func:`set_grippers_command` function does not write to the simulation. The simulation automatically
calls :func:`write_data_to_sim` function to write the command to the simulation. Similarly, the update
function is called automatically for every simulation step, and does not need to be called by the user.
Note:
The SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU.
Use `--device cpu` to run the simulation on CPU.
"""
def __init__(self, cfg: SurfaceGripperCfg):
"""Initialize the surface gripper.
Args:
cfg: A configuration instance.
"""
# copy the configuration
self._cfg = cfg.copy()
isaac_sim_version = get_version()
# checks for Isaac Sim v5.0 to ensure that the surface gripper is supported
if int(isaac_sim_version[2]) < 5:
raise Exception(
"SurfaceGrippers are only supported by IsaacSim 5.0 and newer. Use IsaacSim 5.0 or newer to use this"
" feature."
)
# flag for whether the sensor is initialized
self._is_initialized = False
self._debug_vis_handle = None
# note: Use weakref on callbacks to ensure that this object can be deleted when its destructor is called.
# add callbacks for stage play/stop
# The order is set to 10 which is arbitrary but should be lower priority than the default order of 0
timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream()
self._initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type(
int(omni.timeline.TimelineEventType.PLAY),
lambda event, obj=weakref.proxy(self): obj._initialize_callback(event),
order=10,
)
self._invalidate_initialize_handle = timeline_event_stream.create_subscription_to_pop_by_type(
int(omni.timeline.TimelineEventType.STOP),
lambda event, obj=weakref.proxy(self): obj._invalidate_initialize_callback(event),
order=10,
)
self._prim_deletion_callback_id = SimulationManager.register_callback(
self._on_prim_deletion, event=IsaacEvents.PRIM_DELETION
)
"""
Properties
"""
@property
def data(self):
raise NotImplementedError("SurfaceGripper does have a data interface.")
@property
def num_instances(self) -> int:
"""Number of instances of the gripper.
This is equal to the total number of grippers (the view can only contain one gripper per environment).
"""
return self._num_envs
@property
def state(self) -> torch.Tensor:
"""Returns the gripper state buffer.
The gripper state is a list of integers:
- -1 --> Open
- 0 --> Closing
- 1 --> Closed
"""
return self._gripper_state
@property
def command(self) -> torch.Tensor:
"""Returns the gripper command buffer.
The gripper command is a list of floats:
- [-1, -0.3] --> Open
- [-0.3, 0.3] --> Do nothing
- [0.3, 1] --> Close
"""
return self._gripper_command
@property
def gripper_view(self) -> GripperView:
"""Returns the gripper view object."""
return self._gripper_view
"""
Operations
"""
def update_gripper_properties(
self,
max_grip_distance: torch.Tensor | None = None,
coaxial_force_limit: torch.Tensor | None = None,
shear_force_limit: torch.Tensor | None = None,
retry_interval: torch.Tensor | None = None,
indices: torch.Tensor | None = None,
) -> None:
"""Update the gripper properties.
Args:
max_grip_distance: The maximum grip distance of the gripper. Should be a tensor of shape (num_envs,).
coaxial_force_limit: The coaxial force limit of the gripper. Should be a tensor of shape (num_envs,).
shear_force_limit: The shear force limit of the gripper. Should be a tensor of shape (num_envs,).
retry_interval: The retry interval of the gripper. Should be a tensor of shape (num_envs,).
indices: The indices of the grippers to update the properties for. Can be a tensor of any shape.
"""
if indices is None:
indices = self._ALL_INDICES
indices_as_list = indices.tolist()
if max_grip_distance is not None:
self._max_grip_distance[indices] = max_grip_distance
if coaxial_force_limit is not None:
self._coaxial_force_limit[indices] = coaxial_force_limit
if shear_force_limit is not None:
self._shear_force_limit[indices] = shear_force_limit
if retry_interval is not None:
self._retry_interval[indices] = retry_interval
self._gripper_view.set_surface_gripper_properties(
max_grip_distance=self._max_grip_distance.tolist(),
coaxial_force_limit=self._coaxial_force_limit.tolist(),
shear_force_limit=self._shear_force_limit.tolist(),
retry_interval=self._retry_interval.tolist(),
indices=indices_as_list,
)
def update(self, dt: float) -> None:
"""Update the gripper state using the SurfaceGripperView.
This function is called every simulation step.
The data fetched from the gripper view is a list of strings containing 3 possible states:
- "Open"
- "Closing"
- "Closed"
To make this more neural network friendly, we convert the list of strings to a list of floats:
- "Open" --> -1.0
- "Closing" --> 0.0
- "Closed" --> 1.0
Note:
We need to do this conversion for every single step of the simulation because the gripper can lose contact
with the object if some conditions are met: such as if a large force is applied to the gripped object.
"""
state_list: list[str] = self._gripper_view.get_surface_gripper_status()
state_list_as_int: list[float] = [
-1.0 if state == "Open" else 1.0 if state == "Closed" else 0.0 for state in state_list
]
self._gripper_state = torch.tensor(state_list_as_int, dtype=torch.float32, device=self._device)
def write_data_to_sim(self) -> None:
"""Write the gripper command to the SurfaceGripperView.
The gripper command is a list of integers that needs to be converted to a list of strings:
- [-1, -0.3] --> Open
- ]-0.3, 0.3[ --> Do nothing
- [0.3, 1] --> Closed
The Do nothing command is not applied, and is only used to indicate whether the gripper state has changed.
"""
# Remove the SurfaceGripper indices that have a commanded value of 2
indices = (
torch.argwhere(torch.logical_or(self._gripper_command < -0.3, self._gripper_command > 0.3))
.to(torch.int32)
.tolist()
)
# Write to the SurfaceGripperView if there are any indices to write to
if len(indices) > 0:
self._gripper_view.apply_gripper_action(self._gripper_command.tolist(), indices)
def set_grippers_command(self, states: torch.Tensor, indices: torch.Tensor | None = None) -> None:
"""Set the internal gripper command buffer. This function does not write to the simulation.
Possible values for the gripper command are:
- [-1, -0.3] --> Open
- ]-0.3, 0.3[ --> Do nothing
- [0.3, 1] --> Close
Args:
states: A tensor of integers representing the gripper command. Shape must match that of indices.
indices: A tensor of integers representing the indices of the grippers to set the command for. Defaults
to None, in which case all grippers are set.
"""
if indices is None:
indices = self._ALL_INDICES
self._gripper_command[indices] = states
def reset(self, indices: torch.Tensor | None = None) -> None:
"""Reset the gripper command buffer.
Args:
indices: A tensor of integers representing the indices of the grippers to reset the command for. Defaults
to None, in which case all grippers are reset.
"""
# Would normally set the buffer to 0, for now we won't do that
if indices is None:
indices = self._ALL_INDICES
# Reset the selected grippers to an open status
self._gripper_command[indices] = -1.0
self.write_data_to_sim()
# Sets the gripper last command to be 0.0 (do nothing)
self._gripper_command[indices] = 0
# Force set the state to open. It will read open in the next update call.
self._gripper_state[indices] = -1.0
"""
Initialization.
"""
def _initialize_impl(self) -> None:
"""Initializes the gripper-related handles and internal buffers.
Raises:
ValueError: If the simulation backend is not CPU.
RuntimeError: If the Simulation Context is not initialized.
Note:
The SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU.
Use `--device cpu` to run the simulation on CPU.
"""
# Check that we are using the CPU backend.
if self._device != "cpu":
raise Exception(
"SurfaceGripper is only supported on CPU for now. Please set the simulation backend to run on CPU. Use"
" `--device cpu` to run the simulation on CPU."
)
# Count number of environments
self._prim_expr = self._cfg.prim_expr
env_prim_path_expr = self._prim_expr.rsplit("/", 1)[0]
self._parent_prims = sim_utils.find_matching_prims(env_prim_path_expr)
self._num_envs = len(self._parent_prims)
# Create buffers
self._create_buffers()
# Process the configuration
self._process_cfg()
# Initialize gripper view and set properties. Note we do not set the properties through the gripper view
# to avoid having to convert them to list of floats here. Instead, we do it in the update_gripper_properties
# function which does this conversion internally.
self._gripper_view = GripperView(
self._prim_expr,
)
self.update_gripper_properties(
max_grip_distance=self._max_grip_distance.clone(),
coaxial_force_limit=self._coaxial_force_limit.clone(),
shear_force_limit=self._shear_force_limit.clone(),
retry_interval=self._retry_interval.clone(),
)
# Reset grippers
self.reset()
def _create_buffers(self) -> None:
"""Create the buffers for storing the gripper state, command, and properties."""
self._gripper_state = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
self._gripper_command = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
self._ALL_INDICES = torch.arange(self._num_envs, device=self._device, dtype=torch.long)
self._max_grip_distance = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
self._coaxial_force_limit = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
self._shear_force_limit = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
self._retry_interval = torch.zeros(self._num_envs, device=self._device, dtype=torch.float32)
def _process_cfg(self) -> None:
"""Process the configuration for the gripper properties."""
# Get one of the grippers as defined in the default stage
gripper_prim = self._parent_prims[0]
try:
max_grip_distance = gripper_prim.GetAttribute("isaac:maxGripDistance").Get()
except Exception as e:
warnings.warn(
f"Failed to retrieve max_grip_distance from stage, defaulting to user provided cfg. Exception: {e}"
)
max_grip_distance = None
try:
coaxial_force_limit = gripper_prim.GetAttribute("isaac:coaxialForceLimit").Get()
except Exception as e:
warnings.warn(
f"Failed to retrieve coaxial_force_limit from stage, defaulting to user provided cfg. Exception: {e}"
)
coaxial_force_limit = None
try:
shear_force_limit = gripper_prim.GetAttribute("isaac:shearForceLimit").Get()
except Exception as e:
warnings.warn(
f"Failed to retrieve shear_force_limit from stage, defaulting to user provided cfg. Exception: {e}"
)
shear_force_limit = None
try:
retry_interval = gripper_prim.GetAttribute("isaac:retryInterval").Get()
except Exception as e:
warnings.warn(
f"Failed to retrieve retry_interval from stage defaulting to user provided cfg. Exception: {e}"
)
retry_interval = None
self._max_grip_distance = self.parse_gripper_parameter(self._cfg.max_grip_distance, max_grip_distance)
self._coaxial_force_limit = self.parse_gripper_parameter(self._cfg.coaxial_force_limit, coaxial_force_limit)
self._shear_force_limit = self.parse_gripper_parameter(self._cfg.shear_force_limit, shear_force_limit)
self._retry_interval = self.parse_gripper_parameter(self._cfg.retry_interval, retry_interval)
"""
Helper functions.
"""
def parse_gripper_parameter(
self, cfg_value: float | int | tuple | None, default_value: float | int | tuple | None, ndim: int = 0
) -> torch.Tensor:
"""Parse the gripper parameter.
Args:
cfg_value: The value to parse. Can be a float, int, tuple, or None.
default_value: The default value to use if cfg_value is None. Can be a float, int, tuple, or None.
ndim: The number of dimensions of the parameter. Defaults to 0.
"""
# Adjust the buffer size based on the number of dimensions
if ndim == 0:
param = torch.zeros(self._num_envs, device=self._device)
elif ndim == 3:
param = torch.zeros(self._num_envs, 3, device=self._device)
elif ndim == 4:
param = torch.zeros(self._num_envs, 4, device=self._device)
else:
raise ValueError(f"Invalid number of dimensions: {ndim}")
# Parse the parameter
if cfg_value is not None:
if isinstance(cfg_value, (float, int)):
param[:] = float(cfg_value)
elif isinstance(cfg_value, tuple):
if len(cfg_value) == ndim:
param[:] = torch.tensor(cfg_value, dtype=torch.float, device=self._device)
else:
raise ValueError(f"Invalid number of values for parameter. Got: {len(cfg_value)}\nExpected: {ndim}")
else:
raise TypeError(f"Invalid type for parameter value: {type(cfg_value)}. " + "Expected float or int.")
elif default_value is not None:
if isinstance(default_value, (float, int)):
param[:] = float(default_value)
elif isinstance(default_value, tuple):
assert len(default_value) == ndim, f"Expected {ndim} values, got {len(default_value)}"
param[:] = torch.tensor(default_value, dtype=torch.float, device=self._device)
else:
raise TypeError(
f"Invalid type for default value: {type(default_value)}. " + "Expected float or Tensor."
)
else:
raise ValueError("The parameter value is None and no default value is provided.")
return param
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from dataclasses import MISSING
from isaaclab.utils import configclass
@configclass
class SurfaceGripperCfg:
"""Configuration parameters for a surface gripper actuator."""
prim_expr: str = MISSING
"""The expression to find the grippers in the stage."""
max_grip_distance: float | None = None
"""The maximum grip distance of the gripper."""
coaxial_force_limit: float | None = None
"""The coaxial force limit of the gripper."""
shear_force_limit: float | None = None
"""The shear force limit of the gripper."""
retry_interval: float | None = None
"""The amount of time the gripper will spend trying to grasp an object."""
...@@ -117,6 +117,16 @@ CUBOID_MARKER_CFG = VisualizationMarkersCfg( ...@@ -117,6 +117,16 @@ CUBOID_MARKER_CFG = VisualizationMarkersCfg(
) )
"""Configuration for the cuboid marker.""" """Configuration for the cuboid marker."""
SPHERE_MARKER_CFG = VisualizationMarkersCfg(
markers={
"sphere": sim_utils.SphereCfg(
radius=0.05,
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)),
),
}
)
"""Configuration for the sphere marker."""
POSITION_GOAL_MARKER_CFG = VisualizationMarkersCfg( POSITION_GOAL_MARKER_CFG = VisualizationMarkersCfg(
markers={ markers={
"target_far": sim_utils.SphereCfg( "target_far": sim_utils.SphereCfg(
......
...@@ -26,6 +26,8 @@ from isaaclab.assets import ( ...@@ -26,6 +26,8 @@ from isaaclab.assets import (
RigidObjectCfg, RigidObjectCfg,
RigidObjectCollection, RigidObjectCollection,
RigidObjectCollectionCfg, RigidObjectCollectionCfg,
SurfaceGripper,
SurfaceGripperCfg,
) )
from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg
from isaaclab.sim import SimulationContext from isaaclab.sim import SimulationContext
...@@ -120,6 +122,7 @@ class InteractiveScene: ...@@ -120,6 +122,7 @@ class InteractiveScene:
self._rigid_objects = dict() self._rigid_objects = dict()
self._rigid_object_collections = dict() self._rigid_object_collections = dict()
self._sensors = dict() self._sensors = dict()
self._surface_grippers = dict()
self._extras = dict() self._extras = dict()
# get stage handle # get stage handle
self.sim = SimulationContext.instance() self.sim = SimulationContext.instance()
...@@ -343,6 +346,11 @@ class InteractiveScene: ...@@ -343,6 +346,11 @@ class InteractiveScene:
"""A dictionary of the sensors in the scene, such as cameras and contact reporters.""" """A dictionary of the sensors in the scene, such as cameras and contact reporters."""
return self._sensors return self._sensors
@property
def surface_grippers(self) -> dict[str, SurfaceGripper]:
"""A dictionary of the surface grippers in the scene."""
return self._surface_grippers
@property @property
def extras(self) -> dict[str, XFormPrim]: def extras(self) -> dict[str, XFormPrim]:
"""A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors.
...@@ -388,6 +396,8 @@ class InteractiveScene: ...@@ -388,6 +396,8 @@ class InteractiveScene:
deformable_object.reset(env_ids) deformable_object.reset(env_ids)
for rigid_object in self._rigid_objects.values(): for rigid_object in self._rigid_objects.values():
rigid_object.reset(env_ids) rigid_object.reset(env_ids)
for surface_gripper in self._surface_grippers.values():
surface_gripper.reset(env_ids)
for rigid_object_collection in self._rigid_object_collections.values(): for rigid_object_collection in self._rigid_object_collections.values():
rigid_object_collection.reset(env_ids) rigid_object_collection.reset(env_ids)
# -- sensors # -- sensors
...@@ -403,6 +413,8 @@ class InteractiveScene: ...@@ -403,6 +413,8 @@ class InteractiveScene:
deformable_object.write_data_to_sim() deformable_object.write_data_to_sim()
for rigid_object in self._rigid_objects.values(): for rigid_object in self._rigid_objects.values():
rigid_object.write_data_to_sim() rigid_object.write_data_to_sim()
for surface_gripper in self._surface_grippers.values():
surface_gripper.write_data_to_sim()
for rigid_object_collection in self._rigid_object_collections.values(): for rigid_object_collection in self._rigid_object_collections.values():
rigid_object_collection.write_data_to_sim() rigid_object_collection.write_data_to_sim()
...@@ -421,6 +433,8 @@ class InteractiveScene: ...@@ -421,6 +433,8 @@ class InteractiveScene:
rigid_object.update(dt) rigid_object.update(dt)
for rigid_object_collection in self._rigid_object_collections.values(): for rigid_object_collection in self._rigid_object_collections.values():
rigid_object_collection.update(dt) rigid_object_collection.update(dt)
for surface_gripper in self._surface_grippers.values():
surface_gripper.update(dt)
# -- sensors # -- sensors
for sensor in self._sensors.values(): for sensor in self._sensors.values():
sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update)
...@@ -483,6 +497,10 @@ class InteractiveScene: ...@@ -483,6 +497,10 @@ class InteractiveScene:
root_velocity = asset_state["root_velocity"].clone() root_velocity = asset_state["root_velocity"].clone()
rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids) rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids)
rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids)
# surface grippers
for asset_name, gripper in self._surface_grippers.items():
asset_state = state["gripper"][asset_name]
gripper.write_gripper_state_to_sim(asset_state, env_ids=env_ids)
# write data to simulation to make sure initial state is set # write data to simulation to make sure initial state is set
# this propagates the joint targets to the simulation # this propagates the joint targets to the simulation
...@@ -588,6 +606,7 @@ class InteractiveScene: ...@@ -588,6 +606,7 @@ class InteractiveScene:
self._rigid_objects, self._rigid_objects,
self._rigid_object_collections, self._rigid_object_collections,
self._sensors, self._sensors,
self._surface_grippers,
self._extras, self._extras,
]: ]:
all_keys += list(asset_family.keys()) all_keys += list(asset_family.keys())
...@@ -614,6 +633,7 @@ class InteractiveScene: ...@@ -614,6 +633,7 @@ class InteractiveScene:
self._rigid_objects, self._rigid_objects,
self._rigid_object_collections, self._rigid_object_collections,
self._sensors, self._sensors,
self._surface_grippers,
self._extras, self._extras,
]: ]:
out = asset_family.get(key) out = asset_family.get(key)
...@@ -672,6 +692,8 @@ class InteractiveScene: ...@@ -672,6 +692,8 @@ class InteractiveScene:
if hasattr(rigid_object_cfg, "collision_group") and rigid_object_cfg.collision_group == -1: if hasattr(rigid_object_cfg, "collision_group") and rigid_object_cfg.collision_group == -1:
asset_paths = sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path) asset_paths = sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path)
self._global_prim_paths += asset_paths self._global_prim_paths += asset_paths
elif isinstance(asset_cfg, SurfaceGripperCfg):
pass
elif isinstance(asset_cfg, SensorBaseCfg): elif isinstance(asset_cfg, SensorBaseCfg):
# Update target frame path(s)' regex name space for FrameTransformer # Update target frame path(s)' regex name space for FrameTransformer
if isinstance(asset_cfg, FrameTransformerCfg): if isinstance(asset_cfg, FrameTransformerCfg):
......
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# 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).app
"""Rest everything follows."""
import torch
import isaacsim.core.utils.prims as prim_utils
import pytest
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import (
Articulation,
ArticulationCfg,
RigidObject,
RigidObjectCfg,
SurfaceGripper,
SurfaceGripperCfg,
)
from isaaclab.sim import build_simulation_context
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
# from isaacsim.robot.surface_gripper import GripperView
def generate_surface_gripper_cfgs(
kinematic_enabled: bool = False,
max_grip_distance: float = 0.1,
coaxial_force_limit: float = 100.0,
shear_force_limit: float = 100.0,
retry_interval: float = 0.1,
reset_xform_op_properties: bool = False,
) -> tuple[SurfaceGripperCfg, ArticulationCfg]:
"""Generate a surface gripper cfg and an articulation cfg.
Args:
max_grip_distance: The maximum grip distance of the surface gripper.
coaxial_force_limit: The coaxial force limit of the surface gripper.
shear_force_limit: The shear force limit of the surface gripper.
retry_interval: The retry interval of the surface gripper.
reset_xform_op_properties: Whether to reset the xform op properties of the surface gripper.
Returns:
A tuple containing the surface gripper cfg and the articulation cfg.
"""
articulation_cfg = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/SurfaceGripper/test_gripper.usd",
rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.5),
rot=(1.0, 0.0, 0.0, 0.0),
joint_pos={
".*": 0.0,
},
),
actuators={
"dummy": ImplicitActuatorCfg(
joint_names_expr=[".*"],
stiffness=0.0,
damping=0.0,
),
},
)
surface_gripper_cfg = SurfaceGripperCfg(
max_grip_distance=max_grip_distance,
coaxial_force_limit=coaxial_force_limit,
shear_force_limit=shear_force_limit,
retry_interval=retry_interval,
)
return surface_gripper_cfg, articulation_cfg
def generate_surface_gripper(
surface_gripper_cfg: SurfaceGripperCfg,
articulation_cfg: ArticulationCfg,
num_surface_grippers: int,
device: str,
) -> tuple[SurfaceGripper, Articulation, torch.Tensor]:
"""Generate a surface gripper and an articulation.
Args:
surface_gripper_cfg: The surface gripper cfg.
articulation_cfg: The articulation cfg.
num_surface_grippers: The number of surface grippers to generate.
device: The device to run the test on.
Returns:
A tuple containing the surface gripper, the articulation, and the translations of the surface grippers.
"""
# Generate translations of 2.5 m in x for each articulation
translations = torch.zeros(num_surface_grippers, 3, device=device)
translations[:, 0] = torch.arange(num_surface_grippers) * 2.5
# Create Top-level Xforms, one for each articulation
for i in range(num_surface_grippers):
prim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=translations[i][:3])
articulation = Articulation(articulation_cfg.replace(prim_path="/World/Env_.*/Robot"))
surface_gripper_cfg = surface_gripper_cfg.replace(prim_expr="/World/Env_.*/Robot/Gripper/SurfaceGripper")
surface_gripper = SurfaceGripper(surface_gripper_cfg)
return surface_gripper, articulation, translations
def generate_grippable_object(sim, num_grippable_objects: int):
object_cfg = RigidObjectCfg(
prim_path="/World/Env_.*/Object",
spawn=sim_utils.CuboidCfg(
size=(1.0, 1.0, 1.0),
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
collision_props=sim_utils.CollisionPropertiesCfg(),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)),
),
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)),
)
grippable_object = RigidObject(object_cfg)
return grippable_object
@pytest.fixture
def sim(request):
"""Create simulation context with the specified device."""
device = request.getfixturevalue("device")
if "gravity_enabled" in request.fixturenames:
gravity_enabled = request.getfixturevalue("gravity_enabled")
else:
gravity_enabled = True # default to gravity enabled
if "add_ground_plane" in request.fixturenames:
add_ground_plane = request.getfixturevalue("add_ground_plane")
else:
add_ground_plane = False # default to no ground plane
with build_simulation_context(
device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane
) as sim:
sim._app_control_on_stop_handle = None
yield sim
@pytest.mark.parametrize("num_articulations", [1])
@pytest.mark.parametrize("device", ["cpu"])
@pytest.mark.parametrize("add_ground_plane", [True])
def test_initialization(sim, num_articulations, device, add_ground_plane) -> None:
"""Test initialization for articulation with a surface gripper.
This test verifies that:
1. The surface gripper is initialized correctly.
2. The command and state buffers have the correct shapes.
3. The command and state are initialized to the correct values.
Args:
num_articulations: The number of articulations to initialize.
device: The device to run the test on.
add_ground_plane: Whether to add a ground plane to the simulation.
"""
surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False)
surface_gripper, articulation, _ = generate_surface_gripper(
surface_gripper_cfg, articulation_cfg, num_articulations, device
)
sim.reset()
assert articulation.is_initialized
assert surface_gripper.is_initialized
# Check that the command and state buffers have the correct shapes
assert surface_gripper.command.shape == (num_articulations,)
assert surface_gripper.state.shape == (num_articulations,)
# Check that the command and state are initialized to the correct values
assert surface_gripper.command == 0.0 # Idle command after a reset
assert surface_gripper.state == -1.0 # Open state after a reset
# Simulate physics
for _ in range(10):
# perform rendering
sim.step()
# update articulation
articulation.update(sim.cfg.dt)
surface_gripper.update(sim.cfg.dt)
@pytest.mark.parametrize("device", ["cuda:0"])
@pytest.mark.parametrize("add_ground_plane", [True])
def test_raise_error_if_not_cpu(sim, device, add_ground_plane) -> None:
"""Test that the SurfaceGripper raises an error if the device is not CPU."""
num_articulations = 1
surface_gripper_cfg, articulation_cfg = generate_surface_gripper_cfgs(kinematic_enabled=False)
surface_gripper, articulation, translations = generate_surface_gripper(
surface_gripper_cfg, articulation_cfg, num_articulations, device
)
with pytest.raises(Exception):
sim.reset()
if __name__ == "__main__":
pytest.main([__file__, "-v", "--maxfail=1"])
...@@ -17,6 +17,7 @@ from .franka import * ...@@ -17,6 +17,7 @@ from .franka import *
from .humanoid import * from .humanoid import *
from .humanoid_28 import * from .humanoid_28 import *
from .kinova import * from .kinova import *
from .pick_and_place import *
from .quadcopter import * from .quadcopter import *
from .ridgeback_franka import * from .ridgeback_franka import *
from .sawyer import * from .sawyer import *
......
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for a simple pick and place robot with a suction cup."""
from __future__ import annotations
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
##
# Configuration
##
PICK_AND_PLACE_CFG = ArticulationCfg(
prim_path="{ENV_REGEX_NS}/Robot",
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/PickAndPlace/pick_and_place_robot.usd",
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=10.0,
enable_gyroscopic_forces=True,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False,
solver_position_iteration_count=4,
solver_velocity_iteration_count=0,
sleep_threshold=0.005,
stabilization_threshold=0.001,
),
copy_from_source=False,
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.0),
joint_pos={
"x_axis": 0.0,
"y_axis": 0.0,
"z_axis": 0.0,
},
),
actuators={
"x_gantry": ImplicitActuatorCfg(
joint_names_expr=["x_axis"],
effort_limit=400.0,
velocity_limit=10.0,
stiffness=0.0,
damping=10.0,
),
"y_gantry": ImplicitActuatorCfg(
joint_names_expr=["y_axis"],
effort_limit=400.0,
velocity_limit=10.0,
stiffness=0.0,
damping=10.0,
),
"z_gantry": ImplicitActuatorCfg(
joint_names_expr=["z_axis"],
effort_limit=400.0,
velocity_limit=10.0,
stiffness=0.0,
damping=10.0,
),
},
)
"""Configuration for a simple pick and place robot with a suction cup."""
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment