# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause import math from pathlib import Path from pxr import Usd, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.devices import DevicesCfg from isaaclab.devices.gamepad import Se3GamepadCfg from isaaclab.devices.keyboard import Se3KeyboardCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg 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 RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise # import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG # isort: skip from polaris.environments.robot_cfg import NVIDIA_DROID from isaaclab.markers.config import FRAME_MARKER_CFG from isaaclab.sensors.frame_transformer.frame_transformer_cfg import ( FrameTransformerCfg, OffsetCfg, ) import torch import sim_improvement.environments.mdp as mdp ## # Scene definition ## ## # MDP settings ## class BinaryJointPositionZeroToOneAction(mdp.BinaryJointPositionAction): # override def process_actions(self, actions: torch.Tensor): # store the raw actions self._raw_actions[:] = actions # compute the binary mask if actions.dtype == torch.bool: # true: close, false: open binary_mask = actions == 0 else: # true: close, false: open binary_mask = actions > 0.5 # compute the command self._processed_actions = torch.where( binary_mask, self._close_command, self._open_command ) if self.cfg.clip is not None: self._processed_actions = torch.clamp( self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1], ) @configclass class BinaryJointPositionZeroToOneActionCfg(mdp.BinaryJointPositionActionCfg): """Configuration for the binary joint position action term. See :class:`BinaryJointPositionAction` for more details. """ class_type = BinaryJointPositionZeroToOneAction @configclass class ActionsCfg: """Action specifications for the MDP.""" # Set actions for the specific robot type (franka) arm_action = mdp.DifferentialInverseKinematicsActionCfg( asset_name="robot", joint_names=["panda_joint.*"], body_name="base_link", # body_name="panda_hand", # controller=mdp.DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), controller=mdp.DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), body_offset=mdp.DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=(0.0, 0.0, 0.107)), ) gripper_action = BinaryJointPositionZeroToOneActionCfg( asset_name="robot", joint_names=["finger_joint"], open_command_expr={"finger_joint": 0.0}, close_command_expr={"finger_joint": math.pi / 4}, # joint_names=["panda_finger.*"], # open_command_expr={"panda_finger_.*": 0.04}, # close_command_expr={"panda_finger_.*": 0.0}, ) @configclass class SceneCfg(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, -1.05)), ) light = AssetBaseCfg( prim_path="/World/light", spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), ) # robot: ArticulationCfg = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") robot: ArticulationCfg = NVIDIA_DROID.replace(prim_path="{ENV_REGEX_NS}/Robot") # table = AssetBaseCfg( # prim_path="{ENV_REGEX_NS}/table", # spawn=sim_utils.UsdFileCfg( # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", # ), # init_state=AssetBaseCfg.InitialStateCfg( # pos=(0.55, 0.0, 0.0), rot=(0.70711, 0.0, 0.0, 0.70711) # ), # ) def __post_init__( self, ): marker_cfg = FRAME_MARKER_CFG.copy() marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) marker_cfg.prim_path = "/Visuals/FrameTransformer" self.ee_frame = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/panda_link0", debug_vis=True, visualizer_cfg=marker_cfg, target_frames=[ FrameTransformerCfg.FrameCfg( prim_path="{ENV_REGEX_NS}/Robot/Gripper/Robotiq_2F_85/base_link", # prim_path="{ENV_REGEX_NS}/Robot/panda_hand", name="end_effector", offset=OffsetCfg( pos=(0.0, 0.0, 0.0), ), ), ], ) def dynamic_setup(self, usd_file: str, **kwargs): # environment_path_= Path(environment_path) # environment_path = str(environment_path_.resolve()) environment_path = str(Path(usd_file).resolve()) scene = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/scene", spawn=sim_utils.UsdFileCfg( usd_path=environment_path, activate_contact_sensors=False, ), ) self.scene = scene stage = Usd.Stage.Open(environment_path) scene_prim = stage.GetPrimAtPath("/World") children = scene_prim.GetChildren() for child in children: name = child.GetName() # if its a camera, use the camera pose if UsdPhysics.RigidBodyAPI(child): print(f"Adding rigid object: {name}") pos = child.GetAttribute("xformOp:translate").Get() rot = child.GetAttribute("xformOp:orient").Get() rot = ( rot.GetReal(), rot.GetImaginary()[0], rot.GetImaginary()[1], rot.GetImaginary()[2], ) asset = RigidObjectCfg( prim_path=f"{{ENV_REGEX_NS}}/scene/{name}", spawn=None, init_state=RigidObjectCfg.InitialStateCfg( pos=pos, rot=rot, ), ) setattr(self, name, asset) @configclass class RewardsCfg(): """Reward terms for the PnP environment.""" marker_in_bowl = RewTerm( func=mdp.is_within_xy, weight=50.0, params={ "object1": "marker", "object2": "bowl", "percent_threshold": 0.6, "open_finger_threshold": 0.02 }, ) # object_to_object_distance = RewTerm( # func=mdp.object_to_object_distance, # weight=1.0, # params={ # "std": 0.2, # "object1_cfg": SceneEntityCfg("marker"), # "object2_cfg": SceneEntityCfg("bowl"), # }, # ) # reaching_object = RewTerm( # func=mdp.object_ee_distance, # weight=1.0, # params={ # "std": 0.1, # "object_cfg": SceneEntityCfg("marker"), # "ee_frame_cfg": SceneEntityCfg("ee_frame"), # }, # ) @configclass class EventsCfg(): """Event terms for the PnP environment.""" reset_all = EventTerm( func=mdp.reset_scene_to_default, mode="reset", ) # reset_from_state_dataset = EventTerm( # func=mdp.reset_from_state_dataset("./demonstrations/MarkerInBowl/trajectory.hdf5"), # mode="reset", # ) @configclass class ObservationsCfg: """Observation specifications for the MDP.""" @configclass class PolicyCfg(ObsGroup): """Observations for policy group.""" # observation terms (order preserved) joint_pos = ObsTerm( func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01) ) joint_vel = ObsTerm( func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.01, n_max=0.01) ) actions = ObsTerm(func=mdp.last_action) end_effector_pose = ObsTerm( func=mdp.ee_pose_obs, ) marker_pose = ObsTerm( func = mdp.target_asset_pose_in_root_asset_frame, params = { "root_asset_cfg": SceneEntityCfg("robot"), "target_asset_cfg": SceneEntityCfg("marker"), }, ) bowl_pose = ObsTerm( func = mdp.target_asset_pose_in_root_asset_frame, params = { "root_asset_cfg": SceneEntityCfg("robot"), "target_asset_cfg": SceneEntityCfg("bowl"), }, ) def __post_init__(self): self.enable_corruption = True self.concatenate_terms = True # observation groups policy: PolicyCfg = PolicyCfg() @configclass class TerminationsCfg: """Termination terms for the PnP environment.""" time_out = DoneTerm(func=mdp.time_out, time_out=True) marker_below = DoneTerm( func=mdp.root_height_below_minimum, params={ "asset_cfg": SceneEntityCfg("marker"), "minimum_height": 0.0, }, ) bowl_below = DoneTerm( func=mdp.root_height_below_minimum, params={ "asset_cfg": SceneEntityCfg("bowl"), "minimum_height": 0.0, }, ) @configclass class EnvCfg(ManagerBasedRLEnvCfg): """Configuration for the PnP environment.""" actions = ActionsCfg() scene = SceneCfg(num_envs=4096, env_spacing=2.5) observations = ObservationsCfg() rewards = RewardsCfg() events = EventsCfg() terminations = TerminationsCfg() commands = None curriculum = None def __post_init__(self): """Post initialization.""" # general settings self.decimation = 8 self.sim.render_interval = self.decimation self.episode_length_s = 5.0 self.viewer.eye = (10, 1, 3) self.viewer.lookat = (0, 0, 0) # simulation settings self.sim.dt = 1.0 / (15.0 * self.decimation) self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 self.teleop_devices = DevicesCfg( devices={ "keyboard": Se3KeyboardCfg( gripper_term=False, sim_device=self.sim.device, ), "gamepad": Se3GamepadCfg( gripper_term=False, sim_device=self.sim.device, ), "spacemouse": Se3SpaceMouseCfg( gripper_term=False, sim_device=self.sim.device, ), }, ) def dynamic_setup(self, **kwargs): self.scene.dynamic_setup(**kwargs)