IsaacLab 中实现松灵piper机械臂键盘遥操作与数据采集
·
一、前置工作
检验安装IsaacLab + IsaacSim
cd ~/IsaacLab
./isaaclab.sh -p -c "import isaaclab; import isaacsim; print('✅ IsaacLab + IsaacSim 安装成功！')"
准备好工作目录
/home/agilex/isaac-cosmos/二、创建 IsaacLab 外部项目
1.进入 IsaacLab 根目录
cd ~/IsaacLab
2. 创建外部项目
./isaaclab.sh --new然后按下面选择（直接照抄）：
1. External
2. 项目路径：/home/agilex/isaac-cosmos
3. 项目名：agx_teleop
4. Manager-based | single-agent
5. 随便选一个RL库（比如rsl_rl）
3. 安装项目
cd ~/agx_teleop
pip install -e source/agx_teleop三、准备piper 机械臂模型
1.下载 piper 机械臂 URDF
cd agx_teleop
mkdir -p source/agx_teleop/agx_description
cd source/agx_teleop/agx_description
git clone https://github.com/agilexrobotics/agx_arm_urdf.git2.合并并修正 URDF
新建piper_gripper.urdf，将下面的代码复制粘贴进去
URDF 转 USD
./python.sh ~/IsaacLab/scripts/tools/convert_urdf.py \
/home/hql/agilex/isaac-cosmos/agx_teleop/source/agx_teleop/agx_description/agx_arm_urdf/piper/urdf/piper_gripper.urdf \
/home/hql/agilex/isaac-cosmos/agx_teleop/source/agx_teleop/agx_description/agx_arm_urdf/piper/usd/piper.usd \
--fix-bas检验模型转换成功
1.打开刚刚生成的usd文件
2.点左边的Play
3.Tools → Physics → Physics Inspector
4.点箭头
5.在列表里选你的机器人根节点
6.拖动滑块，关节 / 夹爪全部能动3.piper配置文件
cd agx_teleop
mkdir -p source/agx_teleop/agx_teleop/assets创建assets/__init__.py文件
"""Package containing robot configurations."""
import os
import toml
##
# Configuration for different assets.
##
# Conveniences to other module directories via relative paths
AGX_TELEOP_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../"))
"""Path to the source agx_teleop directory."""
AGX_TELEOP_DESCRIPTION_DIR = os.path.join(AGX_TELEOP_EXT_DIR, "agx_description")
"""Path to the agx_description directory."""创建 assets/piper.py 文件
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg
from agx_teleop.assets import AGX_TELEOP_DESCRIPTION_DIR
PIPER_GRIPPER_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{AGX_TELEOP_DESCRIPTION_DIR}/agx_arm_urdf/piper/usd/piper.usd",
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=5.0,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=0
),
),
init_state=ArticulationCfg.InitialStateCfg(
joint_pos={
"joint1": 0.0,
"joint2": 1.57,
"joint3": -1.57,
"joint4": 0.0,
"joint5": 1.22,
"joint6": 0.0,
"gripper_joint1": 0.0,
"gripper_joint2": 0.0,
},
pos=(0.0, 0.0, 0.0)
),
actuators={
"piper_shoulder": ImplicitActuatorCfg(
joint_names_expr=["joint[1-3]"],
effort_limit_sim=100.0,
velocity_limit_sim=5.0,
stiffness=80.0,
damping=20.0,
),
"piper_forearm": ImplicitActuatorCfg(
joint_names_expr=["joint[4-6]"],
effort_limit_sim=100.0,
velocity_limit_sim=5.0,
stiffness=40.0,
damping=10.0,
),
"gripper": ImplicitActuatorCfg( # 参考 https://github.com/smalleha/isaac_so_arm101/blob/main/src/isaac_so_arm101/robots/piper_description/piper.py
joint_names_expr=["gripper_joint1","gripper_joint2"],
effort_limit_sim=22, # Increased from 1.9 to 2.5 for stronger grip
velocity_limit_sim=1.5,
stiffness=800.0, # Increased from 25.0 to 60.0 for more reliable closing
damping=20.0, # Increased from 10.0 to 20.0 for stability
),
},
soft_joint_pos_limit_factor=0.9,
)
"""Configuration for the Piper robot with parallel gripper."""
PIPER_GRIPPER_HIGH_PD_CFG = PIPER_GRIPPER_CFG.copy()
PIPER_GRIPPER_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True
PIPER_GRIPPER_HIGH_PD_CFG.actuators["piper_shoulder"].stiffness = 400.0
PIPER_GRIPPER_HIGH_PD_CFG.actuators["piper_shoulder"].damping = 80.0
PIPER_GRIPPER_HIGH_PD_CFG.actuators["piper_forearm"].stiffness = 200.0
PIPER_GRIPPER_HIGH_PD_CFG.actuators["piper_forearm"].damping = 40.0
PIPER_GRIPPER_HIGH_PD_CFG.actuators["gripper"].velocity_limit_sim = 0.5
"""Configuration of Piper with gripper robot with stiffer PD control.
This configuration is useful for task-space control using differential IK.
"""四、创建任务目录
1.配置项目结构
cd source/agx_teleop/agx_teleop/tasks/manager_based/
mkdir -p manipulation/stack/mdp
mkdir -p manipulation/stack/config/piper/agents/robomimic
2.创建文件
cd ~/agilex/isaac-cosmos/agx_teleop/source/agx_teleop/agx_teleop/tasks/manager_based
touch manipulation/__init__.py \
manipulation/stack/__init__.py \
manipulation/stack/stack_env_cfg.py \
manipulation/stack/mdp/__init__.py \
manipulation/stack/mdp/observations.py \
manipulation/stack/mdp/piper_stack_events.py \
manipulation/stack/mdp/terminations.py \
manipulation/stack/config/__init__.py \
manipulation/stack/config/piper/__init__.py \
manipulation/stack/config/piper/agents/__init__.py \
manipulation/stack/config/piper/agents/robomimic/bc_rnn_low_dim.json \
manipulation/stack/config/piper/piper_stack_ik_rel_env_cfg.py \
manipulation/stack/config/piper/piper_stack_joint_pos_env_cfg.py修改文件：
将下面的文件直接复制粘贴
/Isaac/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/stack_env_cfg.py
/Isaac/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/__init__.py
/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json
Isaac/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py到
manipulation/stack/stack_env_cfg.py
manipulation/stack/mdp/__init__.py
manipulation/stack/config/piper/agents/robomimic/bc_rnn_low_dim.json
manipulation/stack/mdp/piper_stack_events.py
manipulation/stack/mdp/observations.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
from typing import TYPE_CHECKING, Literal
import torch
import isaaclab.utils.math as math_utils
from isaaclab.assets import Articulation, RigidObject, RigidObjectCollection
from isaaclab.managers import SceneEntityCfg
from isaaclab.sensors import FrameTransformer
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
def cube_positions_in_world_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
) -> torch.Tensor:
"""The position of the cubes in the world frame."""
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
cube_3: RigidObject = env.scene[cube_3_cfg.name]
return torch.cat((cube_1.data.root_pos_w, cube_2.data.root_pos_w, cube_3.data.root_pos_w), dim=1)
def instance_randomize_cube_positions_in_world_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
) -> torch.Tensor:
"""The position of the cubes in the world frame."""
if not hasattr(env, "rigid_objects_in_focus"):
return torch.full((env.num_envs, 9), fill_value=-1)
cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name]
cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name]
cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name]
cube_1_pos_w = []
cube_2_pos_w = []
cube_3_pos_w = []
for env_id in range(env.num_envs):
cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3])
cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3])
cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3])
cube_1_pos_w = torch.stack(cube_1_pos_w)
cube_2_pos_w = torch.stack(cube_2_pos_w)
cube_3_pos_w = torch.stack(cube_3_pos_w)
return torch.cat((cube_1_pos_w, cube_2_pos_w, cube_3_pos_w), dim=1)
def cube_orientations_in_world_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
):
"""The orientation of the cubes in the world frame."""
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
cube_3: RigidObject = env.scene[cube_3_cfg.name]
return torch.cat((cube_1.data.root_quat_w, cube_2.data.root_quat_w, cube_3.data.root_quat_w), dim=1)
def instance_randomize_cube_orientations_in_world_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
) -> torch.Tensor:
"""The orientation of the cubes in the world frame."""
if nothasattr(env, "rigid_objects_in_focus"):
return torch.full((env.num_envs, 9), fill_value=-1)
cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name]
cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name]
cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name]
cube_1_quat_w = []
cube_2_quat_w = []
cube_3_quat_w = []
for env_id in range(env.num_envs):
cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4])
cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4])
cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4])
cube_1_quat_w = torch.stack(cube_1_quat_w)
cube_2_quat_w = torch.stack(cube_2_quat_w)
cube_3_quat_w = torch.stack(cube_3_quat_w)
return torch.cat((cube_1_quat_w, cube_2_quat_w, cube_3_quat_w), dim=1)
def object_obs(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"),
):
"""
Object observations (in world frame):
cube_1 pos,
cube_1 quat,
cube_2 pos,
cube_2 quat,
cube_3 pos,
cube_3 quat,
gripper to cube_1,
gripper to cube_2,
gripper to cube_3,
cube_1 to cube_2,
cube_2 to cube_3,
cube_1 to cube_3,
"""
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
cube_3: RigidObject = env.scene[cube_3_cfg.name]
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
cube_1_pos_w = cube_1.data.root_pos_w
cube_1_quat_w = cube_1.data.root_quat_w
cube_2_pos_w = cube_2.data.root_pos_w
cube_2_quat_w = cube_2.data.root_quat_w
cube_3_pos_w = cube_3.data.root_pos_w
cube_3_quat_w = cube_3.data.root_quat_w
ee_pos_w = ee_frame.data.target_pos_w[:, 0, :]
gripper_to_cube_1 = cube_1_pos_w - ee_pos_w
gripper_to_cube_2 = cube_2_pos_w - ee_pos_w
gripper_to_cube_3 = cube_3_pos_w - ee_pos_w
cube_1_to_2 = cube_1_pos_w - cube_2_pos_w
cube_2_to_3 = cube_2_pos_w - cube_3_pos_w
cube_1_to_3 = cube_1_pos_w - cube_3_pos_w
return torch.cat(
(
cube_1_pos_w - env.scene.env_origins,
cube_1_quat_w,
cube_2_pos_w - env.scene.env_origins,
cube_2_quat_w,
cube_3_pos_w - env.scene.env_origins,
cube_3_quat_w,
gripper_to_cube_1,
gripper_to_cube_2,
gripper_to_cube_3,
cube_1_to_2,
cube_2_to_3,
cube_1_to_3,
),
dim=1,
)
def instance_randomize_object_obs(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"),
):
"""
Object observations (in world frame):
cube_1 pos,
cube_1 quat,
cube_2 pos,
cube_2 quat,
cube_3 pos,
cube_3 quat,
gripper to cube_1,
gripper to cube_2,
gripper to cube_3,
cube_1 to cube_2,
cube_2 to cube_3,
cube_1 to cube_3,
"""
if nothasattr(env, "rigid_objects_in_focus"):
return torch.full((env.num_envs, 9), fill_value=-1)
cube_1: RigidObjectCollection = env.scene[cube_1_cfg.name]
cube_2: RigidObjectCollection = env.scene[cube_2_cfg.name]
cube_3: RigidObjectCollection = env.scene[cube_3_cfg.name]
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
cube_1_pos_w = []
cube_2_pos_w = []
cube_3_pos_w = []
cube_1_quat_w = []
cube_2_quat_w = []
cube_3_quat_w = []
for env_id in range(env.num_envs):
cube_1_pos_w.append(cube_1.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][0], :3])
cube_2_pos_w.append(cube_2.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][1], :3])
cube_3_pos_w.append(cube_3.data.object_pos_w[env_id, env.rigid_objects_in_focus[env_id][2], :3])
cube_1_quat_w.append(cube_1.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][0], :4])
cube_2_quat_w.append(cube_2.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][1], :4])
cube_3_quat_w.append(cube_3.data.object_quat_w[env_id, env.rigid_objects_in_focus[env_id][2], :4])
cube_1_pos_w = torch.stack(cube_1_pos_w)
cube_2_pos_w = torch.stack(cube_2_pos_w)
cube_3_pos_w = torch.stack(cube_3_pos_w)
cube_1_quat_w = torch.stack(cube_1_quat_w)
cube_2_quat_w = torch.stack(cube_2_quat_w)
cube_3_quat_w = torch.stack(cube_3_quat_w)
ee_pos_w = ee_frame.data.target_pos_w[:, 0, :]
gripper_to_cube_1 = cube_1_pos_w - ee_pos_w
gripper_to_cube_2 = cube_2_pos_w - ee_pos_w
gripper_to_cube_3 = cube_3_pos_w - ee_pos_w
cube_1_to_2 = cube_1_pos_w - cube_2_pos_w
cube_2_to_3 = cube_2_pos_w - cube_3_pos_w
cube_1_to_3 = cube_1_pos_w - cube_3_pos_w
return torch.cat(
(
cube_1_pos_w - env.scene.env_origins,
cube_1_quat_w,
cube_2_pos_w - env.scene.env_origins,
cube_2_quat_w,
cube_3_pos_w - env.scene.env_origins,
cube_3_quat_w,
gripper_to_cube_1,
gripper_to_cube_2,
gripper_to_cube_3,
cube_1_to_2,
cube_2_to_3,
cube_1_to_3,
),
dim=1,
)
def ee_frame_pos(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor:
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
ee_frame_pos = ee_frame.data.target_pos_w[:, 0, :] - env.scene.env_origins[:, 0:3]
return ee_frame_pos
def ee_frame_quat(env: ManagerBasedRLEnv, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame")) -> torch.Tensor:
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
ee_frame_quat = ee_frame.data.target_quat_w[:, 0, :]
return ee_frame_quat
def gripper_pos(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
) -> torch.Tensor:
"""
Obtain the versatile gripper position of both Gripper and Suction Cup.
"""
robot: Articulation = env.scene[robot_cfg.name]
if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0:
# Handle multiple surface grippers by concatenating their states
gripper_states = []
for gripper_name, surface_gripper in env.scene.surface_grippers.items():
gripper_states.append(surface_gripper.state.view(-1, 1))
if len(gripper_states) == 1:
return gripper_states[0]
else:
return torch.cat(gripper_states, dim=1)
else:
if hasattr(env.cfg, "gripper_joint_names"):
gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names)
assertlen(gripper_joint_ids) == 2, "Observation gripper_pos only support parallel gripper for now"
finger_joint_1 = robot.data.joint_pos[:, gripper_joint_ids[0]].clone().unsqueeze(1)
finger_joint_2 = -1 * robot.data.joint_pos[:, gripper_joint_ids[1]].clone().unsqueeze(1)
return torch.cat((finger_joint_1, finger_joint_2), dim=1)
else:
raise NotImplementedError("[Error] Cannot find gripper_joint_names in the environment config")
def object_grasped(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg,
ee_frame_cfg: SceneEntityCfg,
object_cfg: SceneEntityCfg,
diff_threshold: float = 0.06,
) -> torch.Tensor:
"""Check if an object is grasped by the specified robot."""
robot: Articulation = env.scene[robot_cfg.name]
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
object: RigidObject = env.scene[object_cfg.name]
object_pos = object.data.root_pos_w
end_effector_pos = ee_frame.data.target_pos_w[:, 0, :]
pose_diff = torch.linalg.vector_norm(object_pos - end_effector_pos, dim=1)
if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0:
surface_gripper = env.scene.surface_grippers["surface_gripper"]
suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open
suction_cup_is_closed = (suction_cup_status == 1).to(torch.float32)
grasped = torch.logical_and(suction_cup_is_closed, pose_diff < diff_threshold)
else:
if hasattr(env.cfg, "gripper_joint_names"):
gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names)
assertlen(gripper_joint_ids) == 2, "Observations only support parallel gripper for now"
# Support per-joint open values (list/tuple) or single scalar (applied to all joints)
if isinstance(env.cfg.gripper_open_val, (list, tuple)):
open_val_0 = env.cfg.gripper_open_val[0]
open_val_1 = env.cfg.gripper_open_val[1]
else:
open_val_0 = env.cfg.gripper_open_val
open_val_1 = env.cfg.gripper_open_val
grasped = torch.logical_and(
pose_diff < diff_threshold,
torch.abs(
robot.data.joint_pos[:, gripper_joint_ids[0]]
- torch.tensor(open_val_0, dtype=torch.float32).to(env.device)
)
> env.cfg.gripper_threshold,
)
grasped = torch.logical_and(
grasped,
torch.abs(
robot.data.joint_pos[:, gripper_joint_ids[1]]
- torch.tensor(open_val_1, dtype=torch.float32).to(env.device)
)
> env.cfg.gripper_threshold,
)
return grasped
def object_stacked(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg,
upper_object_cfg: SceneEntityCfg,
lower_object_cfg: SceneEntityCfg,
xy_threshold: float = 0.05,
height_threshold: float = 0.005,
height_diff: float = 0.0468,
) -> torch.Tensor:
"""Check if an object is stacked by the specified robot."""
robot: Articulation = env.scene[robot_cfg.name]
upper_object: RigidObject = env.scene[upper_object_cfg.name]
lower_object: RigidObject = env.scene[lower_object_cfg.name]
pos_diff = upper_object.data.root_pos_w - lower_object.data.root_pos_w
height_dist = torch.linalg.vector_norm(pos_diff[:, 2:], dim=1)
xy_dist = torch.linalg.vector_norm(pos_diff[:, :2], dim=1)
stacked = torch.logical_and(xy_dist < xy_threshold, (height_dist - height_diff) < height_threshold)
if hasattr(env.scene, "surface_grippers") and len(env.scene.surface_grippers) > 0:
surface_gripper = env.scene.surface_grippers["surface_gripper"]
suction_cup_status = surface_gripper.state.view(-1, 1) # 1: closed, 0: closing, -1: open
suction_cup_is_open = (suction_cup_status == -1).to(torch.float32)
stacked = torch.logical_and(suction_cup_is_open, stacked)
else:
if hasattr(env.cfg, "gripper_joint_names"):
gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names)
assertlen(gripper_joint_ids) == 2, "Observations only support parallel gripper for now"
# Support per-joint open values (list/tuple) or single scalar (applied to all joints)
if isinstance(env.cfg.gripper_open_val, (list, tuple)):
open_val_0 = env.cfg.gripper_open_val[0]
open_val_1 = env.cfg.gripper_open_val[1]
else:
open_val_0 = env.cfg.gripper_open_val
open_val_1 = env.cfg.gripper_open_val
stacked = torch.logical_and(
torch.isclose(
robot.data.joint_pos[:, gripper_joint_ids[0]],
torch.tensor(open_val_0, dtype=torch.float32).to(env.device),
atol=1e-4,
rtol=1e-4,
),
stacked,
)
stacked = torch.logical_and(
torch.isclose(
robot.data.joint_pos[:, gripper_joint_ids[1]],
torch.tensor(open_val_1, dtype=torch.float32).to(env.device),
atol=1e-4,
rtol=1e-4,
),
stacked,
)
else:
raise ValueError("No gripper_joint_names found in environment config")
return stacked
def cube_poses_in_base_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
return_key: Literal["pos", "quat", None] = None,
) -> torch.Tensor:
"""The position and orientation of the cubes in the robot base frame."""
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
cube_3: RigidObject = env.scene[cube_3_cfg.name]
pos_cube_1_world = cube_1.data.root_pos_w
pos_cube_2_world = cube_2.data.root_pos_w
pos_cube_3_world = cube_3.data.root_pos_w
quat_cube_1_world = cube_1.data.root_quat_w
quat_cube_2_world = cube_2.data.root_quat_w
quat_cube_3_world = cube_3.data.root_quat_w
robot: Articulation = env.scene[robot_cfg.name]
root_pos_w = robot.data.root_pos_w
root_quat_w = robot.data.root_quat_w
pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, pos_cube_1_world, quat_cube_1_world
)
pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, pos_cube_2_world, quat_cube_2_world
)
pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, pos_cube_3_world, quat_cube_3_world
)
pos_cubes_base = torch.cat((pos_cube_1_base, pos_cube_2_base, pos_cube_3_base), dim=1)
quat_cubes_base = torch.cat((quat_cube_1_base, quat_cube_2_base, quat_cube_3_base), dim=1)
if return_key == "pos":
return pos_cubes_base
elif return_key == "quat":
return quat_cubes_base
else:
return torch.cat((pos_cubes_base, quat_cubes_base), dim=1)
def object_abs_obs_in_base_frame(
env: ManagerBasedRLEnv,
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg = SceneEntityCfg("cube_3"),
ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"),
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
):
"""
Object Abs observations (in base frame): remove the relative observations,
and add abs gripper pos and quat in robot base frame
cube_1 pos,
cube_1 quat,
cube_2 pos,
cube_2 quat,
cube_3 pos,
cube_3 quat,
gripper pos,
gripper quat,
"""
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
cube_3: RigidObject = env.scene[cube_3_cfg.name]
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
robot: Articulation = env.scene[robot_cfg.name]
root_pos_w = robot.data.root_pos_w
root_quat_w = robot.data.root_quat_w
cube_1_pos_w = cube_1.data.root_pos_w
cube_1_quat_w = cube_1.data.root_quat_w
cube_2_pos_w = cube_2.data.root_pos_w
cube_2_quat_w = cube_2.data.root_quat_w
cube_3_pos_w = cube_3.data.root_pos_w
cube_3_quat_w = cube_3.data.root_quat_w
pos_cube_1_base, quat_cube_1_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, cube_1_pos_w, cube_1_quat_w
)
pos_cube_2_base, quat_cube_2_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, cube_2_pos_w, cube_2_quat_w
)
pos_cube_3_base, quat_cube_3_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, cube_3_pos_w, cube_3_quat_w
)
ee_pos_w = ee_frame.data.target_pos_w[:, 0, :]
ee_quat_w = ee_frame.data.target_quat_w[:, 0, :]
ee_pos_base, ee_quat_base = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
return torch.cat(
(
pos_cube_1_base,
quat_cube_1_base,
pos_cube_2_base,
quat_cube_2_base,
pos_cube_3_base,
quat_cube_3_base,
ee_pos_base,
ee_quat_base,
),
dim=1,
)
def ee_frame_pose_in_base_frame(
env: ManagerBasedRLEnv,
ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"),
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
return_key: Literal["pos", "quat", None] = None,
) -> torch.Tensor:
"""
The end effector pose in the robot base frame.
"""
ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name]
ee_frame_pos_w = ee_frame.data.target_pos_w[:, 0, :]
ee_frame_quat_w = ee_frame.data.target_quat_w[:, 0, :]
robot: Articulation = env.scene[robot_cfg.name]
root_pos_w = robot.data.root_pos_w
root_quat_w = robot.data.root_quat_w
ee_pos_in_base, ee_quat_in_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, ee_frame_pos_w, ee_frame_quat_w
)
if return_key == "pos":
return ee_pos_in_base
elif return_key == "quat":
return ee_quat_in_base
else:
return torch.cat((ee_pos_in_base, ee_quat_in_base), dim=1)manipulation/stack/mdp/terminations.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Common functions that can be used to activate certain terminations for the lift task.
The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable
the termination introduced by the function.
"""
from __future__ import annotations
from typing import TYPE_CHECKING
import torch
from isaaclab.assets import Articulation, RigidObject
from isaaclab.managers import SceneEntityCfg
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
defcubes_stacked(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
cube_1_cfg: SceneEntityCfg = SceneEntityCfg("cube_1"),
cube_2_cfg: SceneEntityCfg = SceneEntityCfg("cube_2"),
cube_3_cfg: SceneEntityCfg | None = SceneEntityCfg("cube_3"),
xy_threshold: float = 0.04,
height_threshold: float = 0.005,
height_diff: float = 0.0468,
atol: float = 0.0001,
rtol: float = 0.0001,
) -> torch.Tensor:
robot: Articulation = env.scene[robot_cfg.name]
cube_1: RigidObject = env.scene[cube_1_cfg.name]
cube_2: RigidObject = env.scene[cube_2_cfg.name]
pos_diff_c12 = cube_1.data.root_pos_w - cube_2.data.root_pos_w
# Compute cube position difference in x-y plane
xy_dist_c12 = torch.norm(pos_diff_c12[:, :2], dim=1)
# Compute cube height difference
h_dist_c12 = torch.norm(pos_diff_c12[:, 2:], dim=1)
# Check cube positions
stacked = xy_dist_c12 < xy_threshold
stacked = torch.logical_and(h_dist_c12 - height_diff < height_threshold, stacked)
stacked = torch.logical_and(pos_diff_c12[:, 2] < 0.0, stacked)
if cube_3_cfg isnotNone:
cube_3: RigidObject = env.scene[cube_3_cfg.name]
pos_diff_c23 = cube_2.data.root_pos_w - cube_3.data.root_pos_w
# Compute cube position difference in x-y plane
xy_dist_c23 = torch.norm(pos_diff_c23[:, :2], dim=1)
# Compute cube height difference
h_dist_c23 = torch.norm(pos_diff_c23[:, 2:], dim=1)
# Check cube positions
stacked = torch.logical_and(xy_dist_c23 < xy_threshold, stacked)
stacked = torch.logical_and(h_dist_c23 - height_diff < height_threshold, stacked)
stacked = torch.logical_and(pos_diff_c23[:, 2] < 0.0, stacked)
# Check gripper positions
ifhasattr(env.scene, "surface_grippers") andlen(env.scene.surface_grippers) > 0:
surface_gripper = env.scene.surface_grippers["surface_gripper"]
suction_cup_status = surface_gripper.state.view(-1) # 1: closed, 0: closing, -1: open
suction_cup_is_open = (suction_cup_status == -1).to(torch.float32)
stacked = torch.logical_and(suction_cup_is_open, stacked)
else:
ifhasattr(env.cfg, "gripper_joint_names"):
gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names)
assertlen(gripper_joint_ids) == 2, "Terminations only support parallel gripper for now"
# Support per-joint open values (list/tuple) or single scalar (applied to all joints)
ifisinstance(env.cfg.gripper_open_val, (list, tuple)):
open_val_0 = env.cfg.gripper_open_val[0]
open_val_1 = env.cfg.gripper_open_val[1]
else:
open_val_0 = env.cfg.gripper_open_val
open_val_1 = env.cfg.gripper_open_val
stacked = torch.logical_and(
torch.isclose(
robot.data.joint_pos[:, gripper_joint_ids[0]],
torch.tensor(open_val_0, dtype=torch.float32).to(env.device),
atol=atol,
rtol=rtol,
),
stacked,
)
stacked = torch.logical_and(
torch.isclose(
robot.data.joint_pos[:, gripper_joint_ids[1]],
torch.tensor(open_val_1, dtype=torch.float32).to(env.device),
atol=atol,
rtol=rtol,
),
stacked,
)
else:
raise ValueError("No gripper_joint_names found in environment config")
return stackedmanipulation/stack/config/piper/piper_stack_joint_pos_env_cfg.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab.assets import RigidObjectCfg
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.sensors import FrameTransformerCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg
from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from agx_teleop.tasks.manager_based.manipulation.stack import mdp
from agx_teleop.tasks.manager_based.manipulation.stack.mdp import piper_stack_events
from agx_teleop.tasks.manager_based.manipulation.stack.stack_env_cfg import StackEnvCfg
##
# Pre-defined configs
##
from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip
from agx_teleop.assets.piper import PIPER_GRIPPER_CFG # isort: skip
@configclass
classEventCfg:
"""Configuration for events."""
init_piper_arm_pose = EventTerm(
func=piper_stack_events.set_default_joint_pose,
mode="reset",
params={
"default_pose": [0.0, 1.57, -1.57, 0.0, 1.22, 0.0, 0.0, 0.0], # 6机械臂+2夹爪，夹爪关闭
},
)
randomize_piper_joint_state = EventTerm(
func=piper_stack_events.randomize_joint_by_gaussian_offset,
mode="reset",
params={
"mean": 0.0,
"std": 0.02,
"asset_cfg": SceneEntityCfg("robot"),
},
)
randomize_cube_positions = EventTerm(
func=piper_stack_events.randomize_object_pose,
mode="reset",
params={
"pose_range": {"x": (0.3, 0.5), "y": (-0.10, 0.10), "z": (0.0203, 0.0203), "yaw": (-1.0, 1, 0)},
"min_separation": 0.12,
"asset_cfgs": [SceneEntityCfg("cube_1"), SceneEntityCfg("cube_2"), SceneEntityCfg("cube_3")],
},
)
@configclass
classPiperCubeStackEnvCfg(StackEnvCfg):
"""Configuration for the Piper Cube Stack Environment."""
def__post_init__(self):
# post init of parent
super().__post_init__()
# Set events
self.events = EventCfg()
# Set Piper as robot
self.scene.robot = PIPER_GRIPPER_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene.robot.spawn.semantic_tags = [("class", "robot")]
# Add semantics to table
self.scene.table.spawn.semantic_tags = [("class", "table")]
# Add semantics to ground
self.scene.plane.semantic_tags = [("class", "ground")]
# Set actions for the specific robot type (piper)
self.actions.arm_action = mdp.JointPositionActionCfg(
asset_name="robot", joint_names=["joint.*"], scale=0.5, use_default_offset=True
)
self.actions.gripper_action = mdp.BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=["gripper_joint.*"],
open_command_expr={
"gripper_joint1": 0.05,
"gripper_joint2": -0.05,
},
close_command_expr={
"gripper_joint1": 0.0,
"gripper_joint2": 0.0,
},
)
# utilities for gripper status check
self.gripper_joint_names = ["gripper_joint.*"]
self.gripper_open_val = [0.05, -0.05]
self.gripper_threshold = 0.005
# Rigid body properties of each cube
cube_properties = RigidBodyPropertiesCfg(
solver_position_iteration_count=16,
solver_velocity_iteration_count=1,
max_angular_velocity=1000.0,
max_linear_velocity=1000.0,
max_depenetration_velocity=5.0,
disable_gravity=False,
)
# Set each stacking cube deterministically
self.scene.cube_1 = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Cube_1",
init_state=RigidObjectCfg.InitialStateCfg(pos=[0.4, 0.0, 0.0203], rot=[1, 0, 0, 0]),
spawn=UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/blue_block.usd",
scale=(1.0, 1.0, 1.0),
rigid_props=cube_properties,
semantic_tags=[("class", "cube_1")],
),
)
self.scene.cube_2 = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Cube_2",
init_state=RigidObjectCfg.InitialStateCfg(pos=[0.55, 0.05, 0.0203], rot=[1, 0, 0, 0]),
spawn=UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/red_block.usd",
scale=(1.0, 1.0, 1.0),
rigid_props=cube_properties,
semantic_tags=[("class", "cube_2")],
),
)
self.scene.cube_3 = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Cube_3",
init_state=RigidObjectCfg.InitialStateCfg(pos=[0.60, -0.1, 0.0203], rot=[1, 0, 0, 0]),
spawn=UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd",
scale=(1.0, 1.0, 1.0),
rigid_props=cube_properties,
semantic_tags=[("class", "cube_3")],
),
)
# Listens to the required transforms
marker_cfg = FRAME_MARKER_CFG.copy()
marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
marker_cfg.prim_path = "/Visuals/FrameTransformer"
self.scene.ee_frame = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base_link",
debug_vis=False,
visualizer_cfg=marker_cfg,
target_frames=[
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/gripper_base",
name="end_effector",
offset=OffsetCfg(
pos=[0.0, 0.0, 0.14], # gripper_base 到 TCP 的距离
),
),
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/gripper_link1",
name="tool_rightfinger",
offset=OffsetCfg(
pos=(0.0, 0.0, 0.0),
),
),
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/gripper_link2",
name="tool_leftfinger",
offset=OffsetCfg(
pos=(0.0, 0.0, 0.0),
),
),
],
)stack/config/piper/piper_stack_ik_rel_env_cfg.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.devices.device_base import DeviceBase, DevicesCfg
from isaaclab.devices.keyboard import Se3KeyboardCfg
from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg
from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg
from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg
from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.utils import configclass
from agx_teleop.tasks.manager_based.manipulation.stack.stack_env_cfg import mdp
from . import piper_stack_joint_pos_env_cfg
##
# Pre-defined configs
##
from agx_teleop.assets.piper import PIPER_GRIPPER_HIGH_PD_CFG # isort: skip
@configclass
classPiperCubeStackEnvCfg(piper_stack_joint_pos_env_cfg.PiperCubeStackEnvCfg):
def__post_init__(self):
# post init of parent
super().__post_init__()
# Set Piper as robot
# We switch here to a stiffer PD controller for IK tracking to be better.
self.scene.robot = PIPER_GRIPPER_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Set actions for the specific robot type (piper)
self.actions.arm_action = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=["joint.*"],
body_name="gripper_base",
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.14]),
)
self.teleop_devices = DevicesCfg(
devices={
"handtracking": OpenXRDeviceCfg(
retargeters=[
Se3RelRetargeterCfg(
bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT,
zero_out_xy_rotation=True,
use_wrist_rotation=False,
use_wrist_position=True,
delta_pos_scale_factor=10.0,
delta_rot_scale_factor=10.0,
sim_device=self.sim.device,
),
GripperRetargeterCfg(
bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device
),
],
sim_device=self.sim.device,
xr_cfg=self.xr,
),
"keyboard": Se3KeyboardCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
}
)
@configclass
classPiperCubeStackRedGreenEnvCfg(PiperCubeStackEnvCfg):
def__post_init__(self):
# post init of parent
super().__post_init__()
self.terminations.success = DoneTerm(
func=mdp.cubes_stacked,
params={"cube_1_cfg": SceneEntityCfg("cube_2"), "cube_2_cfg": SceneEntityCfg("cube_3"), "cube_3_cfg": None},
)
@configclass
classPiperCubeStackRedGreenBlueEnvCfg(PiperCubeStackEnvCfg):
def__post_init__(self):
# post init of parent
super().__post_init__()
self.terminations.success = DoneTerm(
func=mdp.cubes_stacked,
params={
"cube_1_cfg": SceneEntityCfg("cube_2"),
"cube_2_cfg": SceneEntityCfg("cube_3"),
"cube_3_cfg": SceneEntityCfg("cube_1"),
},
)
@configclass
classPiperCubeStackBlueGreenEnvCfg(PiperCubeStackEnvCfg):
def__post_init__(self):
# post init of parent
super().__post_init__()
self.terminations.success = DoneTerm(
func=mdp.cubes_stacked,
params={"cube_1_cfg": SceneEntityCfg("cube_1"), "cube_2_cfg": SceneEntityCfg("cube_3"), "cube_3_cfg": None},
)
@configclass
classPiperCubeStackBlueGreenRedEnvCfg(PiperCubeStackEnvCfg):
def__post_init__(self):
# post init of parent
super().__post_init__()
self.terminations.success = DoneTerm(
func=mdp.cubes_stacked,
params={
"cube_1_cfg": SceneEntityCfg("cube_1"),
"cube_2_cfg": SceneEntityCfg("cube_3"),
"cube_3_cfg": SceneEntityCfg("cube_2"),
},
)stack/config/piper/init.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import gymnasium as gym
# from isaaclab_tasks.manager_based.manipulation.stack.config.franka import agents
from agx_teleop.tasks.manager_based.manipulation.stack.config.piper import agents
##
# Register Gym environments.
##
gym.register(
id="Isaac-Stack-Cube-Piper-IK-Rel-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": f"{__name__}.piper_stack_ik_rel_env_cfg:PiperCubeStackEnvCfg",
"robomimic_bc_cfg_entry_point": f"{agents.__name__}:robomimic/bc_rnn_low_dim.json",
},
disable_env_checker=True,
)修改任务后需要重装外部项目
cd ~/IsaacLab
./isaaclab.sh -p -m pip install -e /home/hql/agilex/isaac-cosmos/agx_teleop/source/agx_teleop五、键盘遥操作与数据采集实现
1.遥操作配置
cd agx_teleop
mkdir -p scripts/tools
touch scripts/tools/record_demos.py \
scripts/tools/repaly_demos.pyrecord_demos.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
Script to 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."""
# Standard library imports
import argparse
import contextlib
# Isaac Lab AppLauncher
from isaaclab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Record demonstrations for Isaac Lab environments.")
parser.add_argument("--task", type=str, required=True, help="Name of the task.")
parser.add_argument(
"--teleop_device",
type=str,
default="keyboard",
help=(
"Teleop device. Set here (legacy) or via the environment config. If using the environment config, pass the"
" device key/name defined under 'teleop_devices' (it can be a custom name, not necessarily 'handtracking')."
" Built-ins: keyboard, spacemouse, gamepad. Not all tasks support all built-ins."
),
)
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.",
)
parser.add_argument(
"--enable_pinocchio",
action="store_true",
default=False,
help="Enable Pinocchio.",
)
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
# Validate required arguments
if args_cli.task is None:
parser.error("--task is required")
app_launcher_args = vars(args_cli)
if args_cli.enable_pinocchio:
# Import pinocchio before AppLauncher to force the use of the version
# installed by IsaacLab and not the one installed by Isaac Sim.
# pinocchio is required by the Pink IK controllers and the GR1T2 retargeter
import pinocchio # noqa: F401
if "handtracking" in args_cli.teleop_device.lower():
app_launcher_args["xr"] = True
# launch the simulator
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
# Third-party imports
import logging
import os
import time
import gymnasium as gym
import torch
import omni.ui as ui
from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg
from isaaclab.devices.openxr import remove_camera_configs
from isaaclab.devices.teleop_device_factory import create_teleop_device
import isaaclab_mimic.envs # noqa: F401
from isaaclab_mimic.ui.instruction_display import InstructionDisplay, show_subtask_instructions
if args_cli.enable_pinocchio:
import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401
import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401
from collections.abc import Callable
from isaaclab.envs import DirectRLEnvCfg, ManagerBasedRLEnvCfg
from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg
from isaaclab.envs.ui import EmptyWindow
from isaaclab.managers import DatasetExportMode
# import isaaclab_tasks # noqa: F401
import agx_teleop.tasks # noqa: F401
from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
# import logger
logger = logging.getLogger(__name__)
class RateLimiter:
"""Convenience class for enforcing rates in loops."""
def __init__(self, hz: int):
"""Initialize a RateLimiter with specified frequency.
Args:
hz: Frequency to enforce in Hertz.
"""
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: gym.Env):
"""Attempt to sleep at the specified rate in hz.
Args:
env: Environment to render during sleep periods.
"""
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 setup_output_directories() -> tuple[str, str]:
"""Set up output directories for saving demonstrations.
Creates the output directory if it doesn't exist and extracts the file name
from the dataset file path.
Returns:
tuple[str, str]: A tuple containing:
- output_dir: The directory path where the dataset will be saved
- output_file_name: The filename (without extension) for the dataset
"""
# 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)
print(f"Created output directory: {output_dir}")
return output_dir, output_file_name
def create_environment_config(
output_dir: str, output_file_name: str
) -> tuple[ManagerBasedRLEnvCfg | DirectRLEnvCfg, object | None]:
"""Create and configure the environment configuration.
Parses the environment configuration and makes necessary adjustments for demo recording.
Extracts the success termination function and configures the recorder manager.
Args:
output_dir: Directory where recorded demonstrations will be saved
output_file_name: Name of the file to store the demonstrations
Returns:
tuple[isaaclab_tasks.utils.parse_cfg.EnvCfg, Optional[object]]: A tuple containing:
- env_cfg: The configured environment configuration
- success_term: The success termination object or None if not available
Raises:
Exception: If parsing the environment configuration fails
"""
# parse configuration
try:
env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=1)
env_cfg.env_name = args_cli.task.split(":")[-1]
except Exception as e:
logger.error(f"Failed to parse environment configuration: {e}")
exit(1)
# 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:
logger.warning(
"No success termination term was found in the environment."
" Will not be able to mark recorded demos as successful."
)
if args_cli.xr:
# If cameras are not enabled and XR is enabled, remove camera configs
if not args_cli.enable_cameras:
env_cfg = remove_camera_configs(env_cfg)
env_cfg.sim.render.antialiasing_mode = "DLSS"
# 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
env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_SUCCEEDED_ONLY
return env_cfg, success_term
def create_environment(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg) -> gym.Env:
"""Create the environment from the configuration.
Args:
env_cfg: The environment configuration object that defines the environment properties.
This should be an instance of EnvCfg created by parse_env_cfg().
Returns:
gym.Env: A Gymnasium environment instance for the specified task.
Raises:
Exception: If environment creation fails for any reason.
"""
try:
env = gym.make(args_cli.task, cfg=env_cfg).unwrapped
return env
except Exception as e:
logger.error(f"Failed to create environment: {e}")
exit(1)
def setup_teleop_device(callbacks: dict[str, Callable]) -> object:
"""Set up the teleoperation device based on configuration.
Attempts to create a teleoperation device based on the environment configuration.
Falls back to default devices if the specified device is not found in the configuration.
Args:
callbacks: Dictionary mapping callback keys to functions that will be
attached to the teleop device
Returns:
object: The configured teleoperation device interface
Raises:
Exception: If teleop device creation fails
"""
teleop_interface = None
try:
if hasattr(env_cfg, "teleop_devices") and args_cli.teleop_device in env_cfg.teleop_devices.devices:
teleop_interface = create_teleop_device(args_cli.teleop_device, env_cfg.teleop_devices.devices, callbacks)
else:
logger.warning(
f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default."
)
# Create fallback teleop device
if args_cli.teleop_device.lower() == "keyboard":
teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.2, rot_sensitivity=0.5))
elif args_cli.teleop_device.lower() == "spacemouse":
teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=0.2, rot_sensitivity=0.5))
else:
logger.error(f"Unsupported teleop device: {args_cli.teleop_device}")
logger.error("Supported devices: keyboard, spacemouse, handtracking")
exit(1)
# Add callbacks to fallback device
for key, callback in callbacks.items():
teleop_interface.add_callback(key, callback)
except Exception as e:
logger.error(f"Failed to create teleop device: {e}")
exit(1)
if teleop_interface is None:
logger.error("Failed to create teleop interface")
exit(1)
return teleop_interface
def setup_ui(label_text: str, env: gym.Env) -> InstructionDisplay:
"""Set up the user interface elements.
Creates instruction display and UI window with labels for showing information
to the user during demonstration recording.
Args:
label_text: Text to display showing current recording status
env: The environment instance for which UI is being created
Returns:
InstructionDisplay: The configured instruction display object
"""
instruction_display = InstructionDisplay(args_cli.xr)
if not args_cli.xr:
window = EmptyWindow(env, "Instruction")
with window.ui_window_elements["main_vstack"]:
demo_label = ui.Label(label_text)
subtask_label = ui.Label("")
instruction_display.set_labels(subtask_label, demo_label)
return instruction_display
def process_success_condition(env: gym.Env, success_term: object | None, success_step_count: int) -> tuple[int, bool]:
"""Process the success condition for the current step.
Checks if the environment has met the success condition for the required
number of consecutive steps. Marks the episode as successful if criteria are met.
Args:
env: The environment instance to check
success_term: The success termination object or None if not available
success_step_count: Current count of consecutive successful steps
Returns:
tuple[int, bool]: A tuple containing:
- updated success_step_count: The updated count of consecutive successful steps
- success_reset_needed: Boolean indicating if reset is needed due to success
"""
if success_term is None:
return success_step_count, False
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])
print("Success condition met! Recording completed.")
return success_step_count, True
else:
success_step_count = 0
return success_step_count, False
def handle_reset(
env: gym.Env, success_step_count: int, instruction_display: InstructionDisplay, label_text: str
) -> int:
"""Handle resetting the environment.
Resets the environment, recorder manager, and related state variables.
Updates the instruction display with current status.
Args:
env: The environment instance to reset
success_step_count: Current count of consecutive successful steps
instruction_display: The display object to update
label_text: Text to display showing current recording status
Returns:
int: Reset success step count (0)
"""
print("Resetting environment...")
env.sim.reset()
env.recorder_manager.reset()
env.reset()
success_step_count = 0
instruction_display.show_demo(label_text)
return success_step_count
def run_simulation_loop(
env: gym.Env,
teleop_interface: object | None,
success_term: object | None,
rate_limiter: RateLimiter | None,
) -> int:
"""Run the main simulation loop for collecting demonstrations.
Sets up callback functions for teleop device, initializes the UI,
and runs the main loop that processes user inputs and environment steps.
Records demonstrations when success conditions are met.
Args:
env: The environment instance
teleop_interface: Optional teleop interface (will be created if None)
success_term: The success termination object or None if not available
rate_limiter: Optional rate limiter to control simulation speed
Returns:
int: Number of successful demonstrations recorded
"""
current_recorded_demo_count = 0
success_step_count = 0
should_reset_recording_instance = False
running_recording_instance = not args_cli.xr
# Callback closures for the teleop device
def reset_recording_instance():
nonlocal should_reset_recording_instance
should_reset_recording_instance = True
print("Recording instance reset requested")
def start_recording_instance():
nonlocal running_recording_instance
running_recording_instance = True
print("Recording started")
def stop_recording_instance():
nonlocal running_recording_instance
running_recording_instance = False
print("Recording paused")
# Set up teleoperation callbacks
teleoperation_callbacks = {
"R": reset_recording_instance,
"START": start_recording_instance,
"STOP": stop_recording_instance,
"RESET": reset_recording_instance,
}
teleop_interface = setup_teleop_device(teleoperation_callbacks)
teleop_interface.add_callback("R", reset_recording_instance)
# Reset before starting
env.sim.reset()
env.reset()
teleop_interface.reset()
label_text = f"Recorded {current_recorded_demo_count} successful demonstrations."
instruction_display = setup_ui(label_text, env)
subtasks = {}
with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode():
while simulation_app.is_running():
# Get keyboard command
action = teleop_interface.advance()
# Expand to batch dimension
actions = action.repeat(env.num_envs, 1)
# Perform action on environment
if running_recording_instance:
# Compute actions based on environment
obv = env.step(actions)
if subtasks is not None:
if subtasks == {}:
subtasks = obv[0].get("subtask_terms")
elif subtasks:
show_subtask_instructions(instruction_display, subtasks, obv, env.cfg)
else:
env.sim.render()
# Check for success condition
success_step_count, success_reset_needed = process_success_condition(env, success_term, success_step_count)
if success_reset_needed:
should_reset_recording_instance = True
# Update 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
label_text = f"Recorded {current_recorded_demo_count} successful demonstrations."
print(label_text)
# Check if we've reached the desired number of demos
if args_cli.num_demos > 0 and env.recorder_manager.exported_successful_episode_count >= args_cli.num_demos:
label_text = f"All {current_recorded_demo_count} demonstrations recorded.\nExiting the app."
instruction_display.show_demo(label_text)
print(label_text)
target_time = time.time() + 0.8
while time.time() < target_time:
if rate_limiter:
rate_limiter.sleep(env)
else:
env.sim.render()
break
# Handle reset if requested
if should_reset_recording_instance:
success_step_count = handle_reset(env, success_step_count, instruction_display, label_text)
should_reset_recording_instance = False
# Check if simulation is stopped
if env.sim.is_stopped():
break
# Rate limiting
if rate_limiter:
rate_limiter.sleep(env)
return current_recorded_demo_count
def main() -> None:
"""Collect demonstrations from the environment using teleop interfaces.
Main function that orchestrates the entire process:
1. Sets up rate limiting based on configuration
2. Creates output directories for saving demonstrations
3. Configures the environment
4. Runs the simulation loop to collect demonstrations
5. Cleans up resources when done
Raises:
Exception: Propagates exceptions from any of the called functions
"""
# if handtracking is selected, rate limiting is achieved via OpenXR
if args_cli.xr:
rate_limiter = None
from isaaclab.ui.xr_widgets import TeleopVisualizationManager, XRVisualization
# Assign the teleop visualization manager to the visualization system
XRVisualization.assign_manager(TeleopVisualizationManager)
else:
rate_limiter = RateLimiter(args_cli.step_hz)
# Set up output directories
output_dir, output_file_name = setup_output_directories()
# Create and configure environment
global env_cfg # Make env_cfg available to setup_teleop_device
env_cfg, success_term = create_environment_config(output_dir, output_file_name)
# Create environment
env = create_environment(env_cfg)
# Run simulation loop
current_recorded_demo_count = run_simulation_loop(env, None, success_term, rate_limiter)
# Clean up
env.close()
print(f"Recording session completed with {current_recorded_demo_count} successful demonstrations")
print(f"Demonstrations saved to: {args_cli.dataset_file}")
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()replay_demos.py
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Script to replay demonstrations with Isaac Lab environments."""
"""Launch Isaac Sim Simulator first."""
import argparse
from isaaclab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Replay demonstrations in Isaac Lab environments.")
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to replay episodes.")
parser.add_argument("--task", type=str, default=None, help="Force to use the specified task.")
parser.add_argument(
"--select_episodes",
type=int,
nargs="+",
default=[],
help="A list of episode indices to be replayed. Keep empty to replay all in the dataset file.",
)
parser.add_argument("--dataset_file", type=str, default="datasets/dataset.hdf5", help="Dataset file to be replayed.")
parser.add_argument(
"--validate_states",
action="store_true",
default=False,
help=(
"Validate if the states, if available, match between loaded from datasets and replayed. Only valid if"
" --num_envs is 1."
),
)
parser.add_argument(
"--validate_success_rate",
action="store_true",
default=False,
help="Validate the replay success rate using the task environment termination criteria",
)
parser.add_argument(
"--enable_pinocchio",
action="store_true",
default=False,
help="Enable Pinocchio.",
)
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
# args_cli.headless = True
if args_cli.enable_pinocchio:
# Import pinocchio before AppLauncher to force the use of the version
# installed by IsaacLab and not the one installed by Isaac Sim.
# pinocchio is required by the Pink IK controllers and the GR1T2 retargeter
import pinocchio # noqa: F401
# launch the simulator
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
import contextlib
import os
import gymnasium as gym
import torch
from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg
from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler
if args_cli.enable_pinocchio:
import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401
import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401
# import isaaclab_tasks # noqa: F401
import agx_teleop.tasks # noqa: F401
from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
is_paused = False
def play_cb():
global is_paused
is_paused = False
def pause_cb():
global is_paused
is_paused = True
def compare_states(state_from_dataset, runtime_state, runtime_env_index) -> (bool, str):
"""Compare states from dataset and runtime.
Args:
state_from_dataset: State from dataset.
runtime_state: State from runtime.
runtime_env_index: Index of the environment in the runtime states to be compared.
Returns:
bool: True if states match, False otherwise.
str: Log message if states don't match.
"""
states_matched = True
output_log = ""
for asset_type in ["articulation", "rigid_object"]:
for asset_name in runtime_state[asset_type].keys():
for state_name in runtime_state[asset_type][asset_name].keys():
runtime_asset_state = runtime_state[asset_type][asset_name][state_name][runtime_env_index]
dataset_asset_state = state_from_dataset[asset_type][asset_name][state_name]
if len(dataset_asset_state) != len(runtime_asset_state):
raise ValueError(f"State shape of {state_name} for asset {asset_name} don't match")
for i in range(len(dataset_asset_state)):
if abs(dataset_asset_state[i] - runtime_asset_state[i]) > 0.01:
states_matched = False
output_log += f'\tState ["{asset_type}"]["{asset_name}"]["{state_name}"][{i}] don\'t match\r\n'
output_log += f"\t Dataset:\t{dataset_asset_state[i]}\r\n"
output_log += f"\t Runtime: \t{runtime_asset_state[i]}\r\n"
return states_matched, output_log
def main():
"""Replay episodes loaded from a file."""
global is_paused
# Load dataset
if not os.path.exists(args_cli.dataset_file):
raise FileNotFoundError(f"The dataset file {args_cli.dataset_file} does not exist.")
dataset_file_handler = HDF5DatasetFileHandler()
dataset_file_handler.open(args_cli.dataset_file)
env_name = dataset_file_handler.get_env_name()
episode_count = dataset_file_handler.get_num_episodes()
if episode_count == 0:
print("No episodes found in the dataset.")
exit()
episode_indices_to_replay = args_cli.select_episodes
if len(episode_indices_to_replay) == 0:
episode_indices_to_replay = list(range(episode_count))
if args_cli.task is not None:
env_name = args_cli.task.split(":")[-1]
if env_name is None:
raise ValueError("Task/env name was not specified nor found in the dataset.")
num_envs = args_cli.num_envs
env_cfg = parse_env_cfg(env_name, device=args_cli.device, num_envs=num_envs)
# extract success checking function to invoke in the main loop
success_term = None
if args_cli.validate_success_rate:
if hasattr(env_cfg.terminations, "success"):
success_term = env_cfg.terminations.success
env_cfg.terminations.success = None
else:
print(
"No success termination term was found in the environment."
" Will not be able to mark recorded demos as successful."
)
# Disable all recorders and terminations
env_cfg.recorders = {}
env_cfg.terminations = {}
# create environment from loaded config
env = gym.make(args_cli.task, cfg=env_cfg).unwrapped
teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.1, rot_sensitivity=0.1))
teleop_interface.add_callback("N", play_cb)
teleop_interface.add_callback("B", pause_cb)
print('Press "B" to pause and "N" to resume the replayed actions.')
# Determine if state validation should be conducted
state_validation_enabled = False
if args_cli.validate_states and num_envs == 1:
state_validation_enabled = True
elif args_cli.validate_states and num_envs > 1:
print("Warning: State validation is only supported with a single environment. Skipping state validation.")
# Get idle action (idle actions are applied to envs without next action)
if hasattr(env_cfg, "idle_action"):
idle_action = env_cfg.idle_action.repeat(num_envs, 1)
else:
idle_action = torch.zeros(env.action_space.shape)
# reset before starting
env.reset()
teleop_interface.reset()
# simulate environment -- run everything in inference mode
episode_names = list(dataset_file_handler.get_episode_names())
replayed_episode_count = 0
recorded_episode_count = 0
# Track current episode indices for each environment
current_episode_indices = [None] * num_envs
# Track failed demo IDs
failed_demo_ids = []
with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode():
while simulation_app.is_running() and not simulation_app.is_exiting():
env_episode_data_map = {index: EpisodeData() for index in range(num_envs)}
first_loop = True
has_next_action = True
episode_ended = [False] * num_envs
while has_next_action:
# initialize actions with idle action so those without next action will not move
actions = idle_action
has_next_action = False
for env_id in range(num_envs):
env_next_action = env_episode_data_map[env_id].get_next_action()
if env_next_action is None:
# check if the episode is successful after the whole episode_data is
if (
(success_term is not None)
and (current_episode_indices[env_id]) is not None
and (not episode_ended[env_id])
):
if bool(success_term.func(env, **success_term.params)[env_id]):
recorded_episode_count += 1
plural_trailing_s = "s" if recorded_episode_count > 1 else ""
print(
f"Successfully replayed {recorded_episode_count} episode{plural_trailing_s} out"
f" of {replayed_episode_count} demos."
)
else:
# if not successful, add to failed demo IDs list
if (
current_episode_indices[env_id] is not None
and current_episode_indices[env_id] not in failed_demo_ids
):
failed_demo_ids.append(current_episode_indices[env_id])
episode_ended[env_id] = True
next_episode_index = None
while episode_indices_to_replay:
next_episode_index = episode_indices_to_replay.pop(0)
if next_episode_index < episode_count:
episode_ended[env_id] = False
break
next_episode_index = None
if next_episode_index is not None:
replayed_episode_count += 1
current_episode_indices[env_id] = next_episode_index
print(f"{replayed_episode_count:4}: Loading #{next_episode_index} episode to env_{env_id}")
episode_data = dataset_file_handler.load_episode(
episode_names[next_episode_index], env.device
)
env_episode_data_map[env_id] = episode_data
# Set initial state for the new episode
initial_state = episode_data.get_initial_state()
env.reset_to(initial_state, torch.tensor([env_id], device=env.device), is_relative=True)
# Get the first action for the new episode
env_next_action = env_episode_data_map[env_id].get_next_action()
has_next_action = True
else:
continue
else:
has_next_action = True
actions[env_id] = env_next_action
if first_loop:
first_loop = False
else:
while is_paused:
env.sim.render()
continue
env.step(actions)
if state_validation_enabled:
state_from_dataset = env_episode_data_map[0].get_next_state()
if state_from_dataset is not None:
print(
f"Validating states at action-index: {env_episode_data_map[0].next_state_index - 1:4}",
end="",
)
current_runtime_state = env.scene.get_state(is_relative=True)
states_matched, comparison_log = compare_states(state_from_dataset, current_runtime_state, 0)
if states_matched:
print("\t- matched.")
else:
print("\t- mismatched.")
print(comparison_log)
break
# Close environment after replay in complete
plural_trailing_s = "s" if replayed_episode_count > 1 else ""
print(f"Finished replaying {replayed_episode_count} episode{plural_trailing_s}.")
# Print success statistics only if validation was enabled
if success_term is not None:
print(f"Successfully replayed: {recorded_episode_count}/{replayed_episode_count}")
# Print failed demo IDs if any
if failed_demo_ids:
print(f"\nFailed demo IDs ({len(failed_demo_ids)} total):")
print(f" {sorted(failed_demo_ids)}")
env.close()
if __name__ == "__main__":
# run the main function
main()
# close sim app
simulation_app.close()2.录制脚本
keyboard遥操作方法
cd /home/hql/agilex/isaac-cosmos/agx_teleop
~/IsaacLab/isaaclab.sh -p ./scripts/tools/record_demos.py \
--task Isaac-Stack-Cube-Piper-IK-Rel-v0 \
--device cpu \
--teleop_device keyboard \
--dataset_file ./datasets/dataset.hdf5 \
--num_demos 10
Keyboard Controller for SE(3): Se3Keyboard
Reset all commands: R
Toggle gripper (open/close): K
Move arm along x-axis: W/S
Move arm along y-axis: A/D
Move arm along z-axis: Q/E
Rotate arm along x-axis: Z/X
Rotate arm along y-axis: T/G
Rotate arm along z-axis: C/V手柄遥操作
cd /home/hql/agilex/isaac-cosmos/agx_teleop
~/IsaacLab/isaaclab.sh -p ./scripts/tools/record_demos.py \
--task Isaac-Stack-Cube-Piper-IK-Rel-v0 \
--device cpu \
--teleop_device gamepad \
--dataset_file ./datasets/dataset_gamepad.hdf5 \
--num_demos 10 \
--step_hz 60
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
0
0- 0
已为社区贡献8条内容
所有评论(0)