Isaac Lab Teleoperation and Data Collection Workflow | Piper Robotic Arm for Embodied AI
0.Preface
This project provides an Isaac Lab-based teleoperation and demonstration collection workflow for the AgileX Piper robotic arm. It is designed for developers working on Embodied AI, robot learning, and imitation learning pipelines using simulation environments.
The workflow supports:
- keyboard-based robot teleoperation
- cube stacking manipulation tasks
- trajectory recording and replay
- imitation learning dataset generation
- ROS2-compatible robotic development
- Isaac Lab manipulation environments
Project Has Already Completed
- employment of the Piper robotic arm inside NVIDIA Isaac Lab
- keyboard teleoperation for robot manipulation
- cube stacking task environment configuration
- URDF-to-USD robot asset conversion
- gripper and actuator parameter configuration
- observation pipeline customization for Piper grippers
- human demonstration data collection workflow
- Robomimic-compatible imitation learning configuration
- replay support for collected robot trajectories
Recommended Environment
- Operating System: Ubuntu 22.04 or compatible Linux distribution
- Simulation Framework: NVIDIA Isaac Lab
- ROS Version: ROS 2 Humble
- Python Version: Python 3.10+
Reference & Resource
-
Official IsaacLab Tutorial: Welcome to Isaac Lab! — Isaac Lab Documentation
-
IsaacLab: GitHub - isaac-sim/IsaacLab: Unified framework for robot learning built on NVIDIA Isaac Sim · GitHub)
-
Project Repository: GitHub - szyzp/IsaacLab_Data_Collection · GitHub
The following section provides how developers can rapidly build a complete robotic manipulation and data collection workflow, steps are shown below:
Pipeline Overview
Keyboard Input
↓
Robot Teleoperation
↓
Isaac Lab Simulation
↓
Trajectory Recording
↓
Dataset Generation
↓
Imitation Learning / RL Training
Step 1.Create an Isaac Lab External Project
Before building the teleoperation and data collection workflow, first create an external Isaac Lab project environment.
1.1 Create a New Isaac Lab External Project
1.1.1 Initialize the Project
Run the following command inside the Isaac Lab root directory.
Make sure Isaac Lab has already been installed and the isaaclab Conda environment is activated.
conda activate isaaclab
./isaaclab.sh --new
1.1.2 Recommended Project Configuration
| Setting | Recommended Value |
|---|---|
| Project Type | External |
| Project Path | /home/agilex/isaac-cosmos/ |
| Project Name | agx_teleop |
| Workflow | Manager-based|single-agent |
| RL Library | optional |
| Project Path | /home/agilex/isaac-cosmos/ |
Configuration are shown above
The initial project structure of agx_teleop is as follows:
├── pyproject.toml
├── README.md
├── scripts
│ ├── list_envs.py
│ ├── random_agent.py
│ ├── sb3
│ │ ├── play.py
│ │ └── train.py
│ └── zero_agent.py
└── source
└── agx_teleop
├── agx_teleop
│ ├── __init__.py
│ ├── tasks
│ │ ├── __init__.py
│ │ └── manager_based
│ │ ├── agx_teleop
│ │ │ ├── agents
│ │ │ │ ├── __init__.py
│ │ │ │ └── sb3_ppo_cfg.yaml
│ │ │ ├── agx_teleop_env_cfg.py
│ │ │ ├── __init__.py
│ │ │ └── mdp
│ │ │ ├── __init__.py
│ │ │ └── rewards.py
│ │ └── __init__.py
│ └── ui_extension_example.py
├── agx_teleop.egg-info
│ ├── dependency_links.txt
│ ├── not-zip-safe
│ ├── PKG-INFO
│ ├── requires.txt
│ ├── SOURCES.txt
│ └── top_level.txt
├── config
│ └── extension.toml
├── docs
│ └── CHANGELOG.rst
├── pyproject.toml
└── setup.py
1.1.3 Install External Projects
Running the following commands:
cd ../agx_teleop
python -m pip install -e source/agx_teleop
1.2 Configure the PiPER USD Asset
1.2.1 Prepare the URDF
Clone the official AgileX PiPER URDF repository:
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.git
Then merge:
piper_with_gripper_description.xacropiper_description.urdf
into a unified:piper_gripper.urdf
Complete PiPER URDF file content:
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot name="piper">
<link name="world"/>
<joint name="world_to_base_link" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="base_link">
<inertial>
<origin xyz="-0.00473641164191482 2.56829134630247E-05 0.041451518036016" rpy="0 0 0"/>
<mass value="1.02" />
<inertia ixx="0.00267433" ixy="-0.00000073" ixz="-0.00017389" iyy="0.00282612" iyz="0.0000004" izz="0.00089624"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/base_link.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/base_link.stl"/>
</geometry>
</collision>
</link>
<link name="link1">
<inertial>
<origin xyz="0.00032 -0.00041 -0.00348" rpy="0 0 0"/>
<mass value="0.71"/>
<inertia ixx="0.00050027" ixy="0.00000078" ixz="0.00000626" iyy="0.00040879" iyz="0.00000462" izz="0.00045802"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link1.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link1.stl"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="revolute">
<origin xyz="0 0 0.123" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<limit lower="-2.6179938" upper="2.6179938" effort="100" velocity="5"/>
</joint>
<link name="link2">
<inertial>
<origin xyz="0.21852 -0.00861 0.00126" rpy="0 0 0"/>
<mass value="1.16"/>
<inertia ixx="0.00111915" ixy="0.00182916" ixz="0.00019233" iyy="0.06763251" iyz="0.00000767" izz="0.067559"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link2.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link2.stl"/>
</geometry>
</collision>
</link>
<joint name="joint2" type="revolute">
<origin xyz="0 0 0" rpy="1.5707963 -0.1357866 -3.1415926"/>
<parent link="link1"/>
<child link="link2"/>
<axis xyz="0 0 1"/>
<limit lower="0" upper="3.1415926" effort="100" velocity="5"/>
</joint>
<link name="link3">
<inertial>
<origin xyz="-0.01999 -0.14808 -0.00053" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.01347608" ixy="0.00161034" ixz="0.00000066" iyy="0.00046377" iyz="0.00000162" izz="0.01363321"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link3.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link3.stl"/>
</geometry>
</collision>
</link>
<joint name="joint3" type="revolute">
<origin xyz="0.28503 0 0" rpy="0 0 -1.7938494"/>
<parent link="link2"/>
<child link="link3"/>
<axis xyz="0 0 1"/>
<limit lower="-2.9670597" upper="0" effort="100" velocity="5"/>
</joint>
<link name="link4">
<inertial>
<origin xyz="0.00013 -0.00076 -0.00394" rpy="0 0 0"/>
<mass value="0.38"/>
<inertia ixx="0.00017844" ixy="0.00000062" ixz="0.00000067" iyy="0.00018914" iyz="0.00000408" izz="0.00014613"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link4.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link4.stl"/>
</geometry>
</collision>
</link>
<joint name="joint4" type="revolute">
<origin xyz="-0.02198 -0.25075 0" rpy="1.5707963 0 0"/>
<parent link="link3"/>
<child link="link4"/>
<axis xyz="0 0 1"/>
<limit lower="-1.7453292" upper="1.7453292" effort="100" velocity="5"/>
</joint>
<link name="link5">
<inertial>
<origin xyz="0.00018 -0.06104 -0.00227" rpy="0 0 0"/>
<mass value="0.383"/>
<inertia ixx="0.00172318" ixy="0.00000259" ixz="0.00000010" iyy="0.00017936" iyz="0.00000990" izz="0.00171172"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link5.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link5.stl"/>
</geometry>
</collision>
</link>
<joint name="joint5" type="revolute">
<origin xyz="0 0 0" rpy="-1.5707963 0 0"/>
<parent link="link4"/>
<child link="link5"/>
<axis xyz="0 0 1"/>
<limit lower="-1.2217304" upper="1.2217304" effort="100" velocity="5"/>
</joint>
<link name="link6">
<inertial>
<origin xyz="-0.00003 0.00006 -0.002" rpy="0 0 0"/>
<mass value="0.007"/>
<inertia ixx="0.00152112" ixy="0.00000202" ixz="0.00000261" iyy="0.00138626" iyz="0.00000002" izz="0.00031152"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/link6.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/link6.stl"/>
</geometry>
</collision>
</link>
<joint name="joint6" type="revolute">
<origin xyz="8.8259E-05 -0.091 0" rpy="1.5707963 0 0"/>
<parent link="link5"/>
<child link="link6"/>
<axis xyz="0 0 1"/>
<limit lower="-2.0943951" upper="2.0943951" effort="100" velocity="5"/>
</joint>
<!-- Gripper components -->
<link name="gripper_base">
<inertial>
<origin xyz="-0.000183807162235591 8.05033155577911E-05 0.0321436689908876" rpy="0 0 0"/>
<mass value="0.45"/>
<inertia ixx="0.00092934" ixy="0.00000034" ixz="-0.00000738" iyy="0.00071447" iyz="0.00000005" izz="0.00039442"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/gripper_base.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/gripper_base.stl"/>
</geometry>
</collision>
</link>
<joint name="gripper_base_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="link6"/>
<child link="gripper_base"/>
</joint>
<link name="gripper_link1">
<inertial>
<origin xyz="0.00065123185041968 -0.0491929869131989 0.00972258769184025" rpy="0 0 0"/>
<mass value="0.025"/>
<inertia ixx="0.00007371" ixy="-0.00000113" ixz="0.00000021" iyy="0.00000781" iyz="-0.00001372" izz="0.0000747"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/gripper_link1.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/gripper_link1.stl"/>
</geometry>
</collision>
</link>
<joint name="gripper_joint1" type="prismatic">
<origin xyz="0 0 0.1358" rpy="1.5707963 0 0"/>
<parent link="gripper_base"/>
<child link="gripper_link1"/>
<axis xyz="0 0 1"/>
<limit lower="0" upper="0.05" effort="10" velocity="3"/>
</joint>
<link name="gripper_link2">
<inertial>
<origin xyz="0.00065123185041968 -0.0491929869131989 0.00972258769184025" rpy="0 0 0"/>
<mass value="0.025"/>
<inertia ixx="0.00007371" ixy="-0.00000113" ixz="0.00000021" iyy="0.00000781" iyz="-0.00001372" izz="0.0000747"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/dae/gripper_link2.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="../meshes/gripper_link2.stl"/>
</geometry>
</collision>
</link>
<joint name="gripper_joint2" type="prismatic">
<origin xyz="0 0 0.1358" rpy="1.5707963 0 -3.1415926"/>
<parent link="gripper_base"/>
<child link="gripper_link2"/>
<axis xyz="0 0 -1"/>
<limit lower="-0.05" upper="0" effort="10" velocity="3"/>
</joint>
</robot>
1.2.2 Convert URDF to USD
Isaac Lab uses USD assets for simulation.
Convert the Piper URDF into a USD robot asset:
python ../IsaacLab/scripts/tools/convert_urdf.py \
/home/agilex/isaac-cosmos/agx_teleop/source/agx_teleop/agx_description/agx_arm_urdf/piper/urdf/piper_gripper.urdf \
/home/agilex/isaac-cosmos/agx_teleop/source/agx_teleop/agx_description/agx_arm_urdf/piper/usd/piper.usd \
--fix-base
Step 2.Configure the PiPER Robot in Isaac Lab
2.1 Create the Robot Asset Configuration
cd agx_teleop
mkdir -p source/agx_teleop/agx_teleop/assets
2.1.1 Create the Asset Package
Create the assets/__init__.py file:
"""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."""
# Robot description assets
AGX_TELEOP_DESCRIPTION_DIR = os.path.join(
AGX_TELEOP_EXT_DIR,
"agx_description"
)
"""Path to the agx_description directory."""
2.1.2 Configure the PiPER Robot Asset
Create the assets/piper.py file
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(
# USD robot asset
spawn=sim_utils.UsdFileCfg(
usd_path=f"{AGX_TELEOP_DESCRIPTION_DIR}/agx_arm_urdf/piper/usd/piper.usd",
# Rigid body physics settings
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=5.0,
),
# Articulation solver settings
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=True,
solver_position_iteration_count=8,
solver_velocity_iteration_count=0,
),
),
# Initial robot pose
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),
),
# Actuator configuration
actuators={
# Shoulder joints
"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,
),
# Forearm joints
"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,
),
# Parallel gripper
"gripper": ImplicitActuatorCfg(
joint_names_expr=[
"gripper_joint1",
"gripper_joint2"
],
effort_limit_sim=22,
velocity_limit_sim=1.5,
stiffness=800.0,
damping=20.0,
),
},
# Soft joint limit scaling
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.
"""
Step 3.Build the Manipulation Environment
3.1 Project Structure Setup
The following structure is modified from the official IsaacLab stack task isaaclab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack. We will create a specialized directory for the PiPER robot under the agx_teleop project.
Create Directories
cd agx_teleop
mkdir -p source/agx_teleop/agx_teleop/tasks/manager_based/manipulation/stack/config/piper/agents/robomimic
mkdir -p source/agx_teleop/agx_teleop/tasks/manager_based/manipulation/stack/mdp
Initialize Files
cd 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
3.2 Core File Modifications
3.2.1 Standard IsaacLab Porting
The following files are ported directly from the IsaacLab Franka stack example without significant modification. They define the base MDP (Markov Decision Process) settings and the Robomimic imitation learning configuration.
- stack/stack_env_cfg.py: Defines the scene (table, lights, ground) and observation groups.
- stack/mdp/init.py: Handles module exports.
- stack/config/piper/agents/robomimic/bc_rnn_low_dim.json: Standard BC-RNN configuration for low-dimensional observations.
Copy the corresponding code from IsaacLab without modification :
/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/__init__.py
/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/agents/robomimic/bc_rnn_low_dim.json
3.2.2 Event Customization (mdp/piper_stack_events.py)
Copy the corresponding code from Issaclab directly, without any modifying :
Isaac/IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/mdp/franka_stack_events.py
3.2.3 Adapting Observations & Terminations for Piper
Copy the corresponding code of isaaclab and modify the part related to gripper_open_val. The Franka panda gripper control uses a single joint value, while the piper gripper control uses two joint values, which need to be adjusted to match the piper gripper.
Key Logic Adjustment (Lines 320-343 & 380-407):
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 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_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 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]
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)
assert len(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)
assert len(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)
assert len(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)
Similarly, the cubes_stacked termination condition is updated to ensure the success criteria check both gripper joints before concluding the task.
Below is the complete code, with relevant modifications in Lines 81-109.
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
def cubes_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 is not None:
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
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: 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)
assert len(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)
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=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 stacked
3.2.4 Piper Joint Position Configuration (piper_stack_joint_pos_env_cfg.py)
This file maps the Piper asset to the environment.
Major changes include:
- Default Pose: Adjusted to [0.0, 1.57, -1.57, 0.0, 1.22, 0.0, 0.0, 0.0] (6 ARM joints + 2 Gripper joints).
- Gripper Command Mapping:
- Open: gripper_joint1: 0.05, gripper_joint2: -0.05
- Close: 0.0 for both.
- End-Effector (EE) Offset: Set to [0.0, 0.0, 0.14] to account for the Piper gripper’s length from the base link to the Tool Center Point (TCP).
# 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
class EventCfg:
"""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
class PiperCubeStackEnvCfg(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),
),
),
],
)
3.2.5 Inverse Kinematics Configuration (piper_stack_ik_rel_env_cfg.py)
This file configures the Differential IK controller. We use a high-gain PD controller (PIPER_GRIPPER_HIGH_PD_CFG) to ensure precise tracking during teleoperation or inference.
# 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
class PiperCubeStackEnvCfg(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
class PiperCubeStackRedGreenEnvCfg(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
class PiperCubeStackRedGreenBlueEnvCfg(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
class PiperCubeStackBlueGreenEnvCfg(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
class PiperCubeStackBlueGreenRedEnvCfg(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"),
},
)
3.2.6 Environment Registration (stack/config/piper/__init__.py)
Finally, register the task in Gymnasium so it can be launched via the IsaacLab runner:
# 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,
)
Step 4.Teleoperation & Data Collection
4.1 Teleoperation Configuration
Create the tool directory and script files within the agx_teleop plugin:
cd agx_teleop
mkdir -p scripts/tools
touch scripts/tools/record_demos.py \
scripts/tools/repaly_demos.py
Copy the corresponding IsaacLab source code and modify the imports for tasks. Required sections are listed below:
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
scripts/tools/repaly_demos.py:
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
4.2 Teleoperation Configuration
# First, create the dataset directory
mkdir -p datasets
# Command for teleoperated data collection; keyboard teleoperation used here. Available devices: spacemouse, keyboard, handtracking
python 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
# Replay the collected dataset
python scripts/tools/replay_demos.py \
--task Isaac-Stack-Cube-Piper-IK-Rel-v0 \
--device cpu --dataset_file ./datasets/dataset.hdf5
Keyboard Teleoperation Method:
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
Q & A
Q1: Why is the End-Effector (EE) offset set to [0.0, 0.0, 0.14] in the IK configuration?
A: This offset accounts for the physical length of the PiPER gripper. It shifts the Tool Center Point (TCP) from the gripper_base link to the actual center of the fingertips. Without this body_offset, the Inverse Kinematics (IK) controller would attempt to align the robot’s “wrist” with the target cube, causing the physical fingers to overshoot and collide with the objects.
Q2: How does the workflow ensure the quality and validity of the recorded .hdf5 datasets?
A: The workflow employs a two-layer validation system:
- Automated Filtering: The
record_demos.pyscript uses DatasetExportMode.EXPORT_SUCCEEDED_ONLY, meaning only episodes that trigger the success termination criteria (cubes stacked for a set number of steps) are saved. - Manual Verification: The
replay_demos.pyscript allows developers to re-simulate recorded actions to confirm that the recorded trajectories consistently result in a successful task outcome before starting training.
Q3: What is the reasoning behind recommending --device cpu during the teleoperation recording phase?
A: Teleoperation is highly sensitive to input latency. When recording demonstrations, the GPU is often heavily taxed by real-time rendering and physics calculations. Running the simulation logic on the CPU can reduce “input lag” and jitter, providing the human operator with a smoother control experience, which leads to higher-quality, more natural trajectory data for imitation learning.
Q4:In piper_stack_ik_rel_env_cfg.py, why choose the relative pose mode with use_relative_mode=True?
A: The Relative Mode is more intuitive for human teleoperation: the input from the keyboard or joystick is mapped to “increments of the current end position” rather than “absolute coordinates”. During training, relative control can also reduce the difficulty for the model to learn spatial features, as it focuses on motion trends rather than fixed global coordinates, which helps improve the model’s generalization ability across different environmental positions.
Q5: Why is the --fix-base parameter required during the URDF to USD conversion process?
A: PiPER robotic arms are usually fixed on workbenches. In Isaac Sim/Isaac Lab, if the robot base is not fixed (Fix), the robotic arm will displace or tip over in the virtual space due to reaction forces when performing high-dynamic actions. The --fix-base parameter rigidly connects the base_link in the URDF to the global coordinate system (World Frame) of the simulation world, ensuring the stability of the robotic arm base.
Have Question?
If you encounter any issues with environment installation, parameter configuration, or RL training, feel free to leave your questions for further discussion.