change rewards

rl_game/get_up/config/t1_env_cfg.py

@@ -14,41 +14,42 @@ from rl_game.get_up.env.t1_env import T1SceneCfg
 import isaaclab.envs.mdp as mdp
 
 
-# --- 1. 自定义 MDP 逻辑函数 (放在配置类之前) ---
+# --- 1. 修正后的自定义 MDP 逻辑函数 ---
 
 def is_standing_still(
         env: ManagerBasedRLEnv,
         minimum_height: float,
         max_angle_error: float,
-        standing_time: float
+        standing_time: float,
+        velocity_threshold: float = 0.2
 ) -> torch.Tensor:
     """
-    判定机器人是否稳定站立了一段时间。
+    判定判定逻辑：高度达标 + 躯干垂直 + 几乎静止 + 维持时间。
-    逻辑：高度达标且姿态垂直，持续时间超过 standing_time 则返回 True。
+    增加了速度判定，彻底杜绝起跳瞬间触发。
     """
-    # 获取当前状态：高度 (Z轴) 和 投影重力 (前两个分量越小越垂直)
+    # 1. 获取 Body 索引
-
-    # 1. 首先获取 H2 (头部) 的索引 (建议在环境初始化时获取一次，或者如下所示获取)
-    # find_bodies 返回 (indices, names)
     head_idx, _ = env.scene["robot"].find_bodies("H2")
 
-    # 2. 修改后的位置获取逻辑
+    # 2. 状态量获取
-    # data.body_link_pos_w 的维度是 (num_envs, num_bodies, 3)
+    current_head_height = env.scene["robot"].data.body_state_w[:, head_idx[0], 2]
-    # 我们取所有环境 (:)，对应的头部索引 (head_idx[0])，以及 Z 轴坐标 (2)
+    # 投影重力误差（越小越垂直）
-    current_head_height = env.scene["robot"].data.body_link_pos_w[:, head_idx[0], 2]
-
-    # 3. 姿态判定保持不变（通常依然以躯干 Trunk 的垂直度为准，因为头部可能会摆动）
     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)
+    root_ang_vel_norm = torch.norm(env.scene["robot"].data.root_ang_vel_w, dim=-1)
 
-    # 4. 更新判断逻辑
+    # 3. 综合判定（这里不强制检查力，改用更稳健的速度限制）
-    is_stable_now = (current_head_height > minimum_height) & (gravity_error < max_angle_error)
+    is_stable_now = (
+            (current_head_height > minimum_height) &
+            (gravity_error < max_angle_error) &
+            (root_vel_norm < velocity_threshold) &
+            (root_ang_vel_norm < velocity_threshold * 2.0)
+    )
 
-    # 在 env.extras 中维护一个计时器
+    # 4. 计时器
     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,
