From c1e3d9382f6cc79de462139fc2b78420828e6b1d Mon Sep 17 00:00:00 2001 From: ChenXi Date: Mon, 16 Mar 2026 09:23:22 -0400 Subject: [PATCH] Add reward to maintain an upright and stable position --- rl_game/get_up/config/t1_env_cfg.py | 158 ++++++++++++++++++++-------- 1 file changed, 114 insertions(+), 44 deletions(-) diff --git a/rl_game/get_up/config/t1_env_cfg.py b/rl_game/get_up/config/t1_env_cfg.py index 66768c8..881c738 100644 --- a/rl_game/get_up/config/t1_env_cfg.py +++ b/rl_game/get_up/config/t1_env_cfg.py @@ -1,5 +1,6 @@ +import torch from isaaclab.assets import ArticulationCfg -from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs import ManagerBasedRLEnvCfg, ManagerBasedRLEnv from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import RewardTermCfg as RewTerm @@ -13,9 +14,62 @@ from rl_game.get_up.env.t1_env import T1SceneCfg import isaaclab.envs.mdp as mdp +# --- 1. 自定义 MDP 逻辑函数 (放在配置类之前) --- + +def is_standing_still( + env: ManagerBasedRLEnv, + minimum_height: float, + max_angle_error: float, + standing_time: float +) -> torch.Tensor: + """ + 判定机器人是否稳定站立了一段时间。 + 逻辑：高度达标且姿态垂直，持续时间超过 standing_time 则返回 True。 + """ + # 获取当前状态：高度 (Z轴) 和 投影重力 (前两个分量越小越垂直) + current_height = env.scene["robot"].data.root_pos_w[:, 2] + gravity_error = torch.norm(env.scene["robot"].data.projected_gravity_b[:, :2], dim=-1) + + # 判断当前时刻是否“达标” + is_stable_now = (current_height > minimum_height) & (gravity_error < max_angle_error) + + # 在 env.extras 中维护一个计时器 + if "stable_timer" not in env.extras: + env.extras["stable_timer"] = torch.zeros(env.num_envs, device=env.device) + + # 核心计时逻辑：达标累加 dt，不达标清零 + # dt = physics_dt * decimation (即控制频率的步长) + dt = env.physics_dt * env.cfg.decimation + env.extras["stable_timer"] = torch.where( + is_stable_now, + env.extras["stable_timer"] + dt, + torch.zeros_like(env.extras["stable_timer"]) + ) + + # 判定是否达到目标时长 + return env.extras["stable_timer"] > standing_time + +def get_success_reward(env: ManagerBasedRLEnv, term_keys: str) -> torch.Tensor: + """检查是否触发了特定的成功终止条件""" + return env.termination_manager.get_term(term_keys) + +def root_lin_vel_norm(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """手动计算 root 线性速度的 L2 范数""" + # 获取速度 (16384, 3) + vel = env.scene[asset_cfg.name].data.root_lin_vel_w + # 返回模长 (16384,) + return torch.norm(vel, dim=-1) + +def root_ang_vel_norm(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """手动计算 root 角速度的 L2 范数""" + vel = env.scene[asset_cfg.name].data.root_ang_vel_w + return torch.norm(vel, dim=-1) + +# --- 2. 配置类定义 --- + @configclass class T1ObservationCfg: - """观察值：必须包含姿态和高度，否则机器人不知道自己是怎么躺着的""" + """观察值配置""" @configclass class PolicyCfg(ObsGroup): @@ -25,8 +79,7 @@ class T1ObservationCfg: base_lin_vel = ObsTerm(func=mdp.base_lin_vel) base_ang_vel = ObsTerm(func=mdp.base_ang_vel) projected_gravity = ObsTerm(func=mdp.projected_gravity) - # ⬅️ 新增：让机器人知道自己离地多高 - root_height = ObsTerm(func=mdp.root_pos_w) + root_height = ObsTerm(func=mdp.root_pos_w) # 高度信息对起身至关重要 joint_pos = ObsTerm(func=mdp.joint_pos_rel) joint_vel = ObsTerm(func=mdp.joint_vel_rel) actions = ObsTerm(func=mdp.last_action) @@ -36,31 +89,28 @@ class T1ObservationCfg: @configclass class T1EventCfg: - """事件配置：实现多种随机躺下姿态的关键""" - - # 每次重置时，随机给基座一个旋转角度 - # 包括：背躺 (Pitch=1.57), 趴着 (Pitch=-1.57), 侧躺 (Roll=1.57) + """重置时的随机姿态：背躺、趴着、侧躺""" reset_robot_rotation = EventTerm( - func=mdp.reset_root_state_uniform, # 修正为报错建议的函数名 + func=mdp.reset_root_state_uniform, params={ "asset_cfg": SceneEntityCfg("robot"), "pose_range": { - "roll": (-1.57, 1.57), # 侧躺随机 - "pitch": (-1.57, 1.57), # 趴下/背躺随机 - "yaw": (-3.14, 3.14), # 航向角随机 - "x": (0.0, 0.0), # 位置不随机（保持在原点） + "roll": (-1.57, 1.57), + "pitch": (-1.57, 1.57), + "yaw": (-3.14, 3.14), + "x": (0.0, 0.0), "y": (0.0, 0.0), - "z": (0.0, 0.0), # 高度由 init_state 决定，这里设为0偏移 + "z": (0.0, 0.0), }, - "velocity_range": {}, # 初始速度默认为0 + "velocity_range": {}, }, mode="reset", ) + @configclass class T1ActionCfg: - """动作空间配置组""" - # 这里的变量名 (例如 joint_pos) 将决定 ActionManager 中的 term 名称 + """动作空间""" joint_pos = JointPositionActionCfg( asset_name="robot", joint_names=[".*"], @@ -68,61 +118,81 @@ class T1ActionCfg: use_default_offset=True ) + @configclass class T1GetUpRewardCfg: - """起身任务专用奖励：快、直、稳""" - - # 1. 核心进度：高度奖励 - # 只要低于目标高度(0.6m)，就会持续根据距离给负分，越高分数越接近0 + """奖励函数：引导、稳定、终点奖""" + # 1. 进度引导：越高分越高 height_progress = RewTerm( func=mdp.root_height_below_minimum, weight=15.0, params={"minimum_height": 0.65} ) - # 2. 速度惩罚：时间就是金钱 - # 每一帧都扣 0.5 分，起身越慢，被扣掉的总分就越多 - time_penalty = RewTerm( - func=mdp.is_alive, - weight=-0.5 - ) + # 2. 时间惩罚：鼓励尽快起身 + time_penalty = RewTerm(func=mdp.is_alive, weight=-0.5) - # 3. 姿态奖励：上半身必须垂直向上 + # 3. 姿态奖：保持躯干垂直 upright = RewTerm(func=mdp.flat_orientation_l2, weight=2.0) - # 4. 平滑惩罚：防止起身时由于急躁导致的剧烈抖动 + # 4. 稳定性奖：站起来后不要乱晃 + root_static = RewTerm( + func=root_lin_vel_norm, # 使用上面定义的本地函数 + weight=-1.5, + params={"asset_cfg": SceneEntityCfg("robot")} + ) + + # 角速度稳定性 + root_ang_static = RewTerm( + func=root_ang_vel_norm, # 使用上面定义的本地函数 + weight=-0.5, + params={"asset_cfg": SceneEntityCfg("robot")} + ) + + # 5. 核心终点奖励：当满足 standing_success 终止条件时，给 500 分 + is_success = RewTerm( + func=get_success_reward, # 使用我们刚刚定义的本地函数 + weight=500.0, + params={"term_keys": "standing_success"} # 确保名字对应 TerminationsCfg 里的变量名 + ) + + # 6. 平滑惩罚 action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.01) joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-0.001) @configclass -class T1TerminationsCfg: - """终止条件""" - # 提前成功：如果站起来了并保持稳定，直接结束 Episode 并拿奖励，节省算力 - is_standing_success = DoneTerm( - func=mdp.root_height_below_minimum, - params={"minimum_height": 0.65}, +class T1GetUpTerminationsCfg: + """终止条件：站稳即算任务完成""" + # 失败：跌倒 + base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 1.0}) + + # 成功：满足高度和角度要求，且维持 1.0 秒 + standing_success = DoneTerm( + func=is_standing_still, + params={ + "minimum_height": 0.63, + "max_angle_error": 0.15, + "standing_time": 1.0 + } ) - time_out = DoneTerm(func=mdp.time_out) @configclass class T1EnvCfg(ManagerBasedRLEnvCfg): - """主环境配置：针对 T1 起身任务""" + """主环境配置""" scene = T1SceneCfg(num_envs=16384, env_spacing=2.5) - # 覆盖初始位置：必须低高度生成，防止随机旋转后高空坠落 def __post_init__(self): super().__post_init__() + # 初始高度设低，配合随机旋转事件实现“从地上爬起来” self.scene.robot.init_state.pos = (0.0, 0.0, 0.2) observations = T1ObservationCfg() rewards = T1GetUpRewardCfg() - terminations = T1TerminationsCfg() - events = T1EventCfg() # ⬅️ 挂载随机姿态事件 - + terminations = T1GetUpTerminationsCfg() + events = T1EventCfg() actions = T1ActionCfg() - # 训练参数 - episode_length_s = 5.0 # 强制 5秒内必须站起来 - decimation = 4 # 提高控制频率 (约 50Hz) 有助于精细动作 \ No newline at end of file + episode_length_s = 5.0 # 5秒强制重置 + decimation = 4 # 控制频率 \ No newline at end of file