Unverified Commit ca2fd911 authored by rebeccazhang0707's avatar rebeccazhang0707 Committed by GitHub

Adds Agibot Humanoid two place tasks (#3228)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->

Adds two place tasks and mimic tasks with Agibot A2D humanoid, using
RMPFlow controller:

- add agibot robot config in agibot.py and .usd asset
- add motion_policy_configs and .urdf for rmpflow controller
- add new task cfg: place_toy2box_rmp_rel_env_cfg, and
place_upright_mug_rmp_rel_env_cfg
- add new mimic task cfg: agibot_place_toy2box_mimic_env_cfg, and
agibot_place_upright_mug_mimic_env_cfg
- add new mimic task: in pick_place_mimic_env.py
- add new subtasks in mdp.observations/terminations: object_grasped,
object_placed_upright, object_a_is_into_b

Notes: This PR relies on PR
(https://github.com/isaac-sim/IsaacLab/pull/3210) for RmpFlowAction
support.

You can test the whole gr00t-mimic workflow by:

1. Record Demos
```
./isaaclab.sh -p scripts/tools/record_demos.py \
  --dataset_file datasets/recorded_demos_agibot_right_arm_rel.hdf5 \
  --task Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 \
  --teleop_device spacemouse \
  --num_demos 1
```
2. Replay Demos
```
./isaaclab.sh -p scripts/tools/replay_demos.py \
  --dataset_file datasets/recorded_demos_agibot_right_arm_rel.hdf5 \
  --task Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 \
  --num_envs 1
```
3. Annotate Demos
```
./isaaclab.sh -p  scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \
  --input_file datasets/recorded_demos_agibot_right_arm_rel.hdf5 \
  --output_file datasets/annotated_demos_agibot_right_arm_rel.hdf5 \
  --task Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-Rel-Mimic-v0 \
  --auto 
```
4. Generate Demos
```
./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \
  --input_file datasets/annotated_demos_agibot_right_arm_rel.hdf5 \
  --output_file datasets/generated_demos_agibot_right_arm_rel.hdf5 \
  --task Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-Rel-Mimic-v0 \
  --num_envs 16 \
  --generation_num_trials 10
```

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- New feature (non-breaking change which adds functionality)

## Screenshot
<img width="1401" height="567" alt="environments_agibot"
src="https://github.com/user-attachments/assets/bc95e83a-e334-4dfc-a65c-3d1d97be0c4b"
/>


## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [ ] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

---------
Signed-off-by: 's avatarrebeccazhang0707 <168459200+rebeccazhang0707@users.noreply.github.com>
Signed-off-by: 's avatarKelly Guo <kellyg@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
parent 82169500
...@@ -149,6 +149,10 @@ for the lift-cube environment: ...@@ -149,6 +149,10 @@ for the lift-cube environment:
| |galbot_stack| | |galbot_stack-link| | Stack three cubes (bottom to top: blue, red, green) with the left arm of | | |galbot_stack| | |galbot_stack-link| | Stack three cubes (bottom to top: blue, red, green) with the left arm of |
| | | a Galbot humanoid robot | | | | a Galbot humanoid robot |
+-------------------------+------------------------------+-----------------------------------------------------------------------------+ +-------------------------+------------------------------+-----------------------------------------------------------------------------+
| |agibot_place_mug| | |agibot_place_mug-link| | Pick up and place a mug upright with a Agibot A2D humanoid robot |
+-------------------------+------------------------------+-----------------------------------------------------------------------------+
| |agibot_place_toy| | |agibot_place_toy-link| | Pick up and place an object in a box with a Agibot A2D humanoid robot |
+-------------------------+------------------------------+-----------------------------------------------------------------------------+
.. |reach-franka| image:: ../_static/tasks/manipulation/franka_reach.jpg .. |reach-franka| image:: ../_static/tasks/manipulation/franka_reach.jpg
.. |reach-ur10| image:: ../_static/tasks/manipulation/ur10_reach.jpg .. |reach-ur10| image:: ../_static/tasks/manipulation/ur10_reach.jpg
...@@ -162,6 +166,8 @@ for the lift-cube environment: ...@@ -162,6 +166,8 @@ for the lift-cube environment:
.. |surface-gripper| image:: ../_static/tasks/manipulation/ur10_stack_surface_gripper.jpg .. |surface-gripper| image:: ../_static/tasks/manipulation/ur10_stack_surface_gripper.jpg
.. |gr1_pp_waist| image:: ../_static/tasks/manipulation/gr-1_pick_place_waist.jpg .. |gr1_pp_waist| image:: ../_static/tasks/manipulation/gr-1_pick_place_waist.jpg
.. |galbot_stack| image:: ../_static/tasks/manipulation/galbot_stack_cube.jpg .. |galbot_stack| image:: ../_static/tasks/manipulation/galbot_stack_cube.jpg
.. |agibot_place_mug| image:: ../_static/tasks/manipulation/agibot_place_mug.jpg
.. |agibot_place_toy| image:: ../_static/tasks/manipulation/agibot_place_toy.jpg
.. |kuka-allegro-lift| image:: ../_static/tasks/manipulation/kuka_allegro_lift.jpg .. |kuka-allegro-lift| image:: ../_static/tasks/manipulation/kuka_allegro_lift.jpg
.. |kuka-allegro-reorient| image:: ../_static/tasks/manipulation/kuka_allegro_reorient.jpg .. |kuka-allegro-reorient| image:: ../_static/tasks/manipulation/kuka_allegro_reorient.jpg
...@@ -190,6 +196,9 @@ for the lift-cube environment: ...@@ -190,6 +196,9 @@ for the lift-cube environment:
.. |cube-shadow-lstm-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py>`__ .. |cube-shadow-lstm-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-LSTM-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py>`__
.. |cube-shadow-vis-link| replace:: `Isaac-Repose-Cube-Shadow-Vision-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py>`__ .. |cube-shadow-vis-link| replace:: `Isaac-Repose-Cube-Shadow-Vision-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py>`__
.. |agibot_place_mug-link| replace:: `Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py>`__
.. |agibot_place_toy-link| replace:: `Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py>`__
Contact-rich Manipulation Contact-rich Manipulation
~~~~~~~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~~~~~~
...@@ -987,6 +996,35 @@ inferencing, including reading from an already trained checkpoint and disabling ...@@ -987,6 +996,35 @@ inferencing, including reading from an already trained checkpoint and disabling
- Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Play-v0 - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Play-v0
- Manager Based - Manager Based
- -
* - Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-v0
- Isaac-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Play-v0
- Manager Based
-
* - Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0
-
- Manager Based
-
* - Isaac-Velocity-Flat-Anymal-B-v0 * - Isaac-Velocity-Flat-Anymal-B-v0
- Isaac-Velocity-Flat-Anymal-B-Play-v0 - Isaac-Velocity-Flat-Anymal-B-Play-v0
- Manager Based - Manager Based
......
...@@ -72,3 +72,23 @@ GALBOT_RIGHT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg( ...@@ -72,3 +72,23 @@ GALBOT_RIGHT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg(
) )
"""Configuration of RMPFlow for Galbot humanoid.""" """Configuration of RMPFlow for Galbot humanoid."""
AGIBOT_LEFT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg(
config_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_left_arm_rmpflow_config.yaml"),
urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "agibot.urdf"),
collision_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_left_arm_gripper.yaml"),
frame_name="gripper_center",
evaluations_per_frame=5,
ignore_robot_state_updates=True,
)
AGIBOT_RIGHT_ARM_RMPFLOW_CFG = RmpFlowControllerCfg(
config_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_right_arm_rmpflow_config.yaml"),
urdf_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "agibot.urdf"),
collision_file=os.path.join(ISAACLAB_NUCLEUS_RMPFLOW_DIR, "agibot", "rmpflow", "agibot_right_arm_gripper.yaml"),
frame_name="right_gripper_center",
evaluations_per_frame=5,
ignore_robot_state_updates=True,
)
"""Configuration of RMPFlow for Agibot humanoid."""
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for the Agibot A2D humanoid robots.
The following configurations are available:
* :obj:`AGIBOT_A2D_CFG`: Agibot A2D robot
"""
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
##
# Configuration
##
AGIBOT_A2D_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Agibot/A2D/A2D_physics.usd",
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_depenetration_velocity=5.0,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False,
solver_position_iteration_count=8,
solver_velocity_iteration_count=0,
),
),
init_state=ArticulationCfg.InitialStateCfg(
joint_pos={
# Body joints
"joint_lift_body": 0.1995,
"joint_body_pitch": 0.6025,
# Head joints
"joint_head_yaw": 0.0,
"joint_head_pitch": 0.6708,
# Left arm joints
"left_arm_joint1": -1.0817,
"left_arm_joint2": 0.5907,
"left_arm_joint3": 0.3442,
"left_arm_joint4": -1.2819,
"left_arm_joint5": 0.6928,
"left_arm_joint6": 1.4725,
"left_arm_joint7": -0.1599,
# Right arm joints
"right_arm_joint1": 1.0817,
"right_arm_joint2": -0.5907,
"right_arm_joint3": -0.3442,
"right_arm_joint4": 1.2819,
"right_arm_joint5": -0.6928,
"right_arm_joint6": -0.7,
"right_arm_joint7": 0.0,
# Left gripper joints
"left_Right_1_Joint": 0.0,
"left_hand_joint1": 0.994,
"left_Right_0_Joint": 0.0,
"left_Left_0_Joint": 0.0,
"left_Right_Support_Joint": 0.994,
"left_Left_Support_Joint": 0.994,
"left_Right_RevoluteJoint": 0.0,
"left_Left_RevoluteJoint": 0.0,
# Right gripper joints
"right_Right_1_Joint": 0.0,
"right_hand_joint1": 0.994,
"right_Right_0_Joint": 0.0,
"right_Left_0_Joint": 0.0,
"right_Right_Support_Joint": 0.994,
"right_Left_Support_Joint": 0.994,
"right_Right_RevoluteJoint": 0.0,
"right_Left_RevoluteJoint": 0.0,
},
pos=(-0.6, 0.0, -1.05), # init pos of the articulation for teleop
),
actuators={
# Body lift and torso actuators
"body": ImplicitActuatorCfg(
joint_names_expr=["joint_lift_body", "joint_body_pitch"],
effort_limit_sim=10000.0,
velocity_limit_sim=2.61,
stiffness=10000000.0,
damping=200.0,
),
# Head actuators
"head": ImplicitActuatorCfg(
joint_names_expr=["joint_head_yaw", "joint_head_pitch"],
effort_limit_sim=50.0,
velocity_limit_sim=1.0,
stiffness=80.0,
damping=4.0,
),
# Left arm actuator
"left_arm": ImplicitActuatorCfg(
joint_names_expr=["left_arm_joint[1-7]"],
effort_limit_sim={
"left_arm_joint1": 2000.0,
"left_arm_joint[2-7]": 1000.0,
},
velocity_limit_sim=1.57,
stiffness={"left_arm_joint1": 10000000.0, "left_arm_joint[2-7]": 20000.0},
damping={"left_arm_joint1": 0.0, "left_arm_joint[2-7]": 0.0},
),
# Right arm actuator
"right_arm": ImplicitActuatorCfg(
joint_names_expr=["right_arm_joint[1-7]"],
effort_limit_sim={
"right_arm_joint1": 2000.0,
"right_arm_joint[2-7]": 1000.0,
},
velocity_limit_sim=1.57,
stiffness={"right_arm_joint1": 10000000.0, "right_arm_joint[2-7]": 20000.0},
damping={"right_arm_joint1": 0.0, "right_arm_joint[2-7]": 0.0},
),
# "left_Right_2_Joint" is excluded from Articulation.
# "left_hand_joint1" is the driver joint, and "left_Right_1_Joint" is the mimic joint.
# "left_.*_Support_Joint" driver joint can be set optionally, to disable the driver, set stiffness and damping to 0.0 below
"left_gripper": ImplicitActuatorCfg(
joint_names_expr=["left_hand_joint1", "left_.*_Support_Joint"],
effort_limit_sim={"left_hand_joint1": 10.0, "left_.*_Support_Joint": 1.0},
velocity_limit_sim=2.0,
stiffness={"left_hand_joint1": 20.0, "left_.*_Support_Joint": 2.0},
damping={"left_hand_joint1": 0.10, "left_.*_Support_Joint": 0.01},
),
# set PD to zero for passive joints in close-loop gripper
"left_gripper_passive": ImplicitActuatorCfg(
joint_names_expr=["left_.*_(0|1)_Joint", "left_.*_RevoluteJoint"],
effort_limit_sim=10.0,
velocity_limit_sim=10.0,
stiffness=0.0,
damping=0.0,
),
# "right_Right_2_Joint" is excluded from Articulation.
# "right_hand_joint1" is the driver joint, and "right_Right_1_Joint" is the mimic joint.
# "right_.*_Support_Joint" driver joint can be set optionally, to disable the driver, set stiffness and damping to 0.0 below
"right_gripper": ImplicitActuatorCfg(
joint_names_expr=["right_hand_joint1", "right_.*_Support_Joint"],
effort_limit_sim={"right_hand_joint1": 100.0, "right_.*_Support_Joint": 100.0},
velocity_limit_sim=10.0,
stiffness={"right_hand_joint1": 20.0, "right_.*_Support_Joint": 2.0},
damping={"right_hand_joint1": 0.10, "right_.*_Support_Joint": 0.01},
),
# set PD to zero for passive joints in close-loop gripper
"right_gripper_passive": ImplicitActuatorCfg(
joint_names_expr=["right_.*_(0|1)_Joint", "right_.*_RevoluteJoint"],
effort_limit_sim=100.0,
velocity_limit_sim=10.0,
stiffness=0.0,
damping=0.0,
),
},
soft_joint_pos_limit_factor=1.0,
)
...@@ -142,3 +142,28 @@ gym.register( ...@@ -142,3 +142,28 @@ gym.register(
}, },
disable_env_checker=True, disable_env_checker=True,
) )
##
# Agibot Left Arm: Place Upright Mug with RmpFlow - Relative Pose Control
##
gym.register(
id="Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-Rel-Mimic-v0",
entry_point=f"{__name__}.pick_place_mimic_env:PickPlaceRelMimicEnv",
kwargs={
"env_cfg_entry_point": (
f"{__name__}.agibot_place_upright_mug_mimic_env_cfg:RmpFlowAgibotPlaceUprightMugMimicEnvCfg"
),
},
disable_env_checker=True,
)
##
# Agibot Right Arm: Place Toy2Box: RmpFlow - Relative Pose Control
##
gym.register(
id="Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-Rel-Mimic-v0",
entry_point=f"{__name__}.pick_place_mimic_env:PickPlaceRelMimicEnv",
kwargs={
"env_cfg_entry_point": f"{__name__}.agibot_place_toy2box_mimic_env_cfg:RmpFlowAgibotPlaceToy2BoxMimicEnvCfg",
},
disable_env_checker=True,
)
# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: Apache-2.0
from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig
from isaaclab.utils import configclass
from isaaclab_tasks.manager_based.manipulation.place.config.agibot.place_toy2box_rmp_rel_env_cfg import (
RmpFlowAgibotPlaceToy2BoxEnvCfg,
)
OBJECT_A_NAME = "toy_truck"
OBJECT_B_NAME = "box"
@configclass
class RmpFlowAgibotPlaceToy2BoxMimicEnvCfg(RmpFlowAgibotPlaceToy2BoxEnvCfg, MimicEnvCfg):
"""
Isaac Lab Mimic environment config class for Agibot Place Toy2Box env.
"""
def __post_init__(self):
# post init of parents
super().__post_init__()
self.datagen_config.name = "demo_src_place_toy2box_task_D0"
self.datagen_config.generation_guarantee = True
self.datagen_config.generation_keep_failed = True
self.datagen_config.generation_num_trials = 10
self.datagen_config.generation_select_src_per_subtask = True
self.datagen_config.generation_transform_first_robot_pose = False
self.datagen_config.generation_interpolate_from_last_target_pose = True
self.datagen_config.max_num_failures = 25
self.datagen_config.seed = 1
# The following are the subtask configurations for the stack task.
subtask_configs = []
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref=OBJECT_A_NAME,
# End of final subtask does not need to be detected
subtask_term_signal="grasp",
# No time offsets for the final subtask
subtask_term_offset_range=(2, 10),
# Selection strategy for source subtask segment
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
# selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.01,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=15,
# Additional fixed steps for the robot to reach the necessary pose
num_fixed_steps=0,
# If True, apply action noise during the interpolation phase and execution
apply_noise_during_interpolation=False,
)
)
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref=OBJECT_B_NAME,
# End of final subtask does not need to be detected
subtask_term_signal=None,
# No time offsets for the final subtask
subtask_term_offset_range=(0, 0),
# Selection strategy for source subtask segment
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
# selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.01,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=15,
# Additional fixed steps for the robot to reach the necessary pose
num_fixed_steps=0,
# If True, apply action noise during the interpolation phase and execution
apply_noise_during_interpolation=False,
)
)
self.subtask_configs["agibot"] = subtask_configs
# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: Apache-2.0
from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig
from isaaclab.utils import configclass
from isaaclab_tasks.manager_based.manipulation.place.config.agibot.place_upright_mug_rmp_rel_env_cfg import (
RmpFlowAgibotPlaceUprightMugEnvCfg,
)
@configclass
class RmpFlowAgibotPlaceUprightMugMimicEnvCfg(RmpFlowAgibotPlaceUprightMugEnvCfg, MimicEnvCfg):
"""
Isaac Lab Mimic environment config class for Agibot Place Upright Mug env.
"""
def __post_init__(self):
# post init of parents
super().__post_init__()
self.datagen_config.name = "demo_src_place_upright_mug_task_D0"
self.datagen_config.generation_guarantee = True
self.datagen_config.generation_keep_failed = True
self.datagen_config.generation_num_trials = 10
self.datagen_config.generation_select_src_per_subtask = True
self.datagen_config.generation_transform_first_robot_pose = False
self.datagen_config.generation_interpolate_from_last_target_pose = True
self.datagen_config.max_num_failures = 25
self.datagen_config.seed = 1
# The following are the subtask configurations for the stack task.
subtask_configs = []
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref="mug",
# End of final subtask does not need to be detected
subtask_term_signal="grasp",
# No time offsets for the final subtask
subtask_term_offset_range=(15, 30),
# Selection strategy for source subtask segment
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
# selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.01,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=5,
# Additional fixed steps for the robot to reach the necessary pose
num_fixed_steps=0,
# If True, apply action noise during the interpolation phase and execution
apply_noise_during_interpolation=False,
)
)
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref="mug",
# End of final subtask does not need to be detected
subtask_term_signal=None,
# No time offsets for the final subtask
subtask_term_offset_range=(0, 0),
# Selection strategy for source subtask segment
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
# selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.01,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=15,
# Additional fixed steps for the robot to reach the necessary pose
num_fixed_steps=0,
# If True, apply action noise during the interpolation phase and execution
apply_noise_during_interpolation=False,
)
)
self.subtask_configs["agibot"] = subtask_configs
# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: Apache-2.0
import torch
from collections.abc import Sequence
import isaaclab.utils.math as PoseUtils
from .franka_stack_ik_abs_mimic_env import FrankaCubeStackIKAbsMimicEnv
from .franka_stack_ik_rel_mimic_env import FrankaCubeStackIKRelMimicEnv
class PickPlaceRelMimicEnv(FrankaCubeStackIKRelMimicEnv):
"""
Isaac Lab Mimic environment wrapper class for DiffIK / RmpFlow Relative Pose Control env.
This MimicEnv is used when all observations are in the robot base frame.
"""
def get_object_poses(self, env_ids: Sequence[int] | None = None):
"""
Gets the pose of each object (including rigid objects and articulated objects) in the robot base frame.
Args:
env_ids: Environment indices to get the pose for. If None, all envs are considered.
Returns:
A dictionary that maps object names to object pose matrix in robot base frame (4x4 torch.Tensor)
"""
if env_ids is None:
env_ids = slice(None)
# Get scene state
scene_state = self.scene.get_state(is_relative=True)
rigid_object_states = scene_state["rigid_object"]
articulation_states = scene_state["articulation"]
# Get robot root pose
robot_root_pose = articulation_states["robot"]["root_pose"]
root_pos = robot_root_pose[env_ids, :3]
root_quat = robot_root_pose[env_ids, 3:7]
object_pose_matrix = dict()
# Process rigid objects
for obj_name, obj_state in rigid_object_states.items():
pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms(
root_pos, root_quat, obj_state["root_pose"][env_ids, :3], obj_state["root_pose"][env_ids, 3:7]
)
rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base)
object_pose_matrix[obj_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base)
# Process articulated objects (except robot)
for art_name, art_state in articulation_states.items():
if art_name != "robot": # Skip robot
pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms(
root_pos, root_quat, art_state["root_pose"][env_ids, :3], art_state["root_pose"][env_ids, 3:7]
)
rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base)
object_pose_matrix[art_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base)
return object_pose_matrix
def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]:
"""
Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1
when the subtask has been completed and 0 otherwise. The implementation of this method is
required if intending to enable automatic subtask term signal annotation when running the
dataset annotation tool. This method can be kept unimplemented if intending to use manual
subtask term signal annotation.
Args:
env_ids: Environment indices to get the termination signals for. If None, all envs are considered.
Returns:
A dictionary termination signal flags (False or True) for each subtask.
"""
if env_ids is None:
env_ids = slice(None)
signals = dict()
subtask_terms = self.obs_buf["subtask_terms"]
if "grasp" in subtask_terms:
signals["grasp"] = subtask_terms["grasp"][env_ids]
# Handle multiple grasp signals
for i in range(0, len(self.cfg.subtask_configs)):
grasp_key = f"grasp_{i + 1}"
if grasp_key in subtask_terms:
signals[grasp_key] = subtask_terms[grasp_key][env_ids]
# final subtask signal is not needed
return signals
class PickPlaceAbsMimicEnv(FrankaCubeStackIKAbsMimicEnv):
"""
Isaac Lab Mimic environment wrapper class for DiffIK / RmpFlow Absolute Pose Control env.
This MimicEnv is used when all observations are in the robot base frame.
"""
def get_object_poses(self, env_ids: Sequence[int] | None = None):
"""
Gets the pose of each object (including rigid objects and articulated objects) in the robot base frame.
Args:
env_ids: Environment indices to get the pose for. If None, all envs are considered.
Returns:
A dictionary that maps object names to object pose matrix in robot base frame (4x4 torch.Tensor)
"""
if env_ids is None:
env_ids = slice(None)
# Get scene state
scene_state = self.scene.get_state(is_relative=True)
rigid_object_states = scene_state["rigid_object"]
articulation_states = scene_state["articulation"]
# Get robot root pose
robot_root_pose = articulation_states["robot"]["root_pose"]
root_pos = robot_root_pose[env_ids, :3]
root_quat = robot_root_pose[env_ids, 3:7]
object_pose_matrix = dict()
# Process rigid objects
for obj_name, obj_state in rigid_object_states.items():
pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms(
root_pos, root_quat, obj_state["root_pose"][env_ids, :3], obj_state["root_pose"][env_ids, 3:7]
)
rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base)
object_pose_matrix[obj_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base)
# Process articulated objects (except robot)
for art_name, art_state in articulation_states.items():
if art_name != "robot": # Skip robot
pos_obj_base, quat_obj_base = PoseUtils.subtract_frame_transforms(
root_pos, root_quat, art_state["root_pose"][env_ids, :3], art_state["root_pose"][env_ids, 3:7]
)
rot_obj_base = PoseUtils.matrix_from_quat(quat_obj_base)
object_pose_matrix[art_name] = PoseUtils.make_pose(pos_obj_base, rot_obj_base)
return object_pose_matrix
def get_subtask_term_signals(self, env_ids: Sequence[int] | None = None) -> dict[str, torch.Tensor]:
"""
Gets a dictionary of termination signal flags for each subtask in a task. The flag is 1
when the subtask has been completed and 0 otherwise. The implementation of this method is
required if intending to enable automatic subtask term signal annotation when running the
dataset annotation tool. This method can be kept unimplemented if intending to use manual
subtask term signal annotation.
Args:
env_ids: Environment indices to get the termination signals for. If None, all envs are considered.
Returns:
A dictionary termination signal flags (False or True) for each subtask.
"""
if env_ids is None:
env_ids = slice(None)
signals = dict()
subtask_terms = self.obs_buf["subtask_terms"]
if "grasp" in subtask_terms:
signals["grasp"] = subtask_terms["grasp"][env_ids]
# Handle multiple grasp signals
for i in range(0, len(self.cfg.subtask_configs)):
grasp_key = f"grasp_{i + 1}"
if grasp_key in subtask_terms:
signals[grasp_key] = subtask_terms[grasp_key][env_ids]
# final subtask signal is not needed
return signals
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configurations for the place environments."""
# We leave this file empty since we don't want to expose any configs in this package directly.
# We still need this file to import the "config" module in the parent package.
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configurations for the place environments."""
# We leave this file empty since we don't want to expose any configs in this package directly.
# We still need this file to import the "config" module in the parent package.
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import gymnasium as gym
##
# Register Gym environments.
##
##
# Agibot Right Arm: place toy2box task, with RmpFlow
##
gym.register(
id="Isaac-Place-Toy2Box-Agibot-Right-Arm-RmpFlow-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": f"{__name__}.place_toy2box_rmp_rel_env_cfg:RmpFlowAgibotPlaceToy2BoxEnvCfg",
},
disable_env_checker=True,
)
##
# Agibot Left Arm: place upright mug task, with RmpFlow
##
gym.register(
id="Isaac-Place-Mug-Agibot-Left-Arm-RmpFlow-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": f"{__name__}.place_upright_mug_rmp_rel_env_cfg:RmpFlowAgibotPlaceUprightMugEnvCfg",
},
disable_env_checker=True,
)
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import os
from dataclasses import MISSING
from isaaclab_assets import ISAACLAB_ASSETS_DATA_DIR
from isaaclab.assets import AssetBaseCfg, RigidObjectCfg
from isaaclab.devices.device_base import DevicesCfg
from isaaclab.devices.keyboard import Se3KeyboardCfg
from isaaclab.devices.spacemouse import Se3SpaceMouseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg
from isaaclab.sim.schemas.schemas_cfg import MassPropertiesCfg, RigidBodyPropertiesCfg
from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab_tasks.manager_based.manipulation.place import mdp as place_mdp
from isaaclab_tasks.manager_based.manipulation.stack import mdp
from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events
from isaaclab_tasks.manager_based.manipulation.stack.stack_env_cfg import ObjectTableSceneCfg
##
# Pre-defined configs
##
from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip
from isaaclab_assets.robots.agibot import AGIBOT_A2D_CFG # isort: skip
from isaaclab.controllers.config.rmp_flow import AGIBOT_RIGHT_ARM_RMPFLOW_CFG # isort: skip
##
# Event settings
##
@configclass
class EventCfgPlaceToy2Box:
"""Configuration for events."""
reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
init_toy_position = EventTerm(
func=franka_stack_events.randomize_object_pose,
mode="reset",
params={
"pose_range": {
"x": (-0.15, 0.20),
"y": (-0.3, -0.15),
"z": (-0.65, -0.65),
"roll": (1.57, 1.57),
"yaw": (-3.14, 3.14),
},
"asset_cfgs": [SceneEntityCfg("toy_truck")],
},
)
init_box_position = EventTerm(
func=franka_stack_events.randomize_object_pose,
mode="reset",
params={
"pose_range": {
"x": (0.25, 0.35),
"y": (0.0, 0.10),
"z": (-0.55, -0.55),
"roll": (1.57, 1.57),
"yaw": (-3.14, 3.14),
},
"asset_cfgs": [SceneEntityCfg("box")],
},
)
#
# MDP settings
##
@configclass
class ObservationsCfg:
"""Observation specifications for the MDP."""
@configclass
class PolicyCfg(ObsGroup):
"""Observations for policy group with state values."""
actions = ObsTerm(func=mdp.last_action)
joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
toy_truck_positions = ObsTerm(
func=place_mdp.object_poses_in_base_frame,
params={"object_cfg": SceneEntityCfg("toy_truck"), "return_key": "pos"},
)
toy_truck_orientations = ObsTerm(
func=place_mdp.object_poses_in_base_frame,
params={"object_cfg": SceneEntityCfg("toy_truck"), "return_key": "quat"},
)
box_positions = ObsTerm(
func=place_mdp.object_poses_in_base_frame, params={"object_cfg": SceneEntityCfg("box"), "return_key": "pos"}
)
box_orientations = ObsTerm(
func=place_mdp.object_poses_in_base_frame,
params={"object_cfg": SceneEntityCfg("box"), "return_key": "quat"},
)
eef_pos = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "pos"})
eef_quat = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "quat"})
gripper_pos = ObsTerm(func=mdp.gripper_pos)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
@configclass
class SubtaskCfg(ObsGroup):
"""Observations for subtask group."""
grasp = ObsTerm(
func=place_mdp.object_grasped,
params={
"robot_cfg": SceneEntityCfg("robot"),
"ee_frame_cfg": SceneEntityCfg("ee_frame"),
"object_cfg": SceneEntityCfg("toy_truck"),
"diff_threshold": 0.05,
},
)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
# observation groups
policy: PolicyCfg = PolicyCfg()
subtask_terms: SubtaskCfg = SubtaskCfg()
@configclass
class ActionsCfg:
"""Action specifications for the MDP."""
# will be set by agent env cfg
arm_action: mdp.JointPositionActionCfg = MISSING
gripper_action: mdp.BinaryJointPositionActionCfg = MISSING
@configclass
class TerminationsCfg:
"""Termination terms for the MDP."""
time_out = DoneTerm(func=mdp.time_out, time_out=True)
toy_truck_dropping = DoneTerm(
func=mdp.root_height_below_minimum, params={"minimum_height": -0.85, "asset_cfg": SceneEntityCfg("toy_truck")}
)
success = DoneTerm(
func=place_mdp.object_a_is_into_b,
params={
"robot_cfg": SceneEntityCfg("robot"),
"object_a_cfg": SceneEntityCfg("toy_truck"),
"object_b_cfg": SceneEntityCfg("box"),
"xy_threshold": 0.10,
"height_diff": 0.06,
"height_threshold": 0.04,
},
)
@configclass
class PlaceToy2BoxEnvCfg(ManagerBasedRLEnvCfg):
"""Configuration for the stacking environment."""
# Scene settings
scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=4096, env_spacing=3.0, replicate_physics=False)
# Basic settings
observations: ObservationsCfg = ObservationsCfg()
actions: ActionsCfg = ActionsCfg()
# MDP settings
terminations: TerminationsCfg = TerminationsCfg()
# Unused managers
commands = None
rewards = None
events = None
curriculum = None
def __post_init__(self):
"""Post initialization."""
self.sim.render_interval = self.decimation
self.sim.physx.bounce_threshold_velocity = 0.2
self.sim.physx.bounce_threshold_velocity = 0.01
self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4
self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024
self.sim.physx.friction_correlation_distance = 0.00625
"""
Env to Replay Sim2Lab Demonstrations with JointSpaceAction
"""
class RmpFlowAgibotPlaceToy2BoxEnvCfg(PlaceToy2BoxEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
self.events = EventCfgPlaceToy2Box()
# Set Agibot as robot
self.scene.robot = AGIBOT_A2D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# add table
self.scene.table = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/Table",
init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0.0, -0.7], rot=[0.707, 0, 0, 0.707]),
spawn=UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd",
scale=(1.8, 1.0, 0.30),
),
)
use_relative_mode_env = os.getenv("USE_RELATIVE_MODE", "True")
self.use_relative_mode = use_relative_mode_env.lower() in ["true", "1", "t"]
# Set actions for the specific robot type (Agibot)
self.actions.arm_action = RMPFlowActionCfg(
asset_name="robot",
joint_names=["right_arm_joint.*"],
body_name="right_gripper_center",
controller=AGIBOT_RIGHT_ARM_RMPFLOW_CFG,
scale=1.0,
body_offset=RMPFlowActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0]),
articulation_prim_expr="/World/envs/env_.*/Robot",
use_relative_mode=self.use_relative_mode,
)
# Enable Parallel Gripper:
self.actions.gripper_action = mdp.BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=["right_hand_joint1", "right_.*_Support_Joint"],
open_command_expr={"right_hand_joint1": 0.994, "right_.*_Support_Joint": 0.994},
close_command_expr={"right_hand_joint1": 0.20, "right_.*_Support_Joint": 0.20},
)
# find joint ids for grippers
self.gripper_joint_names = ["right_hand_joint1", "right_Right_1_Joint"]
self.gripper_open_val = 0.994
self.gripper_threshold = 0.2
# Rigid body properties of toy_truck and box
toy_truck_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,
)
box_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,
)
# Notes: remember to add Physics/Mass properties to the toy_truck mesh to make grasping successful,
# then you can use below MassPropertiesCfg to set the mass of the toy_truck
toy_mass_properties = MassPropertiesCfg(
mass=0.05,
)
self.scene.toy_truck = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/ToyTruck",
init_state=RigidObjectCfg.InitialStateCfg(),
spawn=UsdFileCfg(
usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Objects/toy_truck_022.usd",
scale=(0.001, 0.001, 0.001),
rigid_props=toy_truck_properties,
mass_props=toy_mass_properties,
),
)
self.scene.box = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Box",
init_state=RigidObjectCfg.InitialStateCfg(),
spawn=UsdFileCfg(
usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Objects/box_167.usd",
activate_contact_sensors=True,
scale=(0.001, 0.001, 0.001),
rigid_props=box_properties,
),
)
# Listens to the required transforms
self.marker_cfg = FRAME_MARKER_CFG.copy()
self.marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
self.marker_cfg.prim_path = "/Visuals/FrameTransformer"
self.scene.ee_frame = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base_link",
debug_vis=False,
visualizer_cfg=self.marker_cfg,
target_frames=[
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/right_gripper_center",
name="end_effector",
offset=OffsetCfg(
pos=[0.0, 0.0, 0.0],
),
),
],
)
# add contact force sensor for grasped checking
self.scene.contact_grasp = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot/right_.*_Pad_Link",
update_period=0.0,
history_length=6,
debug_vis=True,
filter_prim_paths_expr=["{ENV_REGEX_NS}/ToyTruck/Aligned"],
)
self.teleop_devices = DevicesCfg(
devices={
"keyboard": Se3KeyboardCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
"spacemouse": Se3SpaceMouseCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
}
)
# Set the simulation parameters
self.sim.dt = 1 / 60
self.sim.render_interval = 6
self.decimation = 3
self.episode_length_s = 30.0
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import os
from dataclasses import MISSING
from isaaclab_assets import ISAACLAB_ASSETS_DATA_DIR
from isaaclab.assets import AssetBaseCfg, RigidObjectCfg
from isaaclab.devices.device_base import DevicesCfg
from isaaclab.devices.keyboard import Se3KeyboardCfg
from isaaclab.devices.spacemouse import Se3SpaceMouseCfg
from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg
from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg
from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg
from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab_tasks.manager_based.manipulation.place import mdp as place_mdp
from isaaclab_tasks.manager_based.manipulation.place.config.agibot import place_toy2box_rmp_rel_env_cfg
from isaaclab_tasks.manager_based.manipulation.stack import mdp
from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events
##
# Pre-defined configs
##
from isaaclab.markers.config import FRAME_MARKER_CFG # isort: skip
from isaaclab_assets.robots.agibot import AGIBOT_A2D_CFG # isort: skip
from isaaclab.controllers.config.rmp_flow import AGIBOT_LEFT_ARM_RMPFLOW_CFG # isort: skip
##
# Event settings
##
@configclass
class EventCfgPlaceUprightMug:
"""Configuration for events."""
reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset", params={"reset_joint_targets": True})
randomize_mug_positions = EventTerm(
func=franka_stack_events.randomize_object_pose,
mode="reset",
params={
"pose_range": {
"x": (-0.05, 0.2),
"y": (-0.10, 0.10),
"z": (0.75, 0.75),
"roll": (-1.57, -1.57),
"yaw": (-0.57, 0.57),
},
"asset_cfgs": [SceneEntityCfg("mug")],
},
)
#
# MDP settings
##
@configclass
class ObservationsCfg:
"""Observation specifications for the MDP."""
@configclass
class PolicyCfg(ObsGroup):
"""Observations for policy group with state values."""
actions = ObsTerm(func=mdp.last_action)
joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
mug_positions = ObsTerm(
func=place_mdp.object_poses_in_base_frame, params={"object_cfg": SceneEntityCfg("mug"), "return_key": "pos"}
)
mug_orientations = ObsTerm(
func=place_mdp.object_poses_in_base_frame,
params={"object_cfg": SceneEntityCfg("mug"), "return_key": "quat"},
)
eef_pos = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "pos"})
eef_quat = ObsTerm(func=mdp.ee_frame_pose_in_base_frame, params={"return_key": "quat"})
gripper_pos = ObsTerm(func=mdp.gripper_pos)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
@configclass
class SubtaskCfg(ObsGroup):
"""Observations for subtask group."""
grasp = ObsTerm(
func=place_mdp.object_grasped,
params={
"robot_cfg": SceneEntityCfg("robot"),
"ee_frame_cfg": SceneEntityCfg("ee_frame"),
"object_cfg": SceneEntityCfg("mug"),
"diff_threshold": 0.05,
},
)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
# observation groups
policy: PolicyCfg = PolicyCfg()
subtask_terms: SubtaskCfg = SubtaskCfg()
@configclass
class ActionsCfg:
"""Action specifications for the MDP."""
# will be set by agent env cfg
arm_action: mdp.JointPositionActionCfg = MISSING
gripper_action: mdp.BinaryJointPositionActionCfg = MISSING
@configclass
class TerminationsCfg:
"""Termination terms for the MDP."""
time_out = DoneTerm(func=mdp.time_out, time_out=True)
mug_dropping = DoneTerm(
func=mdp.root_height_below_minimum, params={"minimum_height": -0.85, "asset_cfg": SceneEntityCfg("mug")}
)
success = DoneTerm(
func=place_mdp.object_placed_upright,
params={
"robot_cfg": SceneEntityCfg("robot"),
"object_cfg": SceneEntityCfg("mug"),
"target_height": 0.6,
},
)
"""
Env to Place Upright Mug with AgiBot Left Arm using RMPFlow
"""
class RmpFlowAgibotPlaceUprightMugEnvCfg(place_toy2box_rmp_rel_env_cfg.PlaceToy2BoxEnvCfg):
def __post_init__(self):
# post init of parent
super().__post_init__()
self.events = EventCfgPlaceUprightMug()
# Set Agibot as robot
self.scene.robot = AGIBOT_A2D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
self.scene.robot.init_state.pos = (-0.60, 0.0, 0.0)
# reset obs and termination terms
self.observations = ObservationsCfg()
self.terminations = TerminationsCfg()
# Table
self.scene.table = AssetBaseCfg(
prim_path="{ENV_REGEX_NS}/Table",
init_state=AssetBaseCfg.InitialStateCfg(pos=[0.50, 0.0, 0.60], rot=[0.707, 0, 0, 0.707]),
spawn=UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd",
scale=(1.0, 1.0, 0.60),
),
)
# add contact force sensor for grasped checking
self.scene.contact_grasp = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Robot/right_.*_Pad_Link",
update_period=0.0,
history_length=6,
debug_vis=True,
filter_prim_paths_expr=["{ENV_REGEX_NS}/Mug"],
)
use_relative_mode_env = os.getenv("USE_RELATIVE_MODE", "True")
self.use_relative_mode = use_relative_mode_env.lower() in ["true", "1", "t"]
# Set actions for the specific robot type (Agibot)
self.actions.arm_action = RMPFlowActionCfg(
asset_name="robot",
joint_names=["left_arm_joint.*"],
body_name="gripper_center",
controller=AGIBOT_LEFT_ARM_RMPFLOW_CFG,
scale=1.0,
body_offset=RMPFlowActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.0], rot=[0.7071, 0.0, -0.7071, 0.0]),
articulation_prim_expr="/World/envs/env_.*/Robot",
use_relative_mode=self.use_relative_mode,
)
# Enable Parallel Gripper:
self.actions.gripper_action = mdp.BinaryJointPositionActionCfg(
asset_name="robot",
joint_names=["left_hand_joint1", "left_.*_Support_Joint"],
open_command_expr={"left_hand_joint1": 0.994, "left_.*_Support_Joint": 0.994},
close_command_expr={"left_hand_joint1": 0.0, "left_.*_Support_Joint": 0.0},
)
# find joint ids for grippers
self.gripper_joint_names = ["left_hand_joint1", "left_Right_1_Joint"]
self.gripper_open_val = 0.994
self.gripper_threshold = 0.2
# Rigid body properties of mug
mug_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,
)
self.scene.mug = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Mug",
init_state=RigidObjectCfg.InitialStateCfg(),
spawn=UsdFileCfg(
usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Objects/mug.usd",
scale=(1.0, 1.0, 1.0),
rigid_props=mug_properties,
),
)
# Listens to the required transforms
self.marker_cfg = FRAME_MARKER_CFG.copy()
self.marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
self.marker_cfg.prim_path = "/Visuals/FrameTransformer"
self.scene.ee_frame = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base_link",
debug_vis=False,
visualizer_cfg=self.marker_cfg,
target_frames=[
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/gripper_center",
name="end_effector",
offset=OffsetCfg(
pos=[0.0, 0.0, 0.0],
rot=[
0.7071,
0.0,
-0.7071,
0.0,
],
),
),
],
)
self.teleop_devices = DevicesCfg(
devices={
"keyboard": Se3KeyboardCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
"spacemouse": Se3SpaceMouseCfg(
pos_sensitivity=0.05,
rot_sensitivity=0.05,
sim_device=self.sim.device,
),
}
)
# Set the simulation parameters
self.sim.dt = 1 / 60
self.sim.render_interval = 6
self.decimation = 3
self.episode_length_s = 10.0
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""This sub-module contains the functions that are specific to the pick and place environments."""
from isaaclab.envs.mdp import * # noqa: F401, F403
from .observations import * # noqa: F401, F403
from .terminations import * # noqa: F401, F403
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import torch
from typing import TYPE_CHECKING, Literal
import isaaclab.utils.math as math_utils
from isaaclab.assets import Articulation, RigidObject
from isaaclab.managers import SceneEntityCfg
from isaaclab.sensors import FrameTransformer
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
def object_poses_in_base_frame(
env: ManagerBasedRLEnv,
object_cfg: SceneEntityCfg = SceneEntityCfg("mug"),
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
return_key: Literal["pos", "quat", None] = None,
) -> torch.Tensor:
"""The pose of the object in the robot base frame."""
object: RigidObject = env.scene[object_cfg.name]
pos_object_world = object.data.root_pos_w
quat_object_world = object.data.root_quat_w
"""The position of the robot in the world frame."""
robot: Articulation = env.scene[robot_cfg.name]
root_pos_w = robot.data.root_pos_w
root_quat_w = robot.data.root_quat_w
pos_object_base, quat_object_base = math_utils.subtract_frame_transforms(
root_pos_w, root_quat_w, pos_object_world, quat_object_world
)
if return_key == "pos":
return pos_object_base
elif return_key == "quat":
return quat_object_base
elif return_key is None:
return torch.cat((pos_object_base, quat_object_base), dim=1)
def object_grasped(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg,
ee_frame_cfg: SceneEntityCfg,
object_cfg: SceneEntityCfg,
diff_threshold: float = 0.06,
force_threshold: float = 1.0,
) -> torch.Tensor:
"""
Check if an object is grasped by the specified robot.
Support both surface gripper and parallel gripper.
If contact_grasp sensor is found, check if the contact force is greater than force_threshold.
"""
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 "contact_grasp" in env.scene.keys() and env.scene["contact_grasp"] is not None:
contact_force_grasp = env.scene["contact_grasp"].data.net_forces_w # shape:(N, 2, 3) for two fingers
contact_force_norm = torch.linalg.vector_norm(
contact_force_grasp, dim=2
) # shape:(N, 2) - force magnitude per finger
both_fingers_force_ok = torch.all(
contact_force_norm > force_threshold, dim=1
) # both fingers must exceed threshold
grasped = torch.logical_and(pose_diff < diff_threshold, both_fingers_force_ok)
elif (
f"contact_grasp_{object_cfg.name}" in env.scene.keys()
and env.scene[f"contact_grasp_{object_cfg.name}"] is not None
):
contact_force_object = env.scene[
f"contact_grasp_{object_cfg.name}"
].data.net_forces_w # shape:(N, 2, 3) for two fingers
contact_force_norm = torch.linalg.vector_norm(
contact_force_object, dim=2
) # shape:(N, 2) - force magnitude per finger
both_fingers_force_ok = torch.all(
contact_force_norm > force_threshold, dim=1
) # both fingers must exceed threshold
grasped = torch.logical_and(pose_diff < diff_threshold, both_fingers_force_ok)
else:
grasped = (pose_diff < diff_threshold).clone().detach()
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)
grasped = torch.logical_and(
grasped,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val)
> env.cfg.gripper_threshold,
)
grasped = torch.logical_and(
grasped,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val)
> env.cfg.gripper_threshold,
)
else:
raise ValueError("No gripper_joint_names found in environment config")
return grasped
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Common functions that can be used to activate certain terminations for the place 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
import torch
from typing import TYPE_CHECKING
import isaaclab.utils.math as math_utils
from isaaclab.assets import Articulation, RigidObject
from isaaclab.managers import SceneEntityCfg
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
def object_placed_upright(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg,
object_cfg: SceneEntityCfg,
target_height: float = 0.927,
euler_xy_threshold: float = 0.10,
):
"""Check if an object placed upright by the specified robot."""
robot: Articulation = env.scene[robot_cfg.name]
object: RigidObject = env.scene[object_cfg.name]
# Compute mug euler angles of X, Y axis, to check if it is placed upright
object_euler_x, object_euler_y, _ = math_utils.euler_xyz_from_quat(object.data.root_quat_w) # (N,4) [0, 2*pi]
object_euler_x_err = torch.abs(math_utils.wrap_to_pi(object_euler_x)) # (N,)
object_euler_y_err = torch.abs(math_utils.wrap_to_pi(object_euler_y)) # (N,)
success = torch.logical_and(object_euler_x_err < euler_xy_threshold, object_euler_y_err < euler_xy_threshold)
# Check if current mug height is greater than target height
height_success = object.data.root_pos_w[:, 2] > target_height
success = torch.logical_and(height_success, success)
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)
success = torch.logical_and(suction_cup_is_open, success)
else:
if hasattr(env.cfg, "gripper_joint_names"):
gripper_joint_ids, _ = robot.find_joints(env.cfg.gripper_joint_names)
success = torch.logical_and(
success,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val)
< env.cfg.gripper_threshold,
)
success = torch.logical_and(
success,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val)
< env.cfg.gripper_threshold,
)
else:
raise ValueError("No gripper_joint_names found in environment config")
return success
def object_a_is_into_b(
env: ManagerBasedRLEnv,
robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
object_a_cfg: SceneEntityCfg = SceneEntityCfg("object_a"),
object_b_cfg: SceneEntityCfg = SceneEntityCfg("object_b"),
xy_threshold: float = 0.03, # xy_distance_threshold
height_threshold: float = 0.04, # height_distance_threshold
height_diff: float = 0.0, # expected height_diff
) -> torch.Tensor:
"""Check if an object a is put into another object b by the specified robot."""
robot: Articulation = env.scene[robot_cfg.name]
object_a: RigidObject = env.scene[object_a_cfg.name]
object_b: RigidObject = env.scene[object_b_cfg.name]
# check object a is into object b
pos_diff = object_a.data.root_pos_w - object_b.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)
success = torch.logical_and(xy_dist < xy_threshold, (height_dist - height_diff) < height_threshold)
# 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) # 1: closed, 0: closing, -1: open
suction_cup_is_open = (suction_cup_status == -1).to(torch.float32)
success = torch.logical_and(suction_cup_is_open, success)
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"
success = torch.logical_and(
success,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[0]]) - env.cfg.gripper_open_val)
< env.cfg.gripper_threshold,
)
success = torch.logical_and(
success,
torch.abs(torch.abs(robot.data.joint_pos[:, gripper_joint_ids[1]]) - env.cfg.gripper_open_val)
< env.cfg.gripper_threshold,
)
else:
raise ValueError("No gripper_joint_names found in environment config")
return success
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment