zhenai
/
TwoFootBot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

can go is Lv0

This commit is contained in:
zhenai 2026-09-13 21:47:49 +08:00
parent eedf98969d
commit fe84eabcdc
8 changed files with 1373 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

262
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/Lv0.py Normal file
View File

@ -0,0 +1,262 @@
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import math
from pathlib import Path
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.managers import EventTermCfg as EventTerm
from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import RewardTermCfg as RewTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.managers import TerminationTermCfg as DoneTerm
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sensors import ContactSensorCfg
from isaaclab.utils.configclass import configclass
from isaaclab.utils.noise import UniformNoiseCfg
import isaaclab_tasks.manager_based.locomotion.velocity.mdp as velocity_mdp
from . import mdp
##
# Pre-defined configs
##
DEMO_ROOT = Path(__file__).resolve().parents[6]
URDF_PATH = DEMO_ROOT / "bot" / "2foot" / "urdf" / "2foot.urdf"
BOT_PACKAGE_PATH = DEMO_ROOT / "bot" / "2foot"
JOINT_NAMES = [
"jlf0_link",
"jlf1_link",
"jlf2_link",
"jlf3_link",
"jrf0_link",
"jrf1_link",
"jrf2_link",
"jrf3_link",
]
TWOFOOTBOT_CFG = ArticulationCfg(
spawn=sim_utils.UrdfFileCfg(
asset_path=str(URDF_PATH),
fix_base=False,
activate_contact_sensors=True,
ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}],
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
drive_type="force",
target_type="position",
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(stiffness=17.0, damping=0.3),
),
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_linear_velocity=100.0,
max_angular_velocity=100.0,
max_depenetration_velocity=5.0,
),
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False,
solver_position_iteration_count=8,
solver_velocity_iteration_count=1,
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.22),
joint_pos={joint_name: 0.0 for joint_name in JOINT_NAMES},
joint_vel={".*": 0.0},
),
actuators={
"legs": ImplicitActuatorCfg(
joint_names_expr=JOINT_NAMES,
effort_limit_sim=0.5,
velocity_limit_sim=5.0,
stiffness=5.0,
damping=0.3,
)
},
)
##
# Scene definition
##
@configclass
class TwofootbotdemoSceneCfg(InteractiveSceneCfg):
"""Flat-ground scene for the 2foot velocity-tracking task."""
# ground plane
ground = AssetBaseCfg(
prim_path="/World/ground",
spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)),
)
# robot
robot: ArticulationCfg = TWOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
contact_forces = None
# lights
dome_light = AssetBaseCfg(
prim_path="/World/DomeLight",
spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0),
)
##
# MDP settings
##
@configclass
class ActionsCfg:
"""Action specifications for the MDP."""
joint_pos = velocity_mdp.JointPositionActionCfg(
asset_name="robot", joint_names=JOINT_NAMES, scale=0.5, use_default_offset=True
)
@configclass
class CommandsCfg:
"""The arrow points along +X at a fixed walking speed."""
base_velocity = velocity_mdp.UniformVelocityCommandCfg(
asset_name="robot",
resampling_time_range=(10.0, 10.0),
rel_standing_envs=0.0,
rel_heading_envs=0.0,
heading_command=False,
debug_vis=True,
ranges=velocity_mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(0.35, 0.35),
lin_vel_y=(0.0, 0.0),
ang_vel_z=(0.0, 0.0),
heading=(-math.pi, math.pi),
),
)
@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=velocity_mdp.base_lin_vel, noise=UniformNoiseCfg(n_min=-0.1, n_max=0.1))
base_ang_vel = ObsTerm(func=velocity_mdp.base_ang_vel, noise=UniformNoiseCfg(n_min=-0.2, n_max=0.2))
projected_gravity = ObsTerm(func=velocity_mdp.projected_gravity)
velocity_commands = ObsTerm(
func=velocity_mdp.generated_commands, params={"command_name": "base_velocity"}
)
joint_pos_rel = ObsTerm(func=velocity_mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("robot")})
joint_vel_rel = ObsTerm(func=velocity_mdp.joint_vel_rel, params={"asset_cfg": SceneEntityCfg("robot")})
actions = ObsTerm(func=velocity_mdp.last_action)
def __post_init__(self) -> None:
self.enable_corruption = False
self.concatenate_terms = True
# observation groups
policy: PolicyCfg = PolicyCfg()
@configclass
class EventCfg:
"""Configuration for events."""
# reset
reset_base = EventTerm(
func=velocity_mdp.reset_root_state_uniform,
mode="reset",
params={
"pose_range": {"x": (-0.2, 0.2), "y": (-0.05, 0.05), "yaw": (-0.1, 0.1)},
"velocity_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.05, 0.05)},
},
)
reset_joints = EventTerm(
func=velocity_mdp.reset_joints_by_scale,
mode="reset",
params={
"position_range": (0.9, 1.1),
"velocity_range": (0.0, 0.0),
},
)
@configclass
class RewardsCfg:
"""Reward terms for the MDP."""
# (1) Constant running reward
# 轨迹线性速度奖励项
track_lin_vel = RewTerm( # 定义一个奖励项，用于跟踪线性速度
func=velocity_mdp.track_lin_vel_xy_exp, # 使用velocity_mdp模块中的track_lin_vel_xy_exp函数作为奖励计算方法
weight=5.0,
params={"command_name": "base_velocity", "std": math.sqrt(0.25)},
)
# 创建奖励项，用于控制机器人的各种行为
upright = RewTerm(func=velocity_mdp.flat_orientation_l2, weight=-1.0) # 控制机器人保持直立姿态，权重为-1.0
vertical_velocity = RewTerm(func=velocity_mdp.lin_vel_z_l2, weight=-1.0) # 控制机器人垂直方向速度，权重为-1.0
angular_velocity = RewTerm(func=velocity_mdp.ang_vel_xy_l2, weight=-0.05) # 控制机器人角速度，权重为-0.05
action_rate = RewTerm(func=velocity_mdp.action_rate_l2, weight=-0.01) # 控制机器人动作变化率，权重为-0.01
joint_acceleration = RewTerm(func=velocity_mdp.joint_acc_l2, weight=-2.5e-7) # 控制机器人关节加速度，权重为-2.5e-7
joint_limits = RewTerm(func=velocity_mdp.joint_pos_limits, weight=-1.0) # 确保机器人关节位置在限制范围内，权重为-1.0
@configclass
class TerminationsCfg:
"""Termination terms for the MDP."""
# (1) Time out
time_out = DoneTerm(func=mdp.time_out, time_out=True)
# (2) The robot has tipped too far from upright.
fallen_orientation = DoneTerm(
func=mdp.bad_orientation,
params={"limit_angle": 0.7},
)
# (3) The base has collapsed onto the ground.
fallen_height = DoneTerm(
func=mdp.root_height_below_minimum,
params={"minimum_height": 0.08},
)
##
# Environment configuration
##
@configclass
class TwofootbotdemoEnvCfg(ManagerBasedRLEnvCfg):
scene: TwofootbotdemoSceneCfg = TwofootbotdemoSceneCfg(num_envs=1024, env_spacing=1.5)
# Basic settings
observations: ObservationsCfg = ObservationsCfg()
actions: ActionsCfg = ActionsCfg()
commands: CommandsCfg = CommandsCfg()
events: EventCfg = EventCfg()
# MDP settings
rewards: RewardsCfg = RewardsCfg()
terminations: TerminationsCfg = TerminationsCfg()
# Post initialization
def __post_init__(self) -> None:
"""Post initialization."""
# general settings
self.decimation = 4
self.episode_length_s = 20.0
# viewer settings
self.viewer.eye = (3.0, 2.5, 1.8)
# simulation settings
self.sim.dt = 1 / 200
self.sim.render_interval = self.decimation
self.sim.visualizer_cfgs = []

