Add weighting function, change the reward logic

rl_game/get_up/config/ppo_cfg.yaml

@@ -39,7 +39,7 @@ params:
     reward_shaper:
       scale_value: 1.0
     normalize_advantage: True
-    gamma: 0.96
+    gamma: 0.98
     tau: 0.95
     learning_rate: 3e-4
     lr_schedule: adaptive

rl_game/get_up/config/t1_env_cfg.py

@@ -26,87 +26,98 @@ def standing_with_feet_reward(
         force_threshold: float = 20.0,
         max_v_z: float = 0.5
 ) -> torch.Tensor:
-    # 增加防护：从场景中安全获取 body 索引
+    """终极高度目标：头高、盆骨高、足部受力稳定"""
     head_idx, _ = env.scene["robot"].find_bodies("H2")
     pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
 
-    # 1. 高度奖励：使用更稳定的归一化，限制范围在 [0, 1]
     curr_head_h = env.scene["robot"].data.body_state_w[:, head_idx[0], 2]
     curr_pelvis_h = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
 
-    # 使用 sigmoid 或简单的 min-max 映射，避免除以极小值
+    # 归一化高度评分
     head_score = torch.clamp(curr_head_h / min_head_height, 0.0, 1.2)
     pelvis_score = torch.clamp(curr_pelvis_h / min_pelvis_height, 0.0, 1.2)
     height_reward = (head_score + pelvis_score) / 2.0
 
-    # 2. 足部受力：增加对 NaN 的防御
+    # 足部受力判定
     contact_sensor = env.scene.sensors.get(sensor_cfg.name)
-    # 某些步数传感器可能未初始化，加个判空
     if contact_sensor is None: return torch.zeros(env.num_envs, device=env.device)
 
     foot_forces_z = torch.sum(contact_sensor.data.net_forces_w[:, :, 2], dim=-1)
-    # 对巨大的冲击力做剪裁，防止 sigmoid 输入过大
-    foot_forces_z = torch.clamp(foot_forces_z, 0.0, 500.0)
     force_weight = torch.sigmoid((foot_forces_z - force_threshold) / 5.0)
 
-    # 3. 垂直速度惩罚：使用更平滑的惩罚
+    # 垂直速度惩罚（防止跳跃不稳）
     root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2]
     vel_penalty = torch.exp(-torch.abs(root_vel_z) / max_v_z)
 
-    # 逻辑组合：高度 * 稳定性
     return height_reward * (0.5 + 0.5 * force_weight * vel_penalty)
 
 
