Add reward to maintain an upright and stable position

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),  # 侧躺随机
+                "roll": (-1.57, 1.57),
-                "pitch": (-1.57, 1.57),  # 趴下/背躺随机
+                "pitch": (-1.57, 1.57),
-                "yaw": (-3.14, 3.14),  # 航向角随机
+                "yaw": (-3.14, 3.14),
-                "x": (0.0, 0.0),  # 位置不随机（保持在原点）
+                "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. 进度引导：越高分越高
-    # 1. 核心进度：高度奖励
-    # 只要低于目标高度(0.6m)，就会持续根据距离给负分，越高分数越接近0
     height_progress = RewTerm(
         func=mdp.root_height_below_minimum,
         weight=15.0,
         params={"minimum_height": 0.65}
     )
 
-    # 2. 速度惩罚：时间就是金钱
+    # 2. 时间惩罚：鼓励尽快起身
-    # 每一帧都扣 0.5 分，起身越慢，被扣掉的总分就越多
+    time_penalty = RewTerm(func=mdp.is_alive, weight=-0.5)
-    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:
+class T1GetUpTerminationsCfg:
-    """终止条件"""
+    """终止条件：站稳即算任务完成"""
-    # 提前成功：如果站起来了并保持稳定，直接结束 Episode 并拿奖励，节省算力
+    # 失败：跌倒
-    is_standing_success = DoneTerm(
+    base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 1.0})
-        func=mdp.root_height_below_minimum,
+
-        params={"minimum_height": 0.65},
+    # 成功：满足高度和角度要求，且维持 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()
+    terminations = T1GetUpTerminationsCfg()
-    events = T1EventCfg()  # ⬅️ 挂载随机姿态事件
+    events = T1EventCfg()
-
     actions = T1ActionCfg()
 
-    # 训练参数
+    episode_length_s = 5.0  # 5秒强制重置
-    episode_length_s = 5.0  # 强制 5秒内必须站起来
+    decimation = 4  # 控制频率
-    decimation = 4  # 提高控制频率 (约 50Hz) 有助于精细动作