# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

"""Configuration for a simple Cartpole robot."""


import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR

##
# Configuration
##

MINDBOT_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        # 1. UPDATE THE USD PATH to your local file
        usd_path="/home/ubuntu/50T/maic_usd_assets/robots/mindrobot-1105-lab/mindrobot_cd.usd",
        # usd_path="/home/ubuntu/50T/maic_usd_assets/robots/mindrobot-1105-lab/mindrobot.usd",
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            max_depenetration_velocity=1.0,
            linear_damping=0.5,
            angular_damping=0.5,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            fix_root_link=True,
            enabled_self_collisions=False,
            solver_position_iteration_count=16,
            solver_velocity_iteration_count=8,
            stabilization_threshold=1e-6,  # 忽略小穿透
        ),
        collision_props=sim_utils.CollisionPropertiesCfg(
            collision_enabled=True,
            contact_offset=0.0005, # 尝试从默认值增大到 0.01 或 0.02
            rest_offset=0,   # 保持略小于 contact_offset
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        # 2. DEFINE THE INITIAL POSITIONS FOR ALL 12 JOINTS
        # All are set to 0.0 here, but you should adjust them for a suitable "home" position.
        joint_pos={
            # Left arm joints
            "l_joint1": 0.0,
            # "l_joint2": -1.5708,
            "l_joint2": 0.0,
            "l_joint3": 0.0,
            "l_joint4": 0.0,
            "l_joint5": 0.0,
            "l_joint6": 0.0,
            # Right arm joints
            "r_joint1": 0.0,
            "r_joint2": -1.5708,
            # "r_joint2": 0.0,
            "r_joint3": 0.0,
            "r_joint4": 0.0,
            "r_joint5": 0.0,
            "r_joint6": 0.0,
            # left wheel
            "left_b_revolute_Joint": 0.0,
            "left_f_revolute_Joint": 0.0,
            # right wheel
            "right_b_revolute_Joint": 0.0,
            "right_f_revolute_Joint": 0.0,
            # left gripper
            # 注意：如果希望初始状态为完全打开，应该设置为 left=0.0, right=0.0
            # 当前设置为 right=0.01 表示略微闭合状态
            "left_hand_joint_left": 0.0,    # 范围：(-0.03, 0.0)，0.0=打开，-0.03=闭合
            "left_hand_joint_right": 0.0,  # 范围：(0.0, 0.03)，0.0=打开，0.03=闭合
            # right gripper
            "right_hand_joint_left": 0.0,
            "right_hand_joint_right": 0.0,
            # trunk
            "PrismaticJoint": 0.3,
            # head
            "head_revoluteJoint": 0.5236
        },
        # The initial (x, y, z) position of the robot's base in the world
        pos=(0, 0, 0.05),
    ),
    actuators={
        # 3. DEFINE ACTUATOR GROUPS FOR THE ARMS
        # Group for the 6 left arm joints using a regular expression
        "left_arm": ImplicitActuatorCfg(
            joint_names_expr=["l_joint[1-6]"], # This matches l_joint1, l_joint2, ..., l_joint6
            effort_limit_sim=100.0,            # Note: Tune this based on your robot's specs
            velocity_limit_sim=100.0,          # Note: Tune this based on your robot's specs
            stiffness=10000.0, #100000.0,                  # Note: Tune this for desired control performance
            damping=1000.0, #10000.0,                     # Note: Tune this for desired control performance
        ),
        # Group for the 6 right arm joints using a regular expression
        "right_arm": ImplicitActuatorCfg(
            joint_names_expr=["r_joint[1-6]"], # This matches r_joint1, r_joint2, ..., r_joint6
            effort_limit_sim=100.0,            # Note: Tune this based on your robot's specs
            velocity_limit_sim=100.0,          # Note: Tune this based on your robot's specs
            stiffness=10000.0, #100000.0,                  # Note: Tune this for desired control performance1
            damping=1000.0, #10000.0,                   # Note: Tune this for desired control performance
        ),
        "head": ImplicitActuatorCfg(joint_names_expr=["head_revoluteJoint"], stiffness=10000.0, damping=1000.0),
        "trunk": ImplicitActuatorCfg(
            joint_names_expr=["PrismaticJoint"],
            stiffness=40000.0,    # 从 10000 增：强持位
            damping=4000.0,      # 从 1000 增：抗振荡
            effort_limit_sim=10000.0,
            velocity_limit_sim=0.2,
        ),
        "wheels": ImplicitActuatorCfg(
            joint_names_expr=[".*_revolute_Joint"], 
            stiffness=0.0, 
            damping=100.0,
            effort_limit_sim=200.0,  # <<<<<< 新增：力矩上限，足够驱动轮子
            velocity_limit_sim=50.0, # <<<<<< 新增：速度上限，防止过快
        ),
        "grippers": ImplicitActuatorCfg(
            joint_names_expr=[".*_hand_joint.*"], 
            stiffness=10000.0, 
            damping=1000.0,
            effort_limit_sim=50.0, # default is 10, gemini said it is too small, so I set it to 50.
        ),
    },
)
