change T1EventCfg to add more initial state

rl_game/get_up/config/t1_env_cfg.py

@@ -10,6 +10,7 @@ from isaaclab.envs.mdp import JointPositionActionCfg
 from isaaclab.managers import SceneEntityCfg
 from isaaclab.utils import configclass
 from rl_game.get_up.env.t1_env import T1SceneCfg
+import isaaclab.utils.math as math_utils
 
 import isaaclab.envs.mdp as mdp
 
@@ -61,36 +62,47 @@ def standing_with_feet_reward(
 
     return torch.where(current_pelvis_h < transition_h, low_height_reward, high_height_reward)
 
+
 def arm_push_up_reward(
         env: ManagerBasedRLEnv,
         sensor_cfg: SceneEntityCfg,
-        height_threshold: float = 0.5
+        height_threshold: float = 0.5,
+        min_force: float = 20.0
 ) -> torch.Tensor:
     """
-    手臂撑地奖励 + 躯干高度线性引导。
-    引导机器人从完全平躺状态通过手臂受力将躯干撑起。
+    强化版手臂撑地奖励：
+    1. 鼓励手臂产生超过阈值的垂直反作用力。
+    2. 当手臂用力且躯干有向上速度时，给予额外加成。
     """
-    # 获取传感器受力
+    # 获取手臂传感器数据
     contact_sensor = env.scene.sensors.get(sensor_cfg.name)
-    if contact_sensor is None: return torch.zeros(env.num_envs, device=env.device)
+    if contact_sensor is None:
+        return torch.zeros(env.num_envs, device=env.device)
 
-    # 获取躯干(Pelvis/Trunk)当前高度
-    pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
-    current_height = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
-
-    # 1. 手臂受力奖：只有在躯干较低时，鼓励手臂产生向上的反作用力
+    # 1. 获取手臂 Z 轴受力 (取所有手臂 Body 的合力或最大力)
     arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
     max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
-    push_reward = torch.clamp(max_arm_force / 50.0, max=1.0)  # 归一化受力奖励
 
-    # 2. 躯干高度线性引导：只要在 height_threshold 以下，越高分越高
-    # 这解决了“动不动奖励都一样”的问题
-    lifting_reward = torch.clamp(current_height / height_threshold, max=1.0)
+    # 归一化受力奖励：在 20N 到 100N 之间线性增长
+    force_reward = torch.clamp((max_arm_force - min_force) / 80.0, min=0.0, max=1.0)
 
-    # 只有在还没站起来（低于阈值）的时候，才给这两个组合奖励
+    # 2. 获取躯干高度和垂直速度
+    pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
+    current_height = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
+    root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2]
+
+    # 3. 协同奖励：当手臂在用力推，且躯干正在上升时，给高分
+    # 只有在高度低于阈值（还在撑起阶段）时生效
+    pushing_up_bonus = torch.where(
+        (max_arm_force > min_force) & (root_vel_z > 0.05),
+        force_reward * (1.0 + root_vel_z * 2.0),  # 速度越快奖励越高
+        force_reward
+    )
+
+    # 只有在躯干较低时才发放此奖励
     return torch.where(current_height < height_threshold,
-                       push_reward + lifting_reward,
-                       torch.zeros_like(push_reward))
+                       pushing_up_bonus,
+                       torch.zeros_like(pushing_up_bonus))
 
 def is_standing_still(
         env: ManagerBasedRLEnv,
@@ -172,6 +184,51 @@ def strict_feet_contact_reward(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCf
 
     return (~all_feet_cond).float()  # 返回1表示违规
 
+
+def reset_root_state_symmetric(
+        env: ManagerBasedRLEnv,
+        env_ids: torch.Tensor,
+        asset_cfg: SceneEntityCfg,
+        pose_range: dict,
+        velocity_range: dict
+):
+    """随机对称采样：让机器人随机以 趴/躺/左倾/右倾 及其组合姿态重置"""
+    robot = env.scene[asset_cfg.name]
+    num_resets = len(env_ids)
+    device = env.device
+
+    # 1. 采样 Euler 角 (roll, pitch, yaw)
+    # 我们先在 positive range 内采样
+    def get_rand(key):
+        low, high = pose_range[key]
+        return (high - low) * torch.rand(num_resets, device=device) + low
+
+    roll = get_rand("roll")
+    pitch = get_rand("pitch")
+    yaw = (pose_range["yaw"][1] - pose_range["yaw"][0]) * torch.rand(num_resets, device=device) + pose_range["yaw"][0]
+
+    # 2. 核心：随机符号化 (Random Sign)
+    # 50% 概率保持正，50% 概率变负。Roll 和 Pitch 独立随机。
+    roll_sign = (torch.randint(0, 2, (num_resets,), device=device) * 2 - 1).float()
+    pitch_sign = (torch.randint(0, 2, (num_resets,), device=device) * 2 - 1).float()
+
+    roll *= roll_sign
+    pitch *= pitch_sign
+
+    # 3. 应用到状态
+    quat = math_utils.quat_from_euler_xyz(roll, pitch, yaw)
+
+    # 获取默认状态并叠加随机位置偏移 (x, y, z)
+    root_states = robot.data.default_root_state[env_ids].clone()
+    for i, key in enumerate(["x", "y", "z"]):
+        low, high = pose_range[key]
+        root_states[:, i] += (high - low) * torch.rand(num_resets, device=device) + low
+
+    root_states[:, 3:7] = quat
+
+    # 4. 写入仿真 (同时清除速度，确保是静态开始)
+    robot.write_root_state_to_sim(root_states, env_ids)
+
 # --- 2. 配置类 ---
 
 T1_JOINT_NAMES = [
@@ -200,18 +257,17 @@ class T1ObservationCfg:
 
 @configclass
 class T1EventCfg:
-    """随机初始化：支持趴、躺、侧身"""
     reset_robot_rotation = EventTerm(
-        func=mdp.reset_root_state_uniform,
+        func=reset_root_state_symmetric, # 使用上面那个通用的函数
         params={
             "asset_cfg": SceneEntityCfg("robot"),
             "pose_range": {
-                "roll": (-0.2, 0.2),#(-1.57, 1.57),
-                "pitch": (-1.6, -1.4),#(-1.57, 1.57),
-                "yaw": (0, 0),#(-3.14, 3.14),
+                "roll": (0, 1.57),   # 左右侧卧
+                "pitch": (1.4, 1.6),  # 仰卧/俯卧
+                "yaw": (-3.14, 3.14), # 全向旋转
                 "x": (0.0, 0.0),
                 "y": (0.0, 0.0),
-                "z": (0.0, 0.0),
+                "z": (0.4, 0.5),
             },
             "velocity_range": {},
         },
@@ -238,7 +294,7 @@ class T1GetUpRewardCfg:
     # 2. 【条件高度奖】：双高度判定（头+盆骨），且必须脚踩地
     height_with_feet = RewTerm(
         func=standing_with_feet_reward,
-        weight=5.0,  # 作为核心引导，增加权重
+        weight=30.0,  # 作为核心引导，增加权重
         params={
             "min_head_height": 1.10,
             "min_pelvis_height": 0.65,
@@ -250,17 +306,21 @@ class T1GetUpRewardCfg:
     # 3. 手臂撑地奖：辅助脱离地面阶段
     arm_push_support = RewTerm(
         func=arm_push_up_reward,
-        weight=3.0,
-        params={"sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_hand_link"])}
+        weight=15.0,  # 显著增加权重（从 3.0 提到 15.0），让它成为起步的关键
+        params={
+            "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_hand_link", "AL3", "AR3"]),
+            "height_threshold": 0.6,  # 躯干升到 0.6m 前都鼓励手臂用力
+            "min_force": 15.0  # 只要有 15N 的力就触发
+        }
     )
 
     # 4. 惩罚项
     undesired_contacts = RewTerm(
         func=mdp.undesired_contacts,
-        weight=-5.0,
+        weight=-10.0,
         params={
-            "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=["H2", "Trunk", ".*_Left", ".*_Right"]),
-            # 注意：这里我们排除了脚部，惩罚大腿/小腿/躯干着地
+            "sensor_cfg": SceneEntityCfg("contact_sensor", body_names=["H2", "Trunk"]),
+            # 注意：此处必须排除手臂相关 link，否则手臂用力时会同时被扣分
             "threshold": 1.0
         }
     )

rl_game/get_up/env/t1_env.py

@@ -12,12 +12,20 @@ T1_USD_PATH = os.path.join(_DEMO_DIR, "asset", "t1", "t1_locomotion_physics.usd"
 
 @configclass
 class T1SceneCfg(InteractiveSceneCfg):
-    """修改后的 T1 机器人场景配置，适配随机姿态起身任务"""
+    """最终修正版：彻底解决 Unknown asset config type 报错"""
 
-    # 1. 地面配置 (如果之前没定义，必须加上)
+    # 1. 地面配置：直接在 spawn 内部定义材质
     ground = AssetBaseCfg(
         prim_path="/World/ground",
-        spawn=sim_utils.GroundPlaneCfg(),
+        spawn=sim_utils.GroundPlaneCfg(
+            physics_material=sim_utils.RigidBodyMaterialCfg(
+                static_friction=1.0,
+                dynamic_friction=1.0,
+                restitution=0.3,
+                friction_combine_mode="average",
+                restitution_combine_mode="average",
+            )
+        ),
     )
 
     # 2. 机器人配置
@@ -28,28 +36,25 @@ class T1SceneCfg(InteractiveSceneCfg):
             activate_contact_sensors=True,
             rigid_props=sim_utils.RigidBodyPropertiesCfg(
                 disable_gravity=False,
-                max_depenetration_velocity=10.0, # ⬅️ 提高穿透修正速度，防止肢体卡在地下
+                max_depenetration_velocity=10.0,
             ),
             articulation_props=sim_utils.ArticulationRootPropertiesCfg(
-                enabled_self_collisions=True,    # 开启自碰撞，起身时肢体可能互碰
-                solver_position_iteration_count=8, # ⬅️ 增加迭代次数，提高物理稳定性
+                enabled_self_collisions=True,
+                solver_position_iteration_count=8,
                 solver_velocity_iteration_count=4,
             ),
         ),
         init_state=ArticulationCfg.InitialStateCfg(
-            # ⬅️ 核心修改：高度降低。因为是躺着生成，0.2m 比较合适
-            pos=(0.0, 0.0, 0.3),
-            # 默认旋转设为单位阵，具体的随机化由 Event 管理器处理
-            rot=(1.0, 0.0, 0.0, 0.0),
+            pos=(0.0, 0.0, 0.4), # 掉落高度
             joint_pos={".*": 0.0},
         ),
         actuators={
-            "legs": ImplicitActuatorCfg(
+            "t1_joints": ImplicitActuatorCfg(
                 joint_names_expr=[".*"],
-                effort_limit=400.0, # T1 通常有较强的力矩输出
-                velocity_limit=20.0, # ⬅️ 调高速度限制，起身需要爆发动作
-                stiffness=150.0,     # ⬅️ 调高刚度：起身需要更“硬”的支撑
-                damping=5.0,         # ⬅️ 增加阻尼：防止机器人站起瞬间晃动太厉害
+                effort_limit=400.0,
+                velocity_limit=20.0,
+                stiffness=150.0,
+                damping=5.0,
             ),
         },
     )