diff --git a/source/extensions/isaacsim.examples.interactive/config/extension.toml b/source/extensions/isaacsim.examples.interactive/config/extension.toml index 4c02c858..c1b34dc8 100644 --- a/source/extensions/isaacsim.examples.interactive/config/extension.toml +++ b/source/extensions/isaacsim.examples.interactive/config/extension.toml @@ -95,6 +95,9 @@ name = "isaacsim.examples.interactive.franka" [[python.module]] name = "isaacsim.examples.interactive.pick_place" +[[python.module]] +name = "isaacsim.examples.interactive.robot_joint_control" + [[test]] timeout = 900 diff --git a/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/__init__.py b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/__init__.py new file mode 100644 index 00000000..3eb575dd --- /dev/null +++ b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/__init__.py @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from isaacsim.examples.interactive.robot_joint_control.robot_joint_control import RobotJointControl +from isaacsim.examples.interactive.robot_joint_control.robot_joint_control_extension import RobotJointControlExtension diff --git a/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control.py b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control.py new file mode 100644 index 00000000..17ac145a --- /dev/null +++ b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control.py @@ -0,0 +1,183 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +from isaacsim.core.api.objects import DynamicCuboid +from isaacsim.core.api.tasks import BaseTask +from isaacsim.core.utils.stage import add_reference_to_stage +from isaacsim.examples.interactive.base_sample import BaseSample +from isaacsim.robot.manipulators.manipulators import SingleManipulator +from isaacsim.robot.manipulators.grippers import ParallelGripper + + +class RobotJointControlTask(BaseTask): + """Task for robot joint control demonstration.""" + + def __init__(self, name: str = "robot_joint_control_task") -> None: + super().__init__(name=name, offset=None) + self._robot = None + self._robot_name = "franka_robot" + + def set_up_scene(self, scene): + super().set_up_scene(scene) + + # Add ground plane + scene.add_default_ground_plane() + + # Add Franka robot + asset_path = "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.2/Isaac/Robots/FrankaRobot/franka_alt_fingers.usd" + add_reference_to_stage(usd_path=asset_path, prim_path="/World/Franka") + + # Define gripper + gripper = ParallelGripper( + end_effector_prim_path="/World/Franka/panda_rightfinger", + joint_prim_names=["panda_finger_joint1", "panda_finger_joint2"], + joint_opened_positions=np.array([0.04, 0.04]), + joint_closed_positions=np.array([0.0, 0.0]), + action_deltas=np.array([0.04, 0.04]), + ) + + # Create manipulator + self._robot = scene.add( + SingleManipulator( + prim_path="/World/Franka", + name=self._robot_name, + end_effector_prim_path="/World/Franka/panda_hand", + gripper=gripper, + ) + ) + + # Add a target cube for reference + scene.add( + DynamicCuboid( + prim_path="/World/TargetCube", + name="target_cube", + position=np.array([0.5, 0.0, 0.5]), + scale=np.array([0.05, 0.05, 0.05]), + color=np.array([1.0, 0.0, 0.0]), + ) + ) + return + + def get_params(self) -> dict: + return {"robot_name": {"value": self._robot_name, "modifiable": False}} + + def get_observations(self) -> dict: + robot = self._robot + observations = { + self._robot_name: { + "joint_positions": robot.get_joint_positions(), + "end_effector_position": robot.end_effector.get_world_pose()[0], + "end_effector_orientation": robot.end_effector.get_world_pose()[1], + } + } + return observations + + def pre_step(self, control_index: int, simulation_time: float) -> None: + return + + def post_reset(self) -> None: + return + + +class RobotJointControl(BaseSample): + """Example demonstrating direct robot joint position control.""" + + def __init__(self) -> None: + super().__init__() + self._robot = None + self._articulation_controller = None + self._joint_positions = None + self._num_joints = 9 # 7 arm joints + 2 gripper joints for Franka + + def setup_scene(self): + world = self.get_world() + world.add_task(RobotJointControlTask()) + return + + async def setup_post_load(self): + self._task = list(self._world.get_current_tasks().values())[0] + self._task_params = self._task.get_params() + self._robot = self._world.scene.get_object(self._task_params["robot_name"]["value"]) + self._articulation_controller = self._robot.get_articulation_controller() + self._joint_positions = self._robot.get_joint_positions() + return + + async def setup_pre_reset(self): + world = self.get_world() + if world.physics_callback_exists("joint_control_step"): + world.remove_physics_callback("joint_control_step") + return + + def world_cleanup(self): + self._robot = None + self._articulation_controller = None + self._joint_positions = None + return + + def get_joint_positions(self) -> np.ndarray: + """Get current joint positions.""" + if self._robot is not None: + return self._robot.get_joint_positions() + return np.zeros(self._num_joints) + + def get_num_joints(self) -> int: + """Get number of joints.""" + return self._num_joints + + def set_joint_position(self, joint_index: int, position: float): + """Set position for a specific joint.""" + if self._joint_positions is not None and 0 <= joint_index < len(self._joint_positions): + self._joint_positions[joint_index] = position + + def apply_joint_positions(self): + """Apply the current joint positions to the robot.""" + if self._articulation_controller is not None and self._joint_positions is not None: + from isaacsim.core.api.utils.types import ArticulationAction + action = ArticulationAction(joint_positions=self._joint_positions) + self._articulation_controller.apply_action(action) + + async def _on_start_control_event_async(self, val): + """Start or stop joint control simulation.""" + world = self.get_world() + if val: + await world.play_async() + world.add_physics_callback("joint_control_step", self._on_joint_control_step) + else: + if world.physics_callback_exists("joint_control_step"): + world.remove_physics_callback("joint_control_step") + return + + def _on_joint_control_step(self, step_size): + """Physics callback for joint control.""" + self.apply_joint_positions() + return + + def move_to_home_position(self): + """Move robot to home position.""" + # Franka home position + home_positions = np.array([0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785, 0.04, 0.04]) + self._joint_positions = home_positions + self.apply_joint_positions() + + def open_gripper(self): + """Open the gripper.""" + if self._robot is not None: + self._robot.gripper.open() + + def close_gripper(self): + """Close the gripper.""" + if self._robot is not None: + self._robot.gripper.close() diff --git a/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control_extension.py b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control_extension.py new file mode 100644 index 00000000..1ec501dd --- /dev/null +++ b/source/extensions/isaacsim.examples.interactive/isaacsim/examples/interactive/robot_joint_control/robot_joint_control_extension.py @@ -0,0 +1,256 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import asyncio +import os + +import omni.ext +import omni.ui as ui +from isaacsim.examples.browser import get_instance as get_browser_instance +from isaacsim.examples.interactive.base_sample import BaseSampleUITemplate +from isaacsim.examples.interactive.robot_joint_control import RobotJointControl +from isaacsim.gui.components.ui_utils import btn_builder, get_style, setup_ui_headers, state_btn_builder, float_builder + + +class RobotJointControlExtension(omni.ext.IExt): + def on_startup(self, ext_id: str): + self.example_name = "Robot Joint Control" + self.category = "Manipulation" + + ui_kwargs = { + "ext_id": ext_id, + "file_path": os.path.abspath(__file__), + "title": "Robot Joint Control", + "doc_link": "https://docs.isaacsim.omniverse.nvidia.com/latest/introduction/examples.html", + "overview": "This example demonstrates direct robot joint position control.\n\n" + "Click 'Load' to load the scene with a Franka robot.\n" + "Use the sliders to control individual joint positions.\n" + "Click 'Start' to begin applying joint positions.\n\n" + "Features:\n" + "- Direct joint position control via sliders\n" + "- Home position preset\n" + "- Gripper open/close control", + "sample": RobotJointControl(), + } + + ui_handle = RobotJointControlUI(**ui_kwargs) + + get_browser_instance().register_example( + name=self.example_name, + execute_entrypoint=ui_handle.build_window, + ui_hook=ui_handle.build_ui, + category=self.category, + ) + return + + def on_shutdown(self): + get_browser_instance().deregister_example(name=self.example_name, category=self.category) + return + + +class RobotJointControlUI(BaseSampleUITemplate): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._joint_sliders = [] + + def build_extra_frames(self): + extra_stacks = self.get_extra_frames_handle() + self.task_ui_elements = {} + + with extra_stacks: + with ui.CollapsableFrame( + title="Joint Control", + width=ui.Fraction(0.33), + height=0, + visible=True, + collapsed=False, + horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ): + self.build_joint_control_ui() + + with ui.CollapsableFrame( + title="Quick Actions", + width=ui.Fraction(0.33), + height=0, + visible=True, + collapsed=False, + horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED, + vertical_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON, + ): + self.build_quick_actions_ui() + return + + def build_joint_control_ui(self): + # Joint limits for Franka robot (approximate) + joint_limits = [ + (-2.8973, 2.8973), # Joint 1 + (-1.7628, 1.7628), # Joint 2 + (-2.8973, 2.8973), # Joint 3 + (-3.0718, -0.0698), # Joint 4 + (-2.8973, 2.8973), # Joint 5 + (-0.0175, 3.7525), # Joint 6 + (-2.8973, 2.8973), # Joint 7 + (0.0, 0.04), # Finger 1 + (0.0, 0.04), # Finger 2 + ] + joint_names = [ + "Joint 1 (Shoulder)", + "Joint 2 (Shoulder)", + "Joint 3 (Elbow)", + "Joint 4 (Elbow)", + "Joint 5 (Wrist)", + "Joint 6 (Wrist)", + "Joint 7 (Wrist)", + "Finger Left", + "Finger Right", + ] + + with ui.VStack(spacing=5): + # Start/Stop control button + dict = { + "label": "Joint Control", + "type": "button", + "a_text": "START", + "b_text": "STOP", + "tooltip": "Start/Stop joint control", + "on_clicked_fn": self._on_start_control_button_event, + } + self.task_ui_elements["Start Control"] = state_btn_builder(**dict) + self.task_ui_elements["Start Control"].enabled = False + + ui.Spacer(height=10) + ui.Label("Joint Positions (radians):", height=20) + ui.Spacer(height=5) + + # Create sliders for each joint + self._joint_sliders = [] + for i, (name, limits) in enumerate(zip(joint_names, joint_limits)): + with ui.HStack(height=25): + ui.Label(name, width=120) + slider = ui.FloatSlider(min=limits[0], max=limits[1], step=0.01) + slider.model.set_value(0.0) + slider.model.add_value_changed_fn(lambda m, idx=i: self._on_joint_slider_changed(idx, m.get_value_as_float())) + self._joint_sliders.append(slider) + slider.enabled = False + + def build_quick_actions_ui(self): + with ui.VStack(spacing=5): + dict = { + "label": "Home Position", + "type": "button", + "text": "GO HOME", + "tooltip": "Move robot to home position", + "on_clicked_fn": self._on_home_button_event, + } + self.task_ui_elements["Home Position"] = btn_builder(**dict) + self.task_ui_elements["Home Position"].enabled = False + + dict = { + "label": "Open Gripper", + "type": "button", + "text": "OPEN", + "tooltip": "Open the gripper", + "on_clicked_fn": self._on_open_gripper_button_event, + } + self.task_ui_elements["Open Gripper"] = btn_builder(**dict) + self.task_ui_elements["Open Gripper"].enabled = False + + dict = { + "label": "Close Gripper", + "type": "button", + "text": "CLOSE", + "tooltip": "Close the gripper", + "on_clicked_fn": self._on_close_gripper_button_event, + } + self.task_ui_elements["Close Gripper"] = btn_builder(**dict) + self.task_ui_elements["Close Gripper"].enabled = False + + def _on_start_control_button_event(self, val): + asyncio.ensure_future(self.sample._on_start_control_event_async(val)) + return + + def _on_joint_slider_changed(self, joint_index: int, value: float): + if self.sample is not None: + self.sample.set_joint_position(joint_index, value) + return + + def _on_home_button_event(self): + if self.sample is not None: + self.sample.move_to_home_position() + # Update sliders to reflect home position + home_positions = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785, 0.04, 0.04] + for i, pos in enumerate(home_positions): + if i < len(self._joint_sliders): + self._joint_sliders[i].model.set_value(pos) + return + + def _on_open_gripper_button_event(self): + if self.sample is not None: + self.sample.open_gripper() + # Update gripper sliders + if len(self._joint_sliders) >= 9: + self._joint_sliders[7].model.set_value(0.04) + self._joint_sliders[8].model.set_value(0.04) + return + + def _on_close_gripper_button_event(self): + if self.sample is not None: + self.sample.close_gripper() + # Update gripper sliders + if len(self._joint_sliders) >= 9: + self._joint_sliders[7].model.set_value(0.0) + self._joint_sliders[8].model.set_value(0.0) + return + + def post_reset_button_event(self): + self.task_ui_elements["Start Control"].enabled = True + self.task_ui_elements["Home Position"].enabled = True + self.task_ui_elements["Open Gripper"].enabled = True + self.task_ui_elements["Close Gripper"].enabled = True + for slider in self._joint_sliders: + slider.enabled = True + if self.task_ui_elements["Start Control"].text == "STOP": + self.task_ui_elements["Start Control"].text = "START" + # Reset sliders to zero + for slider in self._joint_sliders: + slider.model.set_value(0.0) + return + + def post_load_button_event(self): + self.task_ui_elements["Start Control"].enabled = True + self.task_ui_elements["Home Position"].enabled = True + self.task_ui_elements["Open Gripper"].enabled = True + self.task_ui_elements["Close Gripper"].enabled = True + for slider in self._joint_sliders: + slider.enabled = True + # Initialize sliders with current joint positions + if self.sample is not None: + positions = self.sample.get_joint_positions() + for i, pos in enumerate(positions): + if i < len(self._joint_sliders): + self._joint_sliders[i].model.set_value(float(pos)) + return + + def post_clear_button_event(self): + self.task_ui_elements["Start Control"].enabled = False + self.task_ui_elements["Home Position"].enabled = False + self.task_ui_elements["Open Gripper"].enabled = False + self.task_ui_elements["Close Gripper"].enabled = False + for slider in self._joint_sliders: + slider.enabled = False + if self.task_ui_elements["Start Control"].text == "STOP": + self.task_ui_elements["Start Control"].text = "START" + return