45
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/__init__.py
View File

@ -12,12 +12,51 @@ from . import agents
## ##
gym.register( '''gym.register(
id="Template-Twofootbot-v0", id="Template-G1-Twofootbot-mlp-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv", entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True, disable_env_checker=True,
kwargs={ kwargs={
"env_cfg_entry_point": f"{__name__}.twofootbot_env_cfg:TwofootbotEnvCfg", "env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
},
)
gym.register(
id="Template-G1-Twofootbot-cnn-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg",
},
)
gym.register(
id="Template-G1-Twofootbot-mlp-v0-Play",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg_PLAY",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
},
)
gym.register(
id="Template-G1-Twofootbot-cnn-v0-Play",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg_PLAY",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg",
},
)'''
gym.register(
id="Template-Twofootbotv0-Demo",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
disable_env_checker=True,
kwargs={
"env_cfg_entry_point": f"{__name__}.Lv0:TwofootbotdemoEnvCfg",
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
}, },
) )

19
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cfg.py
View File

@ -20,8 +20,9 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
max_iterations = 5000 max_iterations = 5000
save_interval = 200 save_interval = 200
experiment_name = "cartpole_direct" experiment_name = "cartpole_direct"
obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]} #obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]}
actor = RslRlCNNModelCfg( obs_groups = {"actor": ["policy"], "critic": ["policy"]}
'''actor = RslRlCNNModelCfg(
hidden_dims=[512, 256, 256, 128], hidden_dims=[512, 256, 256, 128],
activation="relu", activation="relu",
obs_normalization=False, obs_normalization=False,
@ -32,21 +33,27 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
stride=[8,4,2], stride=[8,4,2],
activation="relu", activation="relu",
), ),
)'''
actor = RslRlMLPModelCfg(
hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
activation="relu",
obs_normalization=True,
distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0),
) )
critic = RslRlMLPModelCfg( critic = RslRlMLPModelCfg(
hidden_dims=[1024, 512, 512, 256, 256, 128], hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
activation="relu", activation="relu",
obs_normalization=False, obs_normalization=True,
) )
algorithm = RslRlPpoAlgorithmCfg( algorithm = RslRlPpoAlgorithmCfg(
value_loss_coef=1.0, #价值函数损失系数 value_loss_coef=1.0, #价值函数损失系数
use_clipped_value_loss=True, #是否使用裁剪值损失 use_clipped_value_loss=True, #是否使用裁剪值损失
clip_param=0.2, #裁剪参数 clip_param=0.2, #裁剪参数
entropy_coef=0.05, #熵系数，探索的奖励 entropy_coef=0.005, #熵系数，探索的奖励
num_learning_epochs=5, #学习的轮数 num_learning_epochs=5, #学习的轮数
num_mini_batches=4, #小批量的数量 num_mini_batches=4, #小批量的数量
learning_rate=5.0e-3, #学习率 learning_rate=5.0e-4, #学习率
schedule="adaptive", #学习率调度器类型 schedule="adaptive", #学习率调度器类型
gamma=0.99, #折扣因子 gamma=0.99, #折扣因子
lam=0.95, #GAE的lambda参数 lam=0.95, #GAE的lambda参数

50
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cnn_cfg.py Normal file
View File

@ -0,0 +1,50 @@
from isaaclab.utils.configclass import configclass
from isaaclab_rl.rsl_rl import (
RslRlCNNModelCfg,
RslRlMLPModelCfg,
RslRlOnPolicyRunnerCfg,
RslRlPpoAlgorithmCfg,
)
@configclass
class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
num_steps_per_env = 16
max_iterations = 15000
save_interval = 200
experiment_name = "cartpole_direct"
obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]}
actor = RslRlCNNModelCfg(
hidden_dims=[1024, 512, 512, 256, 256, 128],
activation="relu",
obs_normalization=False,
distribution_cfg=RslRlCNNModelCfg.GaussianDistributionCfg(init_std=1.0),
cnn_cfg=RslRlCNNModelCfg.CNNCfg(
output_channels=[16,16,16],
kernel_size=[8, 4, 3],
stride=[8,4,2],
activation="relu",
),
)
critic = RslRlMLPModelCfg(
hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
activation="relu",
obs_normalization=True,
)
algorithm = RslRlPpoAlgorithmCfg(
value_loss_coef=1.0, #价值函数损失系数
use_clipped_value_loss=True, #是否使用裁剪值损失
clip_param=0.2, #裁剪参数
entropy_coef=0.005, #熵系数，探索的奖励
num_learning_epochs=5, #学习的轮数
num_mini_batches=4, #小批量的数量
learning_rate=5.0e-4, #学习率
schedule="adaptive", #学习率调度器类型
gamma=0.99, #折扣因子
lam=0.95, #GAE的lambda参数
desired_kl=0.01, #期望的KL散度
max_grad_norm=1.0, #梯度裁剪的最大范数
)

