from isaaclab.assets import ArticulationCfg, AssetBaseCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab import sim as sim_utils

import os

_DEMO_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
T1_USD_PATH = os.path.join(_DEMO_DIR, "asset", "t1", "t1_locomotion_physics.usd")

@configclass
class T1SceneCfg(InteractiveSceneCfg):
    """修改后的 T1 机器人场景配置，适配随机姿态起身任务"""

    # 1. 地面配置 (如果之前没定义，必须加上)
    ground = AssetBaseCfg(
        prim_path="/World/ground",
        spawn=sim_utils.GroundPlaneCfg(),
    )

    # 2. 机器人配置
    robot = ArticulationCfg(
        prim_path="{ENV_REGEX_NS}/Robot",
        spawn=sim_utils.UsdFileCfg(
            usd_path=T1_USD_PATH,
            activate_contact_sensors=True,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False,
                max_depenetration_velocity=10.0, # ⬅️ 提高穿透修正速度，防止肢体卡在地下
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                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.2),
            # 默认旋转设为单位阵，具体的随机化由 Event 管理器处理
            rot=(1.0, 0.0, 0.0, 0.0),
            joint_pos={".*": 0.0},
        ),
        actuators={
            "legs": ImplicitActuatorCfg(
                joint_names_expr=[".*"],
                effort_limit=400.0, # T1 通常有较强的力矩输出
                velocity_limit=20.0, # ⬅️ 调高速度限制，起身需要爆发动作
                stiffness=150.0,     # ⬅️ 调高刚度：起身需要更“硬”的支撑
                damping=5.0,         # ⬅️ 增加阻尼：防止机器人站起瞬间晃动太厉害
            ),
        },
    )

    # 3. 光照配置
    light = AssetBaseCfg(
        prim_path="/World/light",
        spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0),
    )