import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
from isaaclab.sim.schemas.schemas_cfg import (
    RigidBodyPropertiesCfg,
    CollisionPropertiesCfg,
)
from mindbot.utils.assets import MINDBOT_ASSETS_DIR
import math

AUTOSAMPLER_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=f"{MINDBOT_ASSETS_DIR}/devices/Autosampler/Equipment_AA_01.usda",
        scale=(0.8,0.8,0.8),
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            max_depenetration_velocity=1.0
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            fix_root_link=True,  # 由 USD 内的 FixedJoint_ 负责固定底座
            enabled_self_collisions=False,
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=16,
            stabilization_threshold=1e-6,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=[1.95, -0.45, 0.9],
        rot=[-0.7071, 0.0, 0.0, 0.7071],
        joint_pos={
            "RevoluteJoint": math.radians(0.0),  # -100°，盖子打开位置
            # "PrismaticJoint_0":math.radians(0.0),
            # "PrismaticJoint_1":math.radians(0.0),
        },
    ),
    actuators={
        "lid_open": ImplicitActuatorCfg(
            joint_names_expr=['RevoluteJoint'],
            stiffness=0,   # 覆盖 USD 的 100，足够对抗重力保持开盖
            damping=5,      # 阻尼同步加大，防止振荡
            effort_limit_sim=1000.0,
            velocity_limit_sim=100.0,
        ),
    },
)