@@ -56,60 +57,53 @@ def is_standing_still(
         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_vel_z_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor:
-    """专门惩罚 Z 轴方向的速度"""
+    # 严厉惩罚 Z 轴正向速度（向上窜）
-    # 获取根部速度 (num_envs, 3) -> [vx, vy, vz]
+    vel_z = env.scene[asset_cfg.name].data.root_lin_vel_w[:, 2]
-    vel = env.scene[asset_cfg.name].data.root_lin_vel_w
+    return torch.square(torch.clamp(vel_z, min=0.0))
-    # 只取 Z 轴：vel[:, 2]
-    return torch.square(vel[:, 2])
 
-def joint_torques_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor:
-    """计算机器人所有关节施加扭矩的平方和"""
-    # 从 data.applied_torques 获取数据，通常形状为 (num_envs, num_joints)
-    torques = env.scene[asset_cfg.name].data.applied_torque
-    return torch.sum(torch.square(torques), dim=-1)
 
-def joint_vel_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor:
+def feet_airtime_penalty_local(
-    """计算机器人所有关节速度的平方和"""
+        env: ManagerBasedRLEnv,
-    # 从 data.joint_vel 获取数据
+        sensor_cfg: SceneEntityCfg,
-    vel = env.scene[asset_cfg.name].data.joint_vel
+        threshold: float = 1.0
-    return torch.sum(torch.square(vel), dim=-1)
+) -> torch.Tensor:
+    """
+    自定义滞空惩罚逻辑：
+    如果脚部的垂直合力小于阈值，说明脚离地了。
+    返回一个 Tensor，离地时为 1.0，着地时为 0.0。
+    """
+    # 1. 获取传感器对象
+    contact_sensor = env.scene.sensors.get(sensor_cfg.name)
 
-def joint_pos_limits_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor:
+    if contact_sensor is None:
-    """惩罚关节接近或超过限位"""
+        # 如果没搜到传感器，返回全 0，防止程序崩溃
-    # 获取关节位置 (num_envs, num_joints)
+        return torch.zeros(env.num_envs, device=env.device)
-    joint_pos = env.scene[asset_cfg.name].data.joint_pos
-    # 获取限位 (num_joints, 2) -> [lower, upper]
-    limits = env.scene[asset_cfg.name].data.soft_joint_pos_limits
-    lower_limits = limits[..., 0]
-    upper_limits = limits[..., 1]
 
-    # 计算超出限位的部分
+    # 2. 获取触地力 (num_envs, num_bodies_in_sensor, 3)
-    out_of_lower = torch.clamp(lower_limits - joint_pos, min=0.0)
+    # 我们取所有被监测 Body (左右脚) 的 Z 轴推力
-    out_of_upper = torch.clamp(joint_pos - upper_limits, min=0.0)
+    # 如果所有脚的力都小于 threshold，判定为“完全腾空”
+    foot_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
+    is_in_air = torch.all(foot_forces_z < threshold, dim=-1)
 
-    # 返回超出量的平方和
+    return is_in_air.float()
-    return torch.sum(torch.square(out_of_lower + out_of_upper), dim=-1)
 
 # --- 2. 配置类定义 ---
 
-## 1. 定义与你的类一致的关节列表 (按照 ROBOT_MOTORS 的顺序)
 T1_JOINT_NAMES = [
-    'Left_Hip_Pitch', 'Right_Hip_Pitch',
+    'Left_Hip_Pitch', 'Right_Hip_Pitch', 'Left_Hip_Roll', 'Right_Hip_Roll',
-    'Left_Hip_Roll', 'Right_Hip_Roll',
+    'Left_Hip_Yaw', 'Right_Hip_Yaw', 'Left_Knee_Pitch', 'Right_Knee_Pitch',
-    'Left_Hip_Yaw', 'Right_Hip_Yaw',
+    'Left_Ankle_Pitch', 'Right_Ankle_Pitch', 'Left_Ankle_Roll', 'Right_Ankle_Roll'
-    'Left_Knee_Pitch', 'Right_Knee_Pitch',
-    'Left_Ankle_Pitch', 'Right_Ankle_Pitch',
-    'Left_Ankle_Roll', 'Right_Ankle_Roll'
 ]
 
+
 @configclass
 class T1ObservationCfg:
     """观察值空间配置：严格对应你的 Robot 基类数据结构"""
@@ -129,21 +123,10 @@ class T1ObservationCfg:
 
         # 3. 重力投影 (对应 global_orientation_euler/quat 相关的姿态感知)
         projected_gravity = ObsTerm(func=mdp.projected_gravity)
-
+        joint_pos = ObsTerm(func=mdp.joint_pos_rel,
-        # 4. 关节位置 (对应 motor_positions)
+                            params={"asset_cfg": SceneEntityCfg("robot", joint_names=T1_JOINT_NAMES)})
-        # 使用 joint_pos_rel 获取相对于默认姿态的偏差，显式指定关节顺序
+        joint_vel = ObsTerm(func=mdp.joint_vel_rel,
-        joint_pos = ObsTerm(
+                            params={"asset_cfg": SceneEntityCfg("robot", joint_names=T1_JOINT_NAMES)})
-            func=mdp.joint_pos_rel,
-            params={"asset_cfg": SceneEntityCfg("robot", joint_names=T1_JOINT_NAMES)}
-        )
-
-        # 5. 关节速度 (对应 motor_speeds)
-        joint_vel = ObsTerm(
-            func=mdp.joint_vel_rel,
-            params={"asset_cfg": SceneEntityCfg("robot", joint_names=T1_JOINT_NAMES)}
-        )
-
-        # 6. 上一次的动作 (对应 motor_targets)
         actions = ObsTerm(func=mdp.last_action)
 
     policy = PolicyCfg()
@@ -158,7 +141,7 @@ class T1EventCfg:
             "asset_cfg": SceneEntityCfg("robot"),
             "pose_range": {
                 "roll": (0, 0),#(-1.57, 1.57),
-                "pitch": (1.57, 1.57),#(-1.57, 1.57),
+                "pitch": (-1.57, 1.57),#(-1.57, 1.57),
                 "yaw": (0, 0),#(-3.14, 3.14),
                 "x": (0.0, 0.0),
                 "y": (0.0, 0.0),
@@ -172,124 +155,89 @@ class T1EventCfg:
 
 @configclass
 class T1ActionCfg:
-    """动作空间"""
+    """关键修改：降低 scale 让动作变丝滑，增大阻尼效果"""
     joint_pos = JointPositionActionCfg(
         asset_name="robot",
         joint_names=T1_JOINT_NAMES,
-        scale=0.5,
+        scale=0.2,  # 从 0.5 降到 0.2，防止电机暴力抽搐
         use_default_offset=True
     )
 
 
 @configclass
 class T1GetUpRewardCfg:
-    """优化后的奖励函数：抑制跳跃，引导稳健起身"""
+    # 1. 姿态奖 (核心引导)
+    upright = RewTerm(func=mdp.flat_orientation_l2, weight=5.0)
 
-    # 1. 高度引导 (改为平滑的指数奖励)
+    # 2. 只有脚着地时才给的高度奖（模拟逻辑）
-    # 相比 root_height_below_minimum，这个函数会让机器人越接近目标高度得分越高，且曲线平稳
+    # 修正：直接使用 root_height 配合强力的速度惩罚
     height_tracking = RewTerm(
-        func=mdp.root_height_below_minimum,  # 如果没有自定义函数，保留这个但调低权重
+        func=mdp.root_height_below_minimum,
-        weight=2.0,  # 降低权重，防止“弹射”
+        weight=2.0,
         params={
             "minimum_height": 1.05,
             "asset_cfg": SceneEntityCfg("robot", body_names="H2"),
         }
     )
 
-    # 2. 姿态奖 (保持不变，这是核心)
+    # 3. 抑制跳跃：严厉惩罚向上窜的速度
-    upright = RewTerm(func=mdp.flat_orientation_l2, weight=5.0)
-
-    # 3. 稳定性引导 (增加对速度的惩罚，抑制跳跃)
-    # 惩罚过大的垂直速度，防止“跳起”
     root_vel_z_penalty = RewTerm(
-        func=root_vel_z_l2_local,  # 使用本地函数
+        func=root_vel_z_l2_local,
-        weight=-5,
+        weight=-15.0,  # 增大负权重
-        params={"asset_cfg": SceneEntityCfg("robot")}  # 传入资产配置
+        params={"asset_cfg": SceneEntityCfg("robot")}
     )
 
-    feet_contact = RewTerm(
+    # 4. 抑制滞空 (Airtime Penalty)
-        func=mdp.contact_forces,
+    # 如果脚离开地面，按时间扣分
-        weight=0.5,
+    feet_airtime = RewTerm(
+        func=feet_airtime_penalty_local,
+        weight=-15.0,
         params={
             "sensor_cfg": SceneEntityCfg("feet_contact_sensor"),
-            "threshold": 1.0
+            "threshold": 0.2,  # 超过 0.2s 离地就开始扣分
         }
     )
 
-    # 4. 关节与能量约束 (防止 NaN 和乱跳的关键)
+    # 5. 动作平滑 (非常重要：解决视频中的高频抖动)
-    joint_vel = RewTerm(
+    action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1.0)
-        func=joint_vel_l2_local,
+    joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-0.1, params={"asset_cfg": SceneEntityCfg("robot")})
-        weight=-1,
-        params={"asset_cfg": SceneEntityCfg("robot")}
-    )
 
-    applied_torque = RewTerm(
+    # 6. 时间惩罚：逼迫它快点站稳
-        func=joint_torques_l2_local,
+    time_penalty = RewTerm(func=mdp.is_alive, weight=-0.5)
-        weight=-1.0e-2,
-        params={"asset_cfg": SceneEntityCfg("robot")}
-    )
 
-    # 5. 动作平滑 (非常重要)
+    # 7. 终极奖励
-    action_rate = RewTerm(
-        func=mdp.action_rate_l2,
-        weight=-1.0 # 增大权重，强制动作连贯
-    )
-
-    # 6. 软限位惩罚：防止关节撞击
-    joint_limits = RewTerm(
-        func=joint_pos_limits_l2_local,
-        weight=-10.0,
-        params = {"asset_cfg": SceneEntityCfg("robot")}
-    )
-
-    # 7. 时间惩罚 (强制效率)
-    # 每一帧都扣除固定分数，迫使机器人尽快达成 is_success 以停止扣分
-    time_penalty = RewTerm(
-        func=mdp.is_alive,
-        weight=-1.2
-    )
-
-    # 8. 核心终点奖励
     is_success = RewTerm(
         func=get_success_reward,
-        weight=500.0,  # 成功奖励可以给高点，但前提是动作要平稳
+        weight=200.0,  # 成功一次给大奖，但判定条件极严
         params={"term_keys": "standing_success"}
     )
 
 @configclass
 class T1GetUpTerminationsCfg:
-    """终止条件：站稳即算任务完成，且包含强制超时重置"""
-
-    # --- 关键：必须显式添加这一行，episode_length_s 才会生效 ---
     time_out = DoneTerm(func=mdp.time_out)
 
-    # 失败：跌倒 (Trunk 倾斜过大)
+    # 严格的失败判定：躯干倾斜超过 45 度就重置，不让它在地上滚
-    # limit_angle 是弧度，1.0 约等于 57度，如果想严格点可以调小
+    base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 0.785})
-    base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 1.0})
 