-def universal_arm_support_reward(
+def arm_tuck_incremental_reward(
         env: ManagerBasedRLEnv,
-        sensor_cfg: SceneEntityCfg,
+        pitch_threshold: float = 1.4,
-        height_threshold: float = 0.60,
+        shaping_weight: float = 0.2
-        min_force: float = 15.0
 ) -> torch.Tensor:
+    """增量式收手奖励：鼓励向弯曲方向运动，达到阈值给大奖"""
+    joint_names = ["Left_Elbow_Pitch", "Right_Elbow_Pitch"]
+    joint_ids, _ = env.scene["robot"].find_joints(joint_names)
+
+    elbow_pos = env.scene["robot"].data.joint_pos[:, joint_ids]
+    elbow_vel = env.scene["robot"].data.joint_vel[:, joint_ids]
+
+    # 1. 速度引导：只要在收缩（速度为正）就给小奖，伸直则惩罚
+    avg_vel = torch.mean(elbow_vel, dim=-1)
+    shaping_reward = torch.tanh(avg_vel) * shaping_weight
+
+    # 2. 阈值触发：一旦收缩到位，给稳定的静态奖
+    is_tucked = torch.all(elbow_pos > pitch_threshold, dim=-1).float()
+    goal_bonus = is_tucked * 1.5
+
+    return shaping_reward + goal_bonus
+
+
+def dynamic_getup_strategy_reward(env: ManagerBasedRLEnv) -> torch.Tensor:
     """
-    通用手臂支撑奖励：同时支持仰卧起坐支撑和俯卧撑起。
+    状态机奖励切换逻辑：
-    逻辑：只要手臂有向上的推力，且身体正在向上移动，就给奖。
+    - 仰卧时：重点是 翻身 + 缩手。
+    - 俯卧时：重点是 撑地起立。
     """
-    # 1. 获取传感器数据
+    # 获取重力投影：Z轴分量 > 0 表示仰卧
-    contact_sensor = env.scene.sensors.get(sensor_cfg.name)
+    gravity_z = env.scene["robot"].data.projected_gravity_b[:, 2]
-    if contact_sensor is None:
-        return torch.zeros(env.num_envs, device=env.device)
 
-    # 获取所有定义的手臂/手部 link 的垂直总受力 (World Z)
+    # 状态掩码
-    # net_forces_w 形状: (num_envs, num_bodies, 3)
+    is_on_back = (gravity_z > 0.2).float()
-    arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
+    is_on_belly = (gravity_z < -0.2).float()
-    # 取所有受力点的最大值或平均值，代表支撑强度
+    is_transition = (1.0 - is_on_back - is_on_belly)
-    max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
 
-    # 2. 获取状态数据
+    # 1. 翻身势能：引导 gravity_z 向 -1.0 靠拢
-    pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
+    flip_shaping = torch.clamp(-gravity_z, min=-1.0, max=1.0)
-    pelvis_pos_z = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
-    root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2]
 
-    # 3. 计算奖励项
+    # 2. 缩手动作
-    # A. 受力奖励：鼓励手部与地面产生大于 min_force 的推力
+    tuck_rew = arm_tuck_incremental_reward(env)
-    # 使用 tanh 归一化，防止力矩过大导致奖励爆炸 (NaN 风险)
-    force_reward = torch.tanh(torch.clamp(max_arm_force - min_force, min=0.0) / 50.0)
 
-    # B. 速度引导：只有当机器人正在“向上起”时，支撑奖励才翻倍
+    # 3. 撑地动作 (复用原逻辑，但去掉内部的高度衰减，统一由状态机控制)
-    # 这样可以防止它趴在地上乱按手骗分
+    contact_sensor = env.scene.sensors.get("contact_sensor")
-    velocity_factor = torch.clamp(root_vel_z, min=0.0, max=2.0)
+    max_arm_force = torch.zeros(env.num_envs, device=env.device)
+    if contact_sensor is not None:
+        # 假设手臂/手部 link 的受力
+        arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
+        max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
 
-    # C. 姿态惩罚回避：
+    push_rew = torch.tanh(torch.clamp(max_arm_force - 15.0, min=0.0) / 40.0)
-    # 不再检查手是否在盆骨下方，而是检查手是否“在干活”
-    # 只要受力足够大，就认为是在支撑
-    is_supporting = (max_arm_force > min_force).float()
 
-    # 4. 阶段性退出机制 (Curriculum)
+    # --- 权重动态合成 ---
-    # 当盆骨高度超过 height_threshold (0.6m) 时，奖励线性消失
+    # 仰卧区：翻身(8.0) + 缩手(4.0)
-    # 强迫机器人最终依靠腿部力量平衡，而不是一直扶着地
+    back_strategy = is_on_back * (8.0 * flip_shaping + 4.0 * tuck_rew)
-    height_fade = torch.clamp((height_threshold - pelvis_pos_z) / 0.15, min=0.0, max=1.0)
 
-    # 最终组合
+    # 俯卧区：撑地(25.0) + 缩手维持(1.0)
-    # 逻辑：受力 * (1 + 垂直速度) * 高度衰减
+    # 这里撑地权重远高于翻身，确保机器人更愿意待在俯卧区尝试站立
-    total_reward = force_reward * (1.0 + 2.0 * velocity_factor) * is_supporting * height_fade
+    belly_strategy = is_on_belly * (25.0 * push_rew + 1.0 * tuck_rew)
+
+    # 过渡区
+    trans_strategy = is_transition * (4.0 * flip_shaping + 10.0 * push_rew + 2.0 * tuck_rew)
+
+    return back_strategy + belly_strategy + trans_strategy
 
