From a3d0011800c24508b4ce6c2a050d20e821547305 Mon Sep 17 00:00:00 2001 From: Ashwin Varghese Kuruttukulam Date: Fri, 8 Aug 2025 17:22:30 -0700 Subject: [PATCH 1/2] add so-100 envs --- scripts/tools/record_demos.py | 18 +- .../isaaclab_assets/robots/so_100.py | 71 ++++++++ .../envs/pinocchio_envs/__init__.py | 11 ++ .../pinocchio_envs/stack_so100_mimic_env.py | 142 ++++++++++++++++ .../stack_so100_mimic_env_cfg.py | 96 +++++++++++ .../stack/config/so_100/__init__.py | 48 ++++++ .../config/so_100/stack_joint_pos_env_cfg.py | 159 ++++++++++++++++++ .../so_100/stack_pink_ik_rel_env_cfg.py | 128 ++++++++++++++ .../stack_pink_ik_rel_visuomotor_env_cfg.py | 60 +++++++ .../stack/mdp/so100_stack_events.py | 97 +++++++++++ 10 files changed, 828 insertions(+), 2 deletions(-) create mode 100644 source/isaaclab_assets/isaaclab_assets/robots/so_100.py create mode 100644 source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py create mode 100644 source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index d9bacd5c253..dfb87b328ac 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -19,6 +19,8 @@ --step_hz Environment stepping rate in Hz. (default: 30) --num_demos Number of demonstrations to record. (default: 0) --num_success_steps Number of continuous steps with task success for concluding a demo as successful. (default: 10) + --pos_sensitivity Position sensitivity for teleoperation devices. (default: 0.2) + --rot_sensitivity Rotation sensitivity for teleoperation devices. (default: 0.5) """ """Launch Isaac Sim Simulator first.""" @@ -53,6 +55,18 @@ default=False, help="Enable Pinocchio.", ) +parser.add_argument( + "--pos_sensitivity", + type=float, + default=0.2, + help="Position sensitivity for teleoperation devices. Default is 0.2.", +) +parser.add_argument( + "--rot_sensitivity", + type=float, + default=0.5, + help="Rotation sensitivity for teleoperation devices. Default is 0.5.", +) # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) @@ -269,9 +283,9 @@ def setup_teleop_device(callbacks: dict[str, Callable]) -> object: omni.log.warn(f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default.") # Create fallback teleop device if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) + teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity)) elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) + teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity)) else: omni.log.error(f"Unsupported teleop device: {args_cli.teleop_device}") omni.log.error("Supported devices: keyboard, spacemouse, handtracking") diff --git a/source/isaaclab_assets/isaaclab_assets/robots/so_100.py b/source/isaaclab_assets/isaaclab_assets/robots/so_100.py new file mode 100644 index 00000000000..dd0da9919e9 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/so_100.py @@ -0,0 +1,71 @@ +# 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 + +"""Configuration for the Franka Emika robots. + +The following configurations are available: + +* :obj:`SO_100_CFG`: SO-100 robot + +Reference: https://github.com/TheRobotStudio/SO-ARM100 +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Configuration +## + +SO_100_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/RobotStudio/so100/so100.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "shoulder_pan": 0.0, + "shoulder_lift": 0.0, + "elbow_flex": 0.0, + "wrist_flex": 0.0, + "wrist_roll": 0.0, + "gripper": 0.0, + }, + ), + actuators={ + "all_joints": ImplicitActuatorCfg( + joint_names_expr=["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], + effort_limit_sim=None, + velocity_limit_sim=None, + stiffness=None, + damping=None, + armature=0.0, + ), + "gripper": ImplicitActuatorCfg( + joint_names_expr=["gripper"], + effort_limit_sim=0.1, + velocity_limit_sim=2.175, + stiffness=8.0, + damping=0.4, + armature=0.0, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of SO‑100 robot arm.""" \ No newline at end of file diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py index 89dc8459021..e5ef3102655 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -11,6 +11,8 @@ from .nutpour_gr1t2_mimic_env_cfg import NutPourGR1T2MimicEnvCfg from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg +from .stack_so100_mimic_env import StackSO100MimicEnv +from .stack_so100_mimic_env_cfg import StackSO100MimicEnvCfg gym.register( id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0", @@ -34,3 +36,12 @@ kwargs={"env_cfg_entry_point": exhaustpipe_gr1t2_mimic_env_cfg.ExhaustPipeGR1T2MimicEnvCfg}, disable_env_checker=True, ) + +gym.register( + id="Isaac-Stack-SO100-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs.pinocchio_envs:StackSO100MimicEnv", + kwargs={ + "env_cfg_entry_point": stack_so100_mimic_env_cfg.StackSO100MimicEnvCfg, + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py new file mode 100644 index 00000000000..72e53a69b21 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py @@ -0,0 +1,142 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class StackSO100MimicEnv(ManagerBasedRLMimicEnv): + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector (for SO100, this is typically "gripper"). + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + # For SO100, the end effector pose is stored in policy observations + eef_pos_name = "eef_pos" + eef_quat_name = "eef_quat" + + target_eef_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_eef_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + Args: + target_eef_pose_dict: Dictionary containing target eef pose. For SO100, expects key "gripper". + gripper_action_dict: Dictionary containing gripper action. For SO100, expects key "gripper". + action_noise_dict: Noise to add to the action. If None, no noise is added. + env_id: Environment index to get the action for. + + Returns: + An action torch.Tensor that's compatible with env.step(). + """ + + # target position and rotation for single arm + target_eef_pos, target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["gripper"]) + target_eef_rot_quat = PoseUtils.quat_from_matrix(target_rot) + + # gripper action + gripper_action = gripper_action_dict["gripper"] + + if action_noise_dict is not None: + pos_noise = action_noise_dict["gripper"] * torch.randn_like(target_eef_pos) + quat_noise = action_noise_dict["gripper"] * torch.randn_like(target_eef_rot_quat) + + target_eef_pos += pos_noise + target_eef_rot_quat += quat_noise + + return torch.cat( + ( + target_eef_pos, + target_eef_rot_quat, + gripper_action, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + target_poses = {} + + # For SO100: action dimensions are [pos(3), quat(4), gripper(1)] + target_eef_position = action[:, 0:3] + target_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose = PoseUtils.make_pose(target_eef_position, target_rot_mat) + target_poses["gripper"] = target_pose + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key is "gripper". + """ + print("actions", actions) + print("actions", actions.shape) + return {"gripper": actions[:, -1:]} # Gripper is at index 7 (single dimension) + + def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]: + """ + Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1 + when the subtask has been completed and 0 otherwise. + + Args: + env_ids: Environment indices to get the termination signals for. If None, all envs are considered. + + Returns: + A dictionary termination signal flags (False or True) for each subtask. + """ + if env_ids is None: + env_ids = slice(None) + + signals = dict() + subtask_terms = self.obs_buf["subtask_terms"] + signals["grasp_1"] = subtask_terms["grasp_1"][env_ids] + return signals \ No newline at end of file diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py new file mode 100644 index 00000000000..7156badf604 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py @@ -0,0 +1,96 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Copyright (c) 2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.stack.config.so_100.stack_pink_ik_abs_visuomotor_env_cfg import SO100CubeStackPinkIKAbsVisuomotorEnvCfg + + +@configclass +class StackSO100MimicEnvCfg(SO100CubeStackPinkIKAbsVisuomotorEnvCfg, MimicEnvCfg): + + def __post_init__(self): + # Calling post init of parents + super().__post_init__() + + # Override the existing values + self.datagen_config.name = "demo_src_so100_stack_task_D0" + self.datagen_config.generation_guarantee = True + self.datagen_config.generation_keep_failed = False + self.datagen_config.generation_num_trials = 1000 + self.datagen_config.generation_select_src_per_subtask = False + self.datagen_config.generation_select_src_per_arm = False + self.datagen_config.generation_relative = False + self.datagen_config.generation_joint_pos = False + self.datagen_config.generation_transform_first_robot_pose = False + self.datagen_config.generation_interpolate_from_last_target_pose = True + self.datagen_config.max_num_failures = 25 + self.datagen_config.num_demo_to_render = 10 + self.datagen_config.num_fail_demo_to_render = 25 + self.datagen_config.seed = 1 + + # The following are the subtask configurations for the stack task. + # For SO100, we only have one "gripper" arm, so all subtasks are for this single arm + subtask_configs = [] + + # Subtask 1: Pick up cube_2 + subtask_configs.append( + SubTaskConfig( + # Each subtask involves manipulation with respect to a single object frame. + # For picking up cube_2, the object of interest is cube_2 + object_ref="cube_2", + # This key corresponds to the binary indicator in "datagen_info" that signals + # when this subtask is finished (e.g., on a 0 to 1 edge). + subtask_term_signal="grasp_1", + first_subtask_start_offset_range=(0, 0), + # Randomization range for starting index of the first subtask + subtask_term_offset_range=(0, 0), + # Selection strategy for the source subtask segment during data generation + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.0, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=0, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + + # Subtask 2: Place cube_2 on cube_1 + subtask_configs.append( + SubTaskConfig( + # For placing cube_2 on cube_1, the object of interest is cube_1 (target location) + object_ref="cube_1", + # Corresponding key for the binary indicator in "datagen_info" for completion + subtask_term_signal=None, # This is the final subtask, so no termination signal + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for source subtask segment + selection_strategy="nearest_neighbor_object", + # Optional parameters for the selection strategy function + selection_strategy_kwargs={"nn_k": 3}, + # Amount of action noise to apply during this subtask + action_noise=0.0, + # Number of interpolation steps to bridge to this subtask segment + num_interpolation_steps=3, + # Additional fixed steps for the robot to reach the necessary pose + num_fixed_steps=0, + # If True, apply action noise during the interpolation phase and execution + apply_noise_during_interpolation=False, + ) + ) + + # For SO100, we only have one arm "gripper", so we assign all subtasks to it + self.subtask_configs["gripper"] = subtask_configs \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py new file mode 100644 index 00000000000..11f4c8f0a93 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py @@ -0,0 +1,48 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import stack_joint_pos_env_cfg +from . import stack_pink_ik_rel_env_cfg +from . import stack_pink_ik_rel_visuomotor_env_cfg +from . import stack_ik_rel_env_cfg +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Stack-Cube-SO100-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_joint_pos_env_cfg.SO100CubeStackJointPosEnvCfg, + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Relative Pose Control +## +gym.register( + id="Isaac-Stack-Cube-SO100-Pink-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_pink_ik_rel_env_cfg.SO100CubeStackPinkIKRelEnvCfg, + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Stack-Cube-SO100-Pink-IK-Rel-Visuomotor-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": stack_pink_ik_rel_visuomotor_env_cfg.SO100CubeStackPinkIKRelVisuomotorEnvCfg, + }, + disable_env_checker=True, +) \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py new file mode 100644 index 00000000000..54af8b69c63 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py @@ -0,0 +1,159 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import importlib +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.stack import mdp +from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import StackEnvCfg +from isaaclab_tasks.manager_based.manipulation.stack.mdp import so100_stack_events + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip +from isaaclab_assets.robots.so_100 import SO_100_CFG # isort: skip + +import torch + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_offset, + mode="reset", + params={ + "position_range": (-0.1, 0.1), + "velocity_range": (0.0, 0.0), + "asset_cfg": SceneEntityCfg("robot"), + }, + ) + + randomize_cubes_in_focus = EventTerm( + func=so100_stack_events.randomize_single_rigid_objects, + mode="reset", + params={ + "asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")], + "pose_range": {"x": (0.2, 0.3), "y": (-0.10, 0.10), "z": (0.01, 0.01), "yaw": (0.0, 0.0)}, + "min_separation": 0.1, + }, + ) + + +@configclass +class SO100CubeStackJointPosEnvCfg(StackEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set events + self.events = EventCfg() + + # Set SO-100 as robot + self.scene.robot = SO_100_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.0), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + "shoulder_pan": 0.0, + "shoulder_lift": 1.5708, + "elbow_flex": -1.5708, + "wrist_flex": 1.0, + "wrist_roll": 0.0, + "gripper": 0.0, + }, + joint_vel={".*": 0.0}, + )) + self.scene.robot.spawn.semantic_tags = [("class", "robot")] + + # Add semantics to table + self.scene.table.spawn.semantic_tags = [("class", "table")] + + # Add semantics to ground + self.scene.plane.semantic_tags = [("class", "ground")] + + # Set actions for the specific robot type (SO-100) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], scale=1.0, use_default_offset=False + ) + + # Use the last joint as the gripper action + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["gripper"], + open_command_expr={"gripper": 0.5}, + close_command_expr={"gripper": -0.1}, + ) + + # Rigid body properties of each cube + cube_properties = RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ) + + # Set each stacking cube deterministically + self.scene.cube_1 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_1", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.2, 0.0, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd", + scale=(0.5, 0.5, 0.5), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_1")], + ), + ) + self.scene.cube_2 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_2", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.2, 0.05, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd", + scale=(0.5, 0.5, 0.5), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_2")], + ), + ) + self.scene.cube_3 = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube_3", + init_state=RigidObjectCfg.InitialStateCfg(pos=[0.3, -0.05, 0.0203], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd", + scale=(0.5, 0.5, 0.5), + rigid_props=cube_properties, + semantic_tags=[("class", "cube_3")], + ), + ) + + # Listens to the required transforms + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/gripper", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, -0.1, 0.0], + ), + ), + ], + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py new file mode 100644 index 00000000000..b59c70e760c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py @@ -0,0 +1,128 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +from isaaclab.assets import ArticulationCfg +from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg + +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + +from pink.tasks import FrameTask + +import isaaclab.envs.mdp as base_mdp +from ... import mdp + + +from . import stack_joint_pos_env_cfg + +import tempfile + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=base_mdp.last_action) + joint_pos = ObsTerm(func=base_mdp.joint_pos_rel) + joint_vel = ObsTerm(func=base_mdp.joint_vel_rel) + object = ObsTerm(func=mdp.object_obs) + cube_positions = ObsTerm(func=mdp.cube_positions_in_world_frame) + cube_orientations = ObsTerm(func=mdp.cube_orientations_in_world_frame) + eef_pos = ObsTerm(func=mdp.ee_frame_pos) + eef_quat = ObsTerm(func=mdp.ee_frame_quat) + gripper_pos = ObsTerm(func=mdp.gripper_pos) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + + @configclass + class SubtaskCfg(ObsGroup): + """Observations for subtask group.""" + + grasp_1 = ObsTerm( + func=mdp.object_grasped, + params={ + "robot_cfg": SceneEntityCfg("robot"), + "ee_frame_cfg": SceneEntityCfg("ee_frame"), + "object_cfg": SceneEntityCfg("cube_2"), + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + # observation groups + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class SO100CubeStackPinkIKRelEnvCfg(stack_joint_pos_env_cfg.SO100CubeStackJointPosEnvCfg): + observations: ObservationsCfg = ObservationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Temporary directory for URDF files + self.temp_urdf_dir = tempfile.gettempdir() + # Convert USD to URDF and change revolute joints to fixed + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True + ) + + # Set actions to use pink IK controller + self.actions.arm_action = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + "shoulder_pan", + "shoulder_lift", + "elbow_flex", + "wrist_flex", + "wrist_roll", + ], + # Joints to be locked in URDF + ik_urdf_fixed_joint_names=["joints_gripper"], + hand_joint_names=[], + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base", + num_hand_joints=0, + use_relative_mode=True, + show_ik_warnings=True, + variable_input_tasks=[ + FrameTask( + "joints_gripper", + position_cost=1.0, # [cost] / [m] + orientation_cost=1.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.1, + ) + ], + fixed_input_tasks=[], + ), + ) + ControllerUtils.change_revolute_to_fixed( + temp_urdf_output_path, self.actions.arm_action.ik_urdf_fixed_joint_names + ) + + # Set the URDF and mesh paths for the IK controller + self.actions.arm_action.controller.urdf_path = temp_urdf_output_path + self.actions.arm_action.controller.mesh_path = temp_urdf_meshes_output_path + diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py new file mode 100644 index 00000000000..8cc2d61c7dc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py @@ -0,0 +1,60 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import isaaclab.sim as sim_utils +from isaaclab.sensors import CameraCfg +from isaaclab.utils import configclass +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +import isaaclab.envs.mdp as base_mdp + +from ... import mdp +from .stack_pink_ik_rel_env_cfg import SO100CubeStackPinkIKRelEnvCfg, ObservationsCfg + + +@configclass +class VisuomotorObservationsCfg(ObservationsCfg): + """Observation specifications for the visuomotor MDP.""" + + @configclass + class PolicyCfg(ObservationsCfg.PolicyCfg): + """Observations for policy group with state values and camera.""" + + table_cam = ObsTerm( + func=base_mdp.image, params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False} + ) + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class SO100CubeStackPinkIKRelVisuomotorEnvCfg(SO100CubeStackPinkIKRelEnvCfg): + observations: VisuomotorObservationsCfg = VisuomotorObservationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set table view camera + self.scene.table_cam = CameraCfg( + prim_path="{ENV_REGEX_NS}/table_cam", + update_period=0.0, + height=512, + width=512, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 2) + ), + offset=CameraCfg.OffsetCfg( + pos=(1.0, 0.0, 0.4), rot=(0.35355, -0.61237, -0.61237, 0.35355), convention="ros" + ), + ) + + # Set settings for camera rendering + self.rerender_on_reset = True + self.sim.render.antialiasing_mode = "OFF" # disable dlss + \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py new file mode 100644 index 00000000000..82921fffee7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py @@ -0,0 +1,97 @@ +# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import math +import random +import torch +from typing import TYPE_CHECKING + +import isaaclab.utils.math as math_utils +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def sample_object_poses( + num_objects: int, + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + """Sample object poses with minimum separation constraint.""" + range_list = [pose_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z", "roll", "pitch", "yaw"]] + pose_list = [] + + for i in range(num_objects): + for j in range(max_sample_tries): + sample = [random.uniform(range[0], range[1]) for range in range_list] + + # Accept pose if it is the first one, or if reached max num tries + if len(pose_list) == 0 or j == max_sample_tries - 1: + pose_list.append(sample) + break + + # Check if pose of object is sufficiently far away from all other objects + separation_check = [math.dist(sample[:3], pose[:3]) > min_separation for pose in pose_list] + if False not in separation_check: + pose_list.append(sample) + break + + return pose_list + + +def randomize_single_rigid_objects( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + asset_cfgs: list[SceneEntityCfg], + min_separation: float = 0.0, + pose_range: dict[str, tuple[float, float]] = {}, + max_sample_tries: int = 5000, +): + """ + Randomize poses of individual rigid objects (not multi-object rigid bodies). + + This is a version of randomize_rigid_objects_in_focus adapted for single RigidObjects. + Unlike the Franka version which handles multi-object rigid bodies with num_objects attribute, + this function works with individual RigidObject instances in the scene. + + Args: + env: The manager-based environment. + env_ids: Environment indices to apply the randomization to. + asset_cfgs: List of scene entity configurations for the rigid objects to randomize. + min_separation: Minimum separation distance between objects. + pose_range: Dictionary specifying the range for each pose component (x, y, z, roll, pitch, yaw). + max_sample_tries: Maximum number of attempts to sample valid poses. + """ + if env_ids is None: + return + + # Randomize poses in each environment independently + for cur_env in env_ids.tolist(): + pose_list = sample_object_poses( + num_objects=len(asset_cfgs), + min_separation=min_separation, + pose_range=pose_range, + max_sample_tries=max_sample_tries, + ) + + # Randomize pose for each object + for i in range(len(asset_cfgs)): + asset_cfg = asset_cfgs[i] + asset = env.scene[asset_cfg.name] + + # Write pose to simulation + pose_tensor = torch.tensor([pose_list[i]], device=env.device) + positions = pose_tensor[:, 0:3] + env.scene.env_origins[cur_env, 0:3] + orientations = math_utils.quat_from_euler_xyz(pose_tensor[:, 3], pose_tensor[:, 4], pose_tensor[:, 5]) + asset.write_root_pose_to_sim( + torch.cat([positions, orientations], dim=-1), env_ids=torch.tensor([cur_env], device=env.device) + ) + asset.write_root_velocity_to_sim( + torch.zeros(1, 6, device=env.device), env_ids=torch.tensor([cur_env], device=env.device) + ) \ No newline at end of file From 8703792d2c6a4445c65cfe79da0bd193652b7bc3 Mon Sep 17 00:00:00 2001 From: Ashwin Varghese Kuruttukulam Date: Mon, 11 Aug 2025 11:14:57 -0700 Subject: [PATCH 2/2] fix formatting --- scripts/tools/record_demos.py | 10 +++-- .../isaaclab_assets/robots/so_100.py | 2 +- .../pinocchio_envs/stack_so100_mimic_env.py | 2 +- .../stack_so100_mimic_env_cfg.py | 12 +++--- .../stack/config/so_100/__init__.py | 13 +++--- .../config/so_100/stack_joint_pos_env_cfg.py | 40 ++++++++++--------- .../so_100/stack_pink_ik_rel_env_cfg.py | 20 ++++------ .../stack_pink_ik_rel_visuomotor_env_cfg.py | 14 +++---- .../stack/mdp/so100_stack_events.py | 4 +- 9 files changed, 60 insertions(+), 57 deletions(-) diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index dfb87b328ac..1188fe449f5 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -62,7 +62,7 @@ help="Position sensitivity for teleoperation devices. Default is 0.2.", ) parser.add_argument( - "--rot_sensitivity", + "--rot_sensitivity", type=float, default=0.5, help="Rotation sensitivity for teleoperation devices. Default is 0.5.", @@ -283,9 +283,13 @@ def setup_teleop_device(callbacks: dict[str, Callable]) -> object: omni.log.warn(f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default.") # Create fallback teleop device if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity)) + teleop_interface = Se3Keyboard( + Se3KeyboardCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity) + ) elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity)) + teleop_interface = Se3SpaceMouse( + Se3SpaceMouseCfg(pos_sensitivity=args_cli.pos_sensitivity, rot_sensitivity=args_cli.rot_sensitivity) + ) else: omni.log.error(f"Unsupported teleop device: {args_cli.teleop_device}") omni.log.error("Supported devices: keyboard, spacemouse, handtracking") diff --git a/source/isaaclab_assets/isaaclab_assets/robots/so_100.py b/source/isaaclab_assets/isaaclab_assets/robots/so_100.py index dd0da9919e9..1e0daf4aa49 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/so_100.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/so_100.py @@ -68,4 +68,4 @@ }, soft_joint_pos_limit_factor=1.0, ) -"""Configuration of SO‑100 robot arm.""" \ No newline at end of file +"""Configuration of SO‑100 robot arm.""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py index 72e53a69b21..5ad6cc07bbe 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env.py @@ -139,4 +139,4 @@ def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict signals = dict() subtask_terms = self.obs_buf["subtask_terms"] signals["grasp_1"] = subtask_terms["grasp_1"][env_ids] - return signals \ No newline at end of file + return signals diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py index 7156badf604..2c5e1452fa8 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/stack_so100_mimic_env_cfg.py @@ -11,7 +11,9 @@ from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig from isaaclab.utils import configclass -from isaaclab_tasks.manager_based.manipulation.stack.config.so_100.stack_pink_ik_abs_visuomotor_env_cfg import SO100CubeStackPinkIKAbsVisuomotorEnvCfg +from isaaclab_tasks.manager_based.manipulation.stack.config.so_100.stack_pink_ik_abs_visuomotor_env_cfg import ( + SO100CubeStackPinkIKAbsVisuomotorEnvCfg, +) @configclass @@ -40,7 +42,7 @@ def __post_init__(self): # The following are the subtask configurations for the stack task. # For SO100, we only have one "gripper" arm, so all subtasks are for this single arm subtask_configs = [] - + # Subtask 1: Pick up cube_2 subtask_configs.append( SubTaskConfig( @@ -67,7 +69,7 @@ def __post_init__(self): apply_noise_during_interpolation=False, ) ) - + # Subtask 2: Place cube_2 on cube_1 subtask_configs.append( SubTaskConfig( @@ -91,6 +93,6 @@ def __post_init__(self): apply_noise_during_interpolation=False, ) ) - + # For SO100, we only have one arm "gripper", so we assign all subtasks to it - self.subtask_configs["gripper"] = subtask_configs \ No newline at end of file + self.subtask_configs["gripper"] = subtask_configs diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py index 11f4c8f0a93..6988d4bd5de 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/__init__.py @@ -5,10 +5,13 @@ import gymnasium as gym -from . import stack_joint_pos_env_cfg -from . import stack_pink_ik_rel_env_cfg -from . import stack_pink_ik_rel_visuomotor_env_cfg -from . import stack_ik_rel_env_cfg +from . import ( + stack_ik_rel_env_cfg, + stack_joint_pos_env_cfg, + stack_pink_ik_rel_env_cfg, + stack_pink_ik_rel_visuomotor_env_cfg, +) + ## # Register Gym environments. ## @@ -45,4 +48,4 @@ "env_cfg_entry_point": stack_pink_ik_rel_visuomotor_env_cfg.SO100CubeStackPinkIKRelVisuomotorEnvCfg, }, disable_env_checker=True, -) \ No newline at end of file +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py index 54af8b69c63..d664edb04be 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_joint_pos_env_cfg.py @@ -3,7 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -import importlib from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import SceneEntityCfg @@ -15,8 +14,8 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab_tasks.manager_based.manipulation.stack import mdp -from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import StackEnvCfg from isaaclab_tasks.manager_based.manipulation.stack.mdp import so100_stack_events +from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import StackEnvCfg ## # Pre-defined configs @@ -24,8 +23,6 @@ from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip from isaaclab_assets.robots.so_100 import SO_100_CFG # isort: skip -import torch - @configclass class EventCfg: @@ -62,20 +59,22 @@ def __post_init__(self): self.events = EventCfg() # Set SO-100 as robot - self.scene.robot = SO_100_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot", - init_state=ArticulationCfg.InitialStateCfg( - pos=(0, 0, 0.0), - rot=(0.7071, 0, 0, 0.7071), - joint_pos={ - "shoulder_pan": 0.0, - "shoulder_lift": 1.5708, - "elbow_flex": -1.5708, - "wrist_flex": 1.0, - "wrist_roll": 0.0, - "gripper": 0.0, - }, - joint_vel={".*": 0.0}, - )) + self.scene.robot = SO_100_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0, 0, 0.0), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + "shoulder_pan": 0.0, + "shoulder_lift": 1.5708, + "elbow_flex": -1.5708, + "wrist_flex": 1.0, + "wrist_roll": 0.0, + "gripper": 0.0, + }, + joint_vel={".*": 0.0}, + ), + ) self.scene.robot.spawn.semantic_tags = [("class", "robot")] # Add semantics to table @@ -86,7 +85,10 @@ def __post_init__(self): # Set actions for the specific robot type (SO-100) self.actions.arm_action = mdp.JointPositionActionCfg( - asset_name="robot", joint_names=["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], scale=1.0, use_default_offset=False + asset_name="robot", + joint_names=["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], + scale=1.0, + use_default_offset=False, ) # Use the last joint as the gripper action diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py index b59c70e760c..34b2d39a977 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_env_cfg.py @@ -2,27 +2,22 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab.assets import ArticulationCfg -from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg +import tempfile + +from pink.tasks import FrameTask import isaaclab.controllers.utils as ControllerUtils -from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +import isaaclab.envs.mdp as base_mdp +from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import SceneEntityCfg from isaaclab.utils import configclass -from pink.tasks import FrameTask - -import isaaclab.envs.mdp as base_mdp from ... import mdp - - from . import stack_joint_pos_env_cfg -import tempfile - @configclass class ObservationsCfg: @@ -46,7 +41,6 @@ def __post_init__(self): self.enable_corruption = False self.concatenate_terms = False - @configclass class SubtaskCfg(ObsGroup): """Observations for subtask group.""" @@ -63,6 +57,7 @@ class SubtaskCfg(ObsGroup): def __post_init__(self): self.enable_corruption = False self.concatenate_terms = False + # observation groups policy: PolicyCfg = PolicyCfg() subtask_terms: SubtaskCfg = SubtaskCfg() @@ -121,8 +116,7 @@ def __post_init__(self): ControllerUtils.change_revolute_to_fixed( temp_urdf_output_path, self.actions.arm_action.ik_urdf_fixed_joint_names ) - + # Set the URDF and mesh paths for the IK controller self.actions.arm_action.controller.urdf_path = temp_urdf_output_path self.actions.arm_action.controller.mesh_path = temp_urdf_meshes_output_path - diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py index 8cc2d61c7dc..a0f64715b48 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/so_100/stack_pink_ik_rel_visuomotor_env_cfg.py @@ -3,16 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause +import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils -from isaaclab.sensors import CameraCfg -from isaaclab.utils import configclass -from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import SceneEntityCfg -import isaaclab.envs.mdp as base_mdp +from isaaclab.sensors import CameraCfg +from isaaclab.utils import configclass -from ... import mdp -from .stack_pink_ik_rel_env_cfg import SO100CubeStackPinkIKRelEnvCfg, ObservationsCfg +from .stack_pink_ik_rel_env_cfg import ObservationsCfg, SO100CubeStackPinkIKRelEnvCfg @configclass @@ -24,7 +22,8 @@ class PolicyCfg(ObservationsCfg.PolicyCfg): """Observations for policy group with state values and camera.""" table_cam = ObsTerm( - func=base_mdp.image, params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False} + func=base_mdp.image, + params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False}, ) # observation groups @@ -57,4 +56,3 @@ def __post_init__(self): # Set settings for camera rendering self.rerender_on_reset = True self.sim.render.antialiasing_mode = "OFF" # disable dlss - \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py index 82921fffee7..8d4418653ef 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/so100_stack_events.py @@ -55,7 +55,7 @@ def randomize_single_rigid_objects( ): """ Randomize poses of individual rigid objects (not multi-object rigid bodies). - + This is a version of randomize_rigid_objects_in_focus adapted for single RigidObjects. Unlike the Franka version which handles multi-object rigid bodies with num_objects attribute, this function works with individual RigidObject instances in the scene. @@ -94,4 +94,4 @@ def randomize_single_rigid_objects( ) asset.write_root_velocity_to_sim( torch.zeros(1, 6, device=env.device), env_ids=torch.tensor([cur_env], device=env.device) - ) \ No newline at end of file + )