diff --git a/README.md b/README.md index b3d69f3..fd4e300 100644 --- a/README.md +++ b/README.md @@ -98,6 +98,25 @@ python scripts/random_agent.py --task= --- +### Test Placement Agent +**Command:** +```bash +# Use 'FULL_PATH_TO_isaaclab.sh|bat -p' instead of 'python' if Isaac Lab is not installed in Python venv or conda +python scripts/test_placement_agent.py --task=Isaac-Pack-NoArm-v0 --num_envs 5 +``` + +--- + +## Teleoperation and Imitation Learning +**Command:** +```bash +python scripts/teleop_se3_agent.py --task Isaac-Pack-UR5-Teleop-v0 --num_envs 1 --teleop_device mello +``` +### Teleop Device +**Mello** is a teleoperation device designed for intuitive robot control, similar in concept to Gello. It mimics the robot's joint structure, allowing users to control the robot by physically moving the device. Joint positions are sent directly to the robot, eliminating the need for inverse kinematics or physics-based computation. Mello is especially useful for imitation learning, where human demonstrations collected via teleoperation are used to train models. Because Mello closely matches the robot’s kinematics, it enables efficient and accurate data collection for learning from demonstration. + +--- + ## Code Formatting We use a **pre-commit template** to automatically format your code. diff --git a/gcu_objects b/gcu_objects index a582d59..a9a279a 160000 --- a/gcu_objects +++ b/gcu_objects @@ -1 +1 @@ -Subproject commit a582d59dda01b85f5290cb692b2644e9c3d53e9c +Subproject commit a9a279ae03c51947a4b39da9d776adccd9b876bb diff --git a/scripts/teleop_se3_agent.py b/scripts/teleop_se3_agent.py new file mode 100644 index 0000000..b72a0d7 --- /dev/null +++ b/scripts/teleop_se3_agent.py @@ -0,0 +1,318 @@ +# 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 + +"""Script to run a keyboard teleoperation with Isaac Lab manipulation environments.""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Keyboard teleoperation for Isaac Lab environments.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument("--teleop_device", type=str, default="keyboard", help="Device for interacting with environment") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +app_launcher_args = vars(args_cli) + +if args_cli.enable_pinocchio: + # Import pinocchio before AppLauncher to force the use of the version installed by IsaacLab and + # not the one installed by Isaac Sim pinocchio is required by the Pink IK controllers and the + # GR1T2 retargeter + import pinocchio # noqa: F401 +if "handtracking" in args_cli.teleop_device.lower(): + app_launcher_args["xr"] = True + +# launch omniverse app +app_launcher = AppLauncher(app_launcher_args) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import gymnasium as gym +import numpy as np +import omni.log +import torch + +if "handtracking" in args_cli.teleop_device.lower(): + from isaacsim.xr.openxr import OpenXRSpec + +from isaaclab.devices import ( + OpenXRDevice, + Se3Gamepad, + Se3Keyboard, + Se3Mello, + Se3SpaceMouse, +) + +if args_cli.enable_pinocchio: + from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter + import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + +import isaaclab_tasks # noqa: F401 +import tote_consolidation.tasks # noqa: F401 +from isaaclab.devices.openxr.retargeters.manipulator import ( + GripperRetargeter, + Se3AbsRetargeter, + Se3RelRetargeter, +) +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab_tasks.manager_based.manipulation.lift import mdp +from isaaclab_tasks.utils import parse_env_cfg + +smoothed_command = 0.0 +alpha = 0.1 + + +def pre_process_actions( + teleop_data: tuple[np.ndarray, bool] | tuple[np.ndarray, float] | list[tuple[np.ndarray, np.ndarray, np.ndarray]], + num_envs: int, + device: str, +) -> torch.Tensor: + """Convert teleop data to the format expected by the environment action space. + + Args: + teleop_data: Data from the teleoperation device. + num_envs: Number of environments. + device: Device to create tensors on. + + Returns: + Processed actions as a tensor. + """ + # compute actions based on environment + if "Reach" in args_cli.task: + delta_pose, gripper_command = teleop_data + # convert to torch + delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=device).repeat(num_envs, 1) + # note: reach is the only one that uses a different action space + # compute actions + return delta_pose + elif "PickPlace-GR1T2" in args_cli.task: + (left_wrist_pose, right_wrist_pose, hand_joints) = teleop_data[0] + # Reconstruct actions_arms tensor with converted positions and rotations + actions = torch.tensor( + np.concatenate([ + left_wrist_pose, # left ee pose + right_wrist_pose, # right ee pose + hand_joints, # hand joint angles + ]), + device=device, + dtype=torch.float32, + ).unsqueeze(0) + # Concatenate arm poses and hand joint angles + return actions + elif "Pack-UR5" in args_cli.task: + joint_pos, gripper_raw_command = teleop_data + joint_pos = torch.tensor(joint_pos, dtype=torch.float, device=device).repeat(num_envs, 1) + + gripper_binary = torch.tensor( + [[-1.0 if gripper_raw_command else 0.0]], dtype=torch.float, device=device + ).repeat(num_envs, 1) + + return torch.concat([joint_pos, gripper_binary], dim=1) + + """gripper_state = 1 if gripper_command else 0 + # compute actions + return delta_pose + #return delta_pose""" + else: + # resolve gripper command + delta_pose, gripper_command = teleop_data + # convert to torch + delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=device).repeat(num_envs, 1) + gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=device) + gripper_vel[:] = 1 if gripper_command else 0 + # compute actions + return torch.concat([delta_pose, gripper_vel], dim=1) + + +def main(): + """Running keyboard teleoperation with Isaac Lab manipulation environment.""" + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs) + env_cfg.env_name = args_cli.task + # modify configuration + env_cfg.terminations.time_out = None + if "Lift" in args_cli.task: + # set the resampling time range to large number to avoid resampling + env_cfg.commands.object_pose.resampling_time_range = (1.0e9, 1.0e9) + # add termination condition for reaching the goal otherwise the environment won't reset + env_cfg.terminations.object_reached_goal = DoneTerm(func=mdp.object_reached_goal) + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + omni.log.warn( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be ignored." + ) + print(env.scene["right_robot"].data.joint_names) + + # Flags for controlling teleoperation flow + should_reset_recording_instance = False + teleoperation_active = True + + # Callback handlers + def reset_recording_instance(): + """Reset the environment to its initial state. + + This callback is triggered when the user presses the reset key (typically 'R'). + It's useful when: + - The robot gets into an undesirable configuration + - The user wants to start over with the task + - Objects in the scene need to be reset to their initial positions + + The environment will be reset on the next simulation step. + """ + nonlocal should_reset_recording_instance + should_reset_recording_instance = True + + def start_teleoperation(): + """Activate teleoperation control of the robot. + + This callback enables active control of the robot through the input device. + It's typically triggered by a specific gesture or button press and is used when: + - Beginning a new teleoperation session + - Resuming control after temporarily pausing + - Switching from observation mode to control mode + + While active, all commands from the device will be applied to the robot. + """ + nonlocal teleoperation_active + teleoperation_active = True + + def stop_teleoperation(): + """Deactivate teleoperation control of the robot. + + This callback temporarily suspends control of the robot through the input device. + It's typically triggered by a specific gesture or button press and is used when: + - Taking a break from controlling the robot + - Repositioning the input device without moving the robot + - Pausing to observe the scene without interference + + While inactive, the simulation continues to render but device commands are ignored. + """ + nonlocal teleoperation_active + teleoperation_active = False + + # create controller + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard( + pos_sensitivity=0.5 * args_cli.sensitivity, rot_sensitivity=0.5 * args_cli.sensitivity + ) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse( + pos_sensitivity=0.05 * args_cli.sensitivity, rot_sensitivity=0.05 * args_cli.sensitivity + ) + elif args_cli.teleop_device.lower() == "mello": + teleop_interface = Se3Mello() + elif args_cli.teleop_device.lower() == "gamepad": + teleop_interface = Se3Gamepad( + pos_sensitivity=0.1 * args_cli.sensitivity, rot_sensitivity=0.1 * args_cli.sensitivity + ) + elif "dualhandtracking_abs" in args_cli.teleop_device.lower() and "GR1T2" in args_cli.task: + # Create GR1T2 retargeter with desired configuration + gr1t2_retargeter = GR1T2Retargeter( + enable_visualization=True, + num_open_xr_hand_joints=2 * (int(OpenXRSpec.HandJointEXT.XR_HAND_JOINT_LITTLE_TIP_EXT) + 1), + device=env.unwrapped.device, + hand_joint_names=env.scene["robot"].data.joint_names[-22:], + ) + + # Create hand tracking device with retargeter + teleop_interface = OpenXRDevice( + env_cfg.xr, + retargeters=[gr1t2_retargeter], + ) + teleop_interface.add_callback("RESET", reset_recording_instance) + teleop_interface.add_callback("START", start_teleoperation) + teleop_interface.add_callback("STOP", stop_teleoperation) + + # Hand tracking needs explicit start gesture to activate + teleoperation_active = False + + elif "handtracking" in args_cli.teleop_device.lower(): + # Create EE retargeter with desired configuration + if "_abs" in args_cli.teleop_device.lower(): + retargeter_device = Se3AbsRetargeter( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True + ) + else: + retargeter_device = Se3RelRetargeter( + bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True + ) + + grip_retargeter = GripperRetargeter(bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT) + + # Create hand tracking device with retargeter (in a list) + teleop_interface = OpenXRDevice( + env_cfg.xr, + retargeters=[retargeter_device, grip_retargeter], + ) + teleop_interface.add_callback("RESET", reset_recording_instance) + teleop_interface.add_callback("START", start_teleoperation) + teleop_interface.add_callback("STOP", stop_teleoperation) + + # Hand tracking needs explicit start gesture to activate + teleoperation_active = False + else: + raise ValueError( + f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'mello'," + " 'gamepad', 'handtracking', 'handtracking_abs'." + ) + + # add teleoperation key for env reset (for all devices) + teleop_interface.add_callback("R", reset_recording_instance) + print(teleop_interface) + + # reset environment + env.reset() + teleop_interface.reset() + + # simulate environment + while simulation_app.is_running(): + # run everything in inference mode + with torch.inference_mode(): + # get device command + teleop_data = teleop_interface.advance() + + # Only apply teleop commands when active + + if teleoperation_active: + print(f"Teleoperation data: {teleop_data}") + # compute actions based on environment + actions = pre_process_actions(teleop_data, env.num_envs, env.device) + print(f"Actions: {actions}") + # apply actions + env.step(actions) + else: + env.sim.render() + + if should_reset_recording_instance: + env.reset() + should_reset_recording_instance = False + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/source/gculab/gculab/devices/__init__.py b/source/gculab/gculab/devices/__init__.py new file mode 100644 index 0000000..cd6d164 --- /dev/null +++ b/source/gculab/gculab/devices/__init__.py @@ -0,0 +1,19 @@ +# 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 + +"""Sub-package providing interfaces to different teleoperation devices. + +Currently, the following categories of devices are supported: + +* **Keyboard**: Standard keyboard with WASD and arrow keys. + +All device interfaces inherit from the :class:`DeviceBase` class, which provides a +common interface for all devices. The device interface reads the input data when +the :meth:`DeviceBase.advance` method is called. It also provides the function :meth:`DeviceBase.add_callback` +to add user-defined callback functions to be called when a particular input is pressed from +the peripheral device. +""" + +from .mello import Se3Mello diff --git a/source/gculab/gculab/devices/mello/__init__.py b/source/gculab/gculab/devices/mello/__init__.py new file mode 100644 index 0000000..d18a39a --- /dev/null +++ b/source/gculab/gculab/devices/mello/__init__.py @@ -0,0 +1,9 @@ +# 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 + +"""Mello device for SE(3) control.""" + + +from .se3_mello import Se3Mello diff --git a/source/gculab/gculab/devices/mello/se3_mello.py b/source/gculab/gculab/devices/mello/se3_mello.py new file mode 100644 index 0000000..16368c9 --- /dev/null +++ b/source/gculab/gculab/devices/mello/se3_mello.py @@ -0,0 +1,161 @@ +# 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 + +"""Mello conrtroller for SE(3) teleoperation devices.""" + +import ast # For parsing string data from Mello +import threading +import time +from collections.abc import Callable + +import numpy as np +import serial # For serial communication with Mello +from scipy.spatial.transform import Rotation # For handling rotations + +from ..device_base import DeviceBase # Interface for device base class + + +class Se3Mello(DeviceBase): + """A mello controller for SE(3) commands as delta poses. + + This class implements a mello controller to provide commands to a robotic arm with a Mello device. + It uses the mello arm to control the robot's end-effector in SE(3) space, allowing for + teleoperation of the robot's pose in 3D space using motorized input devices. + + The command comprises of two parts: + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + """ + + def __init__( + self, + port: str = "/dev/serial/by-id/usb-M5Stack_Technology_Co.__Ltd_M5Stack_UiFlow_2.0_4827e266dd480000-if00", + baudrate: int = 115200, + ): + """Initialize the Mello Gello controller. + + Args: + pos_sensitivity: Magnitude of input position command scaling. Defaults to 0.4. + rot_sensitivity: Magnitude of scale input rotation commands scaling. Defaults to 0.8. + """ + + # store inputs + self.port = port # serial port for Mello + self.baudrate = baudrate # baudrate for serial communication + + # Serial communication + self.serial = None # serial communication interface for Mello + self._setup_serial() # setup the serial communication with Mello + + # State variables + self.prev_joints = [0] * 6 + self.prev_gripper = 0 + self.latest_values = self.prev_joints + self._close_gripper = False + self.running = True + + # dictionary for additional callbacks + self._additional_callbacks = dict() + + # run a thread for listening to a devite updates + self._start_read_thread() + + def __del__(self): + """Dectructor for the class.""" + self.cleanup() + + def __str__(self) -> str: + """Returns: A string containing the information of joystick.""" + msg = f"Mello Controller for SE(3): {self.__class__.__name__}\n" + msg += "\t----------------------------------------------\n" + msg += "\tScaled version of UR5 robot arm controller\n" + msg += "\tJoints are controlled via motors and controllers\n" + msg += "\tMove the arm in any direction by rotating motors (up, down, left right, diagonal)\n" + msg += "\tUse analog stick to open/close the gripper\n" + return msg + + def cleanup(self): + """Clean up resources.""" + self.running = False + if self.serial and self.serial.is_open: + self.serial.close() + print("Serial port closed") + + """ + Operations + """ + + def reset(self): + """Reset the internals.""" + self._close_gripper = False + self.prev_joints = [0] * 6 + self.prev_gripper = 0 + self.latest_values = self.prev_joints + + def add_callback(self, key: str, func: Callable): + """Add additional functions to bind keyboard.""" + self._additional_callbacks[key] = func + + def advance(self): + """Provides the result from mello event state. + + Returns: + A tuple containing the latest SE(3) delta pose and gripper state. + """ + # rot_vec = Rotation.from_euler("XYZ", self._delta_rot).as_rotvec() + return self.prev_joints, self.prev_gripper + + def _setup_serial(self): + """Set up serial connection to Mello device.""" + try: + self.serial = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=1) # 1 second timeout + print(f"Successfully connected to {self.port}") + except serial.SerialException as e: + print(f"Error opening serial port: {e}") + raise + + def _start_read_thread(self): + """Start a thread to read data from the Mello device.""" + self.read_thread = threading.Thread(target=self._read_thread) + self.read_thread.daemon = True + self.read_thread.start() + + def _read_thread(self): + """Continuously read and parse joint data from Mello.""" + while self.running: + try: + if self.serial.in_waiting: + line = self.serial.readline().decode("utf-8").strip() + try: + data = ast.literal_eval(line) + joint_positions = data.get("joint_positions:", [0] * 7) + + # Extract and process joint data + joints_deg = joint_positions[:6] + joints_deg[2] *= -1 # Flip direction if needed + joints_rad = self._degrees_to_radians(joints_deg) + + gripper_value = joint_positions[-1] if len(joint_positions) > 6 else self.prev_gripper + + # Compute deltas if not zero + if not all(j == 0 for j in joints_rad): + self.prev_joints = joints_rad + + # Gripper state + self.prev_gripper = gripper_value + self.latest_values = self.prev_joints + self._close_gripper = False if gripper_value >= 0 else True + + except (ValueError, SyntaxError) as e: + print(f"Error parsing serial data: {e}") + except Exception as e: + print(f"Error reading serial data: {e}") + + time.sleep(0.01) + + def _degrees_to_radians(self, degrees): + """Convert a list of angles from degrees to radians.""" + return [np.deg2rad(d) for d in degrees] diff --git a/source/gculab_assets/gculab_assets/robots/universal_robots.py b/source/gculab_assets/gculab_assets/robots/universal_robots.py index 4c9988e..6397de4 100644 --- a/source/gculab_assets/gculab_assets/robots/universal_robots.py +++ b/source/gculab_assets/gculab_assets/robots/universal_robots.py @@ -23,6 +23,22 @@ ## +UR5_DEFAULT_JOINT_POS = { + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -1.5708, + "elbow_joint": 1.5708, + "wrist_1_joint": 4.7112, + "wrist_2_joint": -1.5708, + "wrist_3_joint": -1.5708, + "finger_joint": 0.0, + "right_outer.*": 0.0, + "left_outer.*": 0.0, + "left_inner_finger_knuckle_joint": 0.0, + "right_inner_finger_knuckle_joint": 0.0, + "left_inner_finger_joint": -0.785398, + "right_inner_finger_joint": 0.785398, +} + UR10_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd", @@ -62,12 +78,40 @@ ), ) -ur5_robotiq_path = "gcu_objects/assets/ur_description/ur5e_robotiq_2f_140_temp.usd" +ur5_robotiq_path = "gcu_objects/assets/ur_description/ur5e_robotiq.usd" ur5_robotiq_abs_path = os.path.abspath(ur5_robotiq_path) UR5_ROBOTIQ_CFG = UR5_CFG.replace( spawn=sim_utils.UsdFileCfg( usd_path=ur5_robotiq_abs_path, ), + init_state=ArticulationCfg.InitialStateCfg(joint_pos=UR5_DEFAULT_JOINT_POS), ) + +IMPLICIT_UR5_ROBOTIQ = UR5_ROBOTIQ_CFG.copy() # type: ignore +IMPLICIT_UR5_ROBOTIQ.actuators = { + "arm": ImplicitActuatorCfg( + joint_names_expr=["shoulder.*", "elbow.*", "wrist.*"], + stiffness=261.8, + damping=26.18, + # velocity_limit=3.14, + effort_limit_sim={"shoulder.*": 9000, "elbow.*": 9000, "wrist.*": 1680}, + ), + "gripper": ImplicitActuatorCfg( + joint_names_expr=["finger_joint"], + stiffness=17, + damping=5, + # velocity_limit=2.27, + effort_limit_sim=165, + ), + "inner_finger": ImplicitActuatorCfg( + joint_names_expr=[".*_inner_finger_joint"], + stiffness=0.2, + damping=0.02, + # velocity_limit=5.3, + effort_limit_sim=0.5, + ), +} + + """Configuration of UR-10 arm using implicit actuator models.""" diff --git a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/__init__.py b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/__init__.py index 12fee61..09303c1 100644 --- a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/__init__.py +++ b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/__init__.py @@ -17,3 +17,12 @@ "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:UR5PackEnvCfg", }, ) + +gym.register( + id="Isaac-Pack-UR5-Teleop-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_teleop_cfg:UR5PackEnvTeleopCfg", + }, +) diff --git a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_cfg.py b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_cfg.py index f6d7d27..7ad3494 100644 --- a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_cfg.py +++ b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_cfg.py @@ -14,7 +14,7 @@ ## # Pre-defined configs ## -from gculab_assets import UR5_ROBOTIQ_CFG # isort: skip +from gculab_assets import UR5_ROBOTIQ_CFG, IMPLICIT_UR5_ROBOTIQ, UR5_CFG # isort: skip ## @@ -28,7 +28,7 @@ def __post_init__(self): # post init of parent super().__post_init__() - self.scene.right_robot = UR5_ROBOTIQ_CFG.replace( + self.scene.right_robot = IMPLICIT_UR5_ROBOTIQ.replace( prim_path="{ENV_REGEX_NS}/RightRobot", init_state=ArticulationCfg.InitialStateCfg( pos=(0.9, 0.33, 0.75), @@ -44,21 +44,21 @@ def __post_init__(self): ), ) - self.scene.left_robot = UR5_ROBOTIQ_CFG.replace( - prim_path="{ENV_REGEX_NS}/LeftRobot", - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.9, -0.33, 0.75), - rot=(0, -0.7071068, 0, 0.7071068), - joint_pos={ - "shoulder_pan_joint": 0.0, - "shoulder_lift_joint": -2.2, - "elbow_joint": 1.9, - "wrist_1_joint": 1.383, - "wrist_2_joint": 1.57, - "wrist_3_joint": 0.00, - }, - ), - ) + # self.scene.left_robot = UR5_ROBOTIQ_CFG.replace( + # prim_path="{ENV_REGEX_NS}/LeftRobot", + # init_state=ArticulationCfg.InitialStateCfg( + # pos=(0.9, -0.33, 0.75), + # rot=(0, -0.7071068, 0, 0.7071068), + # joint_pos={ + # "shoulder_pan_joint": 0.0, + # "shoulder_lift_joint": -2.2, + # "elbow_joint": 1.9, + # "wrist_1_joint": 1.383, + # "wrist_2_joint": 1.57, + # "wrist_3_joint": 0.00, + # }, + # ), + # ) # override events # self.events.reset_robot_joints = EventTerm( @@ -75,9 +75,53 @@ def __post_init__(self): # self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = ["wrist_3_link"] # self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["wrist_3_link"] # override actions - # self.actions.arm_action = mdp.JointPositionActionCfg( - # asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="right_robot", + joint_names=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + self.actions.gripper_action = mdp.JointPositionActionCfg( + asset_name="right_robot", + joint_names=[ + "finger_joint", + "right_outer_knuckle_joint", + "right_outer_finger_joint", + "left_outer_finger_joint", + "left_inner_finger_pad_joint", + "right_inner_finger_pad_joint", + "left_inner_finger_joint", + "right_inner_finger_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + # self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + # asset_name="right_robot", + # joint_names=["finger_joint", "right_outer_knuckle_joint", "right_outer_finger_joint", "left_outer_finger_joint", + # "left_inner_finger_pad_joint", "right_inner_finger_pad_joint", + # "left_inner_finger_joint", "right_inner_finger_joint"], + # open_command_expr={"finger_joint": 0.0, "right_outer_knuckle_joint": 0.0, "right_outer_finger_joint": 0.785398, + # "left_outer_finger_joint": 0.785398, + # "left_inner_finger_pad_joint": 0.0, "right_inner_finger_pad_joint": 0.0, + # "left_inner_finger_joint": -0.785398, "right_inner_finger_joint": -0.785398}, + # close_command_expr={"finger_joint": 0.785398, "right_outer_knuckle_joint": 0.785398, "right_outer_finger_joint": 0.0, + # "left_outer_finger_joint": 0.0, + # "left_inner_finger_pad_joint": 0.785398, "right_inner_finger_pad_joint": 0.785398, + # "left_inner_finger_joint": -0.785398, "right_inner_finger_joint": -0.785398}, + + # debug_vis=False, # ) + # override command generator body # end-effector is along x-direction # self.commands.ee_pose.body_name = "wrist_3_link" diff --git a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_teleop_cfg.py b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_teleop_cfg.py new file mode 100644 index 0000000..b76552a --- /dev/null +++ b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/config/ur_5/joint_pos_env_teleop_cfg.py @@ -0,0 +1,148 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab.assets import ArticulationCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.utils import configclass +from tote_consolidation.tasks.manager_based.pack.config.ur_5.joint_pos_env_cfg import ( + UR5PackEnvCfg, +) +from tote_consolidation.tasks.manager_based.pack.pack_env_cfg import PackEnvCfg +from tote_consolidation.tasks.manager_based.pack.pack_env_teleop_cfg import ( + TeleopPackEnvCfg, +) + +## +# Pre-defined configs +## +from gculab_assets import UR5_ROBOTIQ_CFG, IMPLICIT_UR5_ROBOTIQ, UR5_CFG # isort: skip + + +## +# Environment configuration +## + + +@configclass +class UR5PackEnvTeleopCfg(TeleopPackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.right_robot = IMPLICIT_UR5_ROBOTIQ.replace( + prim_path="{ENV_REGEX_NS}/RightRobot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.9, 0.33, 0.75), + rot=(0, -0.7071068, 0, 0.7071068), + joint_pos={ + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -2.2, + "elbow_joint": 1.9, + "wrist_1_joint": -1.383, + "wrist_2_joint": -1.57, + "wrist_3_joint": 0.00, + }, + ), + ) + + # self.scene.left_robot = UR5_ROBOTIQ_CFG.replace( + # prim_path="{ENV_REGEX_NS}/LeftRobot", + # init_state=ArticulationCfg.InitialStateCfg( + # pos=(0.9, -0.33, 0.75), + # rot=(0, -0.7071068, 0, 0.7071068), + # joint_pos={ + # "shoulder_pan_joint": 0.0, + # "shoulder_lift_joint": -2.2, + # "elbow_joint": 1.9, + # "wrist_1_joint": 1.383, + # "wrist_2_joint": 1.57, + # "wrist_3_joint": 0.00, + # }, + # ), + # ) + + # override events + # self.events.reset_robot_joints = EventTerm( + # func=mdp.reset_joints_by_scale, + # mode="reset", + # params={ + # "position_range": (0.5, 1.5), + # "velocity_range": (0.0, 0.0), + # }, + # ) + # self.events.reset_robot_joints.params["position_range"] = (0.75, 1.25) + # override rewards + # self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["wrist_3_link"] + # self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = ["wrist_3_link"] + # self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["wrist_3_link"] + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="right_robot", + joint_names=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + scale=1.0, + use_default_offset=False, + ) + + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="right_robot", + joint_names=[ + "finger_joint", + "right_outer_knuckle_joint", + "right_outer_finger_joint", + "left_outer_finger_joint", + "left_inner_finger_pad_joint", + "right_inner_finger_pad_joint", + "left_inner_finger_joint", + "right_inner_finger_joint", + ], + open_command_expr={ + "finger_joint": 0.0, + "right_outer_knuckle_joint": 0.0, + "right_outer_finger_joint": 0.785398, + "left_outer_finger_joint": 0.785398, + "left_inner_finger_pad_joint": 0.0, + "right_inner_finger_pad_joint": 0.0, + "left_inner_finger_joint": -0.785398, + "right_inner_finger_joint": -0.785398, + }, + close_command_expr={ + "finger_joint": 0.785398, + "right_outer_knuckle_joint": 0.785398, + "right_outer_finger_joint": 0.0, + "left_outer_finger_joint": 0.0, + "left_inner_finger_pad_joint": 0.785398, + "right_inner_finger_pad_joint": 0.785398, + "left_inner_finger_joint": -0.785398, + "right_inner_finger_joint": -0.785398, + }, + debug_vis=False, + ) + + # override command generator body + # end-effector is along x-direction + # self.commands.ee_pose.body_name = "wrist_3_link" + # self.commands.ee_pose.ranges.pitch = (math.pi / 2, math.pi / 2) + + +@configclass +class UR5PackEnvTeleopCfg_PLAY(UR5PackEnvTeleopCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_cfg.py b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_cfg.py index 88621bd..d554bcd 100644 --- a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_cfg.py +++ b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_cfg.py @@ -152,7 +152,7 @@ class PackSceneCfg(InteractiveSceneCfg): # robots right_robot: ArticulationCfg | None = None - left_robot: ArticulationCfg | None = None + # left_robot: ArticulationCfg | None = None # lights light = AssetBaseCfg( @@ -309,7 +309,7 @@ class PackEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the reach end-effector pose tracking environment.""" # Scene settings - scene: PackSceneCfg = PackSceneCfg(num_envs=512, env_spacing=2.5, replicate_physics=False, clone_in_fabric=True) + scene: PackSceneCfg = PackSceneCfg(num_envs=512, env_spacing=2.5, replicate_physics=False) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg() diff --git a/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_teleop_cfg.py b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_teleop_cfg.py new file mode 100644 index 0000000..95e3f9e --- /dev/null +++ b/source/tote_consolidation/tote_consolidation/tasks/manager_based/pack/pack_env_teleop_cfg.py @@ -0,0 +1,241 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import glob + +## +# Scene definition +## +import os +from dataclasses import MISSING + +import gculab.sim as gcu_sim_utils +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass +from tote_consolidation.tasks.manager_based.pack.pack_env_cfg import ( + ActionsCfg, + CommandsCfg, + CurriculumCfg, + EventCfg, + PackEnvCfg, + PackSceneCfg, + ToteManagerCfg, +) + +from . import mdp + +tote_usd_path = "gcu_objects/assets/yellow_tote/model.usd" + +vention_table_usd_path = "gcu_objects/assets/vention/vention.usd" + +gcu_objects_path = os.path.abspath("gcu_objects") + +# Dynamically build list of USD paths by scanning the directory +ycb_physics_dir = os.path.join(gcu_objects_path, "YCB/Axis_Aligned_Physics") +all_usd_files = glob.glob(os.path.join(ycb_physics_dir, "*.usd")) + +# Extract all available object IDs and names for reference +available_objects = {} +for usd_file in all_usd_files: + basename = os.path.basename(usd_file) + obj_id = basename[:3] + obj_name = basename[4:].replace(".usd", "") + available_objects[obj_id] = obj_name + +# Print available objects for reference +print("Available YCB objects:") +for obj_id, obj_name in sorted(available_objects.items()): + print(f'"{obj_id}", # {obj_name}') + +# Define which object IDs to include +include_ids = [ + "003", # cracker_box + "004", # sugar_box + "006", # mustard_bottle + "008", # pudding_box + "009", # gelatin_box + "036", # wood_block + "061", # foam_brick +] + +# Filter USD files based on ID prefixes +usd_paths = [] +for usd_file in all_usd_files: + basename = os.path.basename(usd_file) + # Extract the 3-digit ID from filename (assuming format like "003_cracker_box.usd") + if basename[:3] in include_ids: + usd_paths.append(usd_file) + +num_object_per_env = 50 +num_objects_to_reserve = 50 + +# Spacing between totes +tote_spacing = 0.43 # width of tote + gap between totes + + +@configclass +class PackSceneTeleopCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # world + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -0.76)), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=vention_table_usd_path, + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, 0.0, -0.0), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + tote1 = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Tote1", + spawn=sim_utils.UsdFileCfg( + usd_path=tote_usd_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, -1.5 * tote_spacing, 0.0)), + ) + + tote2 = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Tote2", + spawn=sim_utils.UsdFileCfg( + usd_path=tote_usd_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, -0.5 * tote_spacing, 0.0)), + ) + + tote3 = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Tote3", + spawn=sim_utils.UsdFileCfg( + usd_path=tote_usd_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, 0.5 * tote_spacing, 0.0)), + ) + + tote4 = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Tote4", + spawn=sim_utils.UsdFileCfg( + usd_path=tote_usd_path, + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, 1.5 * tote_spacing, 0.0)), + ) + + # robots + right_robot: ArticulationCfg | None = None + left_robot: ArticulationCfg | None = None + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), + ) + + def __post_init__(self): + for i in range(num_object_per_env): + setattr( + self, + f"object{i}", + RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/Object{i}", + spawn=gcu_sim_utils.MultiUsdFromDistFileCfg( + usd_path=usd_paths, + random_choice=True, + distribution=None, # None for uniform distribution + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + # enable_gyroscopic_forces=True, + solver_position_iteration_count=90, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(i / 5.0, 1.2, -0.7)), + ), + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + + policy: PolicyCfg = PolicyCfg() + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + pass + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + pass + +@configclass +class TeleopEventCfg: + """Configuration for events.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + +## +# Environment configuration +## + + +@configclass +class TeleopPackEnvCfg(PackEnvCfg): + """Configuration for the reach end-effector pose tracking environment.""" + + # Scene settings + scene: PackSceneTeleopCfg = PackSceneTeleopCfg(num_envs=4096, env_spacing=2.5, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: TeleopEventCfg = TeleopEventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + tote_manager: ToteManagerCfg = ToteManagerCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.sim.render_interval = self.decimation + self.episode_length_s = 10.0 + self.viewer.eye = (0, 0.1, 5.5) + # simulation settings + self.sim.dt = 1.0 / 90.0