186
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_cnn_env_cfg.py Normal file
View File

@ -0,0 +1,186 @@
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab.managers import RewardTermCfg as RewTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.utils.configclass import configclass
import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp
from TwoFootBot.tasks.manager_based.twofootbot.twofootbot_cnn_env_cfg import (
RewardsCfg,
TwofootbotcnnEnvCfg,
)
##
# Pre-defined configs
##
from isaaclab_assets import G1_MINIMAL_CFG # isort: skip
@configclass
class G1Rewards(RewardsCfg):
"""
G1机器人的奖励函数配置类，继承自RewardsCfg
定义了各种奖励项及其权重，用于训练和评估G1机器人的行为
"""
# 终止惩罚：当机器人终止时给予的惩罚
termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0)
# 跟踪线性速度奖励：鼓励机器人跟踪期望的xy方向线性速度（在坐标系中）
track_lin_vel_xy_exp = RewTerm(
func=mdp.track_lin_vel_xy_yaw_frame_exp,
weight=1.0,
params={"command_name": "base_velocity", "std": 0.5},
)
# 跟踪角速度奖励：鼓励机器人跟踪期望的z方向角速度（在世界坐标系中）
# 奖励项1：跟踪世界坐标系下的z轴角速度期望值
track_ang_vel_z_exp = RewTerm(
func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5}
)
# 定义一个奖励项，用于评估双足机器人的脚部空中时间
#feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0)
feet_air_time = RewTerm(
func=mdp.feet_air_time_positive_biped, # 调用的函数，用于计算双足机器人脚部空中时间
weight=0.25, # 该奖励项的权重，表示在总奖励中的重要性
params={ # 函数所需的参数配置
"command_name": "base_velocity", # 命令名称，可能与基础速度控制相关
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置，指定监测接触力的脚部刚体
"threshold": 0.4, # 阈值，用于判断脚部是否接触地面
},
)
# 创建一个奖励项(RewTerm)，用于计算脚部滑动惩罚
feet_slide = RewTerm(
# 指定使用的函数为mdp.feet_slide
func=mdp.feet_slide,
# 设置权重为-0.1，表示脚部滑动会带来负奖励
weight=-0.1,
# 配置参数，包括传感器和资产的设置
params={
# 配置接触力传感器，监测所有脚踝滚动链接的接触力
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
# 配置机器人实体，指定监测所有脚踝滚动链接
"asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]),
},
)
# 定义一个奖励终端项(Reward Terminal)，用于评估关节位置限制的违反程度
dof_pos_limits = RewTerm(
# 指定评估函数为mdp.joint_pos_limits，该函数用于计算关节位置限制的违反程度
func=mdp.joint_pos_limits,
# 设置权重为-1.0，表示当关节位置限制被违反时，会给予负奖励
weight=-1.0,
#params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])},
)
# 定义髋关节偏差奖励项
joint_deviation_hip = RewTerm(
func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数
weight=-0.1, # 奖励权重，负值表示惩罚偏差
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数，指定机器人髋关节的名称模式
)
@configclass
class G1RoughEnvCfg(TwofootbotcnnEnvCfg):
"""
G1机器人在粗糙环境中的配置类，继承自LocomotionVelocityRoughEnvCfg
定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等
"""
rewards: G1Rewards = G1Rewards()
def __post_init__(self):
# 调用父类的__post_init__方法
super().__post_init__()
# 所有髋关节
hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"]
# ========== 膝关节 ==========
knee_joints = ["jlf2_link", "jrf2_link"]
# ========== 踝关节 ==========
ankle_joints = ["jlf3_link", "jrf3_link"]
# 设置基础速度的偏航成功阈值
self.commands.base_velocity.vel_yaw_success_threshold = 0.8
# 注释：场景中的机器人配置 - 已被注释掉
#self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# 注释：场景中高度扫描器的路径配置 - 已被注释掉
#self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link"
# 初始化事件相关参数
self.events.add_base_mass = None
#elf.events.base_com = None
# 设置外部力矩事件参数，指定作用在机器人躯干链接上
self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link"
# 重置机器人关节的位置范围参数
self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0)
# 设置各种奖励函数的权重
self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0
self.rewards.undesired_contacts = None # 禁用不期望接触的奖励
self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0
self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005
self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7
# 设置自由度加速度L2惩罚的配置参数
self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg(
"robot", joint_names=hip_joints # 只考虑髋关节和膝关节
)
# 设置自由度扭矩L2惩罚的权重和配置参数
self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7
self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg(
"robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节
)
# 设置基础速度命令的范围
self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1
self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0（禁止横向移动）
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1
# 设置终止条件的配置参数
self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link"
# 终止条件基于躯干接触
@configclass
class G1RoughEnvCfg_PLAY(G1RoughEnvCfg):
def __post_init__(self):
# 调用父类的__post_init__方法
super().__post_init__()
# 设置场景参数
self.scene.num_envs = 50 # 环境数量
self.scene.env_spacing = 2.5 # 环境间距
self.episode_length_s = 40.0 # 每集长度（秒）
self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制
# 如果地形生成器存在，则设置其参数
if self.scene.terrain.terrain_generator is not None:
self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数
self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数
self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习
# 设置基础速度命令范围
self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围（固定值1.0）
self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围（固定值0.0）
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围（-1.0到1.0）
self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围（固定值0.0）
# 禁用策略观察值的损坏
self.observations.policy.enable_corruption = False
# 禁用外部力和推力事件
self.events.base_external_force_torque = None
self.events.push_robot = None

