Add standalone Huoyi training project
This commit is contained in:
huangweifeng
@@ -0,0 +1,35 @@
|
||||
[package]
|
||||
|
||||
# Semantic Versioning is used: https://semver.org/
|
||||
version = "1.0.0"
|
||||
|
||||
# Description
|
||||
category = "isaaclab"
|
||||
readme = "README.md"
|
||||
|
||||
title = "RL Training Repo for DeepRobotics"
|
||||
author = "Bo Peng"
|
||||
maintainer = "Bo Peng"
|
||||
description="RL Training Repo for DeepRobotic, Based on IsaacLab."
|
||||
repository = "https://github.com/DeepRoboticsLab/rl_training.git"
|
||||
keywords = ["extension", "rl_training", "isaaclab"]
|
||||
|
||||
[dependencies]
|
||||
"isaaclab" = {}
|
||||
"isaaclab_assets" = {}
|
||||
"isaaclab_mimic" = {}
|
||||
"isaaclab_rl" = {}
|
||||
"isaaclab_tasks" = {}
|
||||
# NOTE: Add additional dependencies here
|
||||
|
||||
[[python.module]]
|
||||
name = "rl_training"
|
||||
|
||||
[isaaclab_settings]
|
||||
# TODO: Uncomment and list any apt dependencies here.
|
||||
# If none, leave it commented out.
|
||||
# apt_deps = ["example_package"]
|
||||
# TODO: Uncomment and provide path to a ros_ws
|
||||
# with rosdeps to be installed. If none,
|
||||
# leave it commented out.
|
||||
# ros_ws = "path/from/extension_root/to/ros_ws"
|
||||
@@ -0,0 +1,3 @@
|
||||
[build-system]
|
||||
requires = ["setuptools", "wheel", "toml"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
@@ -0,0 +1,15 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
"""
|
||||
Python module serving as a project/extension template.
|
||||
"""
|
||||
|
||||
# Register Gym environments.
|
||||
from .tasks import *
|
||||
|
||||
# Register UI extensions.
|
||||
from .ui_extension_example import *
|
||||
@@ -0,0 +1,32 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Package containing asset and sensor configurations."""
|
||||
|
||||
import os
|
||||
import toml
|
||||
|
||||
##
|
||||
# Configuration for different assets.
|
||||
##
|
||||
|
||||
# Conveniences to other module directories via relative paths
|
||||
ISAACLAB_ASSETS_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../"))
|
||||
"""Path to the extension source directory."""
|
||||
|
||||
ISAACLAB_ASSETS_DATA_DIR = os.path.join(ISAACLAB_ASSETS_EXT_DIR, "../../deep_robotics_model")
|
||||
"""Path to the extension data directory."""
|
||||
|
||||
ISAACLAB_ASSETS_METADATA = toml.load(os.path.join(ISAACLAB_ASSETS_EXT_DIR, "config", "extension.toml"))
|
||||
"""Extension metadata dictionary parsed from the extension.toml file."""
|
||||
|
||||
# Configure the module-level variables
|
||||
__version__ = ISAACLAB_ASSETS_METADATA["package"]["version"]
|
||||
@@ -0,0 +1,124 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.actuators import DCMotorCfg, DelayedPDActuatorCfg
|
||||
from isaaclab.assets.articulation import ArticulationCfg
|
||||
|
||||
from rl_training.assets import ISAACLAB_ASSETS_DATA_DIR
|
||||
|
||||
DEEPROBOTICS_LITE3_CFG = ArticulationCfg(
|
||||
spawn=sim_utils.UsdFileCfg(
|
||||
usd_path=f"{ISAACLAB_ASSETS_DATA_DIR}/Lite3/Lite3_usd/Lite3.usd",
|
||||
activate_contact_sensors=True,
|
||||
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||
disable_gravity=False,
|
||||
retain_accelerations=False,
|
||||
linear_damping=0.0,
|
||||
angular_damping=0.0,
|
||||
max_linear_velocity=1000.0,
|
||||
max_angular_velocity=1000.0,
|
||||
max_depenetration_velocity=1.0,
|
||||
),
|
||||
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||
enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=1
|
||||
),
|
||||
),
|
||||
init_state=ArticulationCfg.InitialStateCfg(
|
||||
pos=(0.0, 0.0, 0.35),
|
||||
joint_pos={
|
||||
|
||||
".*Knee_joint": 0.0,
|
||||
},
|
||||
joint_vel={".*": 0.0},
|
||||
),
|
||||
soft_joint_pos_limit_factor=0.99,
|
||||
actuators={
|
||||
# "Hip": DelayedPDActuatorCfg(
|
||||
# joint_names_expr=[".*_Hip[X,Y]_joint"],
|
||||
# effort_limit=24.0,
|
||||
# velocity_limit=26.2,
|
||||
# stiffness=30.0,
|
||||
# damping=1.0,
|
||||
# friction=0.0,
|
||||
# armature=0.0,
|
||||
# min_delay=0,
|
||||
# max_delay=5,
|
||||
# ),
|
||||
"Knee": DelayedPDActuatorCfg(
|
||||
joint_names_expr=[".*_Knee_joint"],
|
||||
effort_limit=36.0,
|
||||
velocity_limit=26.2,
|
||||
stiffness=30.0,
|
||||
damping=1.0,
|
||||
friction=0.0,
|
||||
armature=0.0,
|
||||
min_delay=0,
|
||||
max_delay=5,
|
||||
),
|
||||
},
|
||||
)
|
||||
|
||||
DEEPROBOTICS_M20_CFG = ArticulationCfg(
|
||||
spawn=sim_utils.UrdfFileCfg(
|
||||
asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/M20/M20_urdf/urdf/Solenopsis.urdf",
|
||||
fix_base=False,
|
||||
merge_fixed_joints=True,
|
||||
joint_drive=None,
|
||||
activate_contact_sensors=True,
|
||||
visual_material=sim_utils.PreviewSurfaceCfg(
|
||||
diffuse_color=(0.42, 0.45, 0.50),
|
||||
roughness=0.65,
|
||||
metallic=0.15,
|
||||
),
|
||||
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||
disable_gravity=False,
|
||||
retain_accelerations=False,
|
||||
linear_damping=0.0,
|
||||
angular_damping=0.0,
|
||||
max_linear_velocity=1000.0,
|
||||
max_angular_velocity=1000.0,
|
||||
max_depenetration_velocity=1.0,
|
||||
),
|
||||
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||
enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=1
|
||||
),
|
||||
),
|
||||
init_state=ArticulationCfg.InitialStateCfg(
|
||||
pos=(0.0, 0.0, 0.306),
|
||||
joint_pos={
|
||||
"f[l,r]_knee_joint": 0.5,
|
||||
"h[l,r]_knee_joint": -0.5,
|
||||
".*wheel_joint": 0.0,
|
||||
},
|
||||
joint_vel={".*": 0.0},
|
||||
),
|
||||
soft_joint_pos_limit_factor=0.9,
|
||||
actuators={
|
||||
"joint": DelayedPDActuatorCfg(
|
||||
joint_names_expr=[".*knee_joint"],
|
||||
effort_limit=75.0,
|
||||
velocity_limit=17.8024,
|
||||
stiffness=80.0,
|
||||
damping=2.0,
|
||||
friction=0.0,
|
||||
armature=0.0,
|
||||
min_delay=0,
|
||||
max_delay=5,
|
||||
),
|
||||
"wheel": DelayedPDActuatorCfg(
|
||||
joint_names_expr=[".*_wheel_joint"],
|
||||
effort_limit=28.68,
|
||||
velocity_limit=109.956,
|
||||
stiffness=0.0,
|
||||
damping=0.6,
|
||||
friction=1.0,
|
||||
armature=0.00243216,
|
||||
min_delay=0,
|
||||
max_delay=5,
|
||||
),
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,27 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Package containing task implementations for various robotic environments."""
|
||||
|
||||
import os
|
||||
import toml
|
||||
|
||||
from isaaclab_tasks.utils import import_packages
|
||||
|
||||
##
|
||||
# Register Gym environments.
|
||||
##
|
||||
|
||||
|
||||
# The blacklist is used to prevent importing configs from sub-packages
|
||||
_BLACKLIST_PKGS = ["utils"]
|
||||
# Import all configs in this package
|
||||
import_packages(__name__, _BLACKLIST_PKGS)
|
||||
@@ -0,0 +1,16 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.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
|
||||
|
||||
"""
|
||||
Config-based workflow environments.
|
||||
"""
|
||||
|
||||
import gymnasium as gym
|
||||
@@ -0,0 +1,14 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Locomotion environments for legged robots."""
|
||||
|
||||
from .velocity import * # noqa
|
||||
@@ -0,0 +1,18 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Locomotion environments with velocity-tracking commands.
|
||||
|
||||
These environments are based on the `legged_gym` environments provided by Rudin et al.
|
||||
|
||||
Reference:
|
||||
https://github.com/leggedrobotics/legged_gym
|
||||
"""
|
||||
+15
@@ -0,0 +1,15 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Configurations for velocity-based locomotion 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.
|
||||
+15
@@ -0,0 +1,15 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Configurations for velocity-based locomotion 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.
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
import gymnasium as gym
|
||||
|
||||
from . import agents
|
||||
|
||||
##
|
||||
# Register Gym environments.
|
||||
##
|
||||
|
||||
gym.register(
|
||||
id="Flat-Deeprobotics-Lite3-v0",
|
||||
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||
disable_env_checker=True,
|
||||
kwargs={
|
||||
"env_cfg_entry_point": f"{__name__}.flat_env_cfg:DeeproboticsLite3FlatEnvCfg",
|
||||
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsLite3FlatPPORunnerCfg",
|
||||
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsLite3FlatTrainerCfg",
|
||||
},
|
||||
)
|
||||
|
||||
gym.register(
|
||||
id="Rough-Deeprobotics-Lite3-v0",
|
||||
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||
disable_env_checker=True,
|
||||
kwargs={
|
||||
"env_cfg_entry_point": f"{__name__}.rough_env_cfg:DeeproboticsLite3RoughEnvCfg",
|
||||
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsLite3RoughPPORunnerCfg",
|
||||
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsLite3RoughTrainerCfg",
|
||||
},
|
||||
)
|
||||
|
||||
+5
@@ -0,0 +1,5 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
+48
@@ -0,0 +1,48 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.utils import configclass
|
||||
from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsLite3RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||
num_steps_per_env = 24
|
||||
max_iterations = 10000
|
||||
save_interval = 100
|
||||
experiment_name = "deeprobotics_lite3_rough"
|
||||
empirical_normalization = False
|
||||
clip_actions = 100
|
||||
policy = RslRlPpoActorCriticCfg(
|
||||
init_noise_std=1.0,
|
||||
noise_std_type="log",
|
||||
actor_hidden_dims=[512, 256, 128],
|
||||
critic_hidden_dims=[512, 256, 128],
|
||||
activation="elu",
|
||||
)
|
||||
algorithm = RslRlPpoAlgorithmCfg(
|
||||
value_loss_coef=1.0,
|
||||
use_clipped_value_loss=True,
|
||||
clip_param=0.2,
|
||||
entropy_coef=0.01,
|
||||
num_learning_epochs=5,
|
||||
num_mini_batches=4,
|
||||
learning_rate=1.0e-3,
|
||||
schedule="adaptive",
|
||||
gamma=0.99,
|
||||
lam=0.95,
|
||||
desired_kl=0.01,
|
||||
max_grad_norm=1.0,
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsLite3FlatPPORunnerCfg(DeeproboticsLite3RoughPPORunnerCfg):
|
||||
def __post_init__(self):
|
||||
super().__post_init__()
|
||||
|
||||
self.max_iterations = 10000
|
||||
self.experiment_name = "deeprobotics_lite3_flat"
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.utils import configclass
|
||||
|
||||
from .rough_env_cfg import DeeproboticsLite3RoughEnvCfg
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsLite3FlatEnvCfg(DeeproboticsLite3RoughEnvCfg):
|
||||
def __post_init__(self):
|
||||
# post init of parent
|
||||
super().__post_init__()
|
||||
|
||||
# override rewards
|
||||
# self.rewards.base_height_l2.params["sensor_cfg"] = None
|
||||
# change terrain to flat
|
||||
self.scene.terrain.terrain_type = "plane"
|
||||
self.scene.terrain.terrain_generator = None
|
||||
# no height scan
|
||||
# self.scene.height_scanner = None
|
||||
self.observations.policy.height_scan = None
|
||||
# self.observations.critic.height_scan = None
|
||||
# no terrain curriculum
|
||||
self.curriculum.terrain_levels = None
|
||||
|
||||
# If the weight of rewards is 0, set rewards to None
|
||||
if self.__class__.__name__ == "DeeproboticsLite3FlatEnvCfg":
|
||||
self.disable_zero_weight_rewards()
|
||||
+183
@@ -0,0 +1,183 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.utils import configclass
|
||||
|
||||
from rl_training.tasks.manager_based.locomotion.velocity.velocity_env_cfg import LocomotionVelocityRoughEnvCfg
|
||||
# from isaaclab.sensors.ray_caster import GridPatternCfg
|
||||
##
|
||||
# Pre-defined configs
|
||||
##
|
||||
from rl_training.assets.deeprobotics import DEEPROBOTICS_LITE3_CFG # isort: skip
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsLite3RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
|
||||
base_link_name = "TORSO"
|
||||
foot_link_name = ".*_FOOT"
|
||||
# fmt: off
|
||||
joint_names = [
|
||||
"FL_HipX_joint", "FL_HipY_joint", "FL_Knee_joint",
|
||||
"FR_HipX_joint", "FR_HipY_joint", "FR_Knee_joint",
|
||||
"HL_HipX_joint", "HL_HipY_joint", "HL_Knee_joint",
|
||||
"HR_HipX_joint", "HR_HipY_joint", "HR_Knee_joint",
|
||||
]
|
||||
|
||||
link_names = [
|
||||
'TORSO',
|
||||
'FL_HIP', 'FR_HIP', 'HL_HIP', 'HR_HIP',
|
||||
'FL_THIGH', 'FR_THIGH', 'HL_THIGH', 'HR_THIGH',
|
||||
'FL_SHANK', 'FR_SHANK', 'HL_SHANK', 'HR_SHANK',
|
||||
'FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT',
|
||||
]
|
||||
# fmt: on
|
||||
|
||||
def __post_init__(self):
|
||||
# post init of parent
|
||||
super().__post_init__()
|
||||
|
||||
# ------------------------------Sence------------------------------
|
||||
self.scene.robot = DEEPROBOTICS_LITE3_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||
self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/" + self.base_link_name
|
||||
self.scene.height_scanner_base.prim_path = "{ENV_REGEX_NS}/Robot/" + self.base_link_name
|
||||
self.scene.height_scanner.pattern_cfg.resolution = 0.07 # = GridPatternCfg(resolution=0.07, size=[1.6, 1.0]),
|
||||
|
||||
# ------------------------------Observations------------------------------
|
||||
self.observations.policy.base_lin_vel = None # type: ignore
|
||||
self.observations.policy.height_scan = None # type: ignore
|
||||
self.observations.policy.base_ang_vel.scale = 0.25
|
||||
self.observations.policy.joint_pos.scale = 1.0
|
||||
self.observations.policy.joint_vel.scale = 0.05
|
||||
self.observations.policy.joint_pos.params["asset_cfg"].joint_names = self.joint_names
|
||||
self.observations.policy.joint_vel.params["asset_cfg"].joint_names = self.joint_names
|
||||
|
||||
# ------------------------------Actions------------------------------
|
||||
# reduce action scale
|
||||
self.actions.joint_pos.scale = {".*_HipX_joint": 0.125, "^(?!.*_HipX_joint).*": 0.25}
|
||||
self.actions.joint_pos.clip = {".*": (-100.0, 100.0)}
|
||||
self.actions.joint_pos.joint_names = self.joint_names
|
||||
|
||||
# ------------------------------Events------------------------------
|
||||
self.events.randomize_reset_base.params = {
|
||||
"pose_range": {
|
||||
"x": (-1.0, 1.0),
|
||||
"y": (-1.0, 1.0),
|
||||
"z": (0.0, 0.0),
|
||||
"roll": (-0.3, 0.3),
|
||||
"pitch": (-0.3, 0.3),
|
||||
"yaw": (-3.14, 3.14),
|
||||
},
|
||||
"velocity_range": {
|
||||
"x": (-0.2, 0.2),
|
||||
"y": (-0.2, 0.2),
|
||||
"z": (-0.2, 0.2),
|
||||
"roll": (-0.05, 0.05),
|
||||
"pitch": (-0.05, 0.05),
|
||||
"yaw": (-0.0, 0.0),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
self.events.randomize_rigid_body_mass.params["asset_cfg"].body_names = self.link_names # [self.base_link_name]
|
||||
self.events.randomize_rigid_body_mass_base = None
|
||||
self.events.randomize_com_positions.params["asset_cfg"].body_names = self.base_link_name # [self.base_link_name]
|
||||
# self.events.randomize_com_positions = None
|
||||
self.events.randomize_apply_external_force_torque = None
|
||||
self.events.randomize_push_robot = None
|
||||
self.events.randomize_actuator_gains.params["asset_cfg"].joint_names = self.joint_names
|
||||
|
||||
# set terrain generation probability to 0 for boxes and stairs
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].proportion = 0.4
|
||||
self.scene.terrain.terrain_generator.sub_terrains["hf_pyramid_slope"].proportion = 0.3
|
||||
self.scene.terrain.terrain_generator.sub_terrains["hf_pyramid_slope_inv"].proportion = 0.3
|
||||
self.scene.terrain.terrain_generator.sub_terrains["boxes"].proportion = 0.0
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs"].proportion = 0.0
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs_inv"].proportion = 0.0
|
||||
# scale down the terrains because the robot is small
|
||||
# self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1)
|
||||
# self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_width = 0.8
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01
|
||||
|
||||
# ------------------------------Rewards------------------------------
|
||||
self.rewards.action_rate_l2.weight = -0.02 #-0.02
|
||||
# self.rewards.smoothness_2.weight = -0.0075
|
||||
|
||||
self.rewards.base_height_l2.weight = -10.0
|
||||
self.rewards.base_height_l2.params["target_height"] = 0.35
|
||||
self.rewards.base_height_l2.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
|
||||
self.rewards.feet_air_time.weight = 5.0 # 5.0
|
||||
self.rewards.feet_air_time.params["threshold"] = 0.5
|
||||
self.rewards.feet_air_time.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_air_time_variance.weight = -8.0 # -8.0
|
||||
self.rewards.feet_air_time_variance.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_slide.weight = -0.05
|
||||
self.rewards.feet_slide.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_slide.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.stand_still.weight = -0.5 # -1.0
|
||||
self.rewards.stand_still.params["asset_cfg"].joint_names = self.joint_names
|
||||
self.rewards.stand_still.params["command_threshold"] = 0.1
|
||||
self.rewards.feet_height_body.weight = -2.5 # -2.5
|
||||
self.rewards.feet_height_body.params["target_height"] = -0.35
|
||||
self.rewards.feet_height_body.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_height.weight = -0.2 # -0.2
|
||||
self.rewards.feet_height.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_height.params["target_height"] = 0.05
|
||||
self.rewards.contact_forces.weight = -1e-1 # -2e-2
|
||||
self.rewards.contact_forces.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
|
||||
self.rewards.lin_vel_z_l2.weight = -2.0 #-2.0
|
||||
self.rewards.ang_vel_xy_l2.weight = -0.05 # -0.05
|
||||
|
||||
self.rewards.track_lin_vel_xy_exp.weight = 3.0
|
||||
self.rewards.track_ang_vel_z_exp.weight = 1.5
|
||||
|
||||
self.rewards.undesired_contacts.weight = -0.5
|
||||
self.rewards.undesired_contacts.params["sensor_cfg"].body_names = [f"^(?!.*{self.foot_link_name}).*"]
|
||||
|
||||
self.rewards.joint_torques_l2.weight = -2.5e-5
|
||||
self.rewards.joint_acc_l2.weight = -1e-8
|
||||
self.rewards.joint_deviation_l1.weight = -0.5
|
||||
self.rewards.joint_deviation_l1.params["asset_cfg"].joint_names = [".*HipX.*"]
|
||||
self.rewards.joint_power.weight = -2e-5
|
||||
self.rewards.flat_orientation_l2.weight = -5.0
|
||||
|
||||
# add the following rewards to improve the gait
|
||||
self.rewards.feet_gait.weight = 0.5
|
||||
self.rewards.feet_gait.params["synced_feet_pair_names"] = [
|
||||
["FL_FOOT", "HR_FOOT"],
|
||||
["FR_FOOT", "HL_FOOT"]
|
||||
]
|
||||
|
||||
self.rewards.joint_mirror.weight = -0.05
|
||||
self.rewards.joint_mirror.params["mirror_joints"] = [
|
||||
["FL_(HipX|HipY|Knee).*", "HR_(HipX|HipY|Knee).*"],
|
||||
["FR_(HipX|HipY|Knee).*", "HL_(HipX|HipY|Knee).*"],
|
||||
]
|
||||
|
||||
self.rewards.joint_pos_limits.weight = -5.0
|
||||
# self.rewards.joint_pos_penalty.weight = -1.0
|
||||
self.rewards.feet_contact_without_cmd.weight = 0.1
|
||||
self.rewards.feet_contact_without_cmd.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
|
||||
|
||||
# If the weight of rewards is 0, set rewards to None
|
||||
if self.__class__.__name__ == "DeeproboticsLite3RoughEnvCfg":
|
||||
self.disable_zero_weight_rewards()
|
||||
|
||||
# ------------------------------Terminations------------------------------
|
||||
self.terminations.illegal_contact = None
|
||||
# self.terminations.bad_orientation_2 = None
|
||||
|
||||
# ------------------------------Curriculums------------------------------
|
||||
# self.curriculum.command_levels.params["range_multiplier"] = (0.2, 1.0)
|
||||
self.curriculum.command_levels = None
|
||||
|
||||
# ------------------------------Commands------------------------------
|
||||
self.commands.base_velocity.ranges.lin_vel_x = (-1.5, 1.5)
|
||||
self.commands.base_velocity.ranges.lin_vel_y = (-0.8, 0.8)
|
||||
self.commands.base_velocity.ranges.ang_vel_z = (-0.8, 0.8)
|
||||
+15
@@ -0,0 +1,15 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""Configurations for velocity-based locomotion 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.
|
||||
+46
@@ -0,0 +1,46 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
import gymnasium as gym
|
||||
|
||||
from . import agents
|
||||
|
||||
##
|
||||
# Register Gym environments.
|
||||
##
|
||||
|
||||
gym.register(
|
||||
id="Flat-Deeprobotics-M20-v0",
|
||||
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||
disable_env_checker=True,
|
||||
kwargs={
|
||||
"env_cfg_entry_point": f"{__name__}.flat_env_cfg:DeeproboticsM20FlatEnvCfg",
|
||||
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsM20FlatPPORunnerCfg",
|
||||
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20FlatTrainerCfg",
|
||||
},
|
||||
)
|
||||
|
||||
gym.register(
|
||||
id="Rough-Deeprobotics-M20-v0",
|
||||
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||
disable_env_checker=True,
|
||||
kwargs={
|
||||
"env_cfg_entry_point": f"{__name__}.rough_env_cfg:DeeproboticsM20RoughEnvCfg",
|
||||
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsM20RoughPPORunnerCfg",
|
||||
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg",
|
||||
},
|
||||
)
|
||||
|
||||
gym.register(
|
||||
id="Rough-Deeprobotics-M20-Support-v0",
|
||||
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||
disable_env_checker=True,
|
||||
kwargs={
|
||||
"env_cfg_entry_point": f"{__name__}.rough_env_cfg:DeeproboticsM20SupportRoughEnvCfg",
|
||||
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DeeproboticsM20SupportRoughPPORunnerCfg",
|
||||
"cusrl_cfg_entry_point": f"{agents.__name__}.cusrl_ppo_cfg:DeeproboticsM20RoughTrainerCfg",
|
||||
},
|
||||
)
|
||||
+5
@@ -0,0 +1,5 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
+56
@@ -0,0 +1,56 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.utils import configclass
|
||||
from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||
num_steps_per_env = 24
|
||||
max_iterations = 20000
|
||||
save_interval = 100
|
||||
experiment_name = "deeprobotics_m20_rough"
|
||||
empirical_normalization = False
|
||||
clip_actions = 100
|
||||
policy = RslRlPpoActorCriticCfg(
|
||||
init_noise_std=1.0,
|
||||
noise_std_type="log",
|
||||
actor_hidden_dims=[512, 256, 128],
|
||||
critic_hidden_dims=[512, 256, 128],
|
||||
activation="elu",
|
||||
)
|
||||
algorithm = RslRlPpoAlgorithmCfg(
|
||||
value_loss_coef=1.0,
|
||||
use_clipped_value_loss=True,
|
||||
clip_param=0.2,
|
||||
entropy_coef=0.01,
|
||||
num_learning_epochs=5,
|
||||
num_mini_batches=4,
|
||||
learning_rate=1.0e-3,
|
||||
schedule="adaptive",
|
||||
gamma=0.99,
|
||||
lam=0.95,
|
||||
desired_kl=0.01,
|
||||
max_grad_norm=1.0,
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20FlatPPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg):
|
||||
def __post_init__(self):
|
||||
super().__post_init__()
|
||||
|
||||
self.max_iterations = 5000
|
||||
self.experiment_name = "deeprobotics_m20_flat"
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20SupportRoughPPORunnerCfg(DeeproboticsM20RoughPPORunnerCfg):
|
||||
def __post_init__(self):
|
||||
super().__post_init__()
|
||||
|
||||
self.experiment_name = "deeprobotics_m20_support"
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.utils import configclass
|
||||
|
||||
from .rough_env_cfg import DeeproboticsM20RoughEnvCfg
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20FlatEnvCfg(DeeproboticsM20RoughEnvCfg):
|
||||
def __post_init__(self):
|
||||
# post init of parent
|
||||
super().__post_init__()
|
||||
|
||||
# override rewards
|
||||
self.rewards.base_height_l2.params["sensor_cfg"] = None
|
||||
# change terrain to flat
|
||||
self.scene.terrain.terrain_type = "plane"
|
||||
self.scene.terrain.terrain_generator = None
|
||||
# no height scan
|
||||
self.scene.height_scanner = None
|
||||
self.observations.policy.height_scan = None
|
||||
self.observations.critic.height_scan = None
|
||||
# no terrain curriculum
|
||||
self.curriculum.terrain_levels = None
|
||||
|
||||
# If the weight of rewards is 0, set rewards to None
|
||||
if self.__class__.__name__ == "DeeproboticsM20FlatEnvCfg":
|
||||
self.disable_zero_weight_rewards()
|
||||
+504
@@ -0,0 +1,504 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from isaaclab.managers import CurriculumTermCfg as CurrTerm
|
||||
from isaaclab.managers import EventTermCfg as EventTerm
|
||||
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||
from isaaclab.managers import SceneEntityCfg
|
||||
from isaaclab.utils import configclass
|
||||
|
||||
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
|
||||
from rl_training.tasks.manager_based.locomotion.velocity.velocity_env_cfg import (
|
||||
ActionsCfg,
|
||||
EventCfg,
|
||||
LocomotionVelocityRoughEnvCfg,
|
||||
RewardsCfg,
|
||||
)
|
||||
|
||||
##
|
||||
# Pre-defined configs
|
||||
##
|
||||
from rl_training.assets.deeprobotics import DEEPROBOTICS_M20_CFG # isort: skip
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20ActionsCfg(ActionsCfg):
|
||||
"""Action specifications for the MDP."""
|
||||
|
||||
joint_pos = mdp.JointPositionActionCfg(
|
||||
asset_name="robot", joint_names=[""], scale=0.25, use_default_offset=True, clip=None, preserve_order=True
|
||||
)
|
||||
|
||||
joint_vel = mdp.JointVelocityActionCfg(
|
||||
asset_name="robot", joint_names=[""], scale=20.0, use_default_offset=True, clip=None, preserve_order=True
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20EventCfg(EventCfg):
|
||||
"""Domain randomization with tire-specific contact material."""
|
||||
|
||||
# Replace the inherited all-body material randomization with separate tire and support materials.
|
||||
randomize_rigid_body_material = None
|
||||
|
||||
randomize_wheel_material = EventTerm(
|
||||
func=mdp.randomize_rigid_body_material,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=".*_wheel"),
|
||||
"static_friction_range": (0.9, 1.4),
|
||||
"dynamic_friction_range": (0.6, 0.9),
|
||||
"restitution_range": (0.0, 0.05),
|
||||
"num_buckets": 1024,
|
||||
},
|
||||
)
|
||||
|
||||
randomize_support_material = EventTerm(
|
||||
func=mdp.randomize_rigid_body_material,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=["base_link", ".*_knee"]),
|
||||
"static_friction_range": (0.4, 0.8),
|
||||
"dynamic_friction_range": (0.25, 0.4),
|
||||
"restitution_range": (0.0, 0.05),
|
||||
"num_buckets": 1024,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20RewardsCfg(RewardsCfg):
|
||||
"""Reward terms for the MDP."""
|
||||
|
||||
joint_vel_wheel_l2 = RewTerm(
|
||||
func=mdp.joint_vel_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names="")}
|
||||
)
|
||||
|
||||
joint_acc_wheel_l2 = RewTerm(
|
||||
func=mdp.joint_acc_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names="")}
|
||||
)
|
||||
|
||||
joint_torques_wheel_l2 = RewTerm(
|
||||
func=mdp.joint_torques_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names="")}
|
||||
)
|
||||
|
||||
track_diff_wheel_yaw_exp = RewTerm(
|
||||
func=mdp.track_diff_wheel_yaw_exp,
|
||||
weight=1.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"std": 0.6,
|
||||
"wheel_radius": 0.096,
|
||||
"track_width": 0.50,
|
||||
"left_wheel_indices": (0, 2),
|
||||
"right_wheel_indices": (1, 3),
|
||||
"command_threshold": 0.05,
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=""),
|
||||
},
|
||||
)
|
||||
|
||||
lin_vel_xy_yaw_command_l2 = RewTerm(
|
||||
func=mdp.lin_vel_xy_yaw_command_l2,
|
||||
weight=-0.4,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"yaw_command_threshold": 0.25,
|
||||
"lin_command_threshold": 0.15,
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
|
||||
command_progress = RewTerm(
|
||||
func=mdp.command_progress,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"min_command": 0.12,
|
||||
"max_reward_speed": 0.8,
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
},
|
||||
)
|
||||
|
||||
support_contact_progress = RewTerm(
|
||||
func=mdp.ObstacleSupportEscapeReward,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"wheel_sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"wheel_asset_cfg": SceneEntityCfg("robot", joint_names=""),
|
||||
"wheel_radius": 0.096,
|
||||
"support_force_threshold": 15.0,
|
||||
"wheel_obstacle_force_threshold": 35.0,
|
||||
"max_contact_time": 0.45,
|
||||
"min_command": 0.15,
|
||||
"stall_speed_ratio": 0.35,
|
||||
"min_stall_speed": 0.12,
|
||||
"slip_threshold": 0.30,
|
||||
"obstacle_memory_time": 0.60,
|
||||
"support_window_time": 0.80,
|
||||
"recovery_speed_ratio": 0.60,
|
||||
"min_recovery_speed": 0.18,
|
||||
"recovery_reward_time": 0.25,
|
||||
"cooldown_time": 0.60,
|
||||
"recovery_bonus": 0.25,
|
||||
"max_reward_speed": 0.8,
|
||||
},
|
||||
)
|
||||
|
||||
support_contact_excess = RewTerm(
|
||||
func=mdp.support_contact_excess,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"force_threshold": 120.0,
|
||||
"max_contact_time": 0.6,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
|
||||
"""Stage 1: blind tactile locomotion without body-support rewards."""
|
||||
|
||||
actions: DeeproboticsM20ActionsCfg = DeeproboticsM20ActionsCfg()
|
||||
events: DeeproboticsM20EventCfg = DeeproboticsM20EventCfg()
|
||||
rewards: DeeproboticsM20RewardsCfg = DeeproboticsM20RewardsCfg()
|
||||
|
||||
base_link_name = "base_link"
|
||||
foot_link_name = ".*_wheel"
|
||||
support_link_names = ["base_link", ".*_knee"]
|
||||
|
||||
# fmt: off
|
||||
leg_joint_names = [
|
||||
"fl_knee_joint",
|
||||
"fr_knee_joint",
|
||||
"hl_knee_joint",
|
||||
"hr_knee_joint",
|
||||
]
|
||||
wheel_joint_names = [
|
||||
"fl_wheel_joint", "fr_wheel_joint", "hl_wheel_joint", "hr_wheel_joint",
|
||||
]
|
||||
|
||||
knee_joint_names = [
|
||||
"fl_knee_joint", "fr_knee_joint", "hl_knee_joint", "hr_knee_joint",
|
||||
]
|
||||
joint_names = leg_joint_names + wheel_joint_names
|
||||
# fmt: on
|
||||
|
||||
def __post_init__(self):
|
||||
# post init of parent
|
||||
super().__post_init__()
|
||||
|
||||
# ------------------------------Sence------------------------------
|
||||
self.scene.robot = DEEPROBOTICS_M20_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||
self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/" + self.base_link_name
|
||||
self.scene.height_scanner_base.prim_path = "{ENV_REGEX_NS}/Robot/" + self.base_link_name
|
||||
self.scene.terrain.physics_material.restitution = 0.0
|
||||
self.sim.physics_material = self.scene.terrain.physics_material
|
||||
|
||||
# ------------------------------Observations------------------------------
|
||||
self.observations.policy.joint_pos.func = mdp.joint_pos_rel_without_wheel
|
||||
self.observations.policy.joint_pos.params["wheel_asset_cfg"] = SceneEntityCfg(
|
||||
"robot", joint_names=self.wheel_joint_names
|
||||
)
|
||||
self.observations.critic.joint_pos.func = mdp.joint_pos_rel_without_wheel
|
||||
self.observations.critic.joint_pos.params["wheel_asset_cfg"] = SceneEntityCfg(
|
||||
"robot", joint_names=self.wheel_joint_names
|
||||
)
|
||||
self.observations.policy.base_lin_vel.scale = 2.0
|
||||
self.observations.policy.base_ang_vel.scale = 0.25
|
||||
self.observations.policy.joint_pos.scale = 1.0
|
||||
self.observations.policy.joint_vel.scale = 0.05
|
||||
self.observations.policy.base_lin_vel = None
|
||||
self.observations.policy.height_scan = None
|
||||
self.observations.policy.joint_pos.params["asset_cfg"].joint_names = self.joint_names
|
||||
self.observations.policy.joint_vel.params["asset_cfg"].joint_names = self.joint_names
|
||||
self.observations.policy.wheel_contact = ObsTerm(
|
||||
func=mdp.contact_state,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[self.foot_link_name]),
|
||||
"threshold": 2.0,
|
||||
},
|
||||
clip=(0.0, 1.0),
|
||||
scale=1.0,
|
||||
)
|
||||
self.observations.policy.support_contact = ObsTerm(
|
||||
func=mdp.contact_state,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=self.support_link_names),
|
||||
"threshold": 2.0,
|
||||
},
|
||||
clip=(0.0, 1.0),
|
||||
scale=1.0,
|
||||
)
|
||||
self.observations.policy.wheel_contact_force = ObsTerm(
|
||||
func=mdp.contact_force_norm,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[self.foot_link_name]),
|
||||
"scale": 100.0,
|
||||
"clip": 2.0,
|
||||
},
|
||||
clip=(0.0, 2.0),
|
||||
scale=1.0,
|
||||
)
|
||||
self.observations.policy.support_contact_force = ObsTerm(
|
||||
func=mdp.contact_force_norm,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=self.support_link_names),
|
||||
"scale": 100.0,
|
||||
"clip": 2.0,
|
||||
},
|
||||
clip=(0.0, 2.0),
|
||||
scale=1.0,
|
||||
)
|
||||
self.observations.policy.wheel_slip = ObsTerm(
|
||||
func=mdp.wheel_slip_proxy,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=self.wheel_joint_names, preserve_order=True),
|
||||
"wheel_radius": 0.096,
|
||||
},
|
||||
clip=(0.0, 3.0),
|
||||
scale=0.5,
|
||||
)
|
||||
self.observations.policy.applied_torque = ObsTerm(
|
||||
func=mdp.applied_torque,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=self.joint_names, preserve_order=True),
|
||||
"scale": 80.0,
|
||||
"clip": 2.0,
|
||||
},
|
||||
clip=(-2.0, 2.0),
|
||||
scale=1.0,
|
||||
)
|
||||
|
||||
# ------------------------------Actions------------------------------
|
||||
# reduce action scale
|
||||
self.actions.joint_pos.scale = 0.25
|
||||
self.actions.joint_vel.scale = 5.0
|
||||
self.actions.joint_pos.clip = {".*": (-100.0, 100.0)}
|
||||
self.actions.joint_vel.clip = {".*": (-100.0, 100.0)}
|
||||
self.actions.joint_pos.joint_names = self.leg_joint_names
|
||||
self.actions.joint_vel.joint_names = self.wheel_joint_names
|
||||
|
||||
# ------------------------------Events------------------------------
|
||||
self.events.randomize_reset_base.params = {
|
||||
"pose_range": {
|
||||
"x": (-1.0, 1.0),
|
||||
"y": (-1.0, 1.0),
|
||||
"z": (0.0, 0.0),
|
||||
"roll": (-0.3, 0.3),
|
||||
"pitch": (-0.3, 0.3),
|
||||
"yaw": (-3.14, 3.14),
|
||||
},
|
||||
"velocity_range": {
|
||||
"x": (-0.2, 0.2),
|
||||
"y": (-0.2, 0.2),
|
||||
"z": (-0.2, 0.2),
|
||||
"roll": (-0.05, 0.05),
|
||||
"pitch": (-0.05, 0.05),
|
||||
"yaw": (-0.0, 0.0),
|
||||
},
|
||||
}
|
||||
self.events.randomize_rigid_body_mass_base.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
self.events.randomize_rigid_body_mass.params["asset_cfg"].body_names = [
|
||||
f"^(?!.*{self.base_link_name}).*"
|
||||
]
|
||||
self.events.randomize_com_positions.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
self.events.randomize_apply_external_force_torque.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
|
||||
self.scene.terrain.max_init_terrain_level = 2
|
||||
self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.02, 0.14)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs"].step_height_range = (0.03, 0.16)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs_inv"].step_height_range = (0.03, 0.16)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.10)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_step = 0.01
|
||||
|
||||
# only randomize actuator gains for actuated joints (knees + wheels), exclude hipx/hipy completely
|
||||
self.events.randomize_actuator_gains.params["asset_cfg"].joint_names = self.joint_names
|
||||
|
||||
# ------------------------------Rewards------------------------------
|
||||
# General
|
||||
self.rewards.is_terminated.weight = 0
|
||||
|
||||
# Root penalties
|
||||
self.rewards.lin_vel_z_l2.weight = -2.0
|
||||
self.rewards.ang_vel_xy_l2.weight = -0.02
|
||||
self.rewards.flat_orientation_l2.weight = -0.5
|
||||
self.rewards.base_height_l2.weight = -0.5
|
||||
self.rewards.base_height_l2.params["target_height"] = 0.45
|
||||
self.rewards.base_height_l2.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
self.rewards.body_lin_acc_l2.weight = 0
|
||||
self.rewards.body_lin_acc_l2.params["asset_cfg"].body_names = [self.base_link_name]
|
||||
|
||||
# Joint penalties
|
||||
self.rewards.joint_torques_l2.weight = -2.5e-5
|
||||
self.rewards.joint_torques_l2.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
self.rewards.joint_torques_wheel_l2.weight = 0
|
||||
self.rewards.joint_torques_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.joint_vel_l2.weight = 0
|
||||
self.rewards.joint_vel_l2.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
self.rewards.joint_vel_wheel_l2.weight = 0
|
||||
self.rewards.joint_vel_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.joint_acc_l2.weight = -2e-7
|
||||
self.rewards.joint_acc_l2.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
self.rewards.joint_acc_wheel_l2.weight = -1e-7
|
||||
self.rewards.joint_acc_wheel_l2.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
# self.rewards.create_joint_deviation_l1_rewterm("joint_deviation_hip_l1", -0.2, [".*_hip_joint"])
|
||||
self.rewards.joint_pos_limits.weight = -5.0
|
||||
self.rewards.joint_pos_limits.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
self.rewards.joint_vel_limits.weight = 0
|
||||
self.rewards.joint_vel_limits.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.joint_power.weight = -2e-5
|
||||
self.rewards.joint_power.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
self.rewards.stand_still.weight = -2.0
|
||||
self.rewards.stand_still.params["asset_cfg"].joint_names = self.leg_joint_names
|
||||
|
||||
self.rewards.knee_joint_pos_penalty.weight = -0.1
|
||||
self.rewards.knee_joint_pos_penalty.params["asset_cfg"].joint_names = self.knee_joint_names
|
||||
self.rewards.wheel_vel_penalty.weight = 0
|
||||
self.rewards.wheel_vel_penalty.params["sensor_cfg"].body_names = self.foot_link_name
|
||||
self.rewards.wheel_vel_penalty.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.joint_mirror.weight = -0.03
|
||||
self.rewards.joint_mirror.params["mirror_joints"] = [
|
||||
["fl_knee_joint", "hr_knee_joint"],
|
||||
["fr_knee_joint", "hl_knee_joint"],
|
||||
]
|
||||
|
||||
# Action penalties
|
||||
self.rewards.action_rate_l2.weight = -0.01
|
||||
|
||||
# Contact sensor
|
||||
self.rewards.undesired_contacts.weight = -1.0
|
||||
self.rewards.undesired_contacts.params["sensor_cfg"].body_names = [f"^(?!.*{self.foot_link_name}).*"]
|
||||
self.rewards.undesired_contacts.params["threshold"] = 1.0
|
||||
self.rewards.contact_forces.weight = -1.5e-4
|
||||
self.rewards.contact_forces.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.support_contact_progress.weight = 0.0
|
||||
self.rewards.support_contact_progress.params["sensor_cfg"].body_names = self.support_link_names
|
||||
self.rewards.support_contact_progress.params["wheel_sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.support_contact_progress.params["wheel_asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.support_contact_excess.weight = 0.0
|
||||
self.rewards.support_contact_excess.params["sensor_cfg"].body_names = self.support_link_names
|
||||
|
||||
# Velocity-tracking rewards
|
||||
self.rewards.track_lin_vel_xy_exp.weight = 2.0 # 1.8
|
||||
self.rewards.track_ang_vel_z_exp.weight = 1.5 # 1.2
|
||||
self.rewards.track_ang_vel_z_exp.params["std"] = 0.5
|
||||
self.rewards.track_diff_wheel_yaw_exp.weight = 1.0
|
||||
self.rewards.track_diff_wheel_yaw_exp.params["asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.lin_vel_xy_yaw_command_l2.weight = -0.4
|
||||
|
||||
# Others
|
||||
self.rewards.feet_air_time.weight = 0
|
||||
self.rewards.feet_air_time.params["threshold"] = 0.5
|
||||
self.rewards.feet_air_time.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_contact.weight = 0
|
||||
self.rewards.feet_contact.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_contact_without_cmd.weight = 0.1
|
||||
self.rewards.feet_contact_without_cmd.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_stumble.weight = 0
|
||||
self.rewards.feet_stumble.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_slide.weight = 0
|
||||
self.rewards.feet_slide.params["sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_slide.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_height.weight = 0
|
||||
self.rewards.feet_height.params["target_height"] = 0.1
|
||||
self.rewards.feet_height.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_height_body.weight = 0
|
||||
self.rewards.feet_height_body.params["target_height"] = -0.4
|
||||
self.rewards.feet_height_body.params["asset_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.feet_gait.weight = 0
|
||||
self.rewards.feet_gait.params["synced_feet_pair_names"] = (("fl_wheel", "hr_wheel"), ("fr_wheel", "hl_wheel"))
|
||||
self.rewards.upward.weight = 0.08
|
||||
|
||||
# If the weight of rewards is 0, set rewards to None
|
||||
if self.__class__.__name__ == "DeeproboticsM20RoughEnvCfg":
|
||||
self.disable_zero_weight_rewards()
|
||||
|
||||
# ------------------------------Terminations------------------------------
|
||||
# self.terminations.illegal_contact.params["sensor_cfg"].body_names = [self.base_link_name]
|
||||
self.terminations.illegal_contact = None
|
||||
self.terminations.bad_orientation_2 = None
|
||||
|
||||
# ------------------------------Curriculums------------------------------
|
||||
# self.curriculum.command_levels.params["range_multiplier"] = (0.2, 1.0)
|
||||
self.curriculum.command_levels = None
|
||||
|
||||
# ------------------------------Commands------------------------------
|
||||
self.commands.base_velocity.debug_vis = False
|
||||
self.commands.base_velocity.ranges.lin_vel_x = (-2.0, 2.0)
|
||||
self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0)
|
||||
self.commands.base_velocity.ranges.ang_vel_z = (-1.5, 1.5)
|
||||
|
||||
|
||||
@configclass
|
||||
class DeeproboticsM20SupportRoughEnvCfg(DeeproboticsM20RoughEnvCfg):
|
||||
"""Stage 2: resume from Stage 1 and enable brief body/knee support for obstacle crossing."""
|
||||
|
||||
def __post_init__(self):
|
||||
super().__post_init__()
|
||||
|
||||
# Keep the same policy/action dimensions as Stage 1 for checkpoint resume.
|
||||
# Only rewards, command distribution, and terrain curriculum are changed here.
|
||||
self.scene.terrain.max_init_terrain_level = 1
|
||||
self.scene.terrain.terrain_generator.sub_terrains["boxes"].proportion = 0.25
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs"].proportion = 0.25
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs_inv"].proportion = 0.15
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].proportion = 0.15
|
||||
self.scene.terrain.terrain_generator.sub_terrains["hf_pyramid_slope"].proportion = 0.10
|
||||
self.scene.terrain.terrain_generator.sub_terrains["hf_pyramid_slope_inv"].proportion = 0.10
|
||||
self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.03, 0.22)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs"].step_height_range = (0.04, 0.24)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["pyramid_stairs_inv"].step_height_range = (0.04, 0.20)
|
||||
self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.14)
|
||||
|
||||
# Keep privileged terrain information for the critic only. The actor remains blind/tactile.
|
||||
self.observations.policy.height_scan = None
|
||||
self.observations.critic.height_scan.params["sensor_cfg"] = SceneEntityCfg("height_scanner")
|
||||
self.observations.critic.height_scan.clip = (-1.0, 1.0)
|
||||
self.observations.critic.height_scan.scale = 1.0
|
||||
|
||||
# Body/knee contact is allowed only as short support during forward progress.
|
||||
self.rewards.undesired_contacts.weight = -0.10
|
||||
self.rewards.undesired_contacts.params["sensor_cfg"].body_names = [f"^(?!.*{self.foot_link_name}).*"]
|
||||
self.rewards.undesired_contacts.params["threshold"] = 35.0
|
||||
self.rewards.command_progress.weight = 0.8
|
||||
self.rewards.support_contact_progress.weight = 1.0
|
||||
self.rewards.support_contact_progress.params["sensor_cfg"].body_names = self.support_link_names
|
||||
self.rewards.support_contact_progress.params["wheel_sensor_cfg"].body_names = [self.foot_link_name]
|
||||
self.rewards.support_contact_progress.params["wheel_asset_cfg"].joint_names = self.wheel_joint_names
|
||||
self.rewards.support_contact_excess.weight = -0.60
|
||||
self.rewards.support_contact_excess.params["sensor_cfg"].body_names = self.support_link_names
|
||||
self.rewards.support_contact_excess.params["force_threshold"] = 100.0
|
||||
self.rewards.support_contact_excess.params["max_contact_time"] = 0.45
|
||||
|
||||
# Stage 2 should still avoid the bad solution of crawling on the body.
|
||||
self.rewards.track_lin_vel_xy_exp.weight = 1.5
|
||||
self.rewards.track_ang_vel_z_exp.weight = 1.0
|
||||
self.rewards.flat_orientation_l2.weight = -0.45
|
||||
self.rewards.base_height_l2.weight = -0.35
|
||||
self.rewards.base_height_l2.params["target_height"] = 0.29
|
||||
|
||||
# Stage 2 first focuses on forward obstacle crossing, then expands the speed range.
|
||||
self.commands.base_velocity.heading_command = False
|
||||
self.commands.base_velocity.rel_standing_envs = 0.0
|
||||
self.commands.base_velocity.rel_heading_envs = 0.0
|
||||
self.commands.base_velocity.ranges.lin_vel_x = (0.25, 1.2)
|
||||
self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0)
|
||||
self.commands.base_velocity.ranges.ang_vel_z = (-0.6, 0.6)
|
||||
self.curriculum.command_levels = CurrTerm(
|
||||
func=mdp.command_levels_positive_x,
|
||||
params={
|
||||
"reward_term_name": "track_lin_vel_xy_exp",
|
||||
"initial_max_ratio": 0.35,
|
||||
"increment": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
self.disable_zero_weight_rewards()
|
||||
+21
@@ -0,0 +1,21 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
"""This sub-module contains the functions that are specific to the locomotion environments."""
|
||||
|
||||
from isaaclab.envs.mdp import * # noqa: F401, F403
|
||||
from isaaclab_tasks.manager_based.locomotion.velocity.mdp import * # noqa: F401, F403
|
||||
|
||||
from .commands import * # noqa: F401, F403
|
||||
from .curriculums import * # noqa: F401, F403
|
||||
from .events import * # noqa: F401, F403
|
||||
from .observations import * # noqa: F401, F403
|
||||
from .rewards import * # noqa: F401, F403
|
||||
+130
@@ -0,0 +1,130 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import torch
|
||||
from typing import TYPE_CHECKING, Sequence
|
||||
|
||||
from isaaclab.managers import CommandTerm, CommandTermCfg
|
||||
from isaaclab.utils import configclass
|
||||
|
||||
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from isaaclab.envs import ManagerBasedEnv
|
||||
|
||||
|
||||
class UniformThresholdVelocityCommand(mdp.UniformVelocityCommand):
|
||||
"""Command generator that generates a velocity command in SE(2) from uniform distribution with threshold."""
|
||||
|
||||
cfg: mdp.UniformThresholdVelocityCommandCfg
|
||||
"""The configuration of the command generator."""
|
||||
|
||||
def _resample_command(self, env_ids: Sequence[int]):
|
||||
super()._resample_command(env_ids)
|
||||
# set small commands to zero
|
||||
self.vel_command_b[env_ids, :2] *= (torch.norm(self.vel_command_b[env_ids, :2], dim=1) > 0.2).unsqueeze(1)
|
||||
|
||||
|
||||
@configclass
|
||||
class UniformThresholdVelocityCommandCfg(mdp.UniformVelocityCommandCfg):
|
||||
"""Configuration for the uniform threshold velocity command generator."""
|
||||
|
||||
class_type: type = UniformThresholdVelocityCommand
|
||||
|
||||
|
||||
class DiscreteCommandController(CommandTerm):
|
||||
"""
|
||||
Command generator that assigns discrete commands to environments.
|
||||
|
||||
Commands are stored as a list of predefined integers.
|
||||
The controller maps these commands by their indices (e.g., index 0 -> 10, index 1 -> 20).
|
||||
"""
|
||||
|
||||
cfg: DiscreteCommandControllerCfg
|
||||
"""Configuration for the command controller."""
|
||||
|
||||
def __init__(self, cfg: DiscreteCommandControllerCfg, env: ManagerBasedEnv):
|
||||
"""
|
||||
Initialize the command controller.
|
||||
|
||||
Args:
|
||||
cfg: The configuration of the command controller.
|
||||
env: The environment object.
|
||||
"""
|
||||
# Initialize the base class
|
||||
super().__init__(cfg, env)
|
||||
|
||||
# Validate that available_commands is non-empty
|
||||
if not self.cfg.available_commands:
|
||||
raise ValueError("The available_commands list cannot be empty.")
|
||||
|
||||
# Ensure all elements are integers
|
||||
if not all(isinstance(cmd, int) for cmd in self.cfg.available_commands):
|
||||
raise ValueError("All elements in available_commands must be integers.")
|
||||
|
||||
# Store the available commands
|
||||
self.available_commands = self.cfg.available_commands
|
||||
|
||||
# Create buffers to store the command
|
||||
# -- command buffer: stores discrete action indices for each environment
|
||||
self.command_buffer = torch.zeros(self.num_envs, dtype=torch.int32, device=self.device)
|
||||
|
||||
# -- current_commands: stores a snapshot of the current commands (as integers)
|
||||
self.current_commands = [self.available_commands[0]] * self.num_envs # Default to the first command
|
||||
|
||||
def __str__(self) -> str:
|
||||
"""Return a string representation of the command controller."""
|
||||
return (
|
||||
"DiscreteCommandController:\n"
|
||||
f"\tNumber of environments: {self.num_envs}\n"
|
||||
f"\tAvailable commands: {self.available_commands}\n"
|
||||
)
|
||||
|
||||
"""
|
||||
Properties
|
||||
"""
|
||||
|
||||
@property
|
||||
def command(self) -> torch.Tensor:
|
||||
"""Return the current command buffer. Shape is (num_envs, 1)."""
|
||||
return self.command_buffer
|
||||
|
||||
"""
|
||||
Implementation specific functions.
|
||||
"""
|
||||
|
||||
def _update_metrics(self):
|
||||
"""Update metrics for the command controller."""
|
||||
pass
|
||||
|
||||
def _resample_command(self, env_ids: Sequence[int]):
|
||||
"""Resample commands for the given environments."""
|
||||
sampled_indices = torch.randint(
|
||||
len(self.available_commands), (len(env_ids),), dtype=torch.int32, device=self.device
|
||||
)
|
||||
sampled_commands = torch.tensor(
|
||||
[self.available_commands[idx.item()] for idx in sampled_indices], dtype=torch.int32, device=self.device
|
||||
)
|
||||
self.command_buffer[env_ids] = sampled_commands
|
||||
|
||||
def _update_command(self):
|
||||
"""Update and store the current commands."""
|
||||
self.current_commands = self.command_buffer.tolist()
|
||||
|
||||
|
||||
@configclass
|
||||
class DiscreteCommandControllerCfg(CommandTermCfg):
|
||||
"""Configuration for the discrete command controller."""
|
||||
|
||||
class_type: type = DiscreteCommandController
|
||||
|
||||
available_commands: list[int] = []
|
||||
"""
|
||||
List of available discrete commands, where each element is an integer.
|
||||
Example: [10, 20, 30, 40, 50]
|
||||
"""
|
||||
+95
@@ -0,0 +1,95 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
"""Common functions that can be used to create curriculum for the learning environment.
|
||||
|
||||
The functions can be passed to the :class:`isaaclab.managers.CurriculumTermCfg` object to enable
|
||||
the curriculum introduced by the function.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import torch
|
||||
from collections.abc import Sequence
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from isaaclab.envs import ManagerBasedRLEnv
|
||||
|
||||
|
||||
def command_levels_vel(
|
||||
env: ManagerBasedRLEnv,
|
||||
env_ids: Sequence[int],
|
||||
reward_term_name: str,
|
||||
range_multiplier: Sequence[float] = (0.1, 1.0),
|
||||
) -> None:
|
||||
"""command_levels_vel"""
|
||||
base_velocity_ranges = env.command_manager.get_term("base_velocity").cfg.ranges
|
||||
# Get original velocity ranges (ONLY ON FIRST EPISODE)
|
||||
if env.common_step_counter == 0:
|
||||
env._original_vel_x = torch.tensor(base_velocity_ranges.lin_vel_x, device=env.device)
|
||||
env._original_vel_y = torch.tensor(base_velocity_ranges.lin_vel_y, device=env.device)
|
||||
env._initial_vel_x = env._original_vel_x * range_multiplier[0]
|
||||
env._final_vel_x = env._original_vel_x * range_multiplier[1]
|
||||
env._initial_vel_y = env._original_vel_y * range_multiplier[0]
|
||||
env._final_vel_y = env._original_vel_y * range_multiplier[1]
|
||||
|
||||
# Initialize command ranges to initial values
|
||||
base_velocity_ranges.lin_vel_x = env._initial_vel_x.tolist()
|
||||
base_velocity_ranges.lin_vel_y = env._initial_vel_y.tolist()
|
||||
|
||||
# avoid updating command curriculum at each step since the maximum command is common to all envs
|
||||
if env.common_step_counter % env.max_episode_length == 0:
|
||||
episode_sums = env.reward_manager._episode_sums[reward_term_name]
|
||||
reward_term_cfg = env.reward_manager.get_term_cfg(reward_term_name)
|
||||
delta_command = torch.tensor([-0.1, 0.1], device=env.device)
|
||||
|
||||
# If the tracking reward is above 80% of the maximum, increase the range of commands
|
||||
if torch.mean(episode_sums[env_ids]) / env.max_episode_length_s > 0.8 * reward_term_cfg.weight:
|
||||
new_vel_x = torch.tensor(base_velocity_ranges.lin_vel_x, device=env.device) + delta_command
|
||||
new_vel_y = torch.tensor(base_velocity_ranges.lin_vel_y, device=env.device) + delta_command
|
||||
|
||||
# Clamp to ensure we don't exceed final ranges
|
||||
new_vel_x = torch.clamp(new_vel_x, min=env._final_vel_x[0], max=env._final_vel_x[1])
|
||||
new_vel_y = torch.clamp(new_vel_y, min=env._final_vel_y[0], max=env._final_vel_y[1])
|
||||
|
||||
# Update ranges
|
||||
base_velocity_ranges.lin_vel_x = new_vel_x.tolist()
|
||||
base_velocity_ranges.lin_vel_y = new_vel_y.tolist()
|
||||
|
||||
return torch.tensor(base_velocity_ranges.lin_vel_x[1], device=env.device)
|
||||
|
||||
|
||||
def command_levels_positive_x(
|
||||
env: ManagerBasedRLEnv,
|
||||
env_ids: Sequence[int],
|
||||
reward_term_name: str,
|
||||
initial_max_ratio: float = 0.35,
|
||||
increment: float = 0.1,
|
||||
) -> torch.Tensor:
|
||||
"""Expand only the upper bound of a positive forward-speed command range.
|
||||
|
||||
Unlike :func:`command_levels_vel`, this curriculum preserves the configured minimum forward speed. This avoids
|
||||
producing near-zero commands in obstacle-focused stages where the policy needs enough momentum to learn climbing.
|
||||
"""
|
||||
base_velocity_ranges = env.command_manager.get_term("base_velocity").cfg.ranges
|
||||
if env.common_step_counter == 0:
|
||||
original_x = torch.tensor(base_velocity_ranges.lin_vel_x, device=env.device)
|
||||
env._positive_x_min = original_x[0]
|
||||
env._positive_x_max = original_x[1]
|
||||
initial_x_max = torch.maximum(env._positive_x_min, env._positive_x_max * initial_max_ratio)
|
||||
base_velocity_ranges.lin_vel_x = [env._positive_x_min.item(), initial_x_max.item()]
|
||||
|
||||
if env.common_step_counter % env.max_episode_length == 0:
|
||||
episode_sums = env.reward_manager._episode_sums[reward_term_name]
|
||||
reward_term_cfg = env.reward_manager.get_term_cfg(reward_term_name)
|
||||
mean_reward = torch.mean(episode_sums[env_ids]) / env.max_episode_length_s
|
||||
if mean_reward > 0.8 * reward_term_cfg.weight:
|
||||
current_x_max = torch.tensor(base_velocity_ranges.lin_vel_x[1], device=env.device)
|
||||
new_x_max = torch.clamp(current_x_max + increment, max=env._positive_x_max)
|
||||
base_velocity_ranges.lin_vel_x = [env._positive_x_min.item(), new_x_max.item()]
|
||||
|
||||
return torch.tensor(base_velocity_ranges.lin_vel_x[1], device=env.device)
|
||||
@@ -0,0 +1,213 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
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
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from isaaclab.envs import ManagerBasedEnv
|
||||
|
||||
|
||||
def randomize_rigid_body_inertia(
|
||||
env: ManagerBasedEnv,
|
||||
env_ids: torch.Tensor | None,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
inertia_distribution_params: tuple[float, float],
|
||||
operation: Literal["add", "scale", "abs"],
|
||||
distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform",
|
||||
):
|
||||
"""Randomize the inertia tensors of the bodies by adding, scaling, or setting random values.
|
||||
|
||||
This function allows randomizing only the diagonal inertia tensor components (xx, yy, zz) of the bodies.
|
||||
The function samples random values from the given distribution parameters and adds, scales, or sets the values
|
||||
into the physics simulation based on the operation.
|
||||
|
||||
.. tip::
|
||||
This function uses CPU tensors to assign the body inertias. It is recommended to use this function
|
||||
only during the initialization of the environment.
|
||||
"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject | Articulation = env.scene[asset_cfg.name]
|
||||
|
||||
# resolve environment ids
|
||||
if env_ids is None:
|
||||
env_ids = torch.arange(env.scene.num_envs, device="cpu")
|
||||
else:
|
||||
env_ids = env_ids.cpu()
|
||||
|
||||
# resolve body indices
|
||||
if asset_cfg.body_ids == slice(None):
|
||||
body_ids = torch.arange(asset.num_bodies, dtype=torch.int, device="cpu")
|
||||
else:
|
||||
body_ids = torch.tensor(asset_cfg.body_ids, dtype=torch.int, device="cpu")
|
||||
|
||||
# get the current inertia tensors of the bodies (num_assets, num_bodies, 9 for articulations or 9 for rigid objects)
|
||||
inertias = asset.root_physx_view.get_inertias()
|
||||
|
||||
# apply randomization on default values
|
||||
inertias[env_ids[:, None], body_ids, :] = asset.data.default_inertia[env_ids[:, None], body_ids, :].clone()
|
||||
|
||||
# randomize each diagonal element (xx, yy, zz -> indices 0, 4, 8)
|
||||
for idx in [0, 4, 8]:
|
||||
# Extract and randomize the specific diagonal element
|
||||
randomized_inertias = _randomize_prop_by_op(
|
||||
inertias[:, :, idx],
|
||||
inertia_distribution_params,
|
||||
env_ids,
|
||||
body_ids,
|
||||
operation,
|
||||
distribution,
|
||||
)
|
||||
# Assign the randomized values back to the inertia tensor
|
||||
inertias[env_ids[:, None], body_ids, idx] = randomized_inertias
|
||||
|
||||
# set the inertia tensors into the physics simulation
|
||||
asset.root_physx_view.set_inertias(inertias, env_ids)
|
||||
|
||||
|
||||
def randomize_com_positions(
|
||||
env: ManagerBasedEnv,
|
||||
env_ids: torch.Tensor | None,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
com_distribution_params: tuple[float, float],
|
||||
operation: Literal["add", "scale", "abs"],
|
||||
distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform",
|
||||
):
|
||||
"""Randomize the center of mass (COM) positions for the rigid bodies.
|
||||
|
||||
This function allows randomizing the COM positions of the bodies in the physics simulation. The positions can be
|
||||
randomized by adding, scaling, or setting random values sampled from the specified distribution.
|
||||
|
||||
.. tip::
|
||||
This function is intended for initialization or offline adjustments, as it modifies physics properties directly.
|
||||
|
||||
Args:
|
||||
env (ManagerBasedEnv): The simulation environment.
|
||||
env_ids (torch.Tensor | None): Specific environment indices to apply randomization, or None for all environments.
|
||||
asset_cfg (SceneEntityCfg): The configuration for the target asset whose COM will be randomized.
|
||||
com_distribution_params (tuple[float, float]): Parameters of the distribution (e.g., min and max for uniform).
|
||||
operation (Literal["add", "scale", "abs"]): The operation to apply for randomization.
|
||||
distribution (Literal["uniform", "log_uniform", "gaussian"]): The distribution to sample random values from.
|
||||
"""
|
||||
# Extract the asset (Articulation or RigidObject)
|
||||
asset: RigidObject | Articulation = env.scene[asset_cfg.name]
|
||||
|
||||
# Resolve environment indices
|
||||
if env_ids is None:
|
||||
env_ids = torch.arange(env.scene.num_envs, device="cpu")
|
||||
else:
|
||||
env_ids = env_ids.cpu()
|
||||
|
||||
# Resolve body indices
|
||||
if asset_cfg.body_ids == slice(None):
|
||||
body_ids = torch.arange(asset.num_bodies, dtype=torch.int, device="cpu")
|
||||
else:
|
||||
body_ids = torch.tensor(asset_cfg.body_ids, dtype=torch.int, device="cpu")
|
||||
|
||||
# Get the current COM offsets (num_assets, num_bodies, 3)
|
||||
com_offsets = asset.root_physx_view.get_coms()
|
||||
|
||||
for dim_idx in range(3): # Randomize x, y, z independently
|
||||
randomized_offset = _randomize_prop_by_op(
|
||||
com_offsets[:, :, dim_idx],
|
||||
com_distribution_params,
|
||||
env_ids,
|
||||
body_ids,
|
||||
operation,
|
||||
distribution,
|
||||
)
|
||||
com_offsets[env_ids[:, None], body_ids, dim_idx] = randomized_offset[env_ids[:, None], body_ids]
|
||||
|
||||
# Set the randomized COM offsets into the simulation
|
||||
asset.root_physx_view.set_coms(com_offsets, env_ids)
|
||||
|
||||
|
||||
"""
|
||||
Internal helper functions.
|
||||
"""
|
||||
|
||||
|
||||
def _randomize_prop_by_op(
|
||||
data: torch.Tensor,
|
||||
distribution_parameters: tuple[float | torch.Tensor, float | torch.Tensor],
|
||||
dim_0_ids: torch.Tensor | None,
|
||||
dim_1_ids: torch.Tensor | slice,
|
||||
operation: Literal["add", "scale", "abs"],
|
||||
distribution: Literal["uniform", "log_uniform", "gaussian"],
|
||||
) -> torch.Tensor:
|
||||
"""Perform data randomization based on the given operation and distribution.
|
||||
|
||||
Args:
|
||||
data: The data tensor to be randomized. Shape is (dim_0, dim_1).
|
||||
distribution_parameters: The parameters for the distribution to sample values from.
|
||||
dim_0_ids: The indices of the first dimension to randomize.
|
||||
dim_1_ids: The indices of the second dimension to randomize.
|
||||
operation: The operation to perform on the data. Options: 'add', 'scale', 'abs'.
|
||||
distribution: The distribution to sample the random values from. Options: 'uniform', 'log_uniform'.
|
||||
|
||||
Returns:
|
||||
The data tensor after randomization. Shape is (dim_0, dim_1).
|
||||
|
||||
Raises:
|
||||
NotImplementedError: If the operation or distribution is not supported.
|
||||
"""
|
||||
# resolve shape
|
||||
# -- dim 0
|
||||
if dim_0_ids is None:
|
||||
n_dim_0 = data.shape[0]
|
||||
dim_0_ids = slice(None) # type: ignore
|
||||
else:
|
||||
n_dim_0 = len(dim_0_ids)
|
||||
if not isinstance(dim_1_ids, slice):
|
||||
dim_0_ids = dim_0_ids[:, None]
|
||||
# -- dim 1
|
||||
if isinstance(dim_1_ids, slice):
|
||||
n_dim_1 = data.shape[1]
|
||||
else:
|
||||
n_dim_1 = len(dim_1_ids)
|
||||
|
||||
# resolve the distribution
|
||||
if distribution == "uniform":
|
||||
dist_fn = math_utils.sample_uniform
|
||||
elif distribution == "log_uniform":
|
||||
dist_fn = math_utils.sample_log_uniform
|
||||
elif distribution == "gaussian":
|
||||
dist_fn = math_utils.sample_gaussian
|
||||
else:
|
||||
raise NotImplementedError(
|
||||
f"Unknown distribution: '{distribution}' for joint properties randomization."
|
||||
" Please use 'uniform', 'log_uniform', 'gaussian'."
|
||||
)
|
||||
# perform the operation
|
||||
if operation == "add":
|
||||
data[dim_0_ids, dim_1_ids] += dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device)
|
||||
elif operation == "scale":
|
||||
data[dim_0_ids, dim_1_ids] *= dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device)
|
||||
elif operation == "abs":
|
||||
data[dim_0_ids, dim_1_ids] = dist_fn(*distribution_parameters, (n_dim_0, n_dim_1), device=data.device)
|
||||
else:
|
||||
raise NotImplementedError(
|
||||
f"Unknown operation: '{operation}' for property randomization. Please use 'add', 'scale', or 'abs'."
|
||||
)
|
||||
return data
|
||||
|
||||
|
||||
def bad_orientation_2(
|
||||
env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") # type: ignore
|
||||
) -> torch.Tensor:
|
||||
"""Terminate when the asset's orientation is too far from the desired orientation limits.
|
||||
|
||||
This is computed by checking the angle between the projected gravity vector and the z-axis.
|
||||
"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
return (asset.data.projected_gravity_b[:, 2] > 0) | (asset.data.projected_gravity_b[:, :2].abs() > 0.7).any(-1)
|
||||
+105
@@ -0,0 +1,105 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import torch
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
from isaaclab.assets import Articulation
|
||||
from isaaclab.managers import SceneEntityCfg
|
||||
from isaaclab.sensors import ContactSensor
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv
|
||||
|
||||
|
||||
# def joint_pos_rel_without_wheel(
|
||||
# env: ManagerBasedEnv,
|
||||
# asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
# wheel_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
# ) -> torch.Tensor:
|
||||
# """The joint positions of the asset w.r.t. the default joint positions.(Without the wheel joints)"""
|
||||
# # extract the used quantities (to enable type-hinting)
|
||||
# asset: Articulation = env.scene[asset_cfg.name]
|
||||
# joint_pos_rel = asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids]
|
||||
# joint_pos_rel[:, wheel_asset_cfg.joint_ids] = 0
|
||||
# return joint_pos_rel
|
||||
|
||||
def joint_pos_rel_without_wheel(
|
||||
env: ManagerBasedEnv,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
wheel_asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
|
||||
# 1. 先在“完整 joint 空间”算相对位置
|
||||
joint_pos_rel_full = asset.data.joint_pos - asset.data.default_joint_pos
|
||||
|
||||
# 2. 把 wheel joint 清零（这里的 index 是全局的，合法）
|
||||
joint_pos_rel_full[:, wheel_asset_cfg.joint_ids] = 0.0
|
||||
|
||||
# 3. 最后再裁剪成 policy 需要的 joint
|
||||
joint_pos_rel = joint_pos_rel_full[:, asset_cfg.joint_ids]
|
||||
|
||||
return joint_pos_rel
|
||||
|
||||
|
||||
|
||||
def phase(env: ManagerBasedRLEnv, cycle_time: float) -> torch.Tensor:
|
||||
if not hasattr(env, "episode_length_buf") or env.episode_length_buf is None:
|
||||
env.episode_length_buf = torch.zeros(env.num_envs, device=env.device, dtype=torch.long)
|
||||
phase = env.episode_length_buf[:, None] * env.step_dt / cycle_time
|
||||
phase_tensor = torch.cat([torch.sin(2 * torch.pi * phase), torch.cos(2 * torch.pi * phase)], dim=-1)
|
||||
return phase_tensor
|
||||
|
||||
|
||||
def contact_state(
|
||||
env: ManagerBasedRLEnv,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
threshold: float = 1.0,
|
||||
) -> torch.Tensor:
|
||||
"""Binary contact state for selected bodies."""
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
force_norm = torch.linalg.norm(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, :], dim=-1)
|
||||
return (force_norm > threshold).float()
|
||||
|
||||
|
||||
def contact_force_norm(
|
||||
env: ManagerBasedRLEnv,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
scale: float = 100.0,
|
||||
clip: float = 2.0,
|
||||
) -> torch.Tensor:
|
||||
"""Normalized contact force magnitude for selected bodies."""
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
force_norm = torch.linalg.norm(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, :], dim=-1)
|
||||
return torch.clamp(force_norm / scale, min=0.0, max=clip)
|
||||
|
||||
|
||||
def wheel_slip_proxy(
|
||||
env: ManagerBasedRLEnv,
|
||||
wheel_radius: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""A proprioceptive slip proxy from wheel peripheral speed and base forward speed."""
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
wheel_speed = torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids] * wheel_radius)
|
||||
base_forward_speed = torch.abs(asset.data.root_lin_vel_b[:, 0:1])
|
||||
return torch.clamp(torch.abs(wheel_speed - base_forward_speed), min=0.0, max=3.0)
|
||||
|
||||
|
||||
def applied_torque(
|
||||
env: ManagerBasedRLEnv,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
scale: float = 80.0,
|
||||
clip: float = 2.0,
|
||||
) -> torch.Tensor:
|
||||
"""Normalized applied motor torques."""
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
torque = asset.data.applied_torque[:, asset_cfg.joint_ids]
|
||||
return torch.clamp(torque / scale, min=-clip, max=clip)
|
||||
+985
@@ -0,0 +1,985 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
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 ManagerTermBase
|
||||
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||
from isaaclab.managers import SceneEntityCfg
|
||||
from isaaclab.sensors import ContactSensor, RayCaster
|
||||
from isaaclab.utils.math import quat_apply_inverse, yaw_quat
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from isaaclab.envs import ManagerBasedRLEnv
|
||||
|
||||
|
||||
def track_lin_vel_xy_exp(
|
||||
env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
"""Reward tracking of linear velocity commands (xy axes) using exponential kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
# compute the error
|
||||
lin_vel_error = torch.sum(
|
||||
torch.square(env.command_manager.get_command(command_name)[:, :2] - asset.data.root_lin_vel_b[:, :2]),
|
||||
dim=1,
|
||||
)
|
||||
reward = torch.exp(-lin_vel_error / std**2)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def track_ang_vel_z_exp(
|
||||
env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
"""Reward tracking of angular velocity commands (yaw) using exponential kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
# compute the error
|
||||
ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_b[:, 2])
|
||||
reward = torch.exp(-ang_vel_error / std**2)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def track_diff_wheel_yaw_exp(
|
||||
env: ManagerBasedRLEnv,
|
||||
std: float,
|
||||
command_name: str,
|
||||
wheel_radius: float,
|
||||
track_width: float,
|
||||
left_wheel_indices: tuple[int, ...],
|
||||
right_wheel_indices: tuple[int, ...],
|
||||
command_threshold: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""Reward differential wheel speed matching the commanded yaw rate."""
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
joint_vel = asset.data.joint_vel[:, asset_cfg.joint_ids]
|
||||
|
||||
left_wheel_vel = torch.mean(joint_vel[:, left_wheel_indices], dim=1)
|
||||
right_wheel_vel = torch.mean(joint_vel[:, right_wheel_indices], dim=1)
|
||||
estimated_yaw_rate = wheel_radius * (right_wheel_vel - left_wheel_vel) / track_width
|
||||
yaw_command = env.command_manager.get_command(command_name)[:, 2]
|
||||
|
||||
yaw_error = torch.square(yaw_command - estimated_yaw_rate)
|
||||
reward = torch.exp(-yaw_error / std**2)
|
||||
return torch.where(torch.abs(yaw_command) > command_threshold, reward, torch.zeros_like(reward))
|
||||
|
||||
|
||||
def lin_vel_xy_yaw_command_l2(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
yaw_command_threshold: float,
|
||||
lin_command_threshold: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""Penalize linear drift when the command asks mostly for in-place yaw."""
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
command = env.command_manager.get_command(command_name)
|
||||
is_turn_in_place = (
|
||||
torch.abs(command[:, 2]) > yaw_command_threshold
|
||||
) & (torch.linalg.norm(command[:, :2], dim=1) < lin_command_threshold)
|
||||
lin_vel_error = torch.sum(torch.square(asset.data.root_lin_vel_b[:, :2]), dim=1)
|
||||
return torch.where(is_turn_in_place, lin_vel_error, torch.zeros_like(lin_vel_error))
|
||||
|
||||
|
||||
def command_progress(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
min_command: float,
|
||||
max_reward_speed: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""Reward positive progress along the commanded planar direction."""
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
command_xy = env.command_manager.get_command(command_name)[:, :2]
|
||||
command_norm = torch.linalg.norm(command_xy, dim=1)
|
||||
command_dir = command_xy / torch.clamp(command_norm.unsqueeze(1), min=1e-6)
|
||||
progress_speed = torch.sum(asset.data.root_lin_vel_b[:, :2] * command_dir, dim=1)
|
||||
reward = torch.clamp(progress_speed, min=0.0, max=max_reward_speed) / max_reward_speed
|
||||
return torch.where(command_norm > min_command, reward, torch.zeros_like(reward))
|
||||
|
||||
|
||||
def support_contact_excess(
|
||||
env: ManagerBasedRLEnv,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
force_threshold: float = 120.0,
|
||||
max_contact_time: float = 0.6,
|
||||
) -> torch.Tensor:
|
||||
"""Penalize dragging or high-force support contacts."""
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
force_norm = torch.linalg.norm(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, :], dim=-1)
|
||||
contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids]
|
||||
|
||||
force_excess = torch.clamp(force_norm - force_threshold, min=0.0) / force_threshold
|
||||
time_excess = torch.clamp(contact_time - max_contact_time, min=0.0)
|
||||
return torch.sum(force_excess + time_excess, dim=1)
|
||||
|
||||
|
||||
class ObstacleSupportEscapeReward(ManagerTermBase):
|
||||
"""Reward a blocked -> brief support contact -> recovered progress sequence.
|
||||
|
||||
The state machine prevents ordinary forward motion with incidental support contact from receiving the full reward.
|
||||
Obstacle evidence is inferred from low commanded progress together with either horizontal wheel force or wheel slip,
|
||||
so the actor can remain blind while the training reward uses contact information already available on the robot.
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: RewTerm, env: ManagerBasedRLEnv):
|
||||
super().__init__(cfg, env)
|
||||
self._blocked_memory = torch.zeros(self.num_envs, device=self.device)
|
||||
self._support_window = torch.zeros(self.num_envs, device=self.device)
|
||||
self._recovery_reward_window = torch.zeros(self.num_envs, device=self.device)
|
||||
self._cooldown = torch.zeros(self.num_envs, device=self.device)
|
||||
self._previous_support_active = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
|
||||
|
||||
def reset(self, env_ids=None) -> None:
|
||||
if env_ids is None:
|
||||
env_ids = slice(None)
|
||||
self._blocked_memory[env_ids] = 0.0
|
||||
self._support_window[env_ids] = 0.0
|
||||
self._recovery_reward_window[env_ids] = 0.0
|
||||
self._cooldown[env_ids] = 0.0
|
||||
self._previous_support_active[env_ids] = False
|
||||
|
||||
def __call__(
|
||||
self,
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
wheel_sensor_cfg: SceneEntityCfg,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
wheel_asset_cfg: SceneEntityCfg,
|
||||
wheel_radius: float,
|
||||
support_force_threshold: float,
|
||||
wheel_obstacle_force_threshold: float,
|
||||
max_contact_time: float,
|
||||
min_command: float,
|
||||
stall_speed_ratio: float,
|
||||
min_stall_speed: float,
|
||||
slip_threshold: float,
|
||||
obstacle_memory_time: float,
|
||||
support_window_time: float,
|
||||
recovery_speed_ratio: float,
|
||||
min_recovery_speed: float,
|
||||
recovery_reward_time: float,
|
||||
cooldown_time: float,
|
||||
recovery_bonus: float,
|
||||
max_reward_speed: float,
|
||||
) -> torch.Tensor:
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
dt = float(env.step_dt)
|
||||
|
||||
command_x = env.command_manager.get_command(command_name)[:, 0]
|
||||
command_speed = torch.abs(command_x)
|
||||
direction = torch.where(command_x >= 0.0, torch.ones_like(command_x), -torch.ones_like(command_x))
|
||||
progress_speed = direction * asset.data.root_lin_vel_b[:, 0]
|
||||
commanded_motion = command_speed > min_command
|
||||
|
||||
wheel_forces = contact_sensor.data.net_forces_w[:, wheel_sensor_cfg.body_ids, :]
|
||||
max_horizontal_wheel_force = torch.linalg.norm(wheel_forces[..., :2], dim=-1).amax(dim=1)
|
||||
wheel_joint_vel = asset.data.joint_vel[:, wheel_asset_cfg.joint_ids]
|
||||
mean_wheel_surface_speed = torch.mean(torch.abs(wheel_joint_vel), dim=1) * wheel_radius
|
||||
wheel_slip = torch.clamp(mean_wheel_surface_speed - torch.clamp(progress_speed, min=0.0), min=0.0)
|
||||
|
||||
stall_limit = torch.maximum(stall_speed_ratio * command_speed, torch.full_like(command_speed, min_stall_speed))
|
||||
obstacle_evidence = commanded_motion & (progress_speed < stall_limit) & (
|
||||
(max_horizontal_wheel_force > wheel_obstacle_force_threshold) | (wheel_slip > slip_threshold)
|
||||
)
|
||||
|
||||
self._blocked_memory = torch.clamp(self._blocked_memory - dt, min=0.0)
|
||||
self._support_window = torch.clamp(self._support_window - dt, min=0.0)
|
||||
self._recovery_reward_window = torch.clamp(self._recovery_reward_window - dt, min=0.0)
|
||||
self._cooldown = torch.clamp(self._cooldown - dt, min=0.0)
|
||||
self._blocked_memory = torch.where(
|
||||
obstacle_evidence, torch.full_like(self._blocked_memory, obstacle_memory_time), self._blocked_memory
|
||||
)
|
||||
|
||||
support_force = torch.linalg.norm(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, :], dim=-1)
|
||||
support_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids]
|
||||
support_active = torch.any(support_force > support_force_threshold, dim=1)
|
||||
brief_support = torch.any(
|
||||
(support_force > support_force_threshold) & (support_time <= max_contact_time), dim=1
|
||||
)
|
||||
support_onset = brief_support & ~self._previous_support_active
|
||||
valid_support_onset = support_onset & (self._blocked_memory > 0.0) & (self._cooldown <= 0.0)
|
||||
|
||||
was_waiting_for_recovery = self._support_window > 0.0
|
||||
self._support_window = torch.where(
|
||||
valid_support_onset, torch.full_like(self._support_window, support_window_time), self._support_window
|
||||
)
|
||||
|
||||
recovery_threshold = torch.maximum(
|
||||
recovery_speed_ratio * command_speed, torch.full_like(command_speed, min_recovery_speed)
|
||||
)
|
||||
excessive_support = torch.any(
|
||||
(support_force > support_force_threshold) & (support_time > max_contact_time), dim=1
|
||||
)
|
||||
recovered = (
|
||||
was_waiting_for_recovery
|
||||
& commanded_motion
|
||||
& (progress_speed >= recovery_threshold)
|
||||
& ~excessive_support
|
||||
)
|
||||
self._recovery_reward_window = torch.where(
|
||||
recovered,
|
||||
torch.full_like(self._recovery_reward_window, recovery_reward_time),
|
||||
self._recovery_reward_window,
|
||||
)
|
||||
self._support_window = torch.where(recovered, torch.zeros_like(self._support_window), self._support_window)
|
||||
self._cooldown = torch.where(recovered, torch.full_like(self._cooldown, cooldown_time), self._cooldown)
|
||||
|
||||
recovery_progress = torch.clamp(progress_speed, min=0.0, max=max_reward_speed) / max_reward_speed
|
||||
# Delay the discrete bonus until recovery is observed. This makes support contact alone worthless and
|
||||
# prevents the policy from farming reward by repeatedly tapping the body against an obstacle.
|
||||
reward = recovery_bonus * recovered.float() / dt
|
||||
reward += torch.where(
|
||||
self._recovery_reward_window > 0.0, recovery_progress, torch.zeros_like(recovery_progress)
|
||||
)
|
||||
|
||||
self._previous_support_active = support_active
|
||||
return reward
|
||||
|
||||
|
||||
def track_lin_vel_xy_yaw_frame_exp(
|
||||
env, std: float, command_name: str, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
"""Reward tracking of linear velocity commands (xy axes) in the gravity aligned robot frame using exponential kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset = env.scene[asset_cfg.name]
|
||||
vel_yaw = quat_apply_inverse(yaw_quat(asset.data.root_quat_w), asset.data.root_lin_vel_w[:, :3])
|
||||
lin_vel_error = torch.sum(
|
||||
torch.square(env.command_manager.get_command(command_name)[:, :2] - vel_yaw[:, :2]), dim=1
|
||||
)
|
||||
reward = torch.exp(-lin_vel_error / std**2)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def track_ang_vel_z_world_exp(
|
||||
env, command_name: str, std: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
"""Reward tracking of angular velocity commands (yaw) in world frame using exponential kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset = env.scene[asset_cfg.name]
|
||||
ang_vel_error = torch.square(env.command_manager.get_command(command_name)[:, 2] - asset.data.root_ang_vel_w[:, 2])
|
||||
reward = torch.exp(-ang_vel_error / std**2)
|
||||
reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def joint_power(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
"""Reward joint_power"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
# compute the reward
|
||||
reward = torch.sum(
|
||||
torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids] * asset.data.applied_torque[:, asset_cfg.joint_ids]),
|
||||
dim=1,
|
||||
)
|
||||
return reward
|
||||
|
||||
|
||||
def stand_still_without_cmd(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
command_threshold: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""Penalize joint positions that deviate from the default one when no command."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
# compute out of limits constraints
|
||||
diff_angle = asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids]
|
||||
reward = torch.sum(torch.abs(diff_angle), dim=1)
|
||||
reward *= torch.linalg.norm(env.command_manager.get_command(command_name), dim=1) < command_threshold
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
def joint_pos_penalty(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
stand_still_scale: float,
|
||||
velocity_threshold: float,
|
||||
command_threshold: float,
|
||||
) -> torch.Tensor:
|
||||
"""Penalize joint position error from default on the articulation."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
cmd = torch.linalg.norm(env.command_manager.get_command(command_name), dim=1)
|
||||
body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1)
|
||||
running_reward = torch.linalg.norm(
|
||||
(asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids]), dim=1
|
||||
)
|
||||
reward = torch.where(
|
||||
torch.logical_or(cmd > command_threshold, body_vel > velocity_threshold),
|
||||
running_reward,
|
||||
stand_still_scale * running_reward,
|
||||
)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def wheel_vel_penalty(
|
||||
env: ManagerBasedRLEnv,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
command_name: str,
|
||||
velocity_threshold: float,
|
||||
command_threshold: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
cmd = torch.linalg.norm(env.command_manager.get_command(command_name), dim=1)
|
||||
body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1)
|
||||
joint_vel = torch.abs(asset.data.joint_vel[:, asset_cfg.joint_ids])
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
in_air = contact_sensor.compute_first_air(env.step_dt)[:, sensor_cfg.body_ids]
|
||||
running_reward = torch.sum(in_air * joint_vel, dim=1)
|
||||
standing_reward = torch.sum(joint_vel, dim=1)
|
||||
reward = torch.where(
|
||||
torch.logical_or(cmd > command_threshold, body_vel > velocity_threshold),
|
||||
running_reward,
|
||||
standing_reward,
|
||||
)
|
||||
return reward
|
||||
|
||||
|
||||
class GaitReward(ManagerTermBase):
|
||||
"""Gait enforcing reward term for quadrupeds.
|
||||
|
||||
This reward penalizes contact timing differences between selected foot pairs defined in :attr:`synced_feet_pair_names`
|
||||
to bias the policy towards a desired gait, i.e trotting, bounding, or pacing. Note that this reward is only for
|
||||
quadrupedal gaits with two pairs of synchronized feet.
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: RewTerm, env: ManagerBasedRLEnv):
|
||||
"""Initialize the term.
|
||||
|
||||
Args:
|
||||
cfg: The configuration of the reward.
|
||||
env: The RL environment instance.
|
||||
"""
|
||||
super().__init__(cfg, env)
|
||||
self.std: float = cfg.params["std"]
|
||||
self.command_name: str = cfg.params["command_name"]
|
||||
self.max_err: float = cfg.params["max_err"]
|
||||
self.velocity_threshold: float = cfg.params["velocity_threshold"]
|
||||
self.command_threshold: float = cfg.params["command_threshold"]
|
||||
self.contact_sensor: ContactSensor = env.scene.sensors[cfg.params["sensor_cfg"].name]
|
||||
self.asset: Articulation = env.scene[cfg.params["asset_cfg"].name]
|
||||
# match foot body names with corresponding foot body ids
|
||||
synced_feet_pair_names = cfg.params["synced_feet_pair_names"]
|
||||
if (
|
||||
len(synced_feet_pair_names) != 2
|
||||
or len(synced_feet_pair_names[0]) != 2
|
||||
or len(synced_feet_pair_names[1]) != 2
|
||||
):
|
||||
raise ValueError("This reward only supports gaits with two pairs of synchronized feet, like trotting.")
|
||||
synced_feet_pair_0 = self.contact_sensor.find_bodies(synced_feet_pair_names[0])[0]
|
||||
synced_feet_pair_1 = self.contact_sensor.find_bodies(synced_feet_pair_names[1])[0]
|
||||
self.synced_feet_pairs = [synced_feet_pair_0, synced_feet_pair_1]
|
||||
|
||||
def __call__(
|
||||
self,
|
||||
env: ManagerBasedRLEnv,
|
||||
std: float,
|
||||
command_name: str,
|
||||
max_err: float,
|
||||
velocity_threshold: float,
|
||||
command_threshold: float,
|
||||
synced_feet_pair_names,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
sensor_cfg: SceneEntityCfg,
|
||||
) -> torch.Tensor:
|
||||
"""Compute the reward.
|
||||
|
||||
This reward is defined as a multiplication between six terms where two of them enforce pair feet
|
||||
being in sync and the other four rewards if all the other remaining pairs are out of sync
|
||||
|
||||
Args:
|
||||
env: The RL environment instance.
|
||||
Returns:
|
||||
The reward value.
|
||||
"""
|
||||
# for synchronous feet, the contact (air) times of two feet should match
|
||||
sync_reward_0 = self._sync_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[0][1])
|
||||
sync_reward_1 = self._sync_reward_func(self.synced_feet_pairs[1][0], self.synced_feet_pairs[1][1])
|
||||
sync_reward = sync_reward_0 * sync_reward_1
|
||||
# for asynchronous feet, the contact time of one foot should match the air time of the other one
|
||||
async_reward_0 = self._async_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[1][0])
|
||||
async_reward_1 = self._async_reward_func(self.synced_feet_pairs[0][1], self.synced_feet_pairs[1][1])
|
||||
async_reward_2 = self._async_reward_func(self.synced_feet_pairs[0][0], self.synced_feet_pairs[1][1])
|
||||
async_reward_3 = self._async_reward_func(self.synced_feet_pairs[1][0], self.synced_feet_pairs[0][1])
|
||||
async_reward = async_reward_0 * async_reward_1 * async_reward_2 * async_reward_3
|
||||
# only enforce gait if cmd > 0
|
||||
cmd = torch.linalg.norm(env.command_manager.get_command(self.command_name), dim=1)
|
||||
body_vel = torch.linalg.norm(self.asset.data.root_com_lin_vel_b[:, :2], dim=1)
|
||||
reward = torch.where(
|
||||
torch.logical_or(cmd > self.command_threshold, body_vel > self.velocity_threshold),
|
||||
sync_reward * async_reward,
|
||||
0.0,
|
||||
)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
"""
|
||||
Helper functions.
|
||||
"""
|
||||
|
||||
def _sync_reward_func(self, foot_0: int, foot_1: int) -> torch.Tensor:
|
||||
"""Reward synchronization of two feet."""
|
||||
air_time = self.contact_sensor.data.current_air_time
|
||||
contact_time = self.contact_sensor.data.current_contact_time
|
||||
# penalize the difference between the most recent air time and contact time of synced feet pairs.
|
||||
se_air = torch.clip(torch.square(air_time[:, foot_0] - air_time[:, foot_1]), max=self.max_err**2)
|
||||
se_contact = torch.clip(torch.square(contact_time[:, foot_0] - contact_time[:, foot_1]), max=self.max_err**2)
|
||||
return torch.exp(-(se_air + se_contact) / self.std)
|
||||
|
||||
def _async_reward_func(self, foot_0: int, foot_1: int) -> torch.Tensor:
|
||||
"""Reward anti-synchronization of two feet."""
|
||||
air_time = self.contact_sensor.data.current_air_time
|
||||
contact_time = self.contact_sensor.data.current_contact_time
|
||||
# penalize the difference between opposing contact modes air time of feet 1 to contact time of feet 2
|
||||
# and contact time of feet 1 to air time of feet 2) of feet pairs that are not in sync with each other.
|
||||
se_act_0 = torch.clip(torch.square(air_time[:, foot_0] - contact_time[:, foot_1]), max=self.max_err**2)
|
||||
se_act_1 = torch.clip(torch.square(contact_time[:, foot_0] - air_time[:, foot_1]), max=self.max_err**2)
|
||||
return torch.exp(-(se_act_0 + se_act_1) / self.std)
|
||||
|
||||
|
||||
def joint_mirror(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, mirror_joints: list[list[str]]) -> torch.Tensor:
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
if not hasattr(env, "joint_mirror_joints_cache") or env.joint_mirror_joints_cache is None:
|
||||
# Cache joint positions for all pairs
|
||||
env.joint_mirror_joints_cache = [
|
||||
[asset.find_joints(joint_name) for joint_name in joint_pair] for joint_pair in mirror_joints
|
||||
]
|
||||
reward = torch.zeros(env.num_envs, device=env.device)
|
||||
# Iterate over all joint pairs
|
||||
for joint_pair in env.joint_mirror_joints_cache:
|
||||
# Calculate the difference for each pair and add to the total reward
|
||||
diff = torch.sum(
|
||||
torch.square(asset.data.joint_pos[:, joint_pair[0][0]] - asset.data.joint_pos[:, joint_pair[1][0]]),
|
||||
dim=-1,
|
||||
)
|
||||
reward += diff
|
||||
reward *= 1 / len(mirror_joints) if len(mirror_joints) > 0 else 0
|
||||
reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def action_mirror(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, mirror_joints: list[list[str]]) -> torch.Tensor:
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
if not hasattr(env, "action_mirror_joints_cache") or env.action_mirror_joints_cache is None:
|
||||
# Cache joint positions for all pairs
|
||||
env.action_mirror_joints_cache = [
|
||||
[asset.find_joints(joint_name) for joint_name in joint_pair] for joint_pair in mirror_joints
|
||||
]
|
||||
reward = torch.zeros(env.num_envs, device=env.device)
|
||||
# Iterate over all joint pairs
|
||||
for joint_pair in env.action_mirror_joints_cache:
|
||||
# Calculate the difference for each pair and add to the total reward
|
||||
diff = torch.sum(
|
||||
torch.square(
|
||||
torch.abs(env.action_manager.action[:, joint_pair[0][0]])
|
||||
- torch.abs(env.action_manager.action[:, joint_pair[1][0]])
|
||||
),
|
||||
dim=-1,
|
||||
)
|
||||
reward += diff
|
||||
reward *= 1 / len(mirror_joints) if len(mirror_joints) > 0 else 0
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def action_sync(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, joint_groups: list[list[str]]) -> torch.Tensor:
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: Articulation = env.scene[asset_cfg.name]
|
||||
|
||||
# Cache joint indices if not already done
|
||||
if not hasattr(env, "action_sync_joint_cache") or env.action_sync_joint_cache is None:
|
||||
env.action_sync_joint_cache = [
|
||||
[asset.find_joints(joint_name) for joint_name in joint_group] for joint_group in joint_groups
|
||||
]
|
||||
|
||||
reward = torch.zeros(env.num_envs, device=env.device)
|
||||
# Iterate over each joint group
|
||||
for joint_group in env.action_sync_joint_cache:
|
||||
if len(joint_group) < 2:
|
||||
continue # need at least 2 joints to compare
|
||||
|
||||
# Get absolute actions for all joints in this group
|
||||
actions = torch.stack(
|
||||
[torch.abs(env.action_manager.action[:, joint[0]]) for joint in joint_group], dim=1
|
||||
) # shape: (num_envs, num_joints_in_group)
|
||||
|
||||
# Calculate mean action for each environment
|
||||
mean_actions = torch.mean(actions, dim=1, keepdim=True)
|
||||
|
||||
# Calculate variance from mean for each joint
|
||||
variance = torch.mean(torch.square(actions - mean_actions), dim=1)
|
||||
|
||||
# Add to reward (we want to minimize this variance)
|
||||
reward += variance.squeeze()
|
||||
reward *= 1 / len(joint_groups) if len(joint_groups) > 0 else 0
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
# def feet_air_time(
|
||||
# env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float
|
||||
# ) -> torch.Tensor:
|
||||
# """Reward long steps taken by the feet using L2-kernel.
|
||||
|
||||
# This function rewards the agent for taking steps that are longer than a threshold. This helps ensure
|
||||
# that the robot lifts its feet off the ground and takes steps. The reward is computed as the sum of
|
||||
# the time for which the feet are in the air.
|
||||
|
||||
# If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero.
|
||||
# """
|
||||
# # extract the used quantities (to enable type-hinting)
|
||||
# contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# # compute the reward
|
||||
# first_contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids]
|
||||
# last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids]
|
||||
# reward = torch.sum((last_air_time - threshold) * first_contact, dim=1)
|
||||
# # no reward for zero command
|
||||
# reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1
|
||||
# # print(torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1), "command norm")
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
# return reward
|
||||
|
||||
# def feet_air_time(
|
||||
# env: ManagerBasedRLEnv,
|
||||
# asset_cfg: SceneEntityCfg,
|
||||
# sensor_cfg: SceneEntityCfg,
|
||||
# mode_time: float,
|
||||
# velocity_threshold: float,
|
||||
# ) -> torch.Tensor:
|
||||
# """Reward longer feet air and contact time."""
|
||||
# # extract the used quantities (to enable type-hinting)
|
||||
# contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# asset: Articulation = env.scene[asset_cfg.name]
|
||||
# if contact_sensor.cfg.track_air_time is False:
|
||||
# raise RuntimeError("Activate ContactSensor's track_air_time!")
|
||||
# # compute the reward
|
||||
# current_air_time = contact_sensor.data.current_air_time[:, sensor_cfg.body_ids]
|
||||
# current_contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids]
|
||||
|
||||
# t_max = torch.max(current_air_time, current_contact_time)
|
||||
# t_min = torch.clip(t_max, max=mode_time)
|
||||
# stance_cmd_reward = torch.clip(current_contact_time - current_air_time, -mode_time, mode_time)
|
||||
# cmd = torch.norm(env.command_manager.get_command("base_velocity"), dim=1).unsqueeze(dim=1).expand(-1, 4)
|
||||
# body_vel = torch.linalg.norm(asset.data.root_lin_vel_b[:, :2], dim=1).unsqueeze(dim=1).expand(-1, 4)
|
||||
# reward = torch.where(
|
||||
# torch.logical_or(cmd > 0.0, body_vel > velocity_threshold),
|
||||
# torch.where(t_max < mode_time, t_min, 0),
|
||||
# stance_cmd_reward,
|
||||
# )
|
||||
# return torch.sum(reward, dim=1)
|
||||
|
||||
|
||||
def feet_air_time_positive_biped(env, command_name: str, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||
"""Reward long steps taken by the feet for bipeds.
|
||||
|
||||
This function rewards the agent for taking steps up to a specified threshold and also keep one foot at
|
||||
a time in the air.
|
||||
|
||||
If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero.
|
||||
"""
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# compute the reward
|
||||
air_time = contact_sensor.data.current_air_time[:, sensor_cfg.body_ids]
|
||||
contact_time = contact_sensor.data.current_contact_time[:, sensor_cfg.body_ids]
|
||||
in_contact = contact_time > 0.0
|
||||
in_mode_time = torch.where(in_contact, contact_time, air_time)
|
||||
single_stance = torch.sum(in_contact.int(), dim=1) == 1
|
||||
reward = torch.min(torch.where(single_stance.unsqueeze(-1), in_mode_time, 0.0), dim=1)[0]
|
||||
reward = torch.clamp(reward, max=threshold)
|
||||
# no reward for zero command
|
||||
reward *= torch.norm(env.command_manager.get_command(command_name)[:, :2], dim=1) > 0.1
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_air_time_variance_penalty(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||
"""Penalize variance in the amount of time each foot spends in the air/on the ground relative to each other"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# compute the reward
|
||||
last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids]
|
||||
last_contact_time = contact_sensor.data.last_contact_time[:, sensor_cfg.body_ids]
|
||||
reward = torch.var(torch.clip(last_air_time, max=0.5), dim=1) + torch.var(
|
||||
torch.clip(last_contact_time, max=0.5), dim=1)
|
||||
# print(last_air_time, "last air time")
|
||||
# print(last_contact_time, "last contact time")
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
|
||||
|
||||
def feet_contact(
|
||||
env: ManagerBasedRLEnv, command_name: str, expect_contact_num: int, sensor_cfg: SceneEntityCfg
|
||||
) -> torch.Tensor:
|
||||
"""Reward feet contact"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# compute the reward
|
||||
contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids]
|
||||
contact_num = torch.sum(contact, dim=1)
|
||||
reward = (contact_num != expect_contact_num).float()
|
||||
# no reward for zero command
|
||||
reward *= torch.linalg.norm(env.command_manager.get_command(command_name), dim=1) > 0.5
|
||||
reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_contact_without_cmd(env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||
"""Reward feet contact"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# compute the reward
|
||||
contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids]
|
||||
# print(contact, "contact")
|
||||
reward = torch.sum(contact, dim=-1).float()
|
||||
# print(reward, "reward after sum")
|
||||
reward *= torch.linalg.norm(env.command_manager.get_command(command_name), dim=1) < 0.5
|
||||
# print(env.command_manager.get_command(command_name), "env.command_manager.get_command(command_name)")
|
||||
# print(reward, "reward after multiply")
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_stumble(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
forces_z = torch.abs(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, 2])
|
||||
forces_xy = torch.linalg.norm(contact_sensor.data.net_forces_w[:, sensor_cfg.body_ids, :2], dim=2)
|
||||
# Penalize feet hitting vertical surfaces
|
||||
reward = torch.any(forces_xy > 4 * forces_z, dim=1).float()
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_distance_y_exp(
|
||||
env: ManagerBasedRLEnv, stance_width: float, std: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
cur_footsteps_translated = asset.data.body_link_pos_w[:, asset_cfg.body_ids, :] - asset.data.root_link_pos_w[
|
||||
:, :
|
||||
].unsqueeze(1)
|
||||
n_feet = len(asset_cfg.body_ids)
|
||||
footsteps_in_body_frame = torch.zeros(env.num_envs, n_feet, 3, device=env.device)
|
||||
for i in range(n_feet):
|
||||
footsteps_in_body_frame[:, i, :] = math_utils.quat_apply(
|
||||
math_utils.quat_conjugate(asset.data.root_link_quat_w), cur_footsteps_translated[:, i, :]
|
||||
)
|
||||
side_sign = torch.tensor(
|
||||
[1.0 if i % 2 == 0 else -1.0 for i in range(n_feet)],
|
||||
device=env.device,
|
||||
)
|
||||
stance_width_tensor = stance_width * torch.ones([env.num_envs, 1], device=env.device)
|
||||
desired_ys = stance_width_tensor / 2 * side_sign.unsqueeze(0)
|
||||
stance_diff = torch.square(desired_ys - footsteps_in_body_frame[:, :, 1])
|
||||
reward = torch.exp(-torch.sum(stance_diff, dim=1) / (std**2))
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_distance_xy_exp(
|
||||
env: ManagerBasedRLEnv,
|
||||
stance_width: float,
|
||||
stance_length: float,
|
||||
std: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
|
||||
# Compute the current footstep positions relative to the root
|
||||
cur_footsteps_translated = asset.data.body_link_pos_w[:, asset_cfg.body_ids, :] - asset.data.root_link_pos_w[
|
||||
:, :
|
||||
].unsqueeze(1)
|
||||
|
||||
footsteps_in_body_frame = torch.zeros(env.num_envs, 4, 3, device=env.device)
|
||||
for i in range(4):
|
||||
footsteps_in_body_frame[:, i, :] = math_utils.quat_apply(
|
||||
math_utils.quat_conjugate(asset.data.root_link_quat_w), cur_footsteps_translated[:, i, :]
|
||||
)
|
||||
|
||||
# Desired x and y positions for each foot
|
||||
stance_width_tensor = stance_width * torch.ones([env.num_envs, 1], device=env.device)
|
||||
stance_length_tensor = stance_length * torch.ones([env.num_envs, 1], device=env.device)
|
||||
|
||||
desired_xs = torch.cat(
|
||||
[stance_length_tensor / 2, stance_length_tensor / 2, -stance_length_tensor / 2, -stance_length_tensor / 2],
|
||||
dim=1,
|
||||
)
|
||||
desired_ys = torch.cat(
|
||||
[stance_width_tensor / 2, -stance_width_tensor / 2, stance_width_tensor / 2, -stance_width_tensor / 2], dim=1
|
||||
)
|
||||
|
||||
# Compute differences in x and y
|
||||
stance_diff_x = torch.square(desired_xs - footsteps_in_body_frame[:, :, 0])
|
||||
stance_diff_y = torch.square(desired_ys - footsteps_in_body_frame[:, :, 1])
|
||||
|
||||
# Combine x and y differences and compute the exponential penalty
|
||||
stance_diff = stance_diff_x + stance_diff_y
|
||||
reward = torch.exp(-torch.sum(stance_diff, dim=1) / std**2)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_height(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
target_height: float,
|
||||
tanh_mult: float,
|
||||
) -> torch.Tensor:
|
||||
"""Reward the swinging feet for clearing a specified height off the ground"""
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
foot_z_target_error = torch.square(asset.data.body_pos_w[:, asset_cfg.body_ids, 2] - target_height)
|
||||
# foot_velocity_tanh = torch.tanh(
|
||||
# tanh_mult * torch.linalg.norm(asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2], dim=2)
|
||||
# )
|
||||
# reward = torch.sum(foot_z_target_error * foot_velocity_tanh, dim=1)
|
||||
reward = torch.sum(foot_z_target_error, dim=1)
|
||||
# print(foot_z_target_error, "foot_z_target_error")
|
||||
# no reward for zero command
|
||||
reward *= torch.linalg.norm(env.command_manager.get_command(command_name), dim=1) > 0.2
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_height_body(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
asset_cfg: SceneEntityCfg,
|
||||
target_height: float,
|
||||
tanh_mult: float,
|
||||
) -> torch.Tensor:
|
||||
"""Reward the swinging feet for clearing a specified height off the ground"""
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
cur_footpos_translated = asset.data.body_pos_w[:, asset_cfg.body_ids, :] - asset.data.root_pos_w[:, :].unsqueeze(1)
|
||||
footpos_in_body_frame = torch.zeros(env.num_envs, len(asset_cfg.body_ids), 3, device=env.device)
|
||||
cur_footvel_translated = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :] - asset.data.root_lin_vel_w[
|
||||
:, :
|
||||
].unsqueeze(1)
|
||||
footvel_in_body_frame = torch.zeros(env.num_envs, len(asset_cfg.body_ids), 3, device=env.device)
|
||||
for i in range(len(asset_cfg.body_ids)):
|
||||
footpos_in_body_frame[:, i, :] = math_utils.quat_apply_inverse(
|
||||
asset.data.root_quat_w, cur_footpos_translated[:, i, :]
|
||||
)
|
||||
footvel_in_body_frame[:, i, :] = math_utils.quat_apply_inverse(
|
||||
asset.data.root_quat_w, cur_footvel_translated[:, i, :]
|
||||
)
|
||||
foot_z_target_error = torch.square(footpos_in_body_frame[:, :, 2] - target_height).view(env.num_envs, -1)
|
||||
foot_velocity_tanh = torch.tanh(tanh_mult * torch.norm(footvel_in_body_frame[:, :, :2], dim=2))
|
||||
reward = torch.sum(foot_z_target_error * foot_velocity_tanh, dim=1)
|
||||
reward *= torch.linalg.norm(env.command_manager.get_command(command_name), dim=1) > 0.1
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def feet_slide(
|
||||
env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
) -> torch.Tensor:
|
||||
"""Penalize feet sliding.
|
||||
|
||||
This function penalizes the agent for sliding its feet on the ground. The reward is computed as the
|
||||
norm of the linear velocity of the feet multiplied by a binary contact sensor. This ensures that the
|
||||
agent is penalized only when the feet are in contact with the ground.
|
||||
"""
|
||||
# Penalize feet sliding
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
contacts = contact_sensor.data.net_forces_w_history[:, :, sensor_cfg.body_ids, :].norm(dim=-1).max(dim=1)[0] > 1.0
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
|
||||
# feet_vel = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :2]
|
||||
# reward = torch.sum(feet_vel.norm(dim=-1) * contacts, dim=1)
|
||||
|
||||
cur_footvel_translated = asset.data.body_lin_vel_w[:, asset_cfg.body_ids, :] - asset.data.root_lin_vel_w[
|
||||
:, :
|
||||
].unsqueeze(1)
|
||||
footvel_in_body_frame = torch.zeros(env.num_envs, len(asset_cfg.body_ids), 3, device=env.device)
|
||||
for i in range(len(asset_cfg.body_ids)):
|
||||
footvel_in_body_frame[:, i, :] = math_utils.quat_apply_inverse(
|
||||
asset.data.root_quat_w, cur_footvel_translated[:, i, :]
|
||||
)
|
||||
foot_leteral_vel = torch.sqrt(torch.sum(torch.square(footvel_in_body_frame[:, :, :2]), dim=2)).view(
|
||||
env.num_envs, -1
|
||||
)
|
||||
reward = torch.sum(foot_leteral_vel * contacts, dim=1)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
# def stand_still_joint_deviation_l1(
|
||||
# env, command_name: str, command_threshold: float = 0.06, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")
|
||||
# ) -> torch.Tensor:
|
||||
# """Penalize offsets from the default joint positions when the command is very small."""
|
||||
# command = env.command_manager.get_command(command_name)
|
||||
# # Penalize motion when command is nearly zero.
|
||||
# return joint_deviation_l1(env, asset_cfg) * (torch.norm(command[:, :], dim=1) < command_threshold)
|
||||
|
||||
# def joint_deviation_l1(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
# """Penalize joint positions that deviate from the default one."""
|
||||
# # extract the used quantities (to enable type-hinting)
|
||||
# asset: Articulation = env.scene[asset_cfg.name]
|
||||
# # compute out of limits constraints
|
||||
# angle = asset.data.joint_pos[:, asset_cfg.joint_ids] - asset.data.default_joint_pos[:, asset_cfg.joint_ids]
|
||||
# return torch.sum(torch.abs(angle), dim=1)
|
||||
|
||||
|
||||
# def smoothness_1(env: ManagerBasedRLEnv) -> torch.Tensor:
|
||||
# # Penalize changes in actions
|
||||
# diff = torch.square(env.action_manager.action - env.action_manager.prev_action)
|
||||
# diff = diff * (env.action_manager.prev_action[:, :] != 0) # ignore first step
|
||||
# return torch.sum(diff, dim=1)
|
||||
|
||||
|
||||
# def joint_acc_l2_new(env: ManagerBasedRLEnv) -> torch.Tensor:
|
||||
|
||||
# def smoothness_2(env: ManagerBasedRLEnv) -> torch.Tensor:
|
||||
# # Penalize changes in actions
|
||||
# diff = torch.square(env.action_manager.action - 2 * env.action_manager.prev_action + env.action_manager.prev_prev_action)
|
||||
# diff = diff * (env.action_manager.prev_action[:, :] != 0) # ignore first step
|
||||
# diff = diff * (env.action_manager.prev_prev_action[:, :] != 0) # ignore second step
|
||||
# # print(torch.sum(diff, dim=1), "smoothness l2")
|
||||
# return torch.sum(diff, dim=1)
|
||||
|
||||
|
||||
def upward(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
"""Penalize z-axis base linear velocity using L2 squared kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
reward = torch.square(1 - asset.data.projected_gravity_b[:, 2])
|
||||
return reward
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def base_height_l2(
|
||||
env: ManagerBasedRLEnv,
|
||||
target_height: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
sensor_cfg: SceneEntityCfg | None = None,
|
||||
) -> torch.Tensor:
|
||||
"""Penalize asset height from its target using L2 squared kernel.
|
||||
|
||||
Note:
|
||||
For flat terrain, target height is in the world frame. For rough terrain,
|
||||
sensor readings can adjust the target height to account for the terrain.
|
||||
"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
if sensor_cfg is not None:
|
||||
sensor: RayCaster = env.scene[sensor_cfg.name]
|
||||
# Adjust the target height using the sensor data
|
||||
ray_hits = sensor.data.ray_hits_w[..., 2]
|
||||
if torch.isnan(ray_hits).any() or torch.isinf(ray_hits).any() or torch.max(torch.abs(ray_hits)) > 1e6:
|
||||
adjusted_target_height = asset.data.root_link_pos_w[:, 2]
|
||||
else:
|
||||
adjusted_target_height = target_height + torch.mean(ray_hits, dim=1)
|
||||
else:
|
||||
# Use the provided target height directly for flat terrain
|
||||
adjusted_target_height = target_height
|
||||
# Compute the L2 squared penalty
|
||||
reward = torch.square(asset.data.root_pos_w[:, 2] - adjusted_target_height)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def lin_vel_z_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
"""Penalize z-axis base linear velocity using L2 squared kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
reward = torch.square(asset.data.root_lin_vel_b[:, 2])
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def ang_vel_xy_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
"""Penalize xy-axis base angular velocity using L2 squared kernel."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
reward = torch.sum(torch.square(asset.data.root_ang_vel_b[:, :2]), dim=1)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def undesired_contacts(env: ManagerBasedRLEnv, threshold: float, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||
"""Penalize undesired contacts as the number of violations that are above a threshold."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# check if contact force is above threshold
|
||||
net_contact_forces = contact_sensor.data.net_forces_w_history
|
||||
is_contact = torch.max(torch.norm(net_contact_forces[:, :, sensor_cfg.body_ids], dim=-1), dim=1)[0] > threshold
|
||||
# sum over contacts for each environment
|
||||
reward = torch.sum(is_contact, dim=1).float()
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
|
||||
def flat_orientation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor:
|
||||
"""Penalize non-flat base orientation using L2 squared kernel.
|
||||
|
||||
This is computed by penalizing the xy-components of the projected gravity vector.
|
||||
"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
reward = torch.sum(torch.square(asset.data.projected_gravity_b[:, :2]), dim=1)
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
|
||||
def feet_air_time_including_ang_z(
|
||||
env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, threshold: float
|
||||
) -> torch.Tensor:
|
||||
"""Reward long steps taken by the feet using L2-kernel.
|
||||
|
||||
This function rewards the agent for taking steps that are longer than a threshold. This helps ensure
|
||||
that the robot lifts its feet off the ground and takes steps. The reward is computed as the sum of
|
||||
the time for which the feet are in the air.
|
||||
|
||||
If the commands are small (i.e. the agent is not supposed to take a step), then the reward is zero.
|
||||
"""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name]
|
||||
# compute the reward
|
||||
first_contact = contact_sensor.compute_first_contact(env.step_dt)[:, sensor_cfg.body_ids]
|
||||
last_air_time = contact_sensor.data.last_air_time[:, sensor_cfg.body_ids]
|
||||
reward = torch.sum((last_air_time - threshold) * first_contact, dim=1)
|
||||
# no reward for zero command
|
||||
reward *= torch.norm(env.command_manager.get_command(command_name), dim=1) > 0.1
|
||||
# reward *= torch.norm(env.command_manager.get_command(command_name)[:, :3], dim=1) > 0.1
|
||||
return reward
|
||||
|
||||
def lin_vel_xy_l2_with_ang_z_command(
|
||||
env: ManagerBasedRLEnv,
|
||||
command_name: str,
|
||||
command_threshold: float,
|
||||
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
|
||||
) -> torch.Tensor:
|
||||
"""Penalize xy-axis base linear velocity using L2 squared kernel if command is ang_vel_z."""
|
||||
# extract the used quantities (to enable type-hinting)
|
||||
asset: RigidObject = env.scene[asset_cfg.name]
|
||||
# reward = torch.square(asset.data.root_lin_vel_b[:, 2])
|
||||
reward = torch.sum(torch.square(asset.data.root_lin_vel_b[:, :2]), dim=1)
|
||||
command = env.command_manager.get_command(command_name)
|
||||
reward *= (torch.sum(torch.square(command[:, 2:]), dim=1) > command_threshold) & \
|
||||
(torch.sum(torch.square(command[:, :2]), dim=1) < command_threshold)
|
||||
# reward *= torch.sum(torch.square(env.command_manager.get_command(command_name)[:, 2:]), dim=1) > command_threshold
|
||||
# reward *= torch.clamp(-env.scene["robot"].data.projected_gravity_b[:, 2], 0, 0.7) / 0.7
|
||||
return reward
|
||||
+866
@@ -0,0 +1,866 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
|
||||
# Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
import inspect
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import MISSING
|
||||
|
||||
import isaaclab.sim as sim_utils
|
||||
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
|
||||
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||
from isaaclab.managers import CurriculumTermCfg as CurrTerm
|
||||
from isaaclab.managers import EventTermCfg as EventTerm
|
||||
from isaaclab.managers import ObservationGroupCfg as ObsGroup
|
||||
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||
from isaaclab.managers import SceneEntityCfg
|
||||
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||
from isaaclab.scene import InteractiveSceneCfg
|
||||
from isaaclab.sensors import ContactSensorCfg, RayCasterCfg, patterns
|
||||
from isaaclab.terrains import TerrainImporterCfg
|
||||
from isaaclab.utils import configclass
|
||||
from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise
|
||||
|
||||
import rl_training.tasks.manager_based.locomotion.velocity.mdp as mdp
|
||||
|
||||
##
|
||||
# Pre-defined configs
|
||||
##
|
||||
from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG # isort: skip
|
||||
|
||||
|
||||
##
|
||||
# Scene definition
|
||||
##
|
||||
|
||||
|
||||
@configclass
|
||||
class MySceneCfg(InteractiveSceneCfg):
|
||||
"""Configuration for the terrain scene with a legged robot."""
|
||||
|
||||
# ground terrain
|
||||
terrain = TerrainImporterCfg(
|
||||
prim_path="/World/ground",
|
||||
terrain_type="generator",
|
||||
terrain_generator=ROUGH_TERRAINS_CFG,
|
||||
max_init_terrain_level=5,
|
||||
collision_group=-1,
|
||||
physics_material=sim_utils.RigidBodyMaterialCfg(
|
||||
friction_combine_mode="multiply",
|
||||
restitution_combine_mode="multiply",
|
||||
static_friction=1.0,
|
||||
dynamic_friction=1.0,
|
||||
restitution=1.0,
|
||||
),
|
||||
# Keep scene startup independent of NVIDIA's online asset server.
|
||||
# The robot visual geometry is still loaded from the STL meshes in its URDF.
|
||||
visual_material=sim_utils.PreviewSurfaceCfg(
|
||||
diffuse_color=(0.18, 0.20, 0.22),
|
||||
roughness=0.9,
|
||||
),
|
||||
debug_vis=False,
|
||||
)
|
||||
# robots
|
||||
robot: ArticulationCfg = MISSING
|
||||
# sensors
|
||||
height_scanner = RayCasterCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Robot/base",
|
||||
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
|
||||
ray_alignment="yaw",
|
||||
pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]),
|
||||
debug_vis=False,
|
||||
mesh_prim_paths=["/World/ground"],
|
||||
)
|
||||
height_scanner_base = RayCasterCfg(
|
||||
prim_path="{ENV_REGEX_NS}/Robot/base",
|
||||
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
|
||||
ray_alignment="yaw",
|
||||
pattern_cfg=patterns.GridPatternCfg(resolution=0.05, size=(0.1, 0.1)),
|
||||
debug_vis=False,
|
||||
mesh_prim_paths=["/World/ground"],
|
||||
)
|
||||
contact_forces = ContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||
# lights
|
||||
sky_light = AssetBaseCfg(
|
||||
prim_path="/World/skyLight",
|
||||
spawn=sim_utils.DomeLightCfg(
|
||||
intensity=1200.0,
|
||||
color=(0.75, 0.75, 0.75),
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
##
|
||||
# MDP settings
|
||||
##
|
||||
|
||||
|
||||
@configclass
|
||||
class CommandsCfg:
|
||||
"""Command specifications for the MDP."""
|
||||
|
||||
base_velocity = mdp.UniformThresholdVelocityCommandCfg(
|
||||
asset_name="robot",
|
||||
resampling_time_range=(10.0, 10.0),
|
||||
rel_standing_envs=0.02,
|
||||
rel_heading_envs=1.0,
|
||||
heading_command=True,
|
||||
heading_control_stiffness=0.5,
|
||||
debug_vis=True,
|
||||
ranges=mdp.UniformThresholdVelocityCommandCfg.Ranges(
|
||||
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi)
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class ActionsCfg:
|
||||
"""Action specifications for the MDP."""
|
||||
|
||||
joint_pos = mdp.JointPositionActionCfg(
|
||||
asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True, clip=None, preserve_order=True
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class ObservationsCfg:
|
||||
"""Observation specifications for the MDP."""
|
||||
|
||||
@configclass
|
||||
class PolicyCfg(ObsGroup):
|
||||
"""Observations for policy group."""
|
||||
|
||||
# observation terms (order preserved)
|
||||
base_lin_vel = ObsTerm(
|
||||
func=mdp.base_lin_vel,
|
||||
noise=Unoise(n_min=-0.1, n_max=0.1),
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
base_ang_vel = ObsTerm(
|
||||
func=mdp.base_ang_vel,
|
||||
noise=Unoise(n_min=-0.2, n_max=0.2),
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
projected_gravity = ObsTerm(
|
||||
func=mdp.projected_gravity,
|
||||
noise=Unoise(n_min=-0.05, n_max=0.05),
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
velocity_commands = ObsTerm(
|
||||
func=mdp.generated_commands,
|
||||
params={"command_name": "base_velocity"},
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
joint_pos = ObsTerm(
|
||||
func=mdp.joint_pos_rel,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*", preserve_order=True)},
|
||||
noise=Unoise(n_min=-0.01, n_max=0.01),
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
joint_vel = ObsTerm(
|
||||
func=mdp.joint_vel_rel,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*", preserve_order=True)},
|
||||
noise=Unoise(n_min=-1.5, n_max=1.5),
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
actions = ObsTerm(
|
||||
func=mdp.last_action,
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
height_scan = ObsTerm(
|
||||
func=mdp.height_scan,
|
||||
params={"sensor_cfg": SceneEntityCfg("height_scanner")},
|
||||
noise=Unoise(n_min=-0.1, n_max=0.1),
|
||||
clip=(-1.0, 1.0),
|
||||
scale=1.0,
|
||||
)
|
||||
|
||||
def __post_init__(self):
|
||||
self.enable_corruption = True
|
||||
self.concatenate_terms = True
|
||||
|
||||
@configclass
|
||||
class CriticCfg(ObsGroup):
|
||||
"""Observations for critic group."""
|
||||
|
||||
# observation terms (order preserved)
|
||||
base_lin_vel = ObsTerm(
|
||||
func=mdp.base_lin_vel,
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
base_ang_vel = ObsTerm(
|
||||
func=mdp.base_ang_vel,
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
projected_gravity = ObsTerm(
|
||||
func=mdp.projected_gravity,
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
velocity_commands = ObsTerm(
|
||||
func=mdp.generated_commands,
|
||||
params={"command_name": "base_velocity"},
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
joint_pos = ObsTerm(
|
||||
func=mdp.joint_pos_rel,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*", preserve_order=True)},
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
joint_vel = ObsTerm(
|
||||
func=mdp.joint_vel_rel,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*", preserve_order=True)},
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
actions = ObsTerm(
|
||||
func=mdp.last_action,
|
||||
clip=(-100.0, 100.0),
|
||||
scale=1.0,
|
||||
)
|
||||
height_scan = ObsTerm(
|
||||
func=mdp.height_scan,
|
||||
params={"sensor_cfg": SceneEntityCfg("height_scanner")},
|
||||
clip=(-1.0, 1.0),
|
||||
scale=1.0,
|
||||
)
|
||||
# joint_effort = ObsTerm(
|
||||
# func=mdp.joint_effort,
|
||||
# clip=(-100, 100),
|
||||
# scale=0.01,
|
||||
# )
|
||||
|
||||
def __post_init__(self):
|
||||
self.enable_corruption = False
|
||||
self.concatenate_terms = True
|
||||
|
||||
# observation groups
|
||||
policy: PolicyCfg = PolicyCfg()
|
||||
critic: CriticCfg = CriticCfg()
|
||||
|
||||
|
||||
@configclass
|
||||
class EventCfg:
|
||||
"""Configuration for events."""
|
||||
|
||||
# startup
|
||||
randomize_rigid_body_material = EventTerm(
|
||||
func=mdp.randomize_rigid_body_material,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||
"static_friction_range": (0.3, 1.0),
|
||||
"dynamic_friction_range": (0.3, 0.8),
|
||||
"restitution_range": (0.0, 0.4),
|
||||
"num_buckets": 1024,
|
||||
},
|
||||
)
|
||||
|
||||
randomize_rigid_body_mass = EventTerm(
|
||||
func=mdp.randomize_rigid_body_mass,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"mass_distribution_params": (0.85, 1.15),
|
||||
"operation": "scale",
|
||||
"recompute_inertia": True,
|
||||
},
|
||||
)
|
||||
|
||||
randomize_rigid_body_mass_base = EventTerm(
|
||||
func=mdp.randomize_rigid_body_mass,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"mass_distribution_params": (-1.0, 3.0),
|
||||
"operation": "add",
|
||||
"recompute_inertia": True,
|
||||
},
|
||||
)
|
||||
|
||||
randomize_rigid_body_inertia = EventTerm(
|
||||
func=mdp.randomize_rigid_body_inertia,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||
"inertia_distribution_params": (0.85, 1.15),
|
||||
"operation": "scale",
|
||||
},
|
||||
)
|
||||
|
||||
randomize_com_positions = EventTerm(
|
||||
func=mdp.randomize_rigid_body_com,
|
||||
mode="startup",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||
"com_range": {"x": (-0.03, 0.03), "y": (-0.03, 0.03), "z": (-0.02, 0.02)},
|
||||
},
|
||||
)
|
||||
|
||||
# reset
|
||||
randomize_apply_external_force_torque = EventTerm(
|
||||
func=mdp.apply_external_force_torque,
|
||||
mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"force_range": (-10.0, 10.0),
|
||||
"torque_range": (-10.0, 10.0),
|
||||
},
|
||||
)
|
||||
|
||||
randomize_reset_joints = EventTerm(
|
||||
func=mdp.reset_joints_by_scale,
|
||||
# func=mdp.reset_joints_by_offset,
|
||||
mode="reset",
|
||||
params={
|
||||
"position_range": (1.0, 1.0),
|
||||
"velocity_range": (0.0, 0.0),
|
||||
},
|
||||
)
|
||||
|
||||
randomize_actuator_gains = EventTerm(
|
||||
func=mdp.randomize_actuator_gains,
|
||||
mode="reset",
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
"stiffness_distribution_params": (0.85, 1.15),
|
||||
"damping_distribution_params": (0.85, 1.15),
|
||||
"operation": "scale",
|
||||
"distribution": "uniform",
|
||||
},
|
||||
)
|
||||
|
||||
randomize_reset_base = EventTerm(
|
||||
func=mdp.reset_root_state_uniform,
|
||||
mode="reset",
|
||||
params={
|
||||
"pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)},
|
||||
"velocity_range": {
|
||||
"x": (-0.5, 0.5),
|
||||
"y": (-0.5, 0.5),
|
||||
"z": (-0.5, 0.5),
|
||||
"roll": (-0.5, 0.5),
|
||||
"pitch": (-0.5, 0.5),
|
||||
"yaw": (-0.5, 0.5),
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
# interval
|
||||
randomize_push_robot = EventTerm(
|
||||
func=mdp.push_by_setting_velocity,
|
||||
mode="interval",
|
||||
interval_range_s=(10.0, 15.0),
|
||||
params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}},
|
||||
)
|
||||
|
||||
|
||||
@configclass
|
||||
class RewardsCfg:
|
||||
"""Reward terms for the MDP."""
|
||||
|
||||
# General
|
||||
is_terminated = RewTerm(func=mdp.is_terminated, weight=0.0)
|
||||
|
||||
# Root penalties
|
||||
lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=0.0)
|
||||
ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=0.0)
|
||||
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0)
|
||||
base_height_l2 = RewTerm(
|
||||
func=mdp.base_height_l2,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"sensor_cfg": SceneEntityCfg("height_scanner_base"),
|
||||
"target_height": 0.0,
|
||||
},
|
||||
)
|
||||
body_lin_acc_l2 = RewTerm(
|
||||
func=mdp.body_lin_acc_l2,
|
||||
weight=0.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", body_names="")},
|
||||
)
|
||||
|
||||
# Joint penalties
|
||||
joint_torques_l2 = RewTerm(
|
||||
func=mdp.joint_torques_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}
|
||||
)
|
||||
joint_vel_l2 = RewTerm(
|
||||
func=mdp.joint_vel_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}
|
||||
)
|
||||
joint_acc_l2 = RewTerm(
|
||||
func=mdp.joint_acc_l2, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}
|
||||
)
|
||||
|
||||
joint_deviation_l1 = RewTerm(
|
||||
func=mdp.joint_deviation_l1,
|
||||
weight=0.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")},
|
||||
)
|
||||
|
||||
def create_joint_deviation_l1_rewterm(self, attr_name, weight, joint_names_pattern):
|
||||
rew_term = RewTerm(
|
||||
func=mdp.joint_deviation_l1,
|
||||
weight=weight,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=joint_names_pattern)},
|
||||
)
|
||||
setattr(self, attr_name, rew_term)
|
||||
|
||||
joint_pos_limits = RewTerm(
|
||||
func=mdp.joint_pos_limits, weight=0.0, params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")}
|
||||
)
|
||||
joint_vel_limits = RewTerm(
|
||||
func=mdp.joint_vel_limits,
|
||||
weight=0.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*"), "soft_ratio": 1.0},
|
||||
)
|
||||
joint_power = RewTerm(
|
||||
func=mdp.joint_power,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
},
|
||||
)
|
||||
|
||||
stand_still_without_cmd = RewTerm(
|
||||
func=mdp.stand_still_without_cmd,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"command_threshold": 0.1,
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
},
|
||||
)
|
||||
|
||||
joint_pos_penalty = RewTerm(
|
||||
func=mdp.joint_pos_penalty,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
hipx_joint_pos_penalty = RewTerm(
|
||||
func=mdp.joint_pos_penalty,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
hipy_joint_pos_penalty = RewTerm(
|
||||
func=mdp.joint_pos_penalty,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
knee_joint_pos_penalty = RewTerm(
|
||||
func=mdp.joint_pos_penalty,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=".*"),
|
||||
"stand_still_scale": 5.0,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
wheel_vel_penalty = RewTerm(
|
||||
func=mdp.wheel_vel_penalty,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=""),
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"command_name": "base_velocity",
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
},
|
||||
)
|
||||
|
||||
joint_mirror = RewTerm(
|
||||
func=mdp.joint_mirror,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"mirror_joints": [["FR.*", "RL.*"], ["FL.*", "RR.*"]],
|
||||
},
|
||||
)
|
||||
|
||||
action_mirror = RewTerm(
|
||||
func=mdp.action_mirror,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"mirror_joints": [["FR.*", "RL.*"], ["FL.*", "RR.*"]],
|
||||
},
|
||||
)
|
||||
|
||||
action_sync = RewTerm(
|
||||
func=mdp.action_sync,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"joint_groups": [
|
||||
["FR_hip_joint", "FL_hip_joint", "RL_hip_joint", "RR_hip_joint"],
|
||||
["FR_thigh_joint", "FL_thigh_joint", "RL_thigh_joint", "RR_thigh_joint"],
|
||||
["FR_calf_joint", "FL_calf_joint", "RL_calf_joint", "RR_calf_joint"],
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
# Action penalties
|
||||
applied_torque_limits = RewTerm(
|
||||
func=mdp.applied_torque_limits,
|
||||
weight=0.0,
|
||||
params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*")},
|
||||
)
|
||||
action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=0.0)
|
||||
# smoothness_1 = RewTerm(func=mdp.smoothness_1, weight=0.0) # Same as action_rate_l2
|
||||
# smoothness_2 = RewTerm(func=mdp.smoothness_2, weight=0.0) # Unvaliable now
|
||||
|
||||
# Contact sensor
|
||||
undesired_contacts = RewTerm(
|
||||
func=mdp.undesired_contacts,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"threshold": 1.0,
|
||||
},
|
||||
)
|
||||
contact_forces = RewTerm(
|
||||
func=mdp.contact_forces,
|
||||
weight=0.0,
|
||||
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""), "threshold": 100.0},
|
||||
)
|
||||
|
||||
# Velocity-tracking rewards
|
||||
track_lin_vel_xy_exp = RewTerm(
|
||||
func=mdp.track_lin_vel_xy_exp, weight=0.0, params={"command_name": "base_velocity", "std": math.sqrt(0.5)}
|
||||
)
|
||||
track_ang_vel_z_exp = RewTerm(
|
||||
func=mdp.track_ang_vel_z_exp, weight=0.0, params={"command_name": "base_velocity", "std": math.sqrt(0.5)}
|
||||
)
|
||||
|
||||
# Others
|
||||
# feet_air_time = RewTerm(
|
||||
# func=mdp.feet_air_time,
|
||||
# weight=0.0,
|
||||
# params={
|
||||
# "command_name": "base_velocity",
|
||||
# "threshold": 0.5,
|
||||
# "sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
# },
|
||||
# )
|
||||
|
||||
feet_air_time = RewTerm(
|
||||
func=mdp.feet_air_time_including_ang_z,
|
||||
weight=0.0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"threshold": 0.5,
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
},
|
||||
)
|
||||
|
||||
feet_air_time_variance = RewTerm(
|
||||
func=mdp.feet_air_time_variance_penalty,
|
||||
weight=0,
|
||||
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="")},
|
||||
)
|
||||
|
||||
feet_gait = RewTerm(
|
||||
func=mdp.GaitReward,
|
||||
weight=0.0,
|
||||
params={
|
||||
"std": math.sqrt(0.5),
|
||||
"command_name": "base_velocity",
|
||||
"max_err": 0.2,
|
||||
"velocity_threshold": 0.5,
|
||||
"command_threshold": 0.1,
|
||||
"synced_feet_pair_names": (("", ""), ("", "")),
|
||||
"asset_cfg": SceneEntityCfg("robot"),
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces"),
|
||||
},
|
||||
)
|
||||
|
||||
feet_contact = RewTerm(
|
||||
func=mdp.feet_contact,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"command_name": "base_velocity",
|
||||
"expect_contact_num": 2,
|
||||
},
|
||||
)
|
||||
|
||||
feet_contact_without_cmd = RewTerm(
|
||||
func=mdp.feet_contact_without_cmd,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"command_name": "base_velocity",
|
||||
},
|
||||
)
|
||||
|
||||
feet_stumble = RewTerm(
|
||||
func=mdp.feet_stumble,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
},
|
||||
)
|
||||
|
||||
feet_slide = RewTerm(
|
||||
func=mdp.feet_slide,
|
||||
weight=0.0,
|
||||
params={
|
||||
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""),
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
},
|
||||
)
|
||||
|
||||
stand_still = RewTerm(
|
||||
func=mdp.stand_still_joint_deviation_l1,
|
||||
weight=0,
|
||||
params={
|
||||
"command_name": "base_velocity",
|
||||
"asset_cfg": SceneEntityCfg("robot", joint_names=""),
|
||||
},
|
||||
) # negetive
|
||||
|
||||
feet_height = RewTerm(
|
||||
func=mdp.feet_height,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"tanh_mult": 2.0,
|
||||
"target_height": 0.05,
|
||||
"command_name": "base_velocity",
|
||||
},
|
||||
)
|
||||
|
||||
feet_height_body = RewTerm(
|
||||
func=mdp.feet_height_body,
|
||||
weight=0.0,
|
||||
params={
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"tanh_mult": 2.0,
|
||||
"target_height": -0.3,
|
||||
"command_name": "base_velocity",
|
||||
},
|
||||
)
|
||||
|
||||
feet_distance_y_exp = RewTerm(
|
||||
func=mdp.feet_distance_y_exp,
|
||||
weight=0.0,
|
||||
params={
|
||||
"std": math.sqrt(0.25),
|
||||
"asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
"stance_width": float,
|
||||
},
|
||||
)
|
||||
|
||||
# feet_distance_xy_exp = RewTerm(
|
||||
# func=mdp.feet_distance_xy_exp,
|
||||
# weight=0.0,
|
||||
# params={
|
||||
# "std": math.sqrt(0.25),
|
||||
# "asset_cfg": SceneEntityCfg("robot", body_names=""),
|
||||
# "stance_length": float,
|
||||
# "stance_width": float,
|
||||
# },
|
||||
# )
|
||||
|
||||
upward = RewTerm(func=mdp.upward, weight=0.0)
|
||||
|
||||
# lin_vel_xy_l2_with_ang_z_command = RewTerm(
|
||||
# func=mdp.lin_vel_xy_l2_with_ang_z_command,
|
||||
# weight=0,
|
||||
# params={
|
||||
# "command_name": "base_velocity",
|
||||
# "command_threshold": 0.1,
|
||||
# },
|
||||
# ) # negetive
|
||||
|
||||
|
||||
@configclass
|
||||
class TerminationsCfg:
|
||||
"""Termination terms for the MDP."""
|
||||
|
||||
# MDP terminations
|
||||
time_out = DoneTerm(func=mdp.time_out, time_out=True)
|
||||
# command_resample
|
||||
terrain_out_of_bounds = DoneTerm(
|
||||
func=mdp.terrain_out_of_bounds,
|
||||
params={"asset_cfg": SceneEntityCfg("robot"), "distance_buffer": 3.0},
|
||||
time_out=True,
|
||||
)
|
||||
|
||||
# Contact sensor
|
||||
illegal_contact = DoneTerm(
|
||||
func=mdp.illegal_contact,
|
||||
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=""), "threshold": 1.0},
|
||||
)
|
||||
|
||||
bad_orientation_2 = DoneTerm(func=mdp.bad_orientation_2)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@configclass
|
||||
class CurriculumCfg:
|
||||
"""Curriculum terms for the MDP."""
|
||||
|
||||
terrain_levels = CurrTerm(func=mdp.terrain_levels_vel)
|
||||
|
||||
command_levels = CurrTerm(
|
||||
func=mdp.command_levels_vel,
|
||||
params={
|
||||
"reward_term_name": "track_lin_vel_xy_exp",
|
||||
"range_multiplier": (0.1, 1.0),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
##
|
||||
# Environment configuration
|
||||
##
|
||||
|
||||
|
||||
@configclass
|
||||
class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg):
|
||||
"""Configuration for the locomotion velocity-tracking environment."""
|
||||
|
||||
# Scene settings
|
||||
scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=2.5)
|
||||
# Basic settings
|
||||
observations: ObservationsCfg = ObservationsCfg()
|
||||
actions: ActionsCfg = ActionsCfg()
|
||||
commands: CommandsCfg = CommandsCfg()
|
||||
# MDP settings
|
||||
rewards: RewardsCfg = RewardsCfg()
|
||||
terminations: TerminationsCfg = TerminationsCfg()
|
||||
events: EventCfg = EventCfg()
|
||||
curriculum: CurriculumCfg = CurriculumCfg()
|
||||
|
||||
def __post_init__(self):
|
||||
"""Post initialization."""
|
||||
# general settings
|
||||
self.decimation = 4
|
||||
self.episode_length_s = 20.0
|
||||
# simulation settings
|
||||
self.sim.dt = 0.005
|
||||
self.sim.render_interval = self.decimation
|
||||
self.sim.physics_material = self.scene.terrain.physics_material
|
||||
self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15
|
||||
self.sim.physx.max_position_iteration_count = 4
|
||||
self.sim.physx.max_velocity_iteration_count = 1
|
||||
# update sensor update periods
|
||||
# we tick all the sensors based on the smallest update period (physics update period)
|
||||
if self.scene.height_scanner is not None:
|
||||
self.scene.height_scanner.update_period = self.decimation * self.sim.dt
|
||||
if self.scene.contact_forces is not None:
|
||||
self.scene.contact_forces.update_period = self.sim.dt
|
||||
|
||||
# check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator
|
||||
# this generates terrains with increasing difficulty and is useful for training
|
||||
if getattr(self.curriculum, "terrain_levels", None) is not None:
|
||||
if self.scene.terrain.terrain_generator is not None:
|
||||
self.scene.terrain.terrain_generator.curriculum = True
|
||||
else:
|
||||
if self.scene.terrain.terrain_generator is not None:
|
||||
self.scene.terrain.terrain_generator.curriculum = False
|
||||
|
||||
def disable_zero_weight_rewards(self):
|
||||
"""If the weight of rewards is 0, set rewards to None"""
|
||||
for attr in dir(self.rewards):
|
||||
if not attr.startswith("__"):
|
||||
reward_attr = getattr(self.rewards, attr)
|
||||
if not callable(reward_attr) and reward_attr.weight == 0:
|
||||
setattr(self.rewards, attr, None)
|
||||
|
||||
|
||||
def create_obsgroup_class(class_name, terms, enable_corruption=False, concatenate_terms=True):
|
||||
"""
|
||||
Dynamically create and register a ObsGroup class based on the given configuration terms.
|
||||
|
||||
:param class_name: Name of the configuration class.
|
||||
:param terms: Configuration terms, a dictionary where keys are term names and values are term content.
|
||||
:param enable_corruption: Whether to enable corruption for the observation group. Defaults to False.
|
||||
:param concatenate_terms: Whether to concatenate the observation terms in the group. Defaults to True.
|
||||
:return: The dynamically created class.
|
||||
"""
|
||||
# Dynamically determine the module name
|
||||
module_name = inspect.getmodule(inspect.currentframe()).__name__
|
||||
|
||||
# Define the post-init function
|
||||
def post_init_wrapper(self):
|
||||
setattr(self, "enable_corruption", enable_corruption)
|
||||
setattr(self, "concatenate_terms", concatenate_terms)
|
||||
|
||||
# Dynamically create the class using ObsGroup as the base class
|
||||
terms["__post_init__"] = post_init_wrapper
|
||||
dynamic_class = configclass(type(class_name, (ObsGroup,), terms))
|
||||
|
||||
# Custom serialization and deserialization
|
||||
def __getstate__(self):
|
||||
state = self.__dict__.copy()
|
||||
return state
|
||||
|
||||
def __setstate__(self, state):
|
||||
self.__dict__.update(state)
|
||||
|
||||
# Add custom serialization methods to the class
|
||||
dynamic_class.__getstate__ = __getstate__
|
||||
dynamic_class.__setstate__ = __setstate__
|
||||
|
||||
# Place the class in the global namespace for accessibility
|
||||
globals()[class_name] = dynamic_class
|
||||
|
||||
# Register the dynamic class in the module's dictionary
|
||||
if module_name in sys.modules:
|
||||
sys.modules[module_name].__dict__[class_name] = dynamic_class
|
||||
else:
|
||||
raise ImportError(f"Module {module_name} not found.")
|
||||
|
||||
# Return the class for external instantiation
|
||||
return dynamic_class
|
||||
@@ -0,0 +1,52 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
|
||||
# All rights reserved.
|
||||
#
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
|
||||
import omni.ext
|
||||
|
||||
|
||||
# Functions and vars are available to other extension as usual in python: `example.python_ext.some_public_function(x)`
|
||||
def some_public_function(x: int):
|
||||
print("[rl_training] some_public_function was called with x: ", x)
|
||||
return x**x
|
||||
|
||||
|
||||
# Any class derived from `omni.ext.IExt` in top level module (defined in `python.modules` of `extension.toml`) will be
|
||||
# instantiated when extension gets enabled and `on_startup(ext_id)` will be called. Later when extension gets disabled
|
||||
# on_shutdown() is called.
|
||||
class ExampleExtension(omni.ext.IExt):
|
||||
# ext_id is current extension id. It can be used with extension manager to query additional information, like where
|
||||
# this extension is located on filesystem.
|
||||
def on_startup(self, ext_id):
|
||||
print("[rl_training] startup")
|
||||
|
||||
self._count = 0
|
||||
|
||||
self._window = omni.ui.Window("My Window", width=300, height=300)
|
||||
with self._window.frame:
|
||||
with omni.ui.VStack():
|
||||
label = omni.ui.Label("")
|
||||
|
||||
def on_click():
|
||||
self._count += 1
|
||||
label.text = f"count: {self._count}"
|
||||
|
||||
def on_reset():
|
||||
self._count = 0
|
||||
label.text = "empty"
|
||||
|
||||
on_reset()
|
||||
|
||||
with omni.ui.HStack():
|
||||
omni.ui.Button("Add", clicked_fn=on_click)
|
||||
omni.ui.Button("Reset", clicked_fn=on_reset)
|
||||
|
||||
def on_shutdown(self):
|
||||
print("[rl_training] shutdown")
|
||||
@@ -0,0 +1,55 @@
|
||||
# Copyright (c) 2025 Deep Robotics
|
||||
# SPDX-License-Identifier: BSD 3-Clause
|
||||
#
|
||||
# # Copyright (c) 2024-2025 Ziqi Fan
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
"""Installation script for the 'rl_training' python package."""
|
||||
|
||||
import os
|
||||
import toml
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
# Obtain the extension data from the extension.toml file
|
||||
EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__))
|
||||
# Read the extension.toml file
|
||||
EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml"))
|
||||
|
||||
# Minimum dependencies required prior to installation
|
||||
INSTALL_REQUIRES = [
|
||||
# base
|
||||
"psutil",
|
||||
"colorama",
|
||||
"xacrodoc",
|
||||
# amp
|
||||
"numpy",
|
||||
"pandas",
|
||||
"pinocchio",
|
||||
# rl
|
||||
"cusrl[all]",
|
||||
]
|
||||
|
||||
# Installation operation
|
||||
setup(
|
||||
name="rl_training",
|
||||
packages=["rl_training"],
|
||||
author=EXTENSION_TOML_DATA["package"]["author"],
|
||||
maintainer=EXTENSION_TOML_DATA["package"]["maintainer"],
|
||||
url=EXTENSION_TOML_DATA["package"]["repository"],
|
||||
version=EXTENSION_TOML_DATA["package"]["version"],
|
||||
description=EXTENSION_TOML_DATA["package"]["description"],
|
||||
keywords=EXTENSION_TOML_DATA["package"]["keywords"],
|
||||
install_requires=INSTALL_REQUIRES,
|
||||
license="Apache License 2.0",
|
||||
include_package_data=True,
|
||||
python_requires=">=3.10",
|
||||
classifiers=[
|
||||
"Natural Language :: English",
|
||||
"Programming Language :: Python :: 3.10",
|
||||
"Programming Language :: Python :: 3.11",
|
||||
"Isaac Sim :: 4.5.0",
|
||||
"Isaac Sim :: 5.0.0",
|
||||
],
|
||||
zip_safe=False,
|
||||
)
|
||||
Reference in New Issue
Block a user