-    # 成功：满足“头部高度”和“姿态要求”，且维持 0.8 秒
+    # 严格的成功判定
     standing_success = DoneTerm(
-        func=is_standing_still, # 确保你已经把这个函数里的 current_height 改成了 H2 的 Z 轴
+        func=is_standing_still,
         params={
-            # T1 头部 (H2) 站直高度约 1.15-1.2m，设为 1.1m 比较稳健
+            "minimum_height": 1.05,  # H2 高度
-            "minimum_height": 1.05,
+            "max_angle_error": 0.15,  # 极小角度误差（约 8.6 度）
-            # 姿态误差 (投影重力分量)，0.15 约等于 8.6 度，要求很直
+            "standing_time": 0.8,  # 必须保持 0.8 秒（起跳不可能在空中停这么久）
-            "max_angle_error": 1.0,
+            "velocity_threshold": 0.15  # 速度必须极低
-            # 维持时间
-            "standing_time": 0.8
         }
     )
 
 
 @configclass
 class T1EnvCfg(ManagerBasedRLEnvCfg):
-    """主环境配置"""
+    scene = T1SceneCfg(num_envs=16384, env_spacing=2.5)  # 建议先用 4096 验证逻辑
-    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.4)  # 初始稍微抬高一点点，防止卡地
-        self.scene.robot.init_state.pos = (0.0, 0.0, 0.4)
 
     observations = T1ObservationCfg()
     rewards = T1GetUpRewardCfg()
@@ -297,5 +245,5 @@ class T1EnvCfg(ManagerBasedRLEnvCfg):
     events = T1EventCfg()
     actions = T1ActionCfg()
 
-    episode_length_s = 10.0  # 3秒强制重置
+    episode_length_s = 5.0  # 缩短时长，增加训练效率
-    decimation = 4  # 控制频率
+    decimation = 4