186
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_env_cfg.py Normal file
View File

@ -0,0 +1,186 @@
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from isaaclab.managers import RewardTermCfg as RewTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.utils.configclass import configclass
import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp
from TwoFootBot.tasks.manager_based.twofootbot.velocity_env_cfg import (
RewardsCfg,
LocomotionVelocityRoughEnvCfg,
)
##
# Pre-defined configs
##
from isaaclab_assets import G1_MINIMAL_CFG # isort: skip
@configclass
class G1Rewards(RewardsCfg):
"""
G1机器人的奖励函数配置类，继承自RewardsCfg
定义了各种奖励项及其权重，用于训练和评估G1机器人的行为
"""
# 终止惩罚：当机器人终止时给予的惩罚
termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0)
# 跟踪线性速度奖励：鼓励机器人跟踪期望的xy方向线性速度（在坐标系中）
track_lin_vel_xy_exp = RewTerm(
func=mdp.track_lin_vel_xy_yaw_frame_exp,
weight=1.0,
params={"command_name": "base_velocity", "std": 0.5},
)
# 跟踪角速度奖励：鼓励机器人跟踪期望的z方向角速度（在世界坐标系中）
# 奖励项1：跟踪世界坐标系下的z轴角速度期望值
track_ang_vel_z_exp = RewTerm(
func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5}
)
# 定义一个奖励项，用于评估双足机器人的脚部空中时间
#feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0)
feet_air_time = RewTerm(
func=mdp.feet_air_time_positive_biped, # 调用的函数，用于计算双足机器人脚部空中时间
weight=0.25, # 该奖励项的权重，表示在总奖励中的重要性
params={ # 函数所需的参数配置
"command_name": "base_velocity", # 命令名称，可能与基础速度控制相关
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置，指定监测接触力的脚部刚体
"threshold": 0.4, # 阈值，用于判断脚部是否接触地面
},
)
# 创建一个奖励项(RewTerm)，用于计算脚部滑动惩罚
feet_slide = RewTerm(
# 指定使用的函数为mdp.feet_slide
func=mdp.feet_slide,
# 设置权重为-0.1，表示脚部滑动会带来负奖励
weight=-0.1,
# 配置参数，包括传感器和资产的设置
params={
# 配置接触力传感器，监测所有脚踝滚动链接的接触力
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
# 配置机器人实体，指定监测所有脚踝滚动链接
"asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]),
},
)
# 定义一个奖励终端项(Reward Terminal)，用于评估关节位置限制的违反程度
dof_pos_limits = RewTerm(
# 指定评估函数为mdp.joint_pos_limits，该函数用于计算关节位置限制的违反程度
func=mdp.joint_pos_limits,
# 设置权重为-1.0，表示当关节位置限制被违反时，会给予负奖励
weight=-1.0,
#params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])},
)
# 定义髋关节偏差奖励项
joint_deviation_hip = RewTerm(
func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数
weight=-0.1, # 奖励权重，负值表示惩罚偏差
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数，指定机器人髋关节的名称模式
)
@configclass
class G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
"""
G1机器人在粗糙环境中的配置类，继承自LocomotionVelocityRoughEnvCfg
定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等
"""
rewards: G1Rewards = G1Rewards()
def __post_init__(self):
# 调用父类的__post_init__方法
super().__post_init__()
# 所有髋关节
hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"]
# ========== 膝关节 ==========
knee_joints = ["jlf2_link", "jrf2_link"]
# ========== 踝关节 ==========
ankle_joints = ["jlf3_link", "jrf3_link"]
# 设置基础速度的偏航成功阈值
self.commands.base_velocity.vel_yaw_success_threshold = 0.8
# 注释：场景中的机器人配置 - 已被注释掉
#self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# 注释：场景中高度扫描器的路径配置 - 已被注释掉
#self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link"
# 初始化事件相关参数
self.events.add_base_mass = None
#elf.events.base_com = None
# 设置外部力矩事件参数，指定作用在机器人躯干链接上
self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link"
# 重置机器人关节的位置范围参数
self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0)
# 设置各种奖励函数的权重
self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0
self.rewards.undesired_contacts = None # 禁用不期望接触的奖励
self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0
self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005
self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7
# 设置自由度加速度L2惩罚的配置参数
self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg(
"robot", joint_names=hip_joints # 只考虑髋关节和膝关节
)
# 设置自由度扭矩L2惩罚的权重和配置参数
self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7
self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg(
"robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节
)
# 设置基础速度命令的范围
self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1
self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0（禁止横向移动）
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1
# 设置终止条件的配置参数
self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link"
# 终止条件基于躯干接触
@configclass
class G1RoughEnvCfg_PLAY(G1RoughEnvCfg):
def __post_init__(self):
# 调用父类的__post_init__方法
super().__post_init__()
# 设置场景参数
self.scene.num_envs = 50 # 环境数量
self.scene.env_spacing = 2.5 # 环境间距
self.episode_length_s = 40.0 # 每集长度（秒）
self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制
# 如果地形生成器存在，则设置其参数
if self.scene.terrain.terrain_generator is not None:
self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数
self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数
self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习
# 设置基础速度命令范围
self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围（固定值1.0）
self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围（固定值0.0）
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围（-1.0到1.0）
self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围（固定值0.0）
# 禁用策略观察值的损坏
self.observations.policy.enable_corruption = False
# 禁用外部力和推力事件
self.events.base_external_force_torque = None
self.events.push_robot = None

