From 8a85df8cd78797baab8c270ed02038d4cc4758e8 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Wed, 23 Apr 2025 23:19:50 +0800 Subject: [PATCH 1/9] =?UTF-8?q?=E7=AE=80=E5=8D=95=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../manipulation/cabinet/cabinet_env_cfg.py | 12 +++++++++++- .../cabinet/config/franka/joint_pos_env_cfg.py | 9 ++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index cfdaa4066f6..753f836d4aa 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -53,11 +53,13 @@ class CabinetSceneCfg(InteractiveSceneCfg): ee_frame: FrameTransformerCfg = MISSING cabinet = ArticulationCfg( + # 设置物体的路径，就是在场景树中相对于环境的路径 prim_path="{ENV_REGEX_NS}/Cabinet", spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Sektion_Cabinet/sektion_cabinet_instanceable.usd", activate_contact_sensors=False, ), + # 设置初始状态 init_state=ArticulationCfg.InitialStateCfg( pos=(0.8, 0, 0.4), rot=(0.0, 0.0, 0.0, 1.0), @@ -68,6 +70,7 @@ class CabinetSceneCfg(InteractiveSceneCfg): "drawer_top_joint": 0.0, }, ), + # 设置内部关节限制 actuators={ "drawers": ImplicitActuatorCfg( joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], @@ -86,14 +89,19 @@ class CabinetSceneCfg(InteractiveSceneCfg): }, ) - # Frame definitions for the cabinet. + # 获取相对于柜子的变动的一些位置姿态（变换关系） cabinet_frame = FrameTransformerCfg( + # 设置父坐标系的xform prim_path="{ENV_REGEX_NS}/Cabinet/sektion", debug_vis=True, visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/CabinetFrameTransformer"), target_frames=[ + # 手柄坐标系的名字还是“drawer_handle_top” FrameTransformerCfg.FrameCfg( + # 手柄坐标系在哪里？它要固定附着在场景中已经存在的参考系，两者“相对位姿固定” + # 在这里，用"{ENV_REGEX_NS}/Cabinet/drawer_handle_top"这个xform来作为附着对象 prim_path="{ENV_REGEX_NS}/Cabinet/drawer_handle_top", + # 手柄的坐标系的名称，它起了一个与prim_path中相同的名字 name="drawer_handle_top", offset=OffsetCfg( pos=(0.305, 0.0, 0.01), @@ -150,6 +158,8 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, ) rel_ee_drawer_distance = ObsTerm(func=mdp.rel_ee_drawer_distance) + + actions = ObsTerm(func=mdp.last_action) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py index a485ffdcf9b..681ff72c51c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py @@ -47,12 +47,15 @@ def __post_init__(self): # IMPORTANT: The order of the frames in the list is important. The first frame is the tool center point (TCP) # the other frames are the fingers self.scene.ee_frame = FrameTransformerCfg( + # 父坐标系 prim_path="{ENV_REGEX_NS}/Robot/panda_link0", debug_vis=False, visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/EndEffectorFrameTransformer"), target_frames=[ FrameTransformerCfg.FrameCfg( + # 新的虚拟坐标系附着在usd中定义好的panda_hand这个xfrom上 prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + # 新的虚拟坐标系名字 name="ee_tcp", offset=OffsetCfg( pos=(0.0, 0.0, 0.1034), @@ -66,9 +69,9 @@ def __post_init__(self): ), ), FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", - name="tool_rightfinger", - offset=OffsetCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + name="tool_rightfinger", + offset=OffsetCfg( pos=(0.0, 0.0, 0.046), ), ), From fe54111ecade3fd89b267212174edad63657b4b8 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Thu, 24 Apr 2025 21:36:22 +0800 Subject: [PATCH 2/9] mimic gen --- .gitignore | 2 ++ .../isaaclab_mimic/generate_dataset.py | 10 +++--- .../isaaclab/isaaclab/envs/mimic_env_cfg.py | 9 ++++-- .../isaaclab_mimic/datagen/data_generator.py | 32 +++++++++++++------ .../isaaclab_mimic/datagen/generation.py | 11 ++++--- .../isaaclab_mimic/datagen/waypoint.py | 6 ++++ .../manipulation/cabinet/cabinet_env_cfg.py | 25 ++++++++++++++- .../cabinet/config/franka/ik_abs_env_cfg.py | 1 + .../cabinet/config/franka/ik_rel_env_cfg.py | 1 + 9 files changed, 74 insertions(+), 23 deletions(-) diff --git a/.gitignore b/.gitignore index ea62f833b86..410ae1b4a54 100644 --- a/.gitignore +++ b/.gitignore @@ -60,3 +60,5 @@ _build # Pre-Trained Checkpoints /.pretrained_checkpoints/ + +*hdf5 diff --git a/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py b/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py index 37e993751f8..45950549ad5 100644 --- a/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py +++ b/scripts/imitation_learning/isaaclab_mimic/generate_dataset.py @@ -63,7 +63,7 @@ def main(): output_dir, output_file_name = setup_output_paths(args_cli.output_file) env_name = args_cli.task or get_env_name_from_dataset(args_cli.input_file) - # Configure environment + # Configure environment env_cfg, success_term = setup_env_config( env_name=env_name, output_dir=output_dir, @@ -87,10 +87,10 @@ def main(): # Setup and run async data generation async_components = setup_async_generation( env=env, - num_envs=args_cli.num_envs, - input_file=args_cli.input_file, - success_term=success_term, - pause_subtask=args_cli.pause_subtask, + num_envs=args_cli.num_envs, # 并发环境数量 + input_file=args_cli.input_file, # 输入数据集文件 + success_term=success_term, # 成功终止条件 + pause_subtask=args_cli.pause_subtask, # 是否在每个子任务后暂停 ) try: diff --git a/source/isaaclab/isaaclab/envs/mimic_env_cfg.py b/source/isaaclab/isaaclab/envs/mimic_env_cfg.py index 0e19613113e..266d725482e 100644 --- a/source/isaaclab/isaaclab/envs/mimic_env_cfg.py +++ b/source/isaaclab/isaaclab/envs/mimic_env_cfg.py @@ -78,16 +78,21 @@ class SubTaskConfig: """ Configuration settings specific to the management of individual subtasks. + IsaacLab认为，一整条轨迹，可以被分解为多个子任务，例如“伸手去抓”，“拿起来”等，类似于 + 从当前的位置执行到下个一离散路点的动作。每个子任务都有一个起始点和终止点，起始点是当前的状态，终止点是下一个。 + + 而这个类，就是对每个子任务片段的配置设置； + """ ############################################################## # Mandatory options that should be defined for every subtask # Reference to the object involved in this subtask, None if no - # object is involved (this is rarely the case). + # object is involved (this is rarely the case).这个子任务涉及的参考物体（用于姿态对齐） object_ref: str = None - # Signal for subtask termination + # Signal for subtask termination 哪个 termination signal 表示子任务完成 subtask_term_signal: str = None ############################################################## diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py index 2681e2d9a6f..6eaa31fd86c 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/data_generator.py @@ -70,7 +70,7 @@ def __init__( def __repr__(self): """ - Pretty print this object. + Pretty print this object.用于调试或打印 """ msg = str(self.__class__.__name__) msg += " (\n\tdataset_path={}\n\tdemo_keys={}\n)".format( @@ -84,6 +84,7 @@ def randomize_subtask_boundaries(self): Apply random offsets to sample subtask boundaries according to the task spec. Recall that each demonstration is segmented into a set of subtask segments, and the end index of each subtask can have a random offset. + 为每个演示的子任务边界应用随机偏移，生成新的子任务分割 """ # initial subtask start and end indices - shape (N, S, 2) @@ -117,17 +118,18 @@ def randomize_subtask_boundaries(self): def select_source_demo( self, - eef_pose, - object_pose, - subtask_ind, - src_subtask_inds, - subtask_object_name, - selection_strategy_name, - selection_strategy_kwargs=None, + eef_pose, # 当前末端执行器位姿 + object_pose, # 当前子任务对象的姿态 + subtask_ind, # 子任务索引，没用到 + src_subtask_inds, # 源参考demo的子任务索引 + subtask_object_name, # 子任务的参考对象名称 + selection_strategy_name, # 选择策略名称（如随机选择） + selection_strategy_kwargs=None, # 所选择策略的额外参数 ): """ + 根据当前状态和策略选择一个源演示，用于指导子任务的轨迹生成。 Helper method to run source subtask segment selection. - + Args: eef_pose (np.array): current end effector pose object_pose (np.array): current object pose for this subtask @@ -204,7 +206,7 @@ async def generate( ): """ Attempt to generate a new demonstration. - + 异步生成一条新的演示轨迹，包含状态、动作和观测数据 Args: env_id (int): environment ID @@ -264,15 +266,21 @@ async def generate( ) # like @generated_src_demo_inds, but padded to align with size of @generated_actions prev_src_demo_datagen_info_pool_size = 0 + + # 遍历每个子任务（一个 episode 可能包含多个子任务） + # 每次处理一个小阶段（如“移动到积木”、“抓积木”） for subtask_ind in range(len(self.subtask_configs)): # some things only happen on first subtask + # 是不是第一个子任务 is_first_subtask = subtask_ind == 0 # name of object for this subtask + # 当前子任务的参考对象名称（类似于路点附着在哪个物体上） subtask_object_name = self.subtask_configs[subtask_ind].object_ref # corresponding current object pose + # 当前子任务的参考对象的当前位姿 cur_object_pose = ( self.env.get_object_poses(env_ids=[env_id])[subtask_object_name][0] if (subtask_object_name is not None) @@ -293,6 +301,8 @@ async def generate( need_source_demo_selection = is_first_subtask or select_src_per_subtask # Run source demo selection or use selected demo from previous iteration + # 在源 demo 池中找到一个适合当前子任务的段落 + # 比如我们要做“抓物体”，那就找历史上也在抓同一个物体的位置段 if need_source_demo_selection: selected_src_demo_ind = self.select_source_demo( eef_pose=self.env.get_robot_eef_pose(eef_name, env_ids=[env_id])[0], @@ -311,6 +321,7 @@ async def generate( # get subtask segment, consisting of the sequence of robot eef poses, target poses, gripper actions src_ep_datagen_info = self.src_demo_datagen_info_pool.datagen_infos[selected_src_demo_ind] + # 从源演示中获取子任务段 src_subtask_eef_poses = src_ep_datagen_info.eef_pose[ selected_src_subtask_inds[0] : selected_src_subtask_inds[1] ] @@ -399,6 +410,7 @@ async def generate( traj_to_execute.pop_first() # Execute the trajectory and collect data. + # 执行轨迹并收集数据 exec_results = await traj_to_execute.execute( env=self.env, env_id=env_id, env_action_queue=env_action_queue, success_term=success_term ) diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py index c8536f885cb..24cfc612c3b 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py @@ -31,7 +31,7 @@ async def run_data_generator( success_term: Any, pause_subtask: bool = False, ): - """Run data generator.""" + """Run data generator.运行数据生成器，异步生成数据并记录生成结果""" global num_success, num_failures, num_attempts while True: results = await data_generator.generate( @@ -175,17 +175,18 @@ def setup_async_generation( List of asyncio tasks for data generation """ asyncio_event_loop = asyncio.get_event_loop() - env_action_queue = asyncio.Queue() - shared_datagen_info_pool_lock = asyncio.Lock() - shared_datagen_info_pool = DataGenInfoPool( + env_action_queue = asyncio.Queue() # 创建异步队列 + shared_datagen_info_pool_lock = asyncio.Lock() # 创建异步锁 + shared_datagen_info_pool = DataGenInfoPool( # 创建数据生成信息池 env.unwrapped, env.unwrapped.cfg, env.unwrapped.device, asyncio_lock=shared_datagen_info_pool_lock ) - shared_datagen_info_pool.load_from_dataset_file(input_file) + shared_datagen_info_pool.load_from_dataset_file(input_file) # 从数据集中加载数据生成信息 print(f"Loaded {shared_datagen_info_pool.num_datagen_infos} to datagen info pool") # Create and schedule data generator tasks data_generator = DataGenerator(env=env.unwrapped, src_demo_datagen_info_pool=shared_datagen_info_pool) data_generator_asyncio_tasks = [] + # 创建n个数据生成器的“运行器”，运行器 包含了环境、用于保存动作序列的容器、生成器、成功条件等 for i in range(num_envs): task = asyncio_event_loop.create_task( run_data_generator(env, i, env_action_queue, data_generator, success_term, pause_subtask=pause_subtask) diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py index faab4af86e7..99afa25d58c 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/waypoint.py @@ -405,8 +405,11 @@ async def execute( play_action = play_action.unsqueeze(0) # Reshape to [1, 7] if env_action_queue is None: + # 如果没有使用异步环境，则直接执行动作 obs, _, _, _, _ = env.step(play_action) else: + # 否则将生成的动作存入队列 + # 以便在异步环境中使用 await env_action_queue.put((env_id, play_action[0])) await env_action_queue.join() obs = env.obs_buf @@ -428,3 +431,6 @@ async def execute( success=success, ) return results + + + diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index 753f836d4aa..d3a4e8bfcf8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -18,6 +18,8 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors import TiledCameraCfg + from isaaclab.sensors.frame_transformer import OffsetCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -52,6 +54,21 @@ class CabinetSceneCfg(InteractiveSceneCfg): # End-effector, Will be populated by agent env cfg ee_frame: FrameTransformerCfg = MISSING + + # add camera to the scene + # tiled_camera: TiledCameraCfg = TiledCameraCfg( + # prim_path="{ENV_REGEX_NS}/Camera", + # offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), + # data_types=["rgb"], + # spawn=sim_utils.PinholeCameraCfg( + # focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + # ), + # width=100, + # height=100, + # ) + + + cabinet = ArticulationCfg( # 设置物体的路径，就是在场景树中相对于环境的路径 prim_path="{ENV_REGEX_NS}/Cabinet", @@ -146,7 +163,13 @@ class ObservationsCfg: @configclass class PolicyCfg(ObsGroup): """Observations for policy group.""" - + # # 声明rgb观测 + # rgb_image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), + # "data_type": "rgb"}) + # # 声明depth图观测 + # depth_image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), + # "data_type": "distance_to_camera"}) + joint_pos = ObsTerm(func=mdp.joint_pos_rel) joint_vel = ObsTerm(func=mdp.joint_vel_rel) cabinet_joint_pos = ObsTerm( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py index b8144a34a7e..030385216ee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_abs_env_cfg.py @@ -26,6 +26,7 @@ def __post_init__(self): self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") # Set actions for the specific robot type (franka) + # 动作依旧是末端执行器的动作，但是动作含义为绝对动作 self.actions.arm_action = DifferentialInverseKinematicsActionCfg( asset_name="robot", joint_names=["panda_joint.*"], diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py index 39f9c23d75d..4da4acfd045 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/ik_rel_env_cfg.py @@ -26,6 +26,7 @@ def __post_init__(self): self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") # Set actions for the specific robot type (franka) + # 动作依旧是末端执行器的动作，但是动作含义为相对上一个时刻的偏移动作 self.actions.arm_action = DifferentialInverseKinematicsActionCfg( asset_name="robot", joint_names=["panda_joint.*"], From bc16bafb8c3226be81c4d11da56387f340ff0c07 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Fri, 25 Apr 2025 00:54:55 +0800 Subject: [PATCH 3/9] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E8=B7=AF?= =?UTF-8?q?=E7=82=B9=E8=A7=82=E6=B5=8B=EF=BC=8C=E5=87=86=E5=A4=87=E8=87=AA?= =?UTF-8?q?=E5=8A=A8=E5=8C=96=E6=89=A7=E8=A1=8C=E8=B7=AF=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/tools/record_demos_auto.py | 289 ++++++++++++++++++ scripts/tutorials/05_controllers/run_osc.py | 19 +- source/isaaclab/isaaclab/utils/assets.py | 4 +- .../cabinet/config/franka/__init__.py | 34 +++ .../cabinet/config/franka/osc_env_cfg.py | 75 +++++ .../manipulation/cabinet/mdp/observations.py | 30 +- 6 files changed, 440 insertions(+), 11 deletions(-) create mode 100644 scripts/tools/record_demos_auto.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py diff --git a/scripts/tools/record_demos_auto.py b/scripts/tools/record_demos_auto.py new file mode 100644 index 00000000000..599bce31f54 --- /dev/null +++ b/scripts/tools/record_demos_auto.py @@ -0,0 +1,289 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to record demonstrations with Isaac Lab environments using human teleoperation. + +This script allows users to record demonstrations operated by human teleoperation for a specified task. +The recorded demonstrations are stored as episodes in a hdf5 file. Users can specify the task, teleoperation +device, dataset directory, and environment stepping rate through command-line arguments. + +required arguments: + --task Name of the task. + +optional arguments: + -h, --help Show this help message and exit + --teleop_device Device for interacting with environment. (default: keyboard) + --dataset_file File path to export recorded demos. (default: "./datasets/dataset.hdf5") + --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) +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse +import os + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Record demonstrations for Isaac Lab environments.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--teleop_device", type=str, default="keyboard", help="Device for interacting with environment.") +parser.add_argument( + "--dataset_file", type=str, default="./datasets/dataset.hdf5", help="File path to export recorded demos." +) +parser.add_argument("--step_hz", type=int, default=30, help="Environment stepping rate in Hz.") +parser.add_argument( + "--num_demos", type=int, default=0, help="Number of demonstrations to record. Set to 0 for infinite." +) +parser.add_argument( + "--num_success_steps", + type=int, + default=10, + help="Number of continuous steps with task success for concluding a demo as successful. Default is 10.", +) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +if args_cli.teleop_device.lower() == "handtracking": + vars(args_cli)["experience"] = f'{os.environ["ISAACLAB_PATH"]}/apps/isaaclab.python.xr.openxr.kit' + +# launch the simulator +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import contextlib +import gymnasium as gym +import time +import torch + +import omni.log + +from isaaclab.devices import Se3HandTracking, Se3Keyboard, Se3SpaceMouse +from isaaclab.envs import ViewerCfg +from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg +from isaaclab.envs.ui import ViewportCameraController + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + + +class RateLimiter: + """Convenience class for enforcing rates in loops.""" + + def __init__(self, hz): + """ + Args: + hz (int): frequency to enforce + """ + self.hz = hz + self.last_time = time.time() + self.sleep_duration = 1.0 / hz + self.render_period = min(0.033, self.sleep_duration) + + def sleep(self, env): + """Attempt to sleep at the specified rate in hz.""" + next_wakeup_time = self.last_time + self.sleep_duration + while time.time() < next_wakeup_time: + time.sleep(self.render_period) + env.sim.render() + + self.last_time = self.last_time + self.sleep_duration + + # detect time jumping forwards (e.g. loop is too slow) + if self.last_time < time.time(): + while self.last_time < time.time(): + self.last_time += self.sleep_duration + + +def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torch.Tensor: + """Pre-process actions for the environment.""" + # compute actions based on environment + if "Reach" in args_cli.task: + # note: reach is the only one that uses a different action space + # compute actions + return delta_pose + else: + # resolve gripper command + gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=delta_pose.device) + gripper_vel[:] = -1 if gripper_command else 1 + # compute actions + return torch.concat([delta_pose, gripper_vel], dim=1) + + +def distance_below_threshold(current_pos, desired_pos, threshold: float) -> bool: + return torch.norm(current_pos - desired_pos) < threshold + + +def main(): + """Collect demonstrations from the environment using teleop interfaces.""" + + # if handtracking is selected, rate limiting is achieved via OpenXR + if args_cli.teleop_device.lower() == "handtracking": + rate_limiter = None + else: + rate_limiter = RateLimiter(args_cli.step_hz) + + # get directory path and file name (without extension) from cli arguments + output_dir = os.path.dirname(args_cli.dataset_file) + output_file_name = os.path.splitext(os.path.basename(args_cli.dataset_file))[0] + + # create directory if it does not exist + if not os.path.exists(output_dir): + os.makedirs(output_dir) + + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1) + env_cfg.env_name = args_cli.task + + # extract success checking function to invoke in the main loop + success_term = None + if hasattr(env_cfg.terminations, "success"): + success_term = env_cfg.terminations.success + env_cfg.terminations.success = None + else: + omni.log.warn( + "No success termination term was found in the environment." + " Will not be able to mark recorded demos as successful." + ) + + # modify configuration such that the environment runs indefinitely until + # the goal is reached or other termination conditions are met + env_cfg.terminations.time_out = None + + env_cfg.observations.policy.concatenate_terms = False + + env_cfg.recorders: ActionStateRecorderManagerCfg = ActionStateRecorderManagerCfg() + env_cfg.recorders.dataset_export_dir_path = output_dir + env_cfg.recorders.dataset_filename = output_file_name + + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + + # add teleoperation key for reset current recording instance + should_reset_recording_instance = False + + def reset_recording_instance(): + nonlocal should_reset_recording_instance + should_reset_recording_instance = True + + # create controller + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) + elif args_cli.teleop_device.lower() == "handtracking": + from isaacsim.xr.openxr import OpenXRSpec + + teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) + teleop_interface.add_callback("RESET", reset_recording_instance) + viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") + ViewportCameraController(env, viewer) + else: + raise ValueError( + f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'." + ) + + teleop_interface.add_callback("R", reset_recording_instance) + print(teleop_interface) + + # reset before starting + env.reset() + teleop_interface.reset() + + + # Define targets for the arm + ee_goal_pose_set_tilted_b = torch.tensor( + [ + [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], + ], + device=env.device + ) + ee_goal_wrench_set_tilted_task = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + ], + device=env.device, + ) + kp_set_task = torch.tensor( + [ + [360.0, 360.0, 360.0, 360.0, 360.0, 360.0], + [420.0, 420.0, 420.0, 420.0, 420.0, 420.0], + [320.0, 320.0, 320.0, 320.0, 320.0, 320.0], + ], + device=env.device, + ) + ee_target_set = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task, kp_set_task], dim=-1) + + + # simulate environment -- run everything in inference mode + current_recorded_demo_count = 0 + success_step_count = 0 + with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): + while True: + # get keyboard command + #delta_pose, gripper_command = teleop_interface.advance() + target_pose, gripper_command = teleop_interface.advance() + # convert to torch + target_pose = torch.tensor(target_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1) + # compute actions based on environment + actions = pre_process_actions(target_pose, gripper_command) + + # perform action on environment + #env.step(actions) + + if success_term is not None: + if bool(success_term.func(env, **success_term.params)[0]): + success_step_count += 1 + if success_step_count >= args_cli.num_success_steps: + env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) + env.recorder_manager.set_success_to_episodes( + [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) + ) + env.recorder_manager.export_episodes([0]) + should_reset_recording_instance = True + else: + success_step_count = 0 + + if should_reset_recording_instance: + env.recorder_manager.reset() + env.reset() + should_reset_recording_instance = False + success_step_count = 0 + + # print out the current demo count if it has changed + if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: + current_recorded_demo_count = env.recorder_manager.exported_successful_episode_count + print(f"Recorded {current_recorded_demo_count} successful demonstrations.") + + if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos: + print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.") + break + + # check that simulation is stopped or not + if env.sim.is_stopped(): + break + + if rate_limiter: + rate_limiter.sleep(env) + + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/tutorials/05_controllers/run_osc.py b/scripts/tutorials/05_controllers/run_osc.py index fada4620905..e27255f5e6f 100644 --- a/scripts/tutorials/05_controllers/run_osc.py +++ b/scripts/tutorials/05_controllers/run_osc.py @@ -125,16 +125,17 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Create the OSC osc_cfg = OperationalSpaceControllerCfg( - target_types=["pose_abs", "wrench_abs"], - impedance_mode="variable_kp", - inertial_dynamics_decoupling=True, + target_types=["pose_abs", # 相对于机器人基坐标系的绝对位姿 + "wrench_abs"], # 控制末端施加的力/力矩（也相对于 base） + impedance_mode="variable_kp", # 可变刚度模式（variable stiffness） + inertial_dynamics_decoupling=True, # 启用 惯性解耦 partial_inertial_dynamics_decoupling=False, - gravity_compensation=False, - motion_damping_ratio_task=1.0, - contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], - motion_control_axes_task=[1, 1, 0, 1, 1, 1], - contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], - nullspace_control="position", + gravity_compensation=False, # 不补偿重力 + motion_damping_ratio_task=1.0, # 运动阻尼比，设置为1.0 表示临界阻尼，在执行任务时，机器人不会震荡或过冲 + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], # 力控制方向上的“刚度” 6D 向量（x, y, z, roll, pitch, yaw），仅对 z方向的力 设置了非零刚度 0.1（其他方向是软的） + motion_control_axes_task=[1, 1, 0, 1, 1, 1], # 允许末端哪些自由度运动（x, y, z, roll, pitch, yaw），只让末端 在 xy 平面内自由移动并控制朝向，不允许末端 z 方向乱动（比如浮起来） + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], # 控制末端哪些自由度的力，这里只控制 z 方向的末端力（比如下压） + nullspace_control="position",# 控制多余自由度的方式（Nullspace control），这里使用位置控制去处理 nullspace，比如让手臂靠中、躲避障碍等 ) osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device) diff --git a/source/isaaclab/isaaclab/utils/assets.py b/source/isaaclab/isaaclab/utils/assets.py index 1112902e08d..e5c6c068fa7 100644 --- a/source/isaaclab/isaaclab/utils/assets.py +++ b/source/isaaclab/isaaclab/utils/assets.py @@ -21,7 +21,9 @@ import carb import omni.client -NUCLEUS_ASSET_ROOT_DIR = carb.settings.get_settings().get("/persistent/isaac/asset_root/cloud") +# NUCLEUS_ASSET_ROOT_DIR = carb.settings.get_settings().get("/persistent/isaac/asset_root/cloud") + +NUCLEUS_ASSET_ROOT_DIR = "/home/wgk/isaacsim_assets/Assets/Isaac/4.5/" """Path to the root directory on the Nucleus Server.""" NVIDIA_NUCLEUS_DIR = f"{NUCLEUS_ASSET_ROOT_DIR}/NVIDIA" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py index 980500ed06a..729c34bf28f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/__init__.py @@ -44,6 +44,8 @@ # Inverse Kinematics - Absolute Pose Control ## +# 动作是ee位置的绝对动作，但是只能进行小范围变化，用微分动力学 求解 + gym.register( id="Isaac-Open-Drawer-Franka-IK-Abs-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", @@ -57,6 +59,7 @@ # Inverse Kinematics - Relative Pose Control ## +# 动作是ee位置 的 delta动作， 用微分动力学 求解 gym.register( id="Isaac-Open-Drawer-Franka-IK-Rel-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", @@ -65,3 +68,34 @@ }, disable_env_checker=True, ) + + + +## +# Inverse Kinematics - Operational Space Control +## +# 新添加了笛卡尔空间的动作 +# 动作是末端执行器的笛卡尔空间动作 + +gym.register( + id="Isaac-Open-Drawer-Franka-OSC-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaCabinetEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Operational Space Control +## + +# 动作是末端执行器的笛卡尔空间动作 +gym.register( + id="Isaac-Open-Drawer-Franka-OSC-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaCabinetEnvCfg_PLAY", + }, + disable_env_checker=True, +) \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py new file mode 100644 index 00000000000..5837bdc827f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py @@ -0,0 +1,75 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.operational_space_cfg import OperationalSpaceControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaCabinetEnvCfg(joint_pos_env_cfg.FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We remove stiffness and damping for the shoulder and forearm joints for effort control + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["panda_shoulder"].stiffness = 0.0 + self.scene.robot.actuators["panda_shoulder"].damping = 0.0 + self.scene.robot.actuators["panda_forearm"].stiffness = 0.0 + self.scene.robot.actuators["panda_forearm"].damping = 0.0 + self.scene.robot.spawn.rigid_props.disable_gravity = True + + # If closed-loop contact force control is desired, contact sensors should be enabled for the robot + # self.scene.robot.spawn.activate_contact_sensors = True + + self.actions.arm_action = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + # If a task frame different from articulation root/base is desired, a RigidObject, e.g., "task_frame", + # can be added to the scene and its relative path could provided as task_frame_rel_path + # task_frame_rel_path="task_frame", + controller_cfg=OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=True, # 开启重力补偿 + motion_stiffness_task=100.0, + motion_damping_ratio_task=1.0, + motion_stiffness_limits_task=(50.0, 200.0), + nullspace_control="position", + ), + nullspace_joint_pos_target="center", + position_scale=1.0, + orientation_scale=1.0, + stiffness_scale=100.0, + ) + # Removing these observations as they are not needed for OSC and we want keep the observation space small + self.observations.policy.joint_pos = None + self.observations.policy.joint_vel = None + + + + +@configclass +class FrankaCabinetEnvCfg_PLAY(FrankaCabinetEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 16 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index 5599adb1203..80ff5b702c9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -9,7 +9,9 @@ from typing import TYPE_CHECKING import isaaclab.utils.math as math_utils -from isaaclab.assets import ArticulationData +from isaaclab.assets import ArticulationData,RigidObject +from isaaclab.managers import SceneEntityCfg + from isaaclab.sensors import FrameTransformerData if TYPE_CHECKING: @@ -57,3 +59,29 @@ def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tens ee_quat = ee_tf_data.target_quat_w[..., 0, :] # make first element of quaternion positive return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat + + +def waypoints(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> tuple: + """ + waypoint_idx_gripperaction + """ + rigid_objects = env.scene.rigid_objects + gripper_actions = [] + waypoint_poses = [] + for rigid_obj_name in rigid_objects.keys(): + if "waypoint" in rigid_obj_name: + waypoint: RigidObject = rigid_objects[rigid_obj_name] + waypoint_poses.append(waypoint.data.body_state_w[..., :7]) + + segments = rigid_obj_name.split("_") + if len(segments) > 2: + gripper_actions.append(segments[-1]) + else: + gripper_actions.append("None") + + return waypoint_poses,gripper_actions + + + + + From 7ba97e878611b94dbdc66aa001a0f133c5e55961 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Sat, 26 Apr 2025 00:36:29 +0800 Subject: [PATCH 4/9] =?UTF-8?q?=E5=AE=8C=E6=88=90=E5=9F=BA=E6=9C=AC?= =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E6=95=B4=E4=BD=93=E8=B7=91=E9=80=9A?= =?UTF-8?q?=EF=BC=8C=E4=BD=86=E6=98=AF=E8=BF=98=E9=9C=80debug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/tools/record_demos_auto.py | 245 +++++++++++------- .../manipulation/cabinet/cabinet_env_cfg.py | 9 +- .../cabinet/config/franka/osc_env_cfg.py | 8 + .../manipulation/cabinet/mdp/observations.py | 117 +++++++-- 4 files changed, 274 insertions(+), 105 deletions(-) diff --git a/scripts/tools/record_demos_auto.py b/scripts/tools/record_demos_auto.py index 599bce31f54..a187ce153a3 100644 --- a/scripts/tools/record_demos_auto.py +++ b/scripts/tools/record_demos_auto.py @@ -68,7 +68,7 @@ import omni.log from isaaclab.devices import Se3HandTracking, Se3Keyboard, Se3SpaceMouse -from isaaclab.envs import ViewerCfg +from isaaclab.envs import ViewerCfg,ManagerBasedRLEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg from isaaclab.envs.ui import ViewportCameraController @@ -104,25 +104,145 @@ def sleep(self, env): self.last_time += self.sleep_duration -def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torch.Tensor: +def pre_process_actions(arm_action: torch.Tensor, gripper_command: bool) -> torch.Tensor: """Pre-process actions for the environment.""" # compute actions based on environment if "Reach" in args_cli.task: # note: reach is the only one that uses a different action space # compute actions - return delta_pose + return arm_action else: # resolve gripper command - gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=delta_pose.device) + gripper_vel = torch.zeros((arm_action.shape[0], 1), dtype=torch.float, device=arm_action.device) gripper_vel[:] = -1 if gripper_command else 1 # compute actions - return torch.concat([delta_pose, gripper_vel], dim=1) + return torch.concat([arm_action, gripper_vel], dim=1) def distance_below_threshold(current_pos, desired_pos, threshold: float) -> bool: return torch.norm(current_pos - desired_pos) < threshold + +def get_waypoints(env:ManagerBasedRLEnv): + """从场景中找到其中设定的路径点位置""" + raw_waypoint_states = env.obs_buf["policy"]["waypoint_states"] + waypoint_poses = raw_waypoint_states[:, :7] + waypoint_gripper_actions = raw_waypoint_states[:, 7:] + return waypoint_poses, waypoint_gripper_actions + + +def gen_actions(env:ManagerBasedRLEnv): + """将路点转换为末端执行器(ee)对应要求的任务空间的动作""" + + # 以观测的形式获取场景中定义的路点的位置以及夹爪动作命令 + waypoint_poses, gripper_actions = get_waypoints(env) + + # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 + ee_goal_wrench_set_tilted_task = torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + device=env.device).repeat(waypoint_poses.shape[0], 1) + + # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 + kp_set_task = torch.tensor([420.0, 420.0, 420.0, 420.0, 420.0, 420.0], + device=env.device).repeat(waypoint_poses.shape[0], 1) + + arm_actions = torch.cat([waypoint_poses, kp_set_task], dim=-1) + # gripper 动作命令， 0： 关闭 1： 打开 -1： 不动 + gripper_commands = gripper_actions[:, 0] + return arm_actions, gripper_commands + + + +def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, + gripper_command: torch.Tensor, success_term=None, rate_limiter=None): + """执行单次路点动作，包含ee动作和夹爪动作""" + # + should_reset_recording_instance = False + success_step_count = 0 + # convert to torch + arm_action = torch.tensor(arm_action.clone().detach(), dtype=torch.float, device=env.device).repeat(env.num_envs, 1) + # 夹爪动作置为false, 在执行arm动作时不执行夹爪动作 + ee_action = pre_process_actions(arm_action, False) + # 先判断一下当前状态 + current_gripper_state = env.obs_buf["policy"]["gripper_state"] + + if current_gripper_state == gripper_command or gripper_command == -1: + bool_gripper_command = False + else: + bool_gripper_command = True + + + + gripper_action = pre_process_actions(arm_action, bool_gripper_command) + + # 先执行ee动作,夹爪保持不变 + #while True: # 当夹爪还没有到达目标位置时，不停循环执行动作 + for _ in range(100): + + # perform action on environment + env.step(ee_action) + # 计算当前末端执行器的位姿 + current_ee_pos = env.scene + + # 判断是否成功 + if success_term is not None: + if bool(success_term.func(env, **success_term.params)[0]): + success_step_count += 1 + if success_step_count >= args_cli.num_success_steps: + env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) + env.recorder_manager.set_success_to_episodes( + [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) + ) + env.recorder_manager.export_episodes([0]) + should_reset_recording_instance = True + else: + success_step_count = 0 + + + # TODO: 这里需要检查当前末端执行器的位姿是否到达了目标位置 + if env.sim.is_stopped() or should_reset_recording_instance: + break + # + if rate_limiter: + rate_limiter.sleep(env) + + + # 再执行gripper动作 + if gripper_command and not should_reset_recording_instance: + for _ in range(10): + # perform gripper action on environment + env.step(gripper_action) + + # 判断是否成功 + if success_term is not None: + if bool(success_term.func(env, **success_term.params)[0]): + success_step_count += 1 + if success_step_count >= args_cli.num_success_steps: + env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) + env.recorder_manager.set_success_to_episodes( + [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) + ) + env.recorder_manager.export_episodes([0]) + should_reset_recording_instance = True + else: + success_step_count = 0 + # 检查是否打断 + if env.sim.is_stopped() or should_reset_recording_instance: + break + + if rate_limiter: + rate_limiter.sleep(env) + + + return should_reset_recording_instance + + + + + + + + def main(): """Collect demonstrations from the environment using teleop interfaces.""" @@ -198,89 +318,40 @@ def reset_recording_instance(): # reset before starting env.reset() teleop_interface.reset() - - - # Define targets for the arm - ee_goal_pose_set_tilted_b = torch.tensor( - [ - [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], - [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], - [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], - ], - device=env.device - ) - ee_goal_wrench_set_tilted_task = torch.tensor( - [ - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], - ], - device=env.device, - ) - kp_set_task = torch.tensor( - [ - [360.0, 360.0, 360.0, 360.0, 360.0, 360.0], - [420.0, 420.0, 420.0, 420.0, 420.0, 420.0], - [320.0, 320.0, 320.0, 320.0, 320.0, 320.0], - ], - device=env.device, - ) - ee_target_set = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task, kp_set_task], dim=-1) - - - # simulate environment -- run everything in inference mode + current_recorded_demo_count = 0 - success_step_count = 0 - with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): - while True: - # get keyboard command - #delta_pose, gripper_command = teleop_interface.advance() - target_pose, gripper_command = teleop_interface.advance() - # convert to torch - target_pose = torch.tensor(target_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1) - # compute actions based on environment - actions = pre_process_actions(target_pose, gripper_command) - - # perform action on environment - #env.step(actions) - - if success_term is not None: - if bool(success_term.func(env, **success_term.params)[0]): - success_step_count += 1 - if success_step_count >= args_cli.num_success_steps: - env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) - env.recorder_manager.set_success_to_episodes( - [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) - ) - env.recorder_manager.export_episodes([0]) - should_reset_recording_instance = True - else: - success_step_count = 0 - - if should_reset_recording_instance: - env.recorder_manager.reset() - env.reset() - should_reset_recording_instance = False - success_step_count = 0 - - # print out the current demo count if it has changed - if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: - current_recorded_demo_count = env.recorder_manager.exported_successful_episode_count - print(f"Recorded {current_recorded_demo_count} successful demonstrations.") - - if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos: - print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.") - break - - # check that simulation is stopped or not - if env.sim.is_stopped(): - break - - if rate_limiter: - rate_limiter.sleep(env) - - env.close() - + + # 一直自动生成，直到到达指定的成功demo数量 + while current_recorded_demo_count < args_cli.num_demos: + + # 获取当前回合场景中的路点以及对应的夹爪动作 + arm_actions, gripper_commands = gen_actions(env) + + for waypoint_idx in range(arm_actions.shape[0]): + # 执行动作 + should_reset_recording_instance = execute_action(env, arm_actions[waypoint_idx], + gripper_commands[waypoint_idx], + success_term=success_term, + rate_limiter=rate_limiter) + # if should_reset_recording_instance: + # env.recorder_manager.reset() + # env.reset() + # # 退出当前回合 + # break + + # 执行完一个回合的所有路点之后，打印出当前完成的demo数量 + if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: + current_recorded_demo_count = env.recorder_manager.exported_successful_episode_count + print(f"Recorded {current_recorded_demo_count} successful demonstrations.") + + env.recorder_manager.reset() + env.reset() + + # 完成所有的demo之后退出环境 + print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.") + env.close() + + if __name__ == "__main__": # run the main function diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index d3a4e8bfcf8..e672deaf739 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -15,6 +15,7 @@ from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import ActionTermCfg as ActionTerm from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import FrameTransformerCfg @@ -152,7 +153,7 @@ class CabinetSceneCfg(InteractiveSceneCfg): class ActionsCfg: """Action specifications for the MDP.""" - arm_action: mdp.JointPositionActionCfg = MISSING + arm_action: ActionTerm = MISSING gripper_action: mdp.BinaryJointPositionActionCfg = MISSING @@ -185,6 +186,12 @@ class PolicyCfg(ObsGroup): actions = ObsTerm(func=mdp.last_action) + + # 占位，不一定用 + waypoint_states: ObsTerm = MISSING + # 补充一个gripper的观测，用来判断当前的状态, state = 1 打开，否则是关闭状态 + gripper_state = ObsTerm(func=mdp.gripper_state) + def __post_init__(self): self.enable_corruption = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py index 5837bdc827f..7fbb81ee499 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py @@ -7,7 +7,11 @@ from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg from isaaclab.utils import configclass +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg + from . import joint_pos_env_cfg +from ... import mdp ## # Pre-defined configs @@ -60,6 +64,10 @@ def __post_init__(self): self.observations.policy.joint_pos = None self.observations.policy.joint_vel = None + # 为这种任务空间的形式 添加路点观测 + self.observations.policy.waypoint_states = ObsTerm(func=mdp.waypoints, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index 80ff5b702c9..d8d6d3e129a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -7,11 +7,15 @@ import torch from typing import TYPE_CHECKING +from pxr import UsdGeom import isaaclab.utils.math as math_utils -from isaaclab.assets import ArticulationData,RigidObject +from isaaclab.assets import ArticulationData,RigidObject,Articulation from isaaclab.managers import SceneEntityCfg +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils + from isaaclab.sensors import FrameTransformerData if TYPE_CHECKING: @@ -61,26 +65,105 @@ def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tens return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat -def waypoints(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> tuple: +def gripper_pos(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + robot: Articulation = env.scene[robot_cfg.name] + finger_joint_1 = robot.data.joint_pos[:, -1].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, -2].clone().unsqueeze(1) + + return torch.cat((finger_joint_1, finger_joint_2), dim=1) + +def gripper_state(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + robot: Articulation = env.scene[robot_cfg.name] + finger_joint_1 = robot.data.joint_pos[:, -1].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, -2].clone().unsqueeze(1) + + threshold = 0.075 + is_open = torch.norm((finger_joint_1 - finger_joint_2), dim=1) >= threshold + + return is_open.float() + + +def waypoints(env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + referance: str = "root") -> torch.Tensor: """ waypoint_idx_gripperaction + + 收集场景中所有的路点，并选择是否将其转换为相对于机器人底座坐标系 + 夹爪动作： + - 1.0: open + - 0.0: close + - -1.0: 不动 + Returns: + A tensor of shape (N, 8), where each row = [pose(7), gripper_action] """ - rigid_objects = env.scene.rigid_objects - gripper_actions = [] - waypoint_poses = [] - for rigid_obj_name in rigid_objects.keys(): - if "waypoint" in rigid_obj_name: - waypoint: RigidObject = rigid_objects[rigid_obj_name] - waypoint_poses.append(waypoint.data.body_state_w[..., :7]) - - segments = rigid_obj_name.split("_") - if len(segments) > 2: - gripper_actions.append(segments[-1]) + def find_all_waypoint_paths(max_depth=6): + all_paths = [] + for depth in range(1, max_depth + 1): + if depth == 1: + pattern = "/waypoint_.*" else: - gripper_actions.append("None") - - return waypoint_poses,gripper_actions - + prefix = "/".join([".*?"] * (depth - 1)) + pattern = f"/{prefix}/waypoint_.*" + matches = prim_utils.find_matching_prim_paths(pattern) + all_paths.extend(matches) + return list(set(all_paths)) # 去重 + + + # 这一行可以手动刷新一下场景树 + stage = stage_utils.get_current_stage() + # 获取所有的路点节点路径 + all_waypoint_paths = find_all_waypoint_paths(max_depth=10) + + waypoint_states = [] + + asset: Articulation = env.scene[asset_cfg.name] + # (1,3), (1,4) + root_link_pos , root_link_quat = asset.data.root_state_w[..., :3],asset.data.root_state_w[..., 3:7] + + waypoint_states = torch.empty(len(all_waypoint_paths), 8, device=env.device) + + for path in all_waypoint_paths: + waypoint_name = path.split("/")[-1] # 使用 path，而非 rigid_obj_name + segments = waypoint_name.split("_") + waypoint_idx = int(segments[1]) # 路点索引 + + # 获取到路点的prim + waypoint_prim = UsdGeom.Xformable(stage.GetPrimAtPath(path)) + world_transform = waypoint_prim.ComputeLocalToWorldTransform(0) + world_pos = list(world_transform.ExtractTranslation()) + world_quat = world_transform.ExtractRotationQuat() + + world_quat = [world_quat.GetReal(),*world_quat.GetImaginary()] + # (7,) + waypoint_pose = torch.tensor(world_pos + world_quat, dtype=torch.float32, device=env.device) + + # 变换到以root坐标系为参考系 + if referance == "root": + # 1. 四元数变换（旋转变换到 root 坐标系） + rel_quat = math_utils.quat_mul( + math_utils.quat_inv(root_link_quat), + waypoint_pose[None,3:7] + ) + # 2. 平移向量变换（位置减去 root） + rel_pos = waypoint_pose[:3] - root_link_pos + + # 拼接为新的 pose + waypoint_pose = torch.cat([rel_pos, rel_quat], dim=-1).squeeze(0) + + + # gripper flag: open=1.0, close=0.0, default=-1.0 不动 + if len(segments) > 2: + gripper_flag = 1.0 if segments[-1] == "open" else 0.0 + else: + gripper_flag = -1.0 + + gripper_tensor = torch.tensor([gripper_flag], device=waypoint_pose.device) + full_state = torch.cat([waypoint_pose, gripper_tensor], dim=-1) + waypoint_states[waypoint_idx,:] = full_state + + + return waypoint_states From f88e7c374272a6b83ac2e78dabc89fa7928e9fef Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Sat, 26 Apr 2025 22:18:45 +0800 Subject: [PATCH 5/9] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86=E6=8C=89?= =?UTF-8?q?=E7=85=A7=E9=A2=84=E8=AE=BE=E8=B7=AF=E7=82=B9=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E6=92=AD=E6=94=BE=E8=B7=AF=E5=BE=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/tools/record_demos_auto.py | 102 +++++++++++------- .../manipulation/cabinet/cabinet_env_cfg.py | 36 +++++-- .../config/franka/joint_pos_env_cfg.py | 11 +- .../cabinet/config/franka/osc_env_cfg.py | 5 +- .../manipulation/cabinet/mdp/observations.py | 65 ++++++++--- 5 files changed, 154 insertions(+), 65 deletions(-) diff --git a/scripts/tools/record_demos_auto.py b/scripts/tools/record_demos_auto.py index a187ce153a3..c10de584299 100644 --- a/scripts/tools/record_demos_auto.py +++ b/scripts/tools/record_demos_auto.py @@ -71,6 +71,8 @@ from isaaclab.envs import ViewerCfg,ManagerBasedRLEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg from isaaclab.envs.ui import ViewportCameraController +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg @@ -104,8 +106,10 @@ def sleep(self, env): self.last_time += self.sleep_duration -def pre_process_actions(arm_action: torch.Tensor, gripper_command: bool) -> torch.Tensor: - """Pre-process actions for the environment.""" +def pre_process_actions(arm_action: torch.Tensor, open_gripper: bool) -> torch.Tensor: + """Pre-process actions for the environment. + gripper_command: True 开 False 关 + """ # compute actions based on environment if "Reach" in args_cli.task: # note: reach is the only one that uses a different action space @@ -114,7 +118,7 @@ def pre_process_actions(arm_action: torch.Tensor, gripper_command: bool) -> torc else: # resolve gripper command gripper_vel = torch.zeros((arm_action.shape[0], 1), dtype=torch.float, device=arm_action.device) - gripper_vel[:] = -1 if gripper_command else 1 + gripper_vel[:] = 1 if open_gripper else -1 # compute actions return torch.concat([arm_action, gripper_vel], dim=1) @@ -126,69 +130,73 @@ def distance_below_threshold(current_pos, desired_pos, threshold: float) -> bool def get_waypoints(env:ManagerBasedRLEnv): """从场景中找到其中设定的路径点位置""" - raw_waypoint_states = env.obs_buf["policy"]["waypoint_states"] - waypoint_poses = raw_waypoint_states[:, :7] - waypoint_gripper_actions = raw_waypoint_states[:, 7:] - return waypoint_poses, waypoint_gripper_actions + waypoint_states = env.obs_buf["policy"]["waypoint_states"] + raw_waypoint_poses = waypoint_states[:, :7] + hand_waypoint_poses = waypoint_states[:, 7:-1] + waypoint_gripper_actions = waypoint_states[:, -1:] + return raw_waypoint_poses,hand_waypoint_poses, waypoint_gripper_actions def gen_actions(env:ManagerBasedRLEnv): """将路点转换为末端执行器(ee)对应要求的任务空间的动作""" # 以观测的形式获取场景中定义的路点的位置以及夹爪动作命令 - waypoint_poses, gripper_actions = get_waypoints(env) + raw_waypoint_poses,hand_waypoint_poses, gripper_actions = get_waypoints(env) # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 ee_goal_wrench_set_tilted_task = torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - device=env.device).repeat(waypoint_poses.shape[0], 1) + device=env.device).repeat(raw_waypoint_poses.shape[0], 1) # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 kp_set_task = torch.tensor([420.0, 420.0, 420.0, 420.0, 420.0, 420.0], - device=env.device).repeat(waypoint_poses.shape[0], 1) + device=env.device).repeat(raw_waypoint_poses.shape[0], 1) - arm_actions = torch.cat([waypoint_poses, kp_set_task], dim=-1) + actions = torch.cat([hand_waypoint_poses, kp_set_task], dim=-1) # gripper 动作命令， 0： 关闭 1： 打开 -1： 不动 gripper_commands = gripper_actions[:, 0] - return arm_actions, gripper_commands + return raw_waypoint_poses, actions, gripper_commands def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, - gripper_command: torch.Tensor, success_term=None, rate_limiter=None): + gripper_command: torch.Tensor, success_term=None, + rate_limiter=None,marker:VisualizationMarkers=None,last_gripper_command:bool=None): """执行单次路点动作，包含ee动作和夹爪动作""" # should_reset_recording_instance = False success_step_count = 0 # convert to torch arm_action = torch.tensor(arm_action.clone().detach(), dtype=torch.float, device=env.device).repeat(env.num_envs, 1) - # 夹爪动作置为false, 在执行arm动作时不执行夹爪动作 - ee_action = pre_process_actions(arm_action, False) - # 先判断一下当前状态 - current_gripper_state = env.obs_buf["policy"]["gripper_state"] - if current_gripper_state == gripper_command or gripper_command == -1: - bool_gripper_command = False - else: + if gripper_command == -1: + # 如果不动，则维持上一个夹爪动作 + bool_gripper_command = last_gripper_command + elif gripper_command == 1: # 要求打开 bool_gripper_command = True - + else: # 要求关闭 + bool_gripper_command = False + # 夹爪动作置为false, 在执行arm动作时不执行夹爪动作 + ee_action = pre_process_actions(arm_action, open_gripper=last_gripper_command) - gripper_action = pre_process_actions(arm_action, bool_gripper_command) + gripper_action = pre_process_actions(arm_action, open_gripper=bool_gripper_command) # 先执行ee动作,夹爪保持不变 #while True: # 当夹爪还没有到达目标位置时，不停循环执行动作 - for _ in range(100): + for _ in range(50): # perform action on environment env.step(ee_action) # 计算当前末端执行器的位姿 current_ee_pos = env.scene + # 显示当前ee手指中心的位置 + marker.visualize(env.obs_buf["policy"]["ee_pos"], env.obs_buf["policy"]["ee_quat"]) # 判断是否成功 if success_term is not None: if bool(success_term.func(env, **success_term.params)[0]): success_step_count += 1 - if success_step_count >= args_cli.num_success_steps: + if success_step_count >= args_cli.num_success_steps: # 检查当前连续成功的步数（success_step_count）是否达到预设阈值 env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) env.recorder_manager.set_success_to_episodes( [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) @@ -199,7 +207,7 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, success_step_count = 0 - # TODO: 这里需要检查当前末端执行器的位姿是否到达了目标位置 + # TODO: 这里需要检查当前末端执行器的位姿是否到达了目标位置,我这里简写了一下是固定的循环次数 if env.sim.is_stopped() or should_reset_recording_instance: break # @@ -208,8 +216,9 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, # 再执行gripper动作 - if gripper_command and not should_reset_recording_instance: - for _ in range(10): + current_gripper_state = env.obs_buf["policy"]["gripper_state"] + if bool_gripper_command != current_gripper_state and not should_reset_recording_instance: + for _ in range(30): # perform gripper action on environment env.step(gripper_action) @@ -233,8 +242,10 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, if rate_limiter: rate_limiter.sleep(env) + # 更新上一个指令 + last_gripper_command = bool_gripper_command - return should_reset_recording_instance + return should_reset_recording_instance,last_gripper_command @@ -318,6 +329,14 @@ def reset_recording_instance(): # reset before starting env.reset() teleop_interface.reset() + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + + ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + current_recorded_demo_count = 0 @@ -325,19 +344,26 @@ def reset_recording_instance(): while current_recorded_demo_count < args_cli.num_demos: # 获取当前回合场景中的路点以及对应的夹爪动作 - arm_actions, gripper_commands = gen_actions(env) - - for waypoint_idx in range(arm_actions.shape[0]): + raw_waypoint_poses, actions, gripper_commands = gen_actions(env) + # 默认初始的gripper 是 打开的动作 + last_gripper_command = True + + + + for waypoint_idx in range(actions.shape[0]): + # update marker positions + #ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + goal_marker.visualize(raw_waypoint_poses[waypoint_idx][None,0:3], raw_waypoint_poses[waypoint_idx][None,3:7]) # 执行动作 - should_reset_recording_instance = execute_action(env, arm_actions[waypoint_idx], + should_reset_recording_instance,last_gripper_command = execute_action(env, actions[waypoint_idx], gripper_commands[waypoint_idx], success_term=success_term, - rate_limiter=rate_limiter) - # if should_reset_recording_instance: - # env.recorder_manager.reset() - # env.reset() - # # 退出当前回合 - # break + rate_limiter=rate_limiter, + marker=ee_marker, + last_gripper_command=last_gripper_command) + if should_reset_recording_instance: + # 退出当前回合 + break # 执行完一个回合的所有路点之后，打印出当前完成的demo数量 if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index e672deaf739..43cf51d082e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -21,6 +21,10 @@ from isaaclab.sensors import FrameTransformerCfg from isaaclab.sensors import TiledCameraCfg + +from isaaclab.markers import VisualizationMarkers + + from isaaclab.sensors.frame_transformer import OffsetCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR @@ -50,6 +54,11 @@ class CabinetSceneCfg(InteractiveSceneCfg): which need to set the robot and end-effector frames """ + # Markers + # ee_marker = VisualizationMarkers(FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/ee_current")) + # goal_marker = VisualizationMarkers(FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/ee_goal")) + + # robots, Will be populated by agent env cfg robot: ArticulationCfg = MISSING # End-effector, Will be populated by agent env cfg @@ -92,10 +101,10 @@ class CabinetSceneCfg(InteractiveSceneCfg): actuators={ "drawers": ImplicitActuatorCfg( joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], - effort_limit=87.0, - velocity_limit=100.0, - stiffness=10.0, - damping=1.0, + effort_limit=10.0, # 这里我偷懒，降低了抽屉的容易拉开程度，并不太符合物理标准 原87 + velocity_limit=100.0, # 原100 + stiffness=2.0, # 原10 + damping=0.2, # 原1.0 ), "doors": ImplicitActuatorCfg( joint_names_expr=["door_left_joint", "door_right_joint"], @@ -111,8 +120,8 @@ class CabinetSceneCfg(InteractiveSceneCfg): cabinet_frame = FrameTransformerCfg( # 设置父坐标系的xform prim_path="{ENV_REGEX_NS}/Cabinet/sektion", - debug_vis=True, - visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/CabinetFrameTransformer"), + debug_vis=False, + visualizer_cfg= FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/CabinetFrameTransformer"), target_frames=[ # 手柄坐标系的名字还是“drawer_handle_top” FrameTransformerCfg.FrameCfg( @@ -187,15 +196,24 @@ class PolicyCfg(ObsGroup): actions = ObsTerm(func=mdp.last_action) - # 占位，不一定用 + # 原始的waypoint+gripper命令 waypoint_states: ObsTerm = MISSING + # 需要的ee手指中心目标位置 + # ee_action_targets: ObsTerm = MISSING + # 补充一个gripper的观测，用来判断当前的状态, state = 1 打开，否则是关闭状态 gripper_state = ObsTerm(func=mdp.gripper_state) + # + ee_pos = ObsTerm(func=mdp.ee_pos) + ee_quat = ObsTerm(func=mdp.ee_quat) + # hand_pos = ObsTerm(func=mdp.hand_pos) + # hand_quat = ObsTerm(func=mdp.hand_quat) def __post_init__(self): self.enable_corruption = True - self.concatenate_terms = True + # 这里我改动为False + self.concatenate_terms = False # observation groups policy: PolicyCfg = PolicyCfg() @@ -284,6 +302,8 @@ class TerminationsCfg: """Termination terms for the MDP.""" time_out = DoneTerm(func=mdp.time_out, time_out=True) + # 声明一个简单的成功判断函数，具体的自定义success函数写在了observation.py中 + success = DoneTerm(func=mdp.success) ## diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py index 681ff72c51c..100d86122e2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/joint_pos_env_cfg.py @@ -73,8 +73,17 @@ def __post_init__(self): name="tool_rightfinger", offset=OffsetCfg( pos=(0.0, 0.0, 0.046), - ), + ) ), + # FrameTransformerCfg.FrameCfg( + # # 新的虚拟坐标系附着在usd中定义好的panda_hand这个xfrom上 + # prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + # # 新的虚拟坐标系名字 + # name="hand", + # offset=OffsetCfg( + # pos=(0.0, 0.0, 0.0), + # ), + # ), ], ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py index 7fbb81ee499..55e0c681f1b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py @@ -49,7 +49,7 @@ def __post_init__(self): impedance_mode="variable_kp", inertial_dynamics_decoupling=True, partial_inertial_dynamics_decoupling=False, - gravity_compensation=True, # 开启重力补偿 + gravity_compensation=False, # 重力补偿不能开，好像有问题 motion_stiffness_task=100.0, motion_damping_ratio_task=1.0, motion_stiffness_limits_task=(50.0, 200.0), @@ -68,6 +68,9 @@ def __post_init__(self): self.observations.policy.waypoint_states = ObsTerm(func=mdp.waypoints, params={"asset_cfg": SceneEntityCfg("robot")}, ) + # self.observations.policy.ee_action_targets = ObsTerm(func=mdp.waypoints, + # params={"asset_cfg": SceneEntityCfg("robot")}, + # ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index d8d6d3e129a..b5d94bb3863 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -38,10 +38,18 @@ def rel_ee_drawer_distance(env: ManagerBasedRLEnv) -> torch.Tensor: return cabinet_tf_data.target_pos_w[..., 0, :] - ee_tf_data.target_pos_w[..., 0, :] +# 我这里简单添加了一个成功判断函数 +def success(env: ManagerBasedRLEnv) -> torch.Tensor: + cabinet_tf_data: FrameTransformerData = env.scene["cabinet_frame"].data + threshold = 0.770 + success = torch.norm(cabinet_tf_data.target_pos_w[..., 0, :], dim=1) <= threshold + return success.float() + + def fingertips_pos(env: ManagerBasedRLEnv) -> torch.Tensor: """The position of the fingertips relative to the environment origins.""" ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data - fingertips_pos = ee_tf_data.target_pos_w[..., 1:, :] - env.scene.env_origins.unsqueeze(1) + fingertips_pos = ee_tf_data.target_pos_w[..., 1:3, :] - env.scene.env_origins.unsqueeze(1) return fingertips_pos.view(env.num_envs, -1) @@ -64,13 +72,24 @@ def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tens # make first element of quaternion positive return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat +# def hand_pos(env: ManagerBasedRLEnv) -> torch.Tensor: +# """The position of the end-effector relative to the environment origins.""" +# ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data +# hand_pos = ee_tf_data.target_pos_w[..., 3, :] - env.scene.env_origins + +# return hand_pos -def gripper_pos(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: - robot: Articulation = env.scene[robot_cfg.name] - finger_joint_1 = robot.data.joint_pos[:, -1].clone().unsqueeze(1) - finger_joint_2 = -1 * robot.data.joint_pos[:, -2].clone().unsqueeze(1) - return torch.cat((finger_joint_1, finger_joint_2), dim=1) +# def hand_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tensor: +# """The orientation of the end-effector in the environment frame. + +# If :attr:`make_quat_unique` is True, the quaternion is made unique by ensuring the real part is positive. +# """ +# ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data +# hand_quat = ee_tf_data.target_quat_w[..., 3, :] +# # make first element of quaternion positive +# return math_utils.quat_unique(hand_quat) if make_quat_unique else hand_quat + def gripper_state(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: robot: Articulation = env.scene[robot_cfg.name] @@ -120,8 +139,8 @@ def find_all_waypoint_paths(max_depth=6): asset: Articulation = env.scene[asset_cfg.name] # (1,3), (1,4) root_link_pos , root_link_quat = asset.data.root_state_w[..., :3],asset.data.root_state_w[..., 3:7] - - waypoint_states = torch.empty(len(all_waypoint_paths), 8, device=env.device) + # raw_waypoint (7) hand_waypoint (7) gripper_command (1) + waypoint_states = torch.empty(len(all_waypoint_paths), 15, device=env.device) for path in all_waypoint_paths: waypoint_name = path.split("/")[-1] # 使用 path，而非 rigid_obj_name @@ -135,31 +154,43 @@ def find_all_waypoint_paths(max_depth=6): world_quat = world_transform.ExtractRotationQuat() world_quat = [world_quat.GetReal(),*world_quat.GetImaginary()] - # (7,) - waypoint_pose = torch.tensor(world_pos + world_quat, dtype=torch.float32, device=env.device) + # (7,) 世界参考系 + raw_waypoint_pose = torch.tensor(world_pos + world_quat, dtype=torch.float32, device=env.device) # 变换到以root坐标系为参考系 if referance == "root": # 1. 四元数变换（旋转变换到 root 坐标系） rel_quat = math_utils.quat_mul( math_utils.quat_inv(root_link_quat), - waypoint_pose[None,3:7] + raw_waypoint_pose[None,3:7] ) # 2. 平移向量变换（位置减去 root） - rel_pos = waypoint_pose[:3] - root_link_pos + rel_pos = raw_waypoint_pose[:3] - root_link_pos # 拼接为新的 pose - waypoint_pose = torch.cat([rel_pos, rel_quat], dim=-1).squeeze(0) + raw_waypoint_pose = torch.cat([rel_pos, rel_quat], dim=-1).squeeze(0) + + # ee_target_pose (1,3) 我们希望路点指的是手指中心ee_tcp的路点 + # 但是IK结算器需要给定pand_hand的位置，因此要根据路点计算出panda_hand的对应位置 + # panda_hand坐标系的姿态与ee_tcp相同，但是原点位于ee_tcp的(0,0,-0.1034)的位置 + # TODO: 注意这里我简单设置偏差为(0,0,-0.1034)，当机械臂类型变化后，需要重新设定数值，或者以某种自动化检索的方式获取 + # 我这里没写自动化检索偏差。 + offset_local = torch.tensor([0.0, 0.0, -0.1034], device=env.device) + offset_referance = math_utils.quat_apply(raw_waypoint_pose[3:7], offset_local) + hand_waypoint_pos = torch.tensor(raw_waypoint_pose[:3], device=env.device) + offset_referance + hand_waypoint_pose = torch.cat([hand_waypoint_pos,raw_waypoint_pose[3:7]],dim=-1) + + # gripper flag: open=1.0, close=0.0, default=-1.0 不动 if len(segments) > 2: - gripper_flag = 1.0 if segments[-1] == "open" else 0.0 + gripper_command = 1.0 if segments[-1] == "open" else 0.0 else: - gripper_flag = -1.0 + gripper_command = -1.0 - gripper_tensor = torch.tensor([gripper_flag], device=waypoint_pose.device) - full_state = torch.cat([waypoint_pose, gripper_tensor], dim=-1) + gripper_tensor = torch.tensor([gripper_command], device=raw_waypoint_pose.device) + full_state = torch.cat([raw_waypoint_pose, hand_waypoint_pose, gripper_tensor], dim=-1) waypoint_states[waypoint_idx,:] = full_state From 921307fdb44bf3f831e16dd0308b1baf098789f5 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Sat, 26 Apr 2025 22:22:00 +0800 Subject: [PATCH 6/9] =?UTF-8?q?=E5=B8=A6=E6=9C=89=E9=A2=84=E8=AE=BE?= =?UTF-8?q?=E8=B7=AF=E7=82=B9=E7=9A=84usd=E6=96=87=E4=BB=B6=EF=BC=8C?= =?UTF-8?q?=E6=8B=B7=E8=B4=9D=E6=9B=BF=E6=8D=A2=E6=8E=89=E5=AF=B9=E5=BA=94?= =?UTF-8?q?=E7=9A=84=E6=96=87=E4=BB=B6=E5=8D=B3=E5=8F=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../cabinet/asset/sektion_cabinet_instanceable | Bin 0 -> 16254 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/asset/sektion_cabinet_instanceable diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/asset/sektion_cabinet_instanceable b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/asset/sektion_cabinet_instanceable new file mode 100644 index 0000000000000000000000000000000000000000..5c122ec8aa076423f873bd0dfa2f516617436e1a GIT binary patch literal 16254 zcmeHO34Bvk)<5^5SFhx-*3L}jd2mIO_enGur?HU`3%->Q)0gzAq)EK2~@ z^~#nTF%J3%w!t-CMRs6562vi-$1z;2ui+eGm;f3KdrLA!Q5e@EWd1-=0@qs{@j2$71QBS98CP{>jvF%ei27-^$slN_Z}P3blS-6^zyLzoe{ zh&Dk{n2=k>s6-b9$-^LP+E$TjAdiV$4d7zMHb&Y49rgAU#3&WWEeywWTV>h`os#wx zR9PMn+Dc4&E=+}Bq!Q@_x2F({RAT4$T$l=Cl*(kiM`601-d(R&IaqJH#M)C(D>5V1 z+t{89Qz00s-gI%crx1)(B3-%d=^~@tGF_l@yb$AUoun<%<=&ny3b=HY7b9a^W!n0$ z+SkNXP-VGE+6o=x_H>a^=A+A8UiOrqsy!Al4ly2aH=#CR2=8evCFLQFrtidg7&@zMH$r*YMhab^lZ(Z=E%FXd4rqtF{Eyr7h7VI63VH}>NfJ~wPS$gQhOx@W_I5_SWC-&E>2Kd9QgZ^M#j zhKT7Gj-{^aI-_{Q!gK-Gj^A|CORuF`CObA9nY&0#8=ar5D_=7`eNf%d^|buFAgnzv zOi!mFO>g_^({cc8)doAmp<>w0}LJ-yD>ii_263j61CIookuPT^S4__m<< z)_WRr4AbYXM?;q>B%CW zGIAc}_@#WImEz_J?F&67mR;Ky+BvV=fOo#$Z_0V4U;KumqbAjfdzSz3)?U+jNp`R= z^o{1!lNozIH@%;!Jp0Z^(AZ@DVv||@ttnaDn4RIE)F!nQ9r1$oqp^D448OR{O3a=T(zK*LRw`b1e|6C$76^zfAVbr?qr#f9>ZQfVe!zkejEgEfqZ zLVSX5JGPMVT{j2$PU`2yqD@l&qcYt>p8^&^ZxHSkFMurL3KsinLWL7 z#maw)c9~{;97tbcp98!XpUtKWk^b7o=Z|}R`2GiGwxwm>WlJgjsZG9|FV%0D%sbmW z>EJPz$CvW|Qg*DGo{8uWQrYr;lHciy#j=juvF(!VU_17mWXE<~V#k^#-t|(O9kpYb z)G%D3KfT4Wj@z+CSCQ?PWQVe2oYbeEW5-TQ@*~>K6YXmPC$9_YolrLV8gRpoT|EE) zzuB=XImQ1e{v<9>q$bHOHZ=t9guZSFoE1&Kn7?=>ACVql@}Bv3+t(ejw6CG<>qfEc z+V-`-;SX!KKKG?5dckNi{1M3+UxQ`srrHbVB>Xj+cbOOmVkiE`N-{zwb zr=MfTjNv<|YEhHv&vU9AOJ|%ml??e`N_#8hjo#Wf3b9|X}jyFlXE2TC& zdX6V20#azdBu*^r_&GlFDze>@>`>=;s??{SbB-UBG=DIc=eB~Bh>rIW0?Oq z;t9k&#C*g8g#12o<2?ljBk0f8G({=!KC-PLkh=I3O`hic%kt|3ci@-hQ?H_6^NK6v zzkXSMK>nu7@&j_-{vUe}TGEI1Ikdf*DVAN^esXdC=`lZkVH&9zvMjNw)pYlF7qarZ zt#3Fk$qu%k_ts8+v+%ROH}tyyn>o+aYZ|MW6Fp`{Zfr324d~G%wb@boSuM#4^&V6# zmUY~IZoi7`^^)vR_R}r(>F3zbc&Qg=AT1E{pnwu=+X zu5G){uJ?woee8tkDaU}Aof-wlP1>B4zU8cGy(Bx>c9k(l>!0s(w4wNqo0#X->c$bj zU-7|?-Nzfglz4w9wHbz5UDti_CEsuM{?(QHqZ7+IZoB+fk?oLVhq7G*r9Np;wM?oW zlKl5``kqYdP|}Id(BCW^kmTR+{W@;D=-5%3i+B`qHQOcof{T#vhX03y&_);UspYKw z4^W5h!^`Yezz_T6viuQZg}+>ue=UDbZch%c<#)>VGWh*@u~a!hI><_>aiY|6TlP); zq1;x(du&C#&xbGIUbx-ka{9pS^4fiNmlHg;3VW>=EWF3-=F5HdDLnXWo+{o4h4!jy z9~8UXQ0A(2Rty^sEqJbusc|BY$IIVo_mvKX6A|FToKlCaj`u)Kt&ex|UN3lfX9e$> z0`Br!ug_I8syDO@%Cq@ws7#sNiNY)RN?WbNXYtr;poaIMK-9qMwEOTo&sw*6n%xWT z>N>Bz+*?$RBB5|BG^@ak?8B<LvvISU+!{N;D?m<3cl3tEcN*Fx!UHeaPS7m_PN|f@nh$kN%&@HiOJ%fwlW7_QN%kc zhq)XMyBFsLyYF$ic~p+~9)?C72b>OjMXsx&?t4&lCERG6*BcE_O|g4xZ4NJ#n<4Bc zfGtqq^Vpo;i3DcAaTcp(*_>6_Jx>YmaFyGAb-T#8tP0-g)j`~pM9@t$!QwIQESJa5 zJAKh$^_JTld=!vs7~-(`VxVb^2PYEjP8?BZIsZL=)}Aux84_FSWIB~@g5fE(-iqZU ztP4Dab6&~gyp-!n;qCTme8rc9{S2&SsB!8?08wB_i-K9Ggw0`bd5yrvfVMYe7rK1C zVGKUgHsN2xrV#B$P;>&-d!U>A5K(;uVao~W3gKzMOlR=hdU7WLA7SnQrVB*e4ooMA z&P7k}Zir67FIKyOG6R_1O!!u&SLviQ1=9_f!;I=hqU{3FcYsFy5HMpvGYO(^14SlI zgd2`AH8xy+cAFy$S7rshqxy+xi|7|((H2mJ@1dVLM=b~F1WGd~Q&`0<^i*S3dNg>hYI;AE(dOgMh4fBewr5G6NTby=$B=yQm)Q&xNv2R zve(#uWCqu|-S}r+GrWUyg}cFv+StoS0XfGoV?m!aAI!IV?B$)5>=Iyq1ceEhol1xl zWvU;Cl~o?w6r9yOyJuclqRTlEU?i*P0t-gls&F&aR`6Yb;owkKna5?Ta97*Bd~uz- zD=;V7%wfaw1_5J$=Z9OYd38=(jlG-?Q;Y(p2h1q);XZZPABYT7-wHOb%Yj?I3d~b% z9(!MC*7)jRDsP+2dx~*Y=kZ?Lq7^o$&kG}&*wH17!7R|}RZLPHu-JUnUYLTG#pTJU zj8*LhpNoF?H4Z8rF54Tbqk7y+8I^EsIILSV~Vg;y%L+s$;pd1ICGKNB@AzJ8^hQKc<hVyEOixp4bg73&|Jp#kA8L%LcZ72<{(gdfFpo|pScY4>ew}7Ta}<-z=E+gV_EqunIA7X zbI~ez7qlGeW}?4!iqV15or{mufqF>o4@5;bAg99BI)LjI4xE;AGvSb7guG;k^{FBD zt$2v_>mYWK8rTtF3{zx6Y7*S4!cJ?s%|?hFgA99-VGiOAtZ)$1KVl1=SubIhcnF6?B2{#V2=VSKEh(^SY zOo(m4a1XLIslj-A*a)i61lj{F=h6XZ#JXbNYylky>OjBONL8tb2P095y_;YJW*kW! z$T>TU71NQS7kE(ZBR!EJ9#BQJZ#X^ioG&1!7F!*Qtyf@pKjJrtxril*Rfyjs8W2+~ z@CLvGdAMY7*abIm+fH;hV>hN6LBBJV3g3%t2N2DOY002Jk80~7Lam4U^zh=r4O>Uh z#z74nCUP>@1rQPpj*&T6aV;VlaYJgRX865VZnVFFDBQOd}xe~qk2*Y}2K}fJkfDbOSkZxa5b@$>h~0?ccEl_W^vCNIoDTMO)&nk;wUJoI z-$XF#r<1fN6Lb&a`@nqAk%f@anfo;k@MCVO7#)fi5$h3K5Ia!kf#J76T0HF0L0V@x z?WfNJ;&Y`RE5l|r#O+oCnNJf3kstb?dKfk0_sfH@xpBCPgQOZa!1e`)_2ae%P8qb*V2MDN!%>Axl{6Z=&x z`LfTxPJbso3HcjI{G9aqjIascw)FUG&rdZy{zBNKuUis#4IpIaO|6O#`+bl~$a~p@ zY|A0<mniiX@XZ)JyX$=3WiTl;2q#GkSwH)ls}%8veHw(bwv`i-N>?{oBTj8?2K zXl^JVuNN?{6)4sgu&Y1Sr4-danAbILI zwOais84?497J(sn)|iV0lGOv!7m|6oQZaJ{Ny5xB%uLY2ai|mwM+`qK zL+3`JP#kBKFf&I(G#brbtrB=O<|$4n!LYRpge8Gi11lGhgQ|ln_RvQvHFy+<4jt+y zboGIGvOtUrpzw9lcVAo_e0{$_V$|WWs2Iz>il)5K z3?WF1^ld{FL%(6fm2SX)d4vn+lu!T`#zMF_C|*W1$tzjt>t~eErE3(lJgSMqAJZ12 zqA)XztA@O;uz-%L62jsyoPB@PYn9|l_ldQYCtt0Me^79rdZqI8n#vwi1^1bkE6=U2 zG&f8a{7`hE^{zKzr0{1LA$$NwTJPElX5k&k6}ABc#N_T+?tDsZa98>yIWgnN|KK9(b>nfA)6cWGP40qu3PuOJQ zx2;=X1nye(e2CDJR+d4~5n&ke_N@0oy%ZOWNX;=cSf#{Y_{M>SfKsQ#Pq{dfNASm_ z#a2)nidpDqG2_5kqTqf58l?gy{<8C2qycaYb##E0oKhe+lMh?*<(igP72$=-;vF8cVoUDV!$P=yjpHZJ~Wm=Dl z+5wD`LJ(Nm6w&AB=?n7EO2?bgp&`J&7YBF*HqmdQ+xem#&t3l^9O zj*TG>3ZkI7JK-=!R7_YH%d&{D1IB~hG5F->Pt!3_GMIUuJ+BU1I@t(*rIJ;$xY|)Y z25iTVV>Ct!Hrx}@iVNt6MFNAHaMisj(DztmI?TZG3t@^EOZ!9Og)lwcy>1qTosMY6 z9UCC(PoZZ8ECY*ZoQ@2ur&c`TK9#_nQJtX%fl~39JM-^C7el5A^m-Y|DZObI6&H@Y z3td*3J{%l~$-{p8$ox@x7mLy943z&?a=`-S`NcBJv(lB?VQJ%8%*7+*GFtP|tU&!A Wr~`^rWXT^ETt1cl#0JSE)&DQ#h6k|# literal 0 HcmV?d00001 From 28e75a112b582399bba148d23418681af4c0ebe1 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Mon, 28 Apr 2025 18:04:51 +0800 Subject: [PATCH 7/9] =?UTF-8?q?=E7=AE=80=E5=8C=96=E4=BB=A5=E5=8F=8A?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=83=A8=E5=88=86=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/tools/record_demos_auto.py | 75 ++++++++----------- .../manipulation/cabinet/cabinet_env_cfg.py | 23 ++---- .../cabinet/config/franka/osc_env_cfg.py | 3 - .../manipulation/cabinet/mdp/observations.py | 33 +++----- 4 files changed, 48 insertions(+), 86 deletions(-) diff --git a/scripts/tools/record_demos_auto.py b/scripts/tools/record_demos_auto.py index c10de584299..e59f32cc0d5 100644 --- a/scripts/tools/record_demos_auto.py +++ b/scripts/tools/record_demos_auto.py @@ -123,11 +123,6 @@ def pre_process_actions(arm_action: torch.Tensor, open_gripper: bool) -> torch.T return torch.concat([arm_action, gripper_vel], dim=1) -def distance_below_threshold(current_pos, desired_pos, threshold: float) -> bool: - return torch.norm(current_pos - desired_pos) < threshold - - - def get_waypoints(env:ManagerBasedRLEnv): """从场景中找到其中设定的路径点位置""" waypoint_states = env.obs_buf["policy"]["waypoint_states"] @@ -144,8 +139,8 @@ def gen_actions(env:ManagerBasedRLEnv): raw_waypoint_poses,hand_waypoint_poses, gripper_actions = get_waypoints(env) # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 - ee_goal_wrench_set_tilted_task = torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - device=env.device).repeat(raw_waypoint_poses.shape[0], 1) + # ee_goal_wrench_set_tilted_task = torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + # device=env.device).repeat(raw_waypoint_poses.shape[0], 1) # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 kp_set_task = torch.tensor([420.0, 420.0, 420.0, 420.0, 420.0, 420.0], @@ -166,7 +161,8 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, should_reset_recording_instance = False success_step_count = 0 # convert to torch - arm_action = torch.tensor(arm_action.clone().detach(), dtype=torch.float, device=env.device).repeat(env.num_envs, 1) + arm_action = torch.tensor(arm_action.clone().detach(), + dtype=torch.float, device=env.device).repeat(env.num_envs, 1) if gripper_command == -1: # 如果不动，则维持上一个夹爪动作 @@ -196,7 +192,8 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, if success_term is not None: if bool(success_term.func(env, **success_term.params)[0]): success_step_count += 1 - if success_step_count >= args_cli.num_success_steps: # 检查当前连续成功的步数（success_step_count）是否达到预设阈值 + # 检查当前连续成功的步数（success_step_count）是否达到预设阈值 + if success_step_count >= args_cli.num_success_steps: env.recorder_manager.record_pre_reset([0], force_export_or_skip=False) env.recorder_manager.set_success_to_episodes( [0], torch.tensor([[True]], dtype=torch.bool, device=env.device) @@ -248,12 +245,6 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, return should_reset_recording_instance,last_gripper_command - - - - - - def main(): """Collect demonstrations from the environment using teleop interfaces.""" @@ -299,36 +290,36 @@ def main(): # create environment env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - # add teleoperation key for reset current recording instance - should_reset_recording_instance = False - def reset_recording_instance(): - nonlocal should_reset_recording_instance - should_reset_recording_instance = True - - # create controller - if args_cli.teleop_device.lower() == "keyboard": - teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) - elif args_cli.teleop_device.lower() == "spacemouse": - teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) - elif args_cli.teleop_device.lower() == "handtracking": - from isaacsim.xr.openxr import OpenXRSpec - - teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) - teleop_interface.add_callback("RESET", reset_recording_instance) - viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") - ViewportCameraController(env, viewer) - else: - raise ValueError( - f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'." - ) + should_reset_recording_instance = False - teleop_interface.add_callback("R", reset_recording_instance) - print(teleop_interface) + # def reset_recording_instance(): + # nonlocal should_reset_recording_instance + # should_reset_recording_instance = True + + # # create controller + # if args_cli.teleop_device.lower() == "keyboard": + # teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) + # elif args_cli.teleop_device.lower() == "spacemouse": + # teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) + # elif args_cli.teleop_device.lower() == "handtracking": + # from isaacsim.xr.openxr import OpenXRSpec + + # teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) + # teleop_interface.add_callback("RESET", reset_recording_instance) + # viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") + # ViewportCameraController(env, viewer) + # else: + # raise ValueError( + # f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'." + # ) + + # teleop_interface.add_callback("R", reset_recording_instance) + # print(teleop_interface) # reset before starting env.reset() - teleop_interface.reset() + # teleop_interface.reset() # Markers frame_marker_cfg = FRAME_MARKER_CFG.copy() @@ -348,11 +339,9 @@ def reset_recording_instance(): # 默认初始的gripper 是 打开的动作 last_gripper_command = True - - for waypoint_idx in range(actions.shape[0]): # update marker positions - #ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + # 显示原始的路点位置姿态 goal_marker.visualize(raw_waypoint_poses[waypoint_idx][None,0:3], raw_waypoint_poses[waypoint_idx][None,3:7]) # 执行动作 should_reset_recording_instance,last_gripper_command = execute_action(env, actions[waypoint_idx], diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py index 43cf51d082e..bc8b19eeba5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/cabinet_env_cfg.py @@ -102,9 +102,9 @@ class CabinetSceneCfg(InteractiveSceneCfg): "drawers": ImplicitActuatorCfg( joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], effort_limit=10.0, # 这里我偷懒，降低了抽屉的容易拉开程度，并不太符合物理标准 原87 - velocity_limit=100.0, # 原100 - stiffness=2.0, # 原10 - damping=0.2, # 原1.0 + velocity_limit=200.0, # 原100 + stiffness=0.0, # 原10 + damping=0.0, # 原1.0 ), "doors": ImplicitActuatorCfg( joint_names_expr=["door_left_joint", "door_right_joint"], @@ -191,24 +191,15 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("cabinet", joint_names=["drawer_top_joint"])}, ) rel_ee_drawer_distance = ObsTerm(func=mdp.rel_ee_drawer_distance) - - - actions = ObsTerm(func=mdp.last_action) - # 原始的waypoint+gripper命令 + # waypoint states waypoint_states: ObsTerm = MISSING - # 需要的ee手指中心目标位置 - # ee_action_targets: ObsTerm = MISSING - - # 补充一个gripper的观测，用来判断当前的状态, state = 1 打开，否则是关闭状态 - gripper_state = ObsTerm(func=mdp.gripper_state) - # + # 获取当前的夹爪位置姿态 ee_pos = ObsTerm(func=mdp.ee_pos) ee_quat = ObsTerm(func=mdp.ee_quat) - - # hand_pos = ObsTerm(func=mdp.hand_pos) - # hand_quat = ObsTerm(func=mdp.hand_quat) + # 补充一个gripper的观测，用来判断当前的状态, state = 1 打开，否则是关闭状态 + gripper_state = ObsTerm(func=mdp.gripper_state) def __post_init__(self): self.enable_corruption = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py index 55e0c681f1b..b70c64a7a9b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/osc_env_cfg.py @@ -68,9 +68,6 @@ def __post_init__(self): self.observations.policy.waypoint_states = ObsTerm(func=mdp.waypoints, params={"asset_cfg": SceneEntityCfg("robot")}, ) - # self.observations.policy.ee_action_targets = ObsTerm(func=mdp.waypoints, - # params={"asset_cfg": SceneEntityCfg("robot")}, - # ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py index b5d94bb3863..836906321fe 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/mdp/observations.py @@ -109,12 +109,8 @@ def waypoints(env: ManagerBasedRLEnv, waypoint_idx_gripperaction 收集场景中所有的路点，并选择是否将其转换为相对于机器人底座坐标系 - 夹爪动作： - - 1.0: open - - 0.0: close - - -1.0: 不动 Returns: - A tensor of shape (N, 8), where each row = [pose(7), gripper_action] + A tensor of shape (N, 15) """ def find_all_waypoint_paths(max_depth=6): all_paths = [] @@ -127,7 +123,6 @@ def find_all_waypoint_paths(max_depth=6): matches = prim_utils.find_matching_prim_paths(pattern) all_paths.extend(matches) return list(set(all_paths)) # 去重 - # 这一行可以手动刷新一下场景树 stage = stage_utils.get_current_stage() @@ -137,27 +132,24 @@ def find_all_waypoint_paths(max_depth=6): waypoint_states = [] asset: Articulation = env.scene[asset_cfg.name] - # (1,3), (1,4) + # (1,3), (1,4) 获取机械臂底座位置和姿态备用 root_link_pos , root_link_quat = asset.data.root_state_w[..., :3],asset.data.root_state_w[..., 3:7] - # raw_waypoint (7) hand_waypoint (7) gripper_command (1) + # 空数据，占位raw_waypoint (7) hand_waypoint (7) gripper_command (1) waypoint_states = torch.empty(len(all_waypoint_paths), 15, device=env.device) for path in all_waypoint_paths: waypoint_name = path.split("/")[-1] # 使用 path，而非 rigid_obj_name segments = waypoint_name.split("_") waypoint_idx = int(segments[1]) # 路点索引 - # 获取到路点的prim waypoint_prim = UsdGeom.Xformable(stage.GetPrimAtPath(path)) world_transform = waypoint_prim.ComputeLocalToWorldTransform(0) world_pos = list(world_transform.ExtractTranslation()) world_quat = world_transform.ExtractRotationQuat() - world_quat = [world_quat.GetReal(),*world_quat.GetImaginary()] - # (7,) 世界参考系 + # (7,) 原始的路点 此时以世界坐标系为参考系 raw_waypoint_pose = torch.tensor(world_pos + world_quat, dtype=torch.float32, device=env.device) - - # 变换到以root坐标系为参考系 + # 是否需要将原始路点转换到以机械臂底座为参考系 if referance == "root": # 1. 四元数变换（旋转变换到 root 坐标系） rel_quat = math_utils.quat_mul( @@ -166,12 +158,10 @@ def find_all_waypoint_paths(max_depth=6): ) # 2. 平移向量变换（位置减去 root） rel_pos = raw_waypoint_pose[:3] - root_link_pos - # 拼接为新的 pose raw_waypoint_pose = torch.cat([rel_pos, rel_quat], dim=-1).squeeze(0) - - # ee_target_pose (1,3) 我们希望路点指的是手指中心ee_tcp的路点 - # 但是IK结算器需要给定pand_hand的位置，因此要根据路点计算出panda_hand的对应位置 + # 我们希望路点指的是手指中心ee_tcp的路点但是IK解算器 + # 需要给定pand_hand的位置因此要根据路点计算出panda_hand的对应位置 # panda_hand坐标系的姿态与ee_tcp相同，但是原点位于ee_tcp的(0,0,-0.1034)的位置 # TODO: 注意这里我简单设置偏差为(0,0,-0.1034)，当机械臂类型变化后，需要重新设定数值，或者以某种自动化检索的方式获取 # 我这里没写自动化检索偏差。 @@ -179,21 +169,16 @@ def find_all_waypoint_paths(max_depth=6): offset_referance = math_utils.quat_apply(raw_waypoint_pose[3:7], offset_local) hand_waypoint_pos = torch.tensor(raw_waypoint_pose[:3], device=env.device) + offset_referance hand_waypoint_pose = torch.cat([hand_waypoint_pos,raw_waypoint_pose[3:7]],dim=-1) - - - - - # gripper flag: open=1.0, close=0.0, default=-1.0 不动 + # 根据命令字符串设定对应指令变量open=1.0, close=0.0, default=-1.0 保持 if len(segments) > 2: gripper_command = 1.0 if segments[-1] == "open" else 0.0 else: gripper_command = -1.0 - + # 拼接完整路点 gripper_tensor = torch.tensor([gripper_command], device=raw_waypoint_pose.device) full_state = torch.cat([raw_waypoint_pose, hand_waypoint_pose, gripper_tensor], dim=-1) waypoint_states[waypoint_idx,:] = full_state - return waypoint_states From 3e84bef174ea4d3e1fa8cba087edbb01435be208 Mon Sep 17 00:00:00 2001 From: Hymwgk Date: Mon, 28 Apr 2025 19:07:08 +0800 Subject: [PATCH 8/9] =?UTF-8?q?=E7=AE=80=E5=8C=96=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/tools/record_demos_auto.py | 117 +++++++---------------------- 1 file changed, 28 insertions(+), 89 deletions(-) diff --git a/scripts/tools/record_demos_auto.py b/scripts/tools/record_demos_auto.py index e59f32cc0d5..715a513b3cd 100644 --- a/scripts/tools/record_demos_auto.py +++ b/scripts/tools/record_demos_auto.py @@ -122,7 +122,6 @@ def pre_process_actions(arm_action: torch.Tensor, open_gripper: bool) -> torch.T # compute actions return torch.concat([arm_action, gripper_vel], dim=1) - def get_waypoints(env:ManagerBasedRLEnv): """从场景中找到其中设定的路径点位置""" waypoint_states = env.obs_buf["policy"]["waypoint_states"] @@ -131,17 +130,11 @@ def get_waypoints(env:ManagerBasedRLEnv): waypoint_gripper_actions = waypoint_states[:, -1:] return raw_waypoint_poses,hand_waypoint_poses, waypoint_gripper_actions - def gen_actions(env:ManagerBasedRLEnv): """将路点转换为末端执行器(ee)对应要求的任务空间的动作""" # 以观测的形式获取场景中定义的路点的位置以及夹爪动作命令 raw_waypoint_poses,hand_waypoint_poses, gripper_actions = get_waypoints(env) - - # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 - # ee_goal_wrench_set_tilted_task = torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - # device=env.device).repeat(raw_waypoint_poses.shape[0], 1) - # 随便写的一些动作，仅仅是为了占位，满足任务空间动作的形式要求 kp_set_task = torch.tensor([420.0, 420.0, 420.0, 420.0, 420.0, 420.0], device=env.device).repeat(raw_waypoint_poses.shape[0], 1) @@ -151,8 +144,6 @@ def gen_actions(env:ManagerBasedRLEnv): gripper_commands = gripper_actions[:, 0] return raw_waypoint_poses, actions, gripper_commands - - def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, gripper_command: torch.Tensor, success_term=None, rate_limiter=None,marker:VisualizationMarkers=None,last_gripper_command:bool=None): @@ -163,7 +154,6 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, # convert to torch arm_action = torch.tensor(arm_action.clone().detach(), dtype=torch.float, device=env.device).repeat(env.num_envs, 1) - if gripper_command == -1: # 如果不动，则维持上一个夹爪动作 bool_gripper_command = last_gripper_command @@ -174,21 +164,19 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, # 夹爪动作置为false, 在执行arm动作时不执行夹爪动作 ee_action = pre_process_actions(arm_action, open_gripper=last_gripper_command) - + # 夹爪动作 gripper_action = pre_process_actions(arm_action, open_gripper=bool_gripper_command) - # 先执行ee动作,夹爪保持不变 - #while True: # 当夹爪还没有到达目标位置时，不停循环执行动作 + # 这里设置了固定的时间步长 for _ in range(50): - # perform action on environment env.step(ee_action) - # 计算当前末端执行器的位姿 + # 计算当前末端执行器的位姿，不过这里没用到 current_ee_pos = env.scene - # 显示当前ee手指中心的位置 + # 获取观测，显示当前ee手指中心的位置 marker.visualize(env.obs_buf["policy"]["ee_pos"], env.obs_buf["policy"]["ee_quat"]) - # 判断是否成功 + # 判断回合是否成功 if success_term is not None: if bool(success_term.func(env, **success_term.params)[0]): success_step_count += 1 @@ -244,29 +232,19 @@ def execute_action(env:ManagerBasedRLEnv, arm_action: torch.Tensor, return should_reset_recording_instance,last_gripper_command - def main(): - """Collect demonstrations from the environment using teleop interfaces.""" - - # if handtracking is selected, rate limiting is achieved via OpenXR - if args_cli.teleop_device.lower() == "handtracking": - rate_limiter = None - else: - rate_limiter = RateLimiter(args_cli.step_hz) - - # get directory path and file name (without extension) from cli arguments + """通过回放预设路点的形式来收集任务的演示数据集.""" + rate_limiter = RateLimiter(args_cli.step_hz) + # 获取并创建数据集的存放路径 output_dir = os.path.dirname(args_cli.dataset_file) output_file_name = os.path.splitext(os.path.basename(args_cli.dataset_file))[0] - - # create directory if it does not exist if not os.path.exists(output_dir): os.makedirs(output_dir) - # parse configuration + # 获取当前任务环境的名称 env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1) env_cfg.env_name = args_cli.task - - # extract success checking function to invoke in the main loop + # 从配置文件中获取成功检测函数，这一点就是我们设置的成功检测函数 success_term = None if hasattr(env_cfg.terminations, "success"): success_term = env_cfg.terminations.success @@ -276,97 +254,58 @@ def main(): "No success termination term was found in the environment." " Will not be able to mark recorded demos as successful." ) - - # modify configuration such that the environment runs indefinitely until - # the goal is reached or other termination conditions are met + # 这里禁止了超时判断，使得环境只能在达到你设定的成功条件后再结束一个回合 env_cfg.terminations.time_out = None - + # 这里不允许isaacsim自动把观测信息拼接在一起，而是单独保存 env_cfg.observations.policy.concatenate_terms = False - + # 应该是设置录像器？ env_cfg.recorders: ActionStateRecorderManagerCfg = ActionStateRecorderManagerCfg() env_cfg.recorders.dataset_export_dir_path = output_dir env_cfg.recorders.dataset_filename = output_file_name - - # create environment + # 创建环境对象 env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - - + # 判断是否应该重置录像器的flag should_reset_recording_instance = False - - # def reset_recording_instance(): - # nonlocal should_reset_recording_instance - # should_reset_recording_instance = True - - # # create controller - # if args_cli.teleop_device.lower() == "keyboard": - # teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5) - # elif args_cli.teleop_device.lower() == "spacemouse": - # teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5) - # elif args_cli.teleop_device.lower() == "handtracking": - # from isaacsim.xr.openxr import OpenXRSpec - - # teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True) - # teleop_interface.add_callback("RESET", reset_recording_instance) - # viewer = ViewerCfg(eye=(-0.25, -0.3, 0.5), lookat=(0.6, 0, 0), asset_name="viewer") - # ViewportCameraController(env, viewer) - # else: - # raise ValueError( - # f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'." - # ) - - # teleop_interface.add_callback("R", reset_recording_instance) - # print(teleop_interface) - - # reset before starting + # 开始之前先重置环境 env.reset() - # teleop_interface.reset() - - # Markers + # 在isaacsim仿真器中设置坐标系的marker，用来显示坐标系，调试用 frame_marker_cfg = FRAME_MARKER_CFG.copy() frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - + # 定义了两个marker，一个是末端执行器的坐标系，一个是目标位置的坐标系 ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) - - + # 当前已经记录下来的成功demo回合数量 current_recorded_demo_count = 0 - # 一直自动生成，直到到达指定的成功demo数量 while current_recorded_demo_count < args_cli.num_demos: - - # 获取当前回合场景中的路点以及对应的夹爪动作 + # 从场景中获取当前回合中的路点以及对应的夹爪动作 raw_waypoint_poses, actions, gripper_commands = gen_actions(env) - # 默认初始的gripper 是 打开的动作 + # 默认初始的gripper动作是 open，即使你没有手动给定，例如 waypoint_0 等价于 waypoint_0_open last_gripper_command = True - + # 逐一执行刚收集的所有路点 for waypoint_idx in range(actions.shape[0]): - # update marker positions - # 显示原始的路点位置姿态 + # 显示当前路点的坐标系 goal_marker.visualize(raw_waypoint_poses[waypoint_idx][None,0:3], raw_waypoint_poses[waypoint_idx][None,3:7]) - # 执行动作 + # 执行该路点对应的动作，并判断是否重置回合，回传保存当前的夹爪动作 should_reset_recording_instance,last_gripper_command = execute_action(env, actions[waypoint_idx], gripper_commands[waypoint_idx], success_term=success_term, rate_limiter=rate_limiter, marker=ee_marker, last_gripper_command=last_gripper_command) + # 如果满足了成功条件，或者手动退出了当前回合，则退出当前回合 if should_reset_recording_instance: - # 退出当前回合 break - - # 执行完一个回合的所有路点之后，打印出当前完成的demo数量 + # 如果当前回合成功结束，就打印出当前回合的成功次数，并更新current_recorded_demo_count if env.recorder_manager.exported_successful_episode_count > current_recorded_demo_count: current_recorded_demo_count = env.recorder_manager.exported_successful_episode_count print(f"Recorded {current_recorded_demo_count} successful demonstrations.") - + # 不论回合怎么结束，在这里都要重置录像器和环境自身 env.recorder_manager.reset() env.reset() - - # 完成所有的demo之后退出环境 + # 成功完成要求的n次回合之后，就关闭环境 print(f"All {args_cli.num_demos} demonstrations recorded. Exiting the app.") env.close() - - if __name__ == "__main__": # run the main function From d30838f757ebd5db040470b3e1fa653158facf93 Mon Sep 17 00:00:00 2001 From: Le Date: Wed, 27 Aug 2025 19:32:50 +0800 Subject: [PATCH 9/9] =?UTF-8?q?=E5=94=90=E4=B9=90=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E6=AC=A1=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../manipulation/table/__init__.py | 6 + .../table/asset/sektion_cabinet_instanceable | Bin 0 -> 16254 bytes .../manipulation/table/config/__init__.py | 9 + .../table/config/franka/__init__.py | 101 ++++++ .../table/config/franka/agents/__init__.py | 4 + .../franka/agents/rl_games_ppo_cfg.yaml | 76 ++++ .../config/franka/agents/rsl_rl_ppo_cfg.py | 37 ++ .../config/franka/agents/skrl_ppo_cfg.yaml | 80 +++++ .../table/config/franka/ik_abs_env_cfg.py | 48 +++ .../table/config/franka/ik_rel_env_cfg.py | 49 +++ .../table/config/franka/joint_pos_env_cfg.py | 105 ++++++ .../table/config/franka/osc_env_cfg.py | 83 +++++ .../manipulation/table/mdp/__init__.py | 11 + .../manipulation/table/mdp/observations.py | 186 ++++++++++ .../manipulation/table/mdp/rewards.py | 162 +++++++++ .../manipulation/table/table_env_cfg.py | 331 ++++++++++++++++++ 16 files changed, 1288 insertions(+) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/asset/sektion_cabinet_instanceable create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rl_games_ppo_cfg.yaml create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rsl_rl_ppo_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/skrl_ppo_cfg.yaml create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_abs_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_rel_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/joint_pos_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/osc_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/observations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/rewards.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/table_env_cfg.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/__init__.py new file mode 100644 index 00000000000..0b2ac7bfef5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manipulation environments to open drawers in a table.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/asset/sektion_cabinet_instanceable b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/asset/sektion_cabinet_instanceable new file mode 100644 index 0000000000000000000000000000000000000000..5c122ec8aa076423f873bd0dfa2f516617436e1a GIT binary patch literal 16254 zcmeHO34Bvk)<5^5SFhx-*3L}jd2mIO_enGur?HU`3%->Q)0gzAq)EK2~@ z^~#nTF%J3%w!t-CMRs6562vi-$1z;2ui+eGm;f3KdrLA!Q5e@EWd1-=0@qs{@j2$71QBS98CP{>jvF%ei27-^$slN_Z}P3blS-6^zyLzoe{ zh&Dk{n2=k>s6-b9$-^LP+E$TjAdiV$4d7zMHb&Y49rgAU#3&WWEeywWTV>h`os#wx zR9PMn+Dc4&E=+}Bq!Q@_x2F({RAT4$T$l=Cl*(kiM`601-d(R&IaqJH#M)C(D>5V1 z+t{89Qz00s-gI%crx1)(B3-%d=^~@tGF_l@yb$AUoun<%<=&ny3b=HY7b9a^W!n0$ z+SkNXP-VGE+6o=x_H>a^=A+A8UiOrqsy!Al4ly2aH=#CR2=8evCFLQFrtidg7&@zMH$r*YMhab^lZ(Z=E%FXd4rqtF{Eyr7h7VI63VH}>NfJ~wPS$gQhOx@W_I5_SWC-&E>2Kd9QgZ^M#j zhKT7Gj-{^aI-_{Q!gK-Gj^A|CORuF`CObA9nY&0#8=ar5D_=7`eNf%d^|buFAgnzv zOi!mFO>g_^({cc8)doAmp<>w0}LJ-yD>ii_263j61CIookuPT^S4__m<< z)_WRr4AbYXM?;q>B%CW zGIAc}_@#WImEz_J?F&67mR;Ky+BvV=fOo#$Z_0V4U;KumqbAjfdzSz3)?U+jNp`R= z^o{1!lNozIH@%;!Jp0Z^(AZ@DVv||@ttnaDn4RIE)F!nQ9r1$oqp^D448OR{O3a=T(zK*LRw`b1e|6C$76^zfAVbr?qr#f9>ZQfVe!zkejEgEfqZ zLVSX5JGPMVT{j2$PU`2yqD@l&qcYt>p8^&^ZxHSkFMurL3KsinLWL7 z#maw)c9~{;97tbcp98!XpUtKWk^b7o=Z|}R`2GiGwxwm>WlJgjsZG9|FV%0D%sbmW z>EJPz$CvW|Qg*DGo{8uWQrYr;lHciy#j=juvF(!VU_17mWXE<~V#k^#-t|(O9kpYb z)G%D3KfT4Wj@z+CSCQ?PWQVe2oYbeEW5-TQ@*~>K6YXmPC$9_YolrLV8gRpoT|EE) zzuB=XImQ1e{v<9>q$bHOHZ=t9guZSFoE1&Kn7?=>ACVql@}Bv3+t(ejw6CG<>qfEc z+V-`-;SX!KKKG?5dckNi{1M3+UxQ`srrHbVB>Xj+cbOOmVkiE`N-{zwb zr=MfTjNv<|YEhHv&vU9AOJ|%ml??e`N_#8hjo#Wf3b9|X}jyFlXE2TC& zdX6V20#azdBu*^r_&GlFDze>@>`>=;s??{SbB-UBG=DIc=eB~Bh>rIW0?Oq z;t9k&#C*g8g#12o<2?ljBk0f8G({=!KC-PLkh=I3O`hic%kt|3ci@-hQ?H_6^NK6v zzkXSMK>nu7@&j_-{vUe}TGEI1Ikdf*DVAN^esXdC=`lZkVH&9zvMjNw)pYlF7qarZ zt#3Fk$qu%k_ts8+v+%ROH}tyyn>o+aYZ|MW6Fp`{Zfr324d~G%wb@boSuM#4^&V6# zmUY~IZoi7`^^)vR_R}r(>F3zbc&Qg=AT1E{pnwu=+X zu5G){uJ?woee8tkDaU}Aof-wlP1>B4zU8cGy(Bx>c9k(l>!0s(w4wNqo0#X->c$bj zU-7|?-Nzfglz4w9wHbz5UDti_CEsuM{?(QHqZ7+IZoB+fk?oLVhq7G*r9Np;wM?oW zlKl5``kqYdP|}Id(BCW^kmTR+{W@;D=-5%3i+B`qHQOcof{T#vhX03y&_);UspYKw z4^W5h!^`Yezz_T6viuQZg}+>ue=UDbZch%c<#)>VGWh*@u~a!hI><_>aiY|6TlP); zq1;x(du&C#&xbGIUbx-ka{9pS^4fiNmlHg;3VW>=EWF3-=F5HdDLnXWo+{o4h4!jy z9~8UXQ0A(2Rty^sEqJbusc|BY$IIVo_mvKX6A|FToKlCaj`u)Kt&ex|UN3lfX9e$> z0`Br!ug_I8syDO@%Cq@ws7#sNiNY)RN?WbNXYtr;poaIMK-9qMwEOTo&sw*6n%xWT z>N>Bz+*?$RBB5|BG^@ak?8B<LvvISU+!{N;D?m<3cl3tEcN*Fx!UHeaPS7m_PN|f@nh$kN%&@HiOJ%fwlW7_QN%kc zhq)XMyBFsLyYF$ic~p+~9)?C72b>OjMXsx&?t4&lCERG6*BcE_O|g4xZ4NJ#n<4Bc zfGtqq^Vpo;i3DcAaTcp(*_>6_Jx>YmaFyGAb-T#8tP0-g)j`~pM9@t$!QwIQESJa5 zJAKh$^_JTld=!vs7~-(`VxVb^2PYEjP8?BZIsZL=)}Aux84_FSWIB~@g5fE(-iqZU ztP4Dab6&~gyp-!n;qCTme8rc9{S2&SsB!8?08wB_i-K9Ggw0`bd5yrvfVMYe7rK1C zVGKUgHsN2xrV#B$P;>&-d!U>A5K(;uVao~W3gKzMOlR=hdU7WLA7SnQrVB*e4ooMA z&P7k}Zir67FIKyOG6R_1O!!u&SLviQ1=9_f!;I=hqU{3FcYsFy5HMpvGYO(^14SlI zgd2`AH8xy+cAFy$S7rshqxy+xi|7|((H2mJ@1dVLM=b~F1WGd~Q&`0<^i*S3dNg>hYI;AE(dOgMh4fBewr5G6NTby=$B=yQm)Q&xNv2R zve(#uWCqu|-S}r+GrWUyg}cFv+StoS0XfGoV?m!aAI!IV?B$)5>=Iyq1ceEhol1xl zWvU;Cl~o?w6r9yOyJuclqRTlEU?i*P0t-gls&F&aR`6Yb;owkKna5?Ta97*Bd~uz- zD=;V7%wfaw1_5J$=Z9OYd38=(jlG-?Q;Y(p2h1q);XZZPABYT7-wHOb%Yj?I3d~b% z9(!MC*7)jRDsP+2dx~*Y=kZ?Lq7^o$&kG}&*wH17!7R|}RZLPHu-JUnUYLTG#pTJU zj8*LhpNoF?H4Z8rF54Tbqk7y+8I^EsIILSV~Vg;y%L+s$;pd1ICGKNB@AzJ8^hQKc<hVyEOixp4bg73&|Jp#kA8L%LcZ72<{(gdfFpo|pScY4>ew}7Ta}<-z=E+gV_EqunIA7X zbI~ez7qlGeW}?4!iqV15or{mufqF>o4@5;bAg99BI)LjI4xE;AGvSb7guG;k^{FBD zt$2v_>mYWK8rTtF3{zx6Y7*S4!cJ?s%|?hFgA99-VGiOAtZ)$1KVl1=SubIhcnF6?B2{#V2=VSKEh(^SY zOo(m4a1XLIslj-A*a)i61lj{F=h6XZ#JXbNYylky>OjBONL8tb2P095y_;YJW*kW! z$T>TU71NQS7kE(ZBR!EJ9#BQJZ#X^ioG&1!7F!*Qtyf@pKjJrtxril*Rfyjs8W2+~ z@CLvGdAMY7*abIm+fH;hV>hN6LBBJV3g3%t2N2DOY002Jk80~7Lam4U^zh=r4O>Uh z#z74nCUP>@1rQPpj*&T6aV;VlaYJgRX865VZnVFFDBQOd}xe~qk2*Y}2K}fJkfDbOSkZxa5b@$>h~0?ccEl_W^vCNIoDTMO)&nk;wUJoI z-$XF#r<1fN6Lb&a`@nqAk%f@anfo;k@MCVO7#)fi5$h3K5Ia!kf#J76T0HF0L0V@x z?WfNJ;&Y`RE5l|r#O+oCnNJf3kstb?dKfk0_sfH@xpBCPgQOZa!1e`)_2ae%P8qb*V2MDN!%>Axl{6Z=&x z`LfTxPJbso3HcjI{G9aqjIascw)FUG&rdZy{zBNKuUis#4IpIaO|6O#`+bl~$a~p@ zY|A0<mniiX@XZ)JyX$=3WiTl;2q#GkSwH)ls}%8veHw(bwv`i-N>?{oBTj8?2K zXl^JVuNN?{6)4sgu&Y1Sr4-danAbILI zwOais84?497J(sn)|iV0lGOv!7m|6oQZaJ{Ny5xB%uLY2ai|mwM+`qK zL+3`JP#kBKFf&I(G#brbtrB=O<|$4n!LYRpge8Gi11lGhgQ|ln_RvQvHFy+<4jt+y zboGIGvOtUrpzw9lcVAo_e0{$_V$|WWs2Iz>il)5K z3?WF1^ld{FL%(6fm2SX)d4vn+lu!T`#zMF_C|*W1$tzjt>t~eErE3(lJgSMqAJZ12 zqA)XztA@O;uz-%L62jsyoPB@PYn9|l_ldQYCtt0Me^79rdZqI8n#vwi1^1bkE6=U2 zG&f8a{7`hE^{zKzr0{1LA$$NwTJPElX5k&k6}ABc#N_T+?tDsZa98>yIWgnN|KK9(b>nfA)6cWGP40qu3PuOJQ zx2;=X1nye(e2CDJR+d4~5n&ke_N@0oy%ZOWNX;=cSf#{Y_{M>SfKsQ#Pq{dfNASm_ z#a2)nidpDqG2_5kqTqf58l?gy{<8C2qycaYb##E0oKhe+lMh?*<(igP72$=-;vF8cVoUDV!$P=yjpHZJ~Wm=Dl z+5wD`LJ(Nm6w&AB=?n7EO2?bgp&`J&7YBF*HqmdQ+xem#&t3l^9O zj*TG>3ZkI7JK-=!R7_YH%d&{D1IB~hG5F->Pt!3_GMIUuJ+BU1I@t(*rIJ;$xY|)Y z25iTVV>Ct!Hrx}@iVNt6MFNAHaMisj(DztmI?TZG3t@^EOZ!9Og)lwcy>1qTosMY6 z9UCC(PoZZ8ECY*ZoQ@2ur&c`TK9#_nQJtX%fl~39JM-^C7el5A^m-Y|DZObI6&H@Y z3td*3J{%l~$-{p8$ox@x7mLy943z&?a=`-S`NcBJv(lB?VQJ%8%*7+*GFtP|tU&!A Wr~`^rWXT^ETt1cl#0JSE)&DQ#h6k|# literal 0 HcmV?d00001 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/__init__.py new file mode 100644 index 00000000000..605a62a78d2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the table environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/__init__.py new file mode 100644 index 00000000000..472b9e56f14 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/__init__.py @@ -0,0 +1,101 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Open-Table-Franka-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaTableEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:tablePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Open-Table-Franka-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaTableEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:tablePPORunnerCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + + +## +# Inverse Kinematics - Absolute Pose Control +## + +# 动作是ee位置的绝对动作，但是只能进行小范围变化，用微分动力学 求解 + +gym.register( + id="Isaac-Open-Table-Franka-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:FrankaTableEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Relative Pose Control +## + +# 动作是ee位置 的 delta动作， 用微分动力学 求解 +gym.register( + id="Isaac-Open-Table-Franka-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_rel_env_cfg:FrankaTableEnvCfg", + }, + disable_env_checker=True, +) + + + +## +# Inverse Kinematics - Operational Space Control +## +# 新添加了笛卡尔空间的动作 +# 动作是末端执行器的笛卡尔空间动作 + +gym.register( + id="Isaac-Open-Table-Franka-OSC-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaTableEnvCfg", + }, + disable_env_checker=True, +) + +## +# Inverse Kinematics - Operational Space Control +## + +# 动作是末端执行器的笛卡尔空间动作 +gym.register( + id="Isaac-Open-Table-Franka-OSC-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaTableEnvCfg_PLAY", + }, + disable_env_checker=True, +) \ No newline at end of file diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/__init__.py new file mode 100644 index 00000000000..e75ca2bc3f9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rl_games_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 00000000000..69fbb2a236f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,76 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 5.0 + clip_actions: 1.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False + load_path: '' + + config: + name: franka_open_drawer + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: False + normalize_value: False + num_actors: -1 # configured from the script (based on num_envs) + reward_shaper: + scale_value: 1 + normalize_advantage: False + gamma: 0.99 + tau: 0.95 + learning_rate: 5e-4 + lr_schedule: adaptive + kl_threshold: 0.008 + score_to_win: 200 + max_epochs: 400 + save_best_after: 50 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.001 + truncate_grads: True + e_clip: 0.2 + horizon_length: 96 + minibatch_size: 4096 + mini_epochs: 5 + critic_coef: 4 + clip_value: True + seq_length: 4 + bounds_loss_coef: 0.0001 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 00000000000..85dd0628b67 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class TablePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 96 + max_iterations = 400 + save_interval = 50 + experiment_name = "franka_open_drawer" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=1e-3, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.02, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/skrl_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/skrl_ppo_cfg.yaml new file mode 100644 index 00000000000..dedde5178a2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 96 + learning_epochs: 5 + mini_batches: 96 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 5.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.008 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: null + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.001 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 1 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "franka_open_drawer" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 38400 + environment_info: log diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_abs_env_cfg.py new file mode 100644 index 00000000000..59f5f9422e6 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_abs_env_cfg.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaTableEnvCfg(joint_pos_env_cfg.FrankaTableEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + # 动作依旧是末端执行器的动作，但是动作含义为绝对动作 + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaTableEnvCfg_PLAY(FrankaTableEnvCfg): + 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/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_rel_env_cfg.py new file mode 100644 index 00000000000..987459ec972 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/ik_rel_env_cfg.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip + + +@configclass +class FrankaTableEnvCfg(joint_pos_env_cfg.FrankaTableEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We switch here to a stiffer PD controller for IK tracking to be better. + self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + # 动作依旧是末端执行器的动作，但是动作含义为相对上一个时刻的偏移动作 + self.actions.arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"), + scale=0.5, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + ) + + +@configclass +class FrankaTableEnvCfg_PLAY(FrankaTableEnvCfg): + 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/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/joint_pos_env_cfg.py new file mode 100644 index 00000000000..d9250097ff6 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/joint_pos_env_cfg.py @@ -0,0 +1,105 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.table import mdp + +from isaaclab_tasks.manager_based.manipulation.table.table_env_cfg import ( # isort: skip + FRAME_MARKER_SMALL_CFG, + TableEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaTableEnvCfg(TableEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set Actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + scale=1.0, + use_default_offset=True, + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + + # Listens to the required transforms + # IMPORTANT: The order of the frames in the list is important. The first frame is the tool center point (TCP) + # the other frames are the fingers + self.scene.ee_frame = FrameTransformerCfg( + # 父坐标系 + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/EndEffectorFrameTransformer"), + target_frames=[ + FrameTransformerCfg.FrameCfg( + # 新的虚拟坐标系附着在usd中定义好的panda_hand这个xfrom上 + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + # 新的虚拟坐标系名字 + name="ee_tcp", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.1034), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_leftfinger", + name="tool_leftfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ), + ), + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_rightfinger", + name="tool_rightfinger", + offset=OffsetCfg( + pos=(0.0, 0.0, 0.046), + ) + ), + # FrameTransformerCfg.FrameCfg( + # # 新的虚拟坐标系附着在usd中定义好的panda_hand这个xfrom上 + # prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + # # 新的虚拟坐标系名字 + # name="hand", + # offset=OffsetCfg( + # pos=(0.0, 0.0, 0.0), + # ), + # ), + ], + ) + + # override rewards + self.rewards.approach_gripper_handle.params["offset"] = 0.04 + self.rewards.grasp_handle.params["open_joint_pos"] = 0.04 + self.rewards.grasp_handle.params["asset_cfg"].joint_names = ["panda_finger_.*"] + + +@configclass +class FrankaTableEnvCfg_PLAY(FrankaTableEnvCfg): + 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/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/osc_env_cfg.py new file mode 100644 index 00000000000..1bffa73e595 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/config/franka/osc_env_cfg.py @@ -0,0 +1,83 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.controllers.operational_space_cfg import OperationalSpaceControllerCfg +from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from isaaclab.utils import configclass + +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg + +from . import joint_pos_env_cfg +from ... import mdp + +## +# Pre-defined configs +## +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaTableEnvCfg(joint_pos_env_cfg.FrankaTableEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We remove stiffness and damping for the shoulder and forearm joints for effort control + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["panda_shoulder"].stiffness = 0.0 + self.scene.robot.actuators["panda_shoulder"].damping = 0.0 + self.scene.robot.actuators["panda_forearm"].stiffness = 0.0 + self.scene.robot.actuators["panda_forearm"].damping = 0.0 + self.scene.robot.spawn.rigid_props.disable_gravity = True + + # If closed-loop contact force control is desired, contact sensors should be enabled for the robot + # self.scene.robot.spawn.activate_contact_sensors = True + + self.actions.arm_action = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + # If a task frame different from articulation root/base is desired, a RigidObject, e.g., "task_frame", + # can be added to the scene and its relative path could provided as task_frame_rel_path + # task_frame_rel_path="task_frame", + controller_cfg=OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, # 重力补偿不能开，好像有问题 + motion_stiffness_task=100.0, + motion_damping_ratio_task=1.0, + motion_stiffness_limits_task=(50.0, 200.0), + nullspace_control="position", + ), + nullspace_joint_pos_target="center", + position_scale=1.0, + orientation_scale=1.0, + stiffness_scale=100.0, + ) + # Removing these observations as they are not needed for OSC and we want keep the observation space small + self.observations.policy.joint_pos = None + self.observations.policy.joint_vel = None + + # 为这种任务空间的形式 添加路点观测 + self.observations.policy.waypoint_states = ObsTerm(func=mdp.waypoints, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + + +@configclass +class FrankaTableEnvCfg_PLAY(FrankaTableEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 16 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/__init__.py new file mode 100644 index 00000000000..e6a9c7ac6f3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/__init__.py @@ -0,0 +1,11 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the table environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/observations.py new file mode 100644 index 00000000000..490b78d1226 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/observations.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING +from pxr import UsdGeom + +import isaaclab.utils.math as math_utils +from isaaclab.assets import ArticulationData,RigidObject,Articulation +from isaaclab.managers import SceneEntityCfg + +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils + +from isaaclab.sensors import FrameTransformerData + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def rel_ee_object_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + object_data: ArticulationData = env.scene["object"].data + + return object_data.root_pos_w - ee_tf_data.target_pos_w[..., 0, :] + + +def rel_ee_drawer_distance(env: ManagerBasedRLEnv) -> torch.Tensor: + """The distance between the end-effector and the object.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + table_tf_data: FrameTransformerData = env.scene["table_frame"].data + + return table_tf_data.target_pos_w[..., 0, :] - ee_tf_data.target_pos_w[..., 0, :] + + +# 我这里简单添加了一个成功判断函数 +def success(env: ManagerBasedRLEnv) -> torch.Tensor: + table_tf_data: FrameTransformerData = env.scene["table_frame"].data + threshold = 0.770 + success = torch.norm(table_tf_data.target_pos_w[..., 0, :], dim=1) <= threshold + return success.float() + + +def fingertips_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the fingertips relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + fingertips_pos = ee_tf_data.target_pos_w[..., 1:3, :] - env.scene.env_origins.unsqueeze(1) + + return fingertips_pos.view(env.num_envs, -1) + + +def ee_pos(env: ManagerBasedRLEnv) -> torch.Tensor: + """The position of the end-effector relative to the environment origins.""" + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_pos = ee_tf_data.target_pos_w[..., 0, :] - env.scene.env_origins + + return ee_pos + + +def ee_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tensor: + """The orientation of the end-effector in the environment frame. + + If :attr:`make_quat_unique` is True, the quaternion is made unique by ensuring the real part is positive. + """ + ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data + ee_quat = ee_tf_data.target_quat_w[..., 0, :] + # make first element of quaternion positive + return math_utils.quat_unique(ee_quat) if make_quat_unique else ee_quat + +# def hand_pos(env: ManagerBasedRLEnv) -> torch.Tensor: +# """The position of the end-effector relative to the environment origins.""" +# ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data +# hand_pos = ee_tf_data.target_pos_w[..., 3, :] - env.scene.env_origins + +# return hand_pos + + +# def hand_quat(env: ManagerBasedRLEnv, make_quat_unique: bool = True) -> torch.Tensor: +# """The orientation of the end-effector in the environment frame. + +# If :attr:`make_quat_unique` is True, the quaternion is made unique by ensuring the real part is positive. +# """ +# ee_tf_data: FrameTransformerData = env.scene["ee_frame"].data +# hand_quat = ee_tf_data.target_quat_w[..., 3, :] +# # make first element of quaternion positive +# return math_utils.quat_unique(hand_quat) if make_quat_unique else hand_quat + + +def gripper_state(env: ManagerBasedRLEnv, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + robot: Articulation = env.scene[robot_cfg.name] + finger_joint_1 = robot.data.joint_pos[:, -1].clone().unsqueeze(1) + finger_joint_2 = -1 * robot.data.joint_pos[:, -2].clone().unsqueeze(1) + + threshold = 0.075 + is_open = torch.norm((finger_joint_1 - finger_joint_2), dim=1) >= threshold + + return is_open.float() + + +def waypoints(env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + referance: str = "root") -> torch.Tensor: + """ + waypoint_idx_gripperaction + + 收集场景中所有的路点，并选择是否将其转换为相对于机器人底座坐标系 + Returns: + A tensor of shape (N, 15) + """ + def find_all_waypoint_paths(max_depth=6): + all_paths = [] + for depth in range(1, max_depth + 1): + if depth == 1: + pattern = "/waypoint_.*" + else: + prefix = "/".join([".*?"] * (depth - 1)) + pattern = f"/{prefix}/waypoint_.*" + matches = prim_utils.find_matching_prim_paths(pattern) + all_paths.extend(matches) + return list(set(all_paths)) # 去重 + + # 这一行可以手动刷新一下场景树 + stage = stage_utils.get_current_stage() + # 获取所有的路点节点路径 + all_waypoint_paths = find_all_waypoint_paths(max_depth=10) + + waypoint_states = [] + + asset: Articulation = env.scene[asset_cfg.name] + # (1,3), (1,4) 获取机械臂底座位置和姿态备用 + root_link_pos , root_link_quat = asset.data.root_state_w[..., :3],asset.data.root_state_w[..., 3:7] + # 空数据，占位raw_waypoint (7) hand_waypoint (7) gripper_command (1) + waypoint_states = torch.empty(len(all_waypoint_paths), 15, device=env.device) + + for path in all_waypoint_paths: + waypoint_name = path.split("/")[-1] # 使用 path，而非 rigid_obj_name + segments = waypoint_name.split("_") + waypoint_idx = int(segments[1]) # 路点索引 + # 获取到路点的prim + waypoint_prim = UsdGeom.Xformable(stage.GetPrimAtPath(path)) + world_transform = waypoint_prim.ComputeLocalToWorldTransform(0) + world_pos = list(world_transform.ExtractTranslation()) + world_quat = world_transform.ExtractRotationQuat() + world_quat = [world_quat.GetReal(),*world_quat.GetImaginary()] + # (7,) 原始的路点 此时以世界坐标系为参考系 + raw_waypoint_pose = torch.tensor(world_pos + world_quat, dtype=torch.float32, device=env.device) + # 是否需要将原始路点转换到以机械臂底座为参考系 + if referance == "root": + # 1. 四元数变换（旋转变换到 root 坐标系） + rel_quat = math_utils.quat_mul( + math_utils.quat_inv(root_link_quat), + raw_waypoint_pose[None,3:7] + ) + # 2. 平移向量变换（位置减去 root） + rel_pos = raw_waypoint_pose[:3] - root_link_pos + # 拼接为新的 pose + raw_waypoint_pose = torch.cat([rel_pos, rel_quat], dim=-1).squeeze(0) + # 我们希望路点指的是手指中心ee_tcp的路点但是IK解算器 + # 需要给定pand_hand的位置因此要根据路点计算出panda_hand的对应位置 + # panda_hand坐标系的姿态与ee_tcp相同，但是原点位于ee_tcp的(0,0,-0.1034)的位置 + # TODO: 注意这里我简单设置偏差为(0,0,-0.1034)，当机械臂类型变化后，需要重新设定数值，或者以某种自动化检索的方式获取 + # 我这里没写自动化检索偏差。 + offset_local = torch.tensor([0.0, 0.0, -0.1034], device=env.device) + offset_referance = math_utils.quat_apply(raw_waypoint_pose[3:7], offset_local) + hand_waypoint_pos = torch.tensor(raw_waypoint_pose[:3], device=env.device) + offset_referance + hand_waypoint_pose = torch.cat([hand_waypoint_pos,raw_waypoint_pose[3:7]],dim=-1) + # 根据命令字符串设定对应指令变量open=1.0, close=0.0, default=-1.0 保持 + if len(segments) > 2: + gripper_command = 1.0 if segments[-1] == "open" else 0.0 + else: + gripper_command = -1.0 + # 拼接完整路点 + gripper_tensor = torch.tensor([gripper_command], device=raw_waypoint_pose.device) + full_state = torch.cat([raw_waypoint_pose, hand_waypoint_pose, gripper_tensor], dim=-1) + waypoint_states[waypoint_idx,:] = full_state + + return waypoint_states + + + + diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/rewards.py new file mode 100644 index 00000000000..45ea4badbec --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/mdp/rewards.py @@ -0,0 +1,162 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import matrix_from_quat + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def approach_ee_handle(env: ManagerBasedRLEnv, threshold: float) -> torch.Tensor: + r"""Reward the robot for reaching the drawer handle using inverse-square law. + + It uses a piecewise function to reward the robot for reaching the handle. + + .. math:: + + reward = \begin{cases} + 2 * (1 / (1 + distance^2))^2 & \text{if } distance \leq threshold \\ + (1 / (1 + distance^2))^2 & \text{otherwise} + \end{cases} + + """ + ee_tcp_pos = env.scene["ee_frame"].data.target_pos_w[..., 0, :] + handle_pos = env.scene["table_frame"].data.target_pos_w[..., 0, :] + + # Compute the distance of the end-effector to the handle + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + + # Reward the robot for reaching the handle + reward = 1.0 / (1.0 + distance**2) + reward = torch.pow(reward, 2) + return torch.where(distance <= threshold, 2 * reward, reward) + + +def align_ee_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Reward for aligning the end-effector with the handle. + + The reward is based on the alignment of the gripper with the handle. It is computed as follows: + + .. math:: + + reward = 0.5 * (align_z^2 + align_x^2) + + where :math:`align_z` is the dot product of the z direction of the gripper and the -x direction of the handle + and :math:`align_x` is the dot product of the x direction of the gripper and the -y direction of the handle. + """ + ee_tcp_quat = env.scene["ee_frame"].data.target_quat_w[..., 0, :] + handle_quat = env.scene["table_frame"].data.target_quat_w[..., 0, :] + + ee_tcp_rot_mat = matrix_from_quat(ee_tcp_quat) + handle_mat = matrix_from_quat(handle_quat) + + # get current x and y direction of the handle + handle_x, handle_y = handle_mat[..., 0], handle_mat[..., 1] + # get current x and z direction of the gripper + ee_tcp_x, ee_tcp_z = ee_tcp_rot_mat[..., 0], ee_tcp_rot_mat[..., 2] + + # make sure gripper aligns with the handle + # in this case, the z direction of the gripper should be close to the -x direction of the handle + # and the x direction of the gripper should be close to the -y direction of the handle + # dot product of z and x should be large + align_z = torch.bmm(ee_tcp_z.unsqueeze(1), -handle_x.unsqueeze(-1)).squeeze(-1).squeeze(-1) + align_x = torch.bmm(ee_tcp_x.unsqueeze(1), -handle_y.unsqueeze(-1)).squeeze(-1).squeeze(-1) + return 0.5 * (torch.sign(align_z) * align_z**2 + torch.sign(align_x) * align_x**2) + + +def align_grasp_around_handle(env: ManagerBasedRLEnv) -> torch.Tensor: + """Bonus for correct hand orientation around the handle. + + The correct hand orientation is when the left finger is above the handle and the right finger is below the handle. + """ + # Target object position: (num_envs, 3) + handle_pos = env.scene["table_frame"].data.target_pos_w[..., 0, :] + # Fingertips position: (num_envs, n_fingertips, 3) + ee_fingertips_w = env.scene["ee_frame"].data.target_pos_w[..., 1:, :] + lfinger_pos = ee_fingertips_w[..., 0, :] + rfinger_pos = ee_fingertips_w[..., 1, :] + + # Check if hand is in a graspable pose + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + + # bonus if left finger is above the drawer handle and right below + return is_graspable + + +def approach_gripper_handle(env: ManagerBasedRLEnv, offset: float = 0.04) -> torch.Tensor: + """Reward the robot's gripper reaching the drawer handle with the right pose. + + This function returns the distance of fingertips to the handle when the fingers are in a grasping orientation + (i.e., the left finger is above the handle and the right finger is below the handle). Otherwise, it returns zero. + """ + # Target object position: (num_envs, 3) + handle_pos = env.scene["table_frame"].data.target_pos_w[..., 0, :] + # Fingertips position: (num_envs, n_fingertips, 3) + ee_fingertips_w = env.scene["ee_frame"].data.target_pos_w[..., 1:, :] + lfinger_pos = ee_fingertips_w[..., 0, :] + rfinger_pos = ee_fingertips_w[..., 1, :] + + # Compute the distance of each finger from the handle + lfinger_dist = torch.abs(lfinger_pos[:, 2] - handle_pos[:, 2]) + rfinger_dist = torch.abs(rfinger_pos[:, 2] - handle_pos[:, 2]) + + # Check if hand is in a graspable pose + is_graspable = (rfinger_pos[:, 2] < handle_pos[:, 2]) & (lfinger_pos[:, 2] > handle_pos[:, 2]) + + return is_graspable * ((offset - lfinger_dist) + (offset - rfinger_dist)) + + +def grasp_handle( + env: ManagerBasedRLEnv, threshold: float, open_joint_pos: float, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward for closing the fingers when being close to the handle. + + The :attr:`threshold` is the distance from the handle at which the fingers should be closed. + The :attr:`open_joint_pos` is the joint position when the fingers are open. + + Note: + It is assumed that zero joint position corresponds to the fingers being closed. + """ + ee_tcp_pos = env.scene["ee_frame"].data.target_pos_w[..., 0, :] + handle_pos = env.scene["table_frame"].data.target_pos_w[..., 0, :] + gripper_joint_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids] + + distance = torch.norm(handle_pos - ee_tcp_pos, dim=-1, p=2) + is_close = distance <= threshold + + return is_close * torch.sum(open_joint_pos - gripper_joint_pos, dim=-1) + + +def open_drawer_bonus(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Bonus for opening the drawer given by the joint position of the drawer. + + The bonus is given when the drawer is open. If the grasp is around the handle, the bonus is doubled. + """ + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + is_graspable = align_grasp_around_handle(env).float() + + return (is_graspable + 1.0) * drawer_pos + + +def multi_stage_open_drawer(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Multi-stage bonus for opening the drawer. + + Depending on the drawer's position, the reward is given in three stages: easy, medium, and hard. + This helps the agent to learn to open the drawer in a controlled manner. + """ + drawer_pos = env.scene[asset_cfg.name].data.joint_pos[:, asset_cfg.joint_ids[0]] + is_graspable = align_grasp_around_handle(env).float() + + open_easy = (drawer_pos > 0.01) * 0.5 + open_medium = (drawer_pos > 0.2) * is_graspable + open_hard = (drawer_pos > 0.3) * is_graspable + + return open_easy + open_medium + open_hard diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/table_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/table_env_cfg.py new file mode 100644 index 00000000000..0044bf5fd25 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/table/table_env_cfg.py @@ -0,0 +1,331 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.actuators.actuator_cfg import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +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 ActionTermCfg as ActionTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors import TiledCameraCfg + + +from isaaclab.markers import VisualizationMarkers + + +from isaaclab.sensors.frame_transformer import OffsetCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + +## +# Pre-defined configs +## +from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip + + +FRAME_MARKER_SMALL_CFG = FRAME_MARKER_CFG.copy() +FRAME_MARKER_SMALL_CFG.markers["frame"].scale = (0.10, 0.10, 0.10) + + +## +# Scene definition +## + + +@configclass +class TableSceneCfg(InteractiveSceneCfg): + """Configuration for the table scene with a robot and a table. + + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the robot and end-effector frames + """ + + # Markers + # ee_marker = VisualizationMarkers(FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/ee_current")) + # goal_marker = VisualizationMarkers(FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/ee_goal")) + + + # robots, Will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # End-effector, Will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + + + # add camera to the scene + tiled_camera: TiledCameraCfg = TiledCameraCfg( + prim_path="{ENV_REGEX_NS}/Camera", + offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) + ), + width=100, + height=100, + ) + + + + table = ArticulationCfg( + # 设置物体的路径，就是在场景树中相对于环境的路径 + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Sektion_Table/sektion_table_instanceable.usd", + activate_contact_sensors=False, + ), + # 设置初始状态 + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.8, 0, 0.4), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "door_left_joint": 0.0, + "door_right_joint": 0.0, + "drawer_bottom_joint": 0.0, + "drawer_top_joint": 0.0, + }, + ), + # 设置内部关节限制 + actuators={ + "drawers": ImplicitActuatorCfg( + joint_names_expr=["drawer_top_joint", "drawer_bottom_joint"], + effort_limit=10.0, # 这里我偷懒，降低了抽屉的容易拉开程度，并不太符合物理标准 原87 + velocity_limit=200.0, # 原100 + stiffness=0.0, # 原10 + damping=0.0, # 原1.0 + ), + "doors": ImplicitActuatorCfg( + joint_names_expr=["door_left_joint", "door_right_joint"], + effort_limit=87.0, + velocity_limit=100.0, + stiffness=10.0, + damping=2.5, + ), + }, + ) + + # 获取相对于柜子的变动的一些位置姿态（变换关系） + table_frame = FrameTransformerCfg( + # 设置父坐标系的xform + prim_path="{ENV_REGEX_NS}/Table/sektion", + debug_vis=False, + visualizer_cfg= FRAME_MARKER_SMALL_CFG.replace(prim_path="/Visuals/TableFrameTransformer"), + target_frames=[ + # 手柄坐标系的名字还是“drawer_handle_top” + FrameTransformerCfg.FrameCfg( + # 手柄坐标系在哪里？它要固定附着在场景中已经存在的参考系，两者“相对位姿固定” + # 在这里，用"{ENV_REGEX_NS}/Table/drawer_handle_top"这个xform来作为附着对象 + prim_path="{ENV_REGEX_NS}/Table/drawer_handle_top", + # 手柄的坐标系的名称，它起了一个与prim_path中相同的名字 + name="drawer_handle_top", + offset=OffsetCfg( + pos=(0.305, 0.0, 0.01), + rot=(0.5, 0.5, -0.5, -0.5), # align with end-effector frame + ), + ), + ], + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(), + spawn=sim_utils.GroundPlaneCfg(), + collision_group=-1, + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action: ActionTerm = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + # 声明rgb观测 + rgb_image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), + "data_type": "rgb"}) + # # 声明depth图观测 + # depth_image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), + # "data_type": "distance_to_camera"}) + + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + table_joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("table", joint_names=["drawer_top_joint"])}, + ) + table_joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("table", joint_names=["drawer_top_joint"])}, + ) + rel_ee_drawer_distance = ObsTerm(func=mdp.rel_ee_drawer_distance) + actions = ObsTerm(func=mdp.last_action) + + # waypoint states + waypoint_states: ObsTerm = MISSING + # 获取当前的夹爪位置姿态 + ee_pos = ObsTerm(func=mdp.ee_pos) + ee_quat = ObsTerm(func=mdp.ee_quat) + # 补充一个gripper的观测，用来判断当前的状态, state = 1 打开，否则是关闭状态 + gripper_state = ObsTerm(func=mdp.gripper_state) + + def __post_init__(self): + self.enable_corruption = True + # 这里我改动为False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + robot_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + "static_friction_range": (0.8, 1.25), + "dynamic_friction_range": (0.8, 1.25), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + table_physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("table", body_names="drawer_handle_top"), + "static_friction_range": (1.0, 1.25), + "dynamic_friction_range": (1.25, 1.5), + "restitution_range": (0.0, 0.0), + "num_buckets": 16, + }, + ) + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + 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), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # 1. Approach the handle + approach_ee_handle = RewTerm(func=mdp.approach_ee_handle, weight=2.0, params={"threshold": 0.2}) + align_ee_handle = RewTerm(func=mdp.align_ee_handle, weight=0.5) + + # 2. Grasp the handle + approach_gripper_handle = RewTerm(func=mdp.approach_gripper_handle, weight=5.0, params={"offset": MISSING}) + align_grasp_around_handle = RewTerm(func=mdp.align_grasp_around_handle, weight=0.125) + grasp_handle = RewTerm( + func=mdp.grasp_handle, + weight=0.5, + params={ + "threshold": 0.03, + "open_joint_pos": MISSING, + "asset_cfg": SceneEntityCfg("robot", joint_names=MISSING), + }, + ) + + # 3. Open the drawer + open_drawer_bonus = RewTerm( + func=mdp.open_drawer_bonus, + weight=7.5, + params={"asset_cfg": SceneEntityCfg("table", joint_names=["drawer_top_joint"])}, + ) + multi_stage_open_drawer = RewTerm( + func=mdp.multi_stage_open_drawer, + weight=1.0, + params={"asset_cfg": SceneEntityCfg("table", joint_names=["drawer_top_joint"])}, + ) + + # 4. Penalize actions for cosmetic reasons + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1e-2) + joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-0.0001) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # 声明一个简单的成功判断函数，具体的自定义success函数写在了observation.py中 + success = DoneTerm(func=mdp.success) + + +## +# Environment configuration +## + + +@configclass +class TableEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the table environment.""" + + # Scene settings + scene: TableSceneCfg = TableSceneCfg(num_envs=4096, env_spacing=2.0) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 1 + self.episode_length_s = 8.0 + self.viewer.eye = (-2.0, 2.0, 2.0) + self.viewer.lookat = (0.8, 0.0, 0.5) + # simulation settings + self.sim.dt = 1 / 60 # 60Hz + self.sim.render_interval = self.decimation + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.friction_correlation_distance = 0.00625