-    return total_reward
 
 def is_standing_still(
         env: ManagerBasedRLEnv,
@@ -126,7 +137,6 @@ def is_standing_still(
     gravity_error = torch.norm(env.scene["robot"].data.projected_gravity_b[:, :2], dim=-1)
     root_vel_norm = torch.norm(env.scene["robot"].data.root_lin_vel_w, dim=-1)
 
-    # 判定条件：头够高 且 盆骨够高 且 垂直误差小 且 速度低
     is_stable_now = (
             (current_head_h > min_head_height) &
             (current_pelvis_h > min_pelvis_height) &
@@ -143,21 +153,17 @@ def is_standing_still(
 
     return env.extras["stable_timer"] > standing_time
 
+
 # --- 2. 配置类 ---
 
 T1_JOINT_NAMES = [
-
     'AAHead_yaw', 'Head_pitch',
-                
     'Left_Shoulder_Pitch', 'Left_Shoulder_Roll', 'Left_Elbow_Pitch', 'Left_Elbow_Yaw',
     'Right_Shoulder_Pitch', 'Right_Shoulder_Roll', 'Right_Elbow_Pitch', 'Right_Elbow_Yaw',
-
     'Waist',
-    
     'Left_Hip_Pitch', 'Right_Hip_Pitch', 'Left_Hip_Roll', 'Right_Hip_Roll',
     'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch',
     'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll'
-
 ]
 
 
@@ -186,14 +192,13 @@ class T1EventCfg:
         params={
             "asset_cfg": SceneEntityCfg("robot"),
             "pose_range": {
-                "roll": (-1.57, 1.57),   # 左右侧卧
+                "roll": (-1.57, 1.57),
                 "pitch": tuple(numpy.array([1.4, 1.6], dtype=np.float32) * random.choice([-1 , 1])),  # 仰卧/俯卧
-                "yaw": (-3.14, 3.14), # 全向旋转
+                "yaw": (-3.14, 3.14),
                 "x": (0.0, 0.0),
                 "y": (0.0, 0.0),
                 "z": (0.3, 0.4),
             },
-            "velocity_range": {},
         },
         mode="reset",
     )
@@ -201,61 +206,41 @@ class T1EventCfg:
 
 @configclass
 class T1ActionCfg:
-    """关键修改：降低 scale 让动作变丝滑，增大阻尼效果"""
     joint_pos = JointPositionActionCfg(
-        asset_name="robot",
+        asset_name="robot", joint_names=T1_JOINT_NAMES, scale=0.5, use_default_offset=True
-        joint_names=T1_JOINT_NAMES,
-        scale=0.5,
-        use_default_offset=True
     )
 
 
 @configclass
 class T1GetUpRewardCfg:
-    # 1. 姿态基础奖 (引导身体变正)
+    # --- 1. 动态策略整合奖励 (包含了翻身、缩手、撑地的逻辑切换) ---
-    upright = RewTerm(func=mdp.flat_orientation_l2, weight=2.0)
+    adaptive_strategy = RewTerm(
+        func=dynamic_getup_strategy_reward,
+        weight=1.0  # 内部已经有细分权重
+    )
 
-    # 2. 【条件高度奖】：双高度判定（头+盆骨），且必须脚踩地
+    # --- 2. 核心高度目标 (维持最高优先级) ---
     height_with_feet = RewTerm(
         func=standing_with_feet_reward,
-        weight=20.0,  # 作为核心引导，增加权重
+        weight=15.0,
         params={
-            "min_head_height": 1.10,
+            "min_head_height": 1.1,
             "min_pelvis_height": 0.7,
             "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_foot_link"]),
-            "force_threshold": 20.0,
+            "force_threshold": 30.0,
             "max_v_z": 0.3
         }
     )
 
-    # 3. 手臂撑地奖：辅助脱离地面阶段
+    # --- 3. 辅助约束与惩罚 ---
-    arm_push_support = RewTerm(
+    upright = RewTerm(func=mdp.flat_orientation_l2, weight=1.0)
-        func=universal_arm_support_reward,
+    joint_limits = RewTerm(func=mdp.joint_pos_limits, weight=-20.0, params={"asset_cfg": SceneEntityCfg("robot")})
-        weight=15.0,  # 显著增加权重（从 3.0 提到 15.0），让它成为起步的关键
+    action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.01)
-        params={
-            "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_hand_link", "AL3", "AR3"]),
-            "height_threshold": 0.65,  # 躯干升到 0.6m 前都鼓励手臂用力
-            "min_force": 8.0  # 只要有 15N 的力就触发
-        }
-    )
 