111
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_env_cfg.py → source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_cnn_env_cfg.py
View File

@ -236,11 +236,36 @@ JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。
BOT_PACKAGE_PATH = Path(__file__).resolve().parents[6] / "bot/2foot" # 获取机器人 ROS 包目录。 BOT_PACKAGE_PATH = Path(__file__).resolve().parents[6] / "bot/2foot" # 获取机器人 ROS 包目录。
URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。 URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。
def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs):
"""Spawn the URDF and enable contact reporting on every rigid link."""
from pxr import Usd, UsdPhysics
from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf
from isaaclab.sim.utils import safe_set_attribute_on_usd_prim
spawn_cfg = cfg.replace(activate_contact_sensors=False)
prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs)
for child_prim in Usd.PrimRange(prim):
if not child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
continue
applied_schemas = child_prim.GetAppliedSchemas()
if "PhysxRigidBodyAPI" not in applied_schemas:
child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
if "PhysxContactReportAPI" not in applied_schemas:
child_prim.AddAppliedSchema("PhysxContactReportAPI")
safe_set_attribute_on_usd_prim(
child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False
)
safe_set_attribute_on_usd_prim(
child_prim, "physxContactReport:threshold", 0.0, camel_case=False
)
return prim
TOFOOTBOT_CFG = ArticulationCfg( TOFOOTBOT_CFG = ArticulationCfg(
spawn=sim_utils.UrdfFileCfg( spawn=sim_utils.UrdfFileCfg(
func=spawn_twofootbot_from_urdf,
asset_path=str(URDF_PATH), asset_path=str(URDF_PATH),
fix_base=False, fix_base=False,
activate_contact_sensors=True, activate_contact_sensors=False,
ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}], ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}],
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg( joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
drive_type="force", drive_type="force",
@ -268,7 +293,8 @@ TOFOOTBOT_CFG = ArticulationCfg(
actuators={ # 动力配置。 actuators={ # 动力配置。
"legs": ImplicitActuatorCfg( "legs": ImplicitActuatorCfg(
joint_names_expr=JOINT_NAMES, # 控制所有腿的关节。 joint_names_expr=JOINT_NAMES, # 控制所有腿的关节。
effort_limit_sim=0.08, # 力矩限制。单位：牛顿米。 effort_limit_sim=10, # 力矩限制。单位：牛顿米。
velocity_limit_sim=8.0554, # 速度限制 (rad/s)
stiffness=2.0, # 刚度。单位：牛顿/米。 stiffness=2.0, # 刚度。单位：牛顿/米。
damping=0.08, # 阻尼。单位：牛顿秒/米。 damping=0.08, # 阻尼。单位：牛顿秒/米。
) )
@ -365,15 +391,15 @@ class MySceneCfg(InteractiveSceneCfg):
# robots # robots
robot: ArticulationCfg = TOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") robot: ArticulationCfg = TOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# sensors # sensors
#height_scanner = None height_scanner = None
height_scanner = RayCasterCfg( '''height_scanner = RayCasterCfg(
prim_path="{ENV_REGEX_NS}/Robot/head_link", prim_path="{ENV_REGEX_NS}/Robot/head_link",
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
ray_alignment="yaw", ray_alignment="yaw",
pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[0.40, 0.25]), pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[0.40, 0.25]),
debug_vis=False, debug_vis=False,
mesh_prim_paths=["/World/ground"], mesh_prim_paths=["/World/ground"],
) )'''
contact_forces = VelocityEnvContactSensorCfg() contact_forces = VelocityEnvContactSensorCfg()
# lights # lights
@ -404,7 +430,7 @@ class CommandsCfg:
heading_control_stiffness=0.5, heading_control_stiffness=0.5,
debug_vis=True, debug_vis=True,
ranges=mdp.UniformVelocityCommandCfg.Ranges( ranges=mdp.UniformVelocityCommandCfg.Ranges(
lin_vel_x=(-0.5, 0.5), lin_vel_y=(-0.3, 0.3), ang_vel_z=(-0.5, 0.5), heading=(-math.pi, math.pi) 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)
), ),
) )
@ -559,23 +585,6 @@ class EventsCfg:
) )
def feet_air_time_alt(env):
"""替代奖励：基于足端高度估计空中时间，鼓励足部腾空。"""
# 获取左右足端位置（世界坐标）
foot_l_pos = env.scene["robot"].data.body_pos_w[:, env.scene["robot"].body_names.index("lf3_link")]
foot_r_pos = env.scene["robot"].data.body_pos_w[:, env.scene["robot"].body_names.index("rf3_link")]
foot_z = torch.stack([foot_l_pos[:, 2], foot_r_pos[:, 2]], dim=1) # (num_envs, 2)
# 触地判断（高度低于阈值）
contact_threshold = 0.02 # 可根据机器人尺寸调整
in_air = foot_z > contact_threshold # True表示在空中
# 计算每只脚累计空中步数（可在环境状态中存储历史）
# 这里简化：给予即时奖励：空中高度*系数
#air_time_reward = torch.sum(foot_z * in_air.float(), dim=1) * 1 # 系数可调
air_time_reward = torch.sum(in_air.float(), dim=1) * 1 # 每只脚在空中给予奖励
return air_time_reward
@configclass @configclass
class RewardsCfg: class RewardsCfg:
"""Reward terms for the MDP.""" """Reward terms for the MDP."""
@ -593,37 +602,25 @@ class RewardsCfg:
dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)#关节力矩平方和 dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)#关节力矩平方和
dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)#关节加速度平方和 dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)#关节加速度平方和
action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3)#惩罚相邻动作之间的变化量平方 action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3)#惩罚相邻动作之间的变化量平方
alive = RewTerm( #固定时间惩罚
func=mdp.is_alive, # 该函数通常返回1（若机器人存活）
weight=-0.25,
)
fall_penalty = RewTerm( #摔倒惩罚
func=mdp.is_terminated_term,
weight=-100.0,
params={"term_keys": "base_contact"},
)
feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.1) # 足端空中时间奖励 #feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.1) # 足端空中时间奖励
'''feet_air_time = RewTerm( feet_air_time = RewTerm(
func=mdp.feet_air_time, func=mdp.feet_air_time,
weight=0.125, weight=0.125,
params={ params={
#"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*3_link"), "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
"sensor_cfg": SceneEntityCfg("robot", body_names=".*3_link"), "command_name": "base_velocity",
"command_name": "base_velocity", "threshold": 0.5,
"threshold": 0.5,
}, },
) )
# 定义一个不期望接触的惩罚项，用于优化机器人运动
# 使用RewTerm类创建，其权重为负值，表示对不期望接触的行为进行惩罚
undesired_contacts = RewTerm( undesired_contacts = RewTerm(
func=mdp.undesired_contacts, func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts，用于计算不期望接触的惩罚
weight=-1.0, weight=-1.0, # 设置权重为-1.0，表示不期望接触的行为会带来负奖励
params={ params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0},
#"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*0_link"), )
"sensor_cfg": SceneEntityCfg("robot", body_names=".*0_link"),
"threshold": 1.0
},
)'''
# -- optional penalties # -- optional penalties
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=-1.5) #惩罚倾斜角度 flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=-1.5) #惩罚倾斜角度
@ -635,10 +632,13 @@ class TerminationsCfg:
time_out = DoneTerm(func=mdp.time_out, time_out=True) time_out = DoneTerm(func=mdp.time_out, time_out=True)
base_contact = DoneTerm( base_contact = DoneTerm(
func=mdp.illegal_contact, func=mdp.illegal_contact,
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="head_link"), "threshold": 1.0}, params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[
#params={"asset_cfg": SceneEntityCfg("robot", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0}, "head_link",
) "lf0_link", "lf1_link", "lf2_link",
"rf0_link", "rf1_link", "rf2_link",
]), "threshold": 1.0},
)
#机器人Z轴与世界Z轴夹角超90度则终止 #机器人Z轴与世界Z轴夹角超90度则终止
'''upside_down = DoneTerm( '''upside_down = DoneTerm(
func=mdp.upside_down, func=mdp.upside_down,
@ -649,7 +649,8 @@ class TerminationsCfg:
class CurriculumCfg: class CurriculumCfg:
"""Curriculum terms for the MDP.""" """Curriculum terms for the MDP."""
terrain_levels = CurrTerm(func=mdp.terrain_levels_vel) # The scene uses a flat plane, so there is no terrain generator to update.
terrain_levels = None
## ##
@ -658,7 +659,7 @@ class CurriculumCfg:
@configclass @configclass
class TwofootbotEnvCfg(ManagerBasedRLEnvCfg): class TwofootbotcnnEnvCfg(ManagerBasedRLEnvCfg):
"""Configuration for the locomotion velocity-tracking environment.""" """Configuration for the locomotion velocity-tracking environment."""
# Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs # Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs

578
source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/velocity_env_cfg.py Normal file
View File

@ -0,0 +1,578 @@
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import math
from dataclasses import MISSING
from pathlib import Path
import torch
from isaaclab.sensors import ContactSensorCfg
from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg
from isaaclab_newton.sensors import ContactSensorCfg as NewtonContactSensorCfg
from isaaclab_ovphysx.sensors import ContactSensorCfg as OvPhysXContactSensorCfg
from isaaclab_physx.physics import PhysxCfg
from isaaclab_physx.sensors import ContactSensorCfg as PhysXContactSensorCfg
from isaaclab.actuators import ImplicitActuatorCfg
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 RayCasterCfg, patterns
from isaaclab.sim import SimulationCfg
from isaaclab.terrains import TerrainImporterCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
from isaaclab.utils.configclass import configclass
from isaaclab.utils.noise import UniformNoiseCfg as Unoise
import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp
#from isaaclab.managers import mdp as base_mdp
from isaaclab_tasks.utils import PresetCfg, preset
from isaaclab.sensors import CameraCfg
from isaaclab.sensors import TiledCameraCfg
from isaaclab.sim import PinholeCameraCfg
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from isaaclab.managers import (
RewardTermCfg,
TerminationTermCfg,
EventTermCfg as EventTerm,
ObservationGroupCfg as ObsGroup,
ObservationTermCfg as ObsTerm,
CurriculumTermCfg as CurriculumTerm,
SceneEntityCfg,
)
JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。
"jlf0_link",
"jlf1_link",
"jlf2_link",
"jlf3_link",
"jrf0_link",
"jrf1_link",
"jrf2_link",
"jrf3_link",
]
BOT_PACKAGE_PATH = Path(__file__).resolve().parents[6] / "bot/2foot" # 获取机器人 ROS 包目录。
URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。
URDF_PATH = "/home/zhenai/AI/RL/bot/2foot/urdf/2foot.urdf"
BOT_PACKAGE_PATH = "/home/zhenai/AI/RL/bot/2foot" # ⚠️ 确保这个目录下有 meshes/ 文件夹
def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs):
"""Spawn the URDF and enable contact reporting on every rigid link."""
from pxr import Usd, UsdPhysics
from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf
from isaaclab.sim.utils import safe_set_attribute_on_usd_prim
spawn_cfg = cfg.replace(activate_contact_sensors=False)
prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs)
for child_prim in Usd.PrimRange(prim):
if child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
applied_schemas = child_prim.GetAppliedSchemas()
if "PhysxRigidBodyAPI" not in applied_schemas:
child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
if "PhysxContactReportAPI" not in applied_schemas:
child_prim.AddAppliedSchema("PhysxContactReportAPI")
safe_set_attribute_on_usd_prim(
child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False
)
safe_set_attribute_on_usd_prim(
child_prim, "physxContactReport:threshold", 0.0, camel_case=False
)
return prim
# 双足机器人URDF配置对象
TWOFOOTBOT_URDF_CFG = ArticulationCfg(
# 配置双足机器人从URDF文件生成
spawn=sim_utils.UrdfFileCfg(
# 指定用于生成机器人的函数
func=spawn_twofootbot_from_urdf,
asset_path=URDF_PATH,
# 设置参数：是否固定基座
fix_base=False,
# 设置参数：是否激活接触传感器
activate_contact_sensors=False,
# ROS包路径配置
ros_package_paths=[{"name": "2foot", "path": BOT_PACKAGE_PATH}],
# 关节驱动配置
# 创建一个关节驱动配置，使用URDF文件配置中的JointDriveCfg类
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
# 设置驱动类型为"force"，表示通过力来控制关节
drive_type="force",
# 设置目标类型为"position"，表示目标是控制关节位置
target_type="position",
# 设置控制增益参数，使用PD控制器配置
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(
stiffness=17,
damping=0
),
),
# 刚体属性配置
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=False,
max_linear_velocity=100.0,
max_angular_velocity=100.0,
max_depenetration_velocity=5.0,
),
# 关节属性配置
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=False,
solver_position_iteration_count=4,
solver_velocity_iteration_count=0,
),
),
# 初始状态配置
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.22),
joint_pos={
"jlf0_link": 0.0, "jlf1_link": 0.0, "jlf2_link": 0.0, "jlf3_link": 0.0,
"jrf0_link": 0.0, "jrf1_link": 0.0, "jrf2_link": 0.0, "jrf3_link": 0.0,
},
joint_vel={".*": 0.0},
),
# 执行器配置
actuators={
"legs": ImplicitActuatorCfg(
# 关节名称表达式列表
joint_names_expr=[
"jlf0_link", "jlf1_link", "jlf2_link", "jlf3_link",
"jrf0_link", "jrf1_link", "jrf2_link", "jrf3_link",
],
# 仿真中的力限制
effort_limit_sim=0.5,
# 仿真中的速度限制
velocity_limit_sim=5.0, # 速度限制 (rad/s)
# 刚度系数
stiffness=5,
# 阻尼系数
damping=0.3,
)
},
)
##
# Physics presets
##
@configclass
class RoughPhysicsCfg(PresetCfg):
"""Shared physics preset for all rough-terrain locomotion envs."""
default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15)
newton_mjwarp = NewtonCfg(
solver_cfg=MJWarpSolverCfg(
njmax=200,
nconmax=100,
cone="pyramidal",
impratio=1.0,
integrator="implicitfast",
use_mujoco_contacts=False,
),
collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000),
num_substeps=1,
debug_mode=False,
# 1 cm shape margin is the single most important Newton setting for rough
# terrain — without it, non-Anymal-D robots fail to learn stable contact
# on triangle-mesh terrain. See isaaclab_newton 0.5.22 changelog.
default_shape_cfg=NewtonShapeCfg(margin=0.01),
)
physx = default
##
# Scene definition
##
@configclass
class VelocityEnvContactSensorCfg(PresetCfg):
default = PhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
newton_mjwarp = NewtonContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
physx = default
ovphysx = OvPhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
@configclass
class MySceneCfg(InteractiveSceneCfg):
"""Configuration for the terrain scene with a legged robot."""
# ground terrain
terrain = TerrainImporterCfg(
prim_path="/World/ground",
terrain_type="plane",
collision_group=-1,
physics_material=sim_utils.RigidBodyMaterialCfg(
friction_combine_mode="multiply",
restitution_combine_mode="multiply",
static_friction=1.0,
dynamic_friction=1.0,
),
visual_material=sim_utils.MdlFileCfg(
mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl",
project_uvw=True,
texture_scale=(0.25, 0.25),
),
debug_vis=False,
)
# robots
robot: ArticulationCfg = TWOFOOTBOT_URDF_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# sensors
height_scanner = None
'''height_scanner = RayCasterCfg(
prim_path="{ENV_REGEX_NS}/Robot/head_link",
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"],
)'''
contact_forces = VelocityEnvContactSensorCfg()
# lights
sky_light = AssetBaseCfg(
prim_path="/World/skyLight",
spawn=sim_utils.DomeLightCfg(
intensity=750.0,
texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr",
),
)
##
# MDP settings
##
@configclass
class CommandsCfg:
"""Command specifications for the MDP."""
base_velocity = mdp.UniformVelocityCommandCfg(
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.UniformVelocityCommandCfg.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)
@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))
base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2))
projected_gravity = ObsTerm(
func=mdp.projected_gravity,
noise=Unoise(n_min=-0.05, n_max=0.05),
)
velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"})
joint_pos = ObsTerm(
# 指定计算函数，用于获取关节相对位置
func=mdp.joint_pos_rel,
# 设置噪声参数，使用均匀分布噪声，最小值为-1e-3，最大值为1e-3
noise=Unoise(n_min=-1e-3, n_max=1e-3),
# 配置观测参数
params={
# 指定观测的实体为机器人，并配置关节名称和顺序
"asset_cfg": SceneEntityCfg(
"robot", # 实体名称为"robot"
joint_names=JOINT_NAMES, # 指定要观测的关节名称列表
preserve_order=True, # 保持关节顺序不变
)
},
# 设置历史记录长度为5，即保留最近5个时间步的观测数据
history_length=5,
)
joint_vel = ObsTerm(
func=mdp.joint_vel,
noise=Unoise(n_min=-0.04, n_max=0.04),
scale=0.05,
params={
"asset_cfg": SceneEntityCfg(
"robot",
joint_names=JOINT_NAMES,
preserve_order=True,
)
},
history_length=5,
)
actions = ObsTerm(func=mdp.last_action)
velocity_commands = ObsTerm(
func=mdp.generated_commands, # 指定调用的函数，即MDP中的generated_commands方法
params={"command_name": "base_velocity"}, # 传递给函数的参数，指定命令名称为"base_velocity"
scale=5.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),
)'''
def __post_init__(self):
self.enable_corruption = True
self.concatenate_terms = True
# observation groups
policy: PolicyCfg = PolicyCfg()
@configclass
class EventsCfg:
"""Configuration for events."""
# startup
# 创建一个物理材质事件项
physics_material = EventTerm(
# 指定函数为随机化刚体材质的函数
func=mdp.randomize_rigid_body_material,
# 设置事件模式为启动时执行
mode="startup",
# 设置事件参数
params={
# 配置场景实体，指定为机器人，并匹配所有身体部位
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
# 设置静态摩擦力的范围为固定值0.8
"static_friction_range": (0.8, 0.8),
# 设置动态摩擦力的范围为固定值0.6
"dynamic_friction_range": (0.6, 0.6),
# 设置恢复系数的范围为固定值0.0（完全非弹性碰撞）
"restitution_range": (0.0, 0.0),
# 设置材质分桶数量为64
"num_buckets": 64,
},
)
add_base_mass = EventTerm(
func=mdp.randomize_rigid_body_mass,
mode="startup",
params={
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
# Multiplicative ±25% log-uniform. Scale-invariant across robot sizes
# (no per-robot kg overrides needed) with geometric mean 1.0 and
# symmetric inverse perturbation (acceleration symmetric around nominal).
"mass_distribution_params": (1 / 1.25, 1.25),
"operation": "scale",
"distribution": "log_uniform",
},
)
# 预设基座质心随机化配置
base_com = preset( # 使用preset函数创建预设配置
default=EventTerm( # 默认事件项配置
func=mdp.randomize_rigid_body_com, # 执行的函数：随机化刚体质心
mode="startup", # 事件模式：启动时执行
params={ # 函数参数配置
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"), # 场景实体配置：机器人基座
"com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)},
},
),
newton_mjwarp=None,
)
# reset
# 创建一个外部力矩事件项，用于在重置时对机器人基座施加外部力和力矩
base_external_force_torque = EventTerm(
# 指定事件处理函数为mdp.apply_external_force_torque
func=mdp.apply_external_force_torque,
# 设置事件模式为"reset"，表示在环境重置时触发
mode="reset",
# 设置事件参数
params={
# 指定目标实体为机器人，并指定施加力的部位为基座
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
# 设置力的范围为(0.0, 0.0)，表示不施加任何力
"force_range": (0.0, 0.0),
# 设置力矩的范围为(-0.0, 0.0)，表示不施加任何力矩
"torque_range": (-0.0, 0.0),
},
)
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),
},
},
)
# 定义一个重置机器人关节的事件项
reset_robot_joints = EventTerm(
# 指定事件处理函数为mdp.reset_joints_by_scale
func=mdp.reset_joints_by_scale,
# 设置事件模式为"reset"
mode="reset",
# 设置事件参数
params={
# 设置位置范围为(0.5, 1.5)
"position_range": (0.5, 1.5),
# 设置速度范围为(0.0, 0.0)
"velocity_range": (0.0, 0.0),
},
)
# interval
# 创建一个名为push_robot的EventTerm对象，用于执行机器人推动操作
push_robot = EventTerm(
func=mdp.push_by_setting_velocity, # 指定执行函数为mdp中的push_by_setting_velocity
mode="interval", # 设置执行模式为间隔模式
interval_range_s=(10.0, 15.0), # 设置执行间隔时间范围为10.0到15.0秒
params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}}, # 设置推动速度范围，x和y方向的速度范围均为-0.5到0.5
)
@configclass
class RewardsCfg:
"""Reward terms for the MDP."""
# -- task
track_lin_vel_xy_exp = RewTerm(
func=mdp.track_lin_vel_xy_exp, weight=1.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
)
track_ang_vel_z_exp = RewTerm(
func=mdp.track_ang_vel_z_exp, weight=0.5, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
)
# -- penalties
lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=-2.0)
ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=-0.05)
dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)
dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)
action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.01)
feet_air_time = RewTerm(
func=mdp.feet_air_time,
weight=0.125,
params={
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
"command_name": "base_velocity",
"threshold": 0.5,
},
)
# 定义一个不期望接触的惩罚项，用于优化机器人运动
# 使用RewTerm类创建，其权重为负值，表示对不期望接触的行为进行惩罚
undesired_contacts = RewTerm(
func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts，用于计算不期望接触的惩罚
weight=-1.0, # 设置权重为-1.0，表示不期望接触的行为会带来负奖励
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0},
)
# -- optional penalties
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0)
dof_pos_limits = RewTerm(func=mdp.joint_pos_limits, weight=0.0)
@configclass
class TerminationsCfg:
"""Termination terms for the MDP."""
time_out = DoneTerm(func=mdp.time_out, time_out=True)
base_contact = DoneTerm(
func=mdp.illegal_contact,
params={"sensor_cfg": SceneEntityCfg("contact_forces",
body_names=[
"head_link",
"lf0_link", "lf1_link", "lf2_link",
"rf0_link", "rf1_link", "rf2_link",
]), "threshold": 1.0},
)
@configclass
class CurriculumCfg:
"""Curriculum terms for the MDP."""
# The scene uses a flat plane, so there is no terrain generator to update.
terrain_levels = None
##
# Environment configuration base
##
@configclass
class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg):
"""Configuration for the locomotion velocity-tracking environment."""
# Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs
sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg())
# Scene settings
scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=0.5)
# Basic settings
observations: ObservationsCfg = ObservationsCfg()
actions: ActionsCfg = ActionsCfg()
commands: CommandsCfg = CommandsCfg()
# MDP settings
rewards: RewardsCfg = RewardsCfg()
terminations: TerminationsCfg = TerminationsCfg()
events: EventsCfg = EventsCfg()
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
# 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