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 机器人的场景配置，包含地面、机器人和光照"""

    # T1 机器人配置
    # 注意：你需要将 usd_path 替换为你真实的 T1 机器人 USD 文件路径
    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,
                retain_accelerations=False,
                linear_damping=0.0,
                angular_damping=0.0,
                max_linear_velocity=1000.0,
                max_angular_velocity=1000.0,
                max_depenetration_velocity=1.0,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=True,
                solver_position_iteration_count=4,
                solver_velocity_iteration_count=0
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(0.0, 0.0, 0.7), # 初始高度，确保机器人双脚着地而非穿模
            joint_pos={".*": 0.0}, # 所有关节初始角度为 0
        ),
        actuators={
            "legs": ImplicitActuatorCfg(
                joint_names_expr=[".*"], # 匹配所有关节，也可以指定具体名称如 ["L_Hip.*", "R_Hip.*"]
                effort_limit=400.0,
                velocity_limit=10.0,
                stiffness=85.0, # P 增益
                damping=2.0,    # D 增益
            ),
        },
    )