-    # 4. 关节限位惩罚 (新增：防止关节撞死导致数值问题)
+    # --- 4. 成功奖励 ---
-    joint_limits = RewTerm(
+    is_success_bonus = RewTerm(
-        func=mdp.joint_pos_limits,
-        weight=-1.0,
-        params={"asset_cfg": SceneEntityCfg("robot")}
-    )
-
-    # 5. 时间惩罚 (强制效率)
-    time_penalty = RewTerm(
-        func=mdp.is_alive,
-        weight=-1.2
-    )
-
-    # 6. 成功终极大奖
-    is_success = RewTerm(
         func=is_standing_still,
-        weight=800.0,
+        weight=1000.0,
         params={
             "min_head_height": 1.05,
             "min_pelvis_height": 0.75,
@@ -268,11 +253,6 @@ class T1GetUpRewardCfg:
 @configclass
 class T1GetUpTerminationsCfg:
     time_out = DoneTerm(func=mdp.time_out)
-
-    # 失败判定：躯干倾斜超过 45 度重置
-    #base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 0.785})
-
-    # 成功判定：双高度 + 稳定
     standing_success = DoneTerm(
         func=is_standing_still,
         params={
@@ -287,7 +267,7 @@ class T1GetUpTerminationsCfg:
 
 @configclass
 class T1EnvCfg(ManagerBasedRLEnvCfg):
-    scene = T1SceneCfg(num_envs=8192, env_spacing=2.5)  # 5090 性能全开
+    scene = T1SceneCfg(num_envs=8192, env_spacing=2.5)
 
     def __post_init__(self):
         super().__post_init__()

rl_game/get_up/env/t1_env.py

@@ -51,10 +51,10 @@ class T1SceneCfg(InteractiveSceneCfg):
         actuators={
             "t1_joints": ImplicitActuatorCfg(
                 joint_names_expr=[".*"],
-                effort_limit=400.0,
+                effort_limit=800.0,  # 翻倍，确保电机有力气
                 velocity_limit=20.0,
-                stiffness=150.0,
+                stiffness=500.0,  # 【关键】从 150 提到 500-800 之间
-                damping=5.0,
+                damping=40.0,  # 【关键】从 5 提到 30-50 之间，抑制乱抖
             ),
         },
     )

rl_game/get_up/train.py

@@ -9,7 +9,7 @@ from isaaclab.app import AppLauncher
 
 # 1. 配置启动参数
 parser = argparse.ArgumentParser(description="Train T1 robot to Get-Up with RL-Games.")
-parser.add_argument("--num_envs", type=int, default=16384, help="起身任务建议并行 4096 即可")
+parser.add_argument("--num_envs", type=int, default=8192, help="起身任务建议并行 4096 即可")
 parser.add_argument("--task", type=str, default="Isaac-T1-GetUp-v0", help="任务 ID")
 parser.add_argument("--seed", type=int, default=42, help="随机种子")
 AppLauncher.add_app_launcher_args(parser)