# Copyright (c) 2022-2026, 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 the Fourier Robots.

The following configuration parameters are available:

* :obj:'MINDBOT_CFG'
* :obj:`GR1T2_HIGH_PD_CFG`: The GR1T2 humanoid configured with high PD gains on upper body joints for pick-place manipulation tasks.

Reference: https://www.fftai.com/products-gr1
"""

import torch

import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from mindbot.utils.assets import MINDBOT_ASSETS_DIR

##
# Configuration
##


MINDBOT_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=(
                f"{MINDBOT_ASSETS_DIR}/robots/mindrobot-1105-lab/mindrobot_cd2.usd",
            ),
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=True,
            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=16, 
            solver_velocity_iteration_count=8,
            stablization_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(
        pos=(0.0, 0.0, 0.7),
        joint_pos={
            # ====== Left arm (RM-65) — away from singularities ======
            # Elbow singularity: q3=0; Wrist singularity: q5=0.
            # Small offsets on q2/q4/q6 to avoid exact 90° Jacobian symmetry.
            "l_joint1":  1.5708,   #  90°
            "l_joint2": -1.2217,   # -70°  (offset from -90° for better elbow workspace)
            "l_joint3": -1.5708,   # -90°  (away from elbow singularity q3=0)
            "l_joint4":  1.3963,   #  80°  (offset from 90° to break wrist symmetry)
            "l_joint5":  1.5708,   #  90°  (away from wrist singularity q5=0)
            "l_joint6": -1.3963,   # -80°  (offset from -90° to break wrist symmetry)
            # ====== Right arm (RM-65) — same joint values as left (verified visually) ======
            "r_joint1":  1.5708,   #  90°
            "r_joint2": -1.2217,   # -70°
            "r_joint3": -1.5708,   # -90°  (away from elbow singularity q3=0)
            "r_joint4":  1.3963,   #  80°
            "r_joint5":  1.5708,   #  90°  (away from wrist singularity q5=0)
            "r_joint6": 1.3963,   # -80°
            # ====== Grippers (0=open) ======
            "left_hand_joint_left": 0.0,
            "left_hand_joint_right": 0.0,
            "right_hand_joint_left": 0.0,
            "right_hand_joint_right": 0.0,
            # ====== Trunk & Head ======
            # NOTE: pos z = 0.44 (base clearance) + PrismaticJoint value.
            # If you change PrismaticJoint, update pos z accordingly to avoid
            # geometry clipping that causes the robot to fall on spawn.
            "PrismaticJoint": 0.26,
            "head_pitch_Joint": 0.0,
            "head_yaw_Joint": 0.0,
        },
        joint_vel={".*": 0.0},
    ),
    actuators={
        "head": ImplicitActuatorCfg(
            joint_names_expr=[
                "head_.*",
            ],
            effort_limit=None,
            velocity_limit=None,
            stiffness=None,
            damping=None,
        ),
        "trunk": ImplicitActuatorCfg(
            joint_names_expr=[
                "PrismaticJoint",
            ],
            effort_limit=None,
            velocity_limit=None,
            stiffness=None,
            damping=None,
        ),
        "wheels": ImplicitActuatorCfg(
            # joint_names_expr=[".*_revolute_Joint"],
            joint_names_expr=[
                "right_b_revolute_Joint",
                "left_b_revolute_Joint",
                "left_f_revolute_Joint",
                "right_f_revolute_Joint",
            ],
            effort_limit=None,
            velocity_limit=None,
            stiffness=None,
            damping=None,
        ),
        "left_arm_shoulder": ImplicitActuatorCfg(
            joint_names_expr=["l_joint[1-3]"],
            effort_limit=torch.inf,
            velocity_limit=torch.inf,
            stiffness=None,
            damping=None,
            armature=0.0,
        ),
        # RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s
        "left_arm_wrist": ImplicitActuatorCfg(
            joint_names_expr=["l_joint[4-6]"],
            effort_limit=torch.inf,
            velocity_limit=torch.inf,
            stiffness=None,
            damping=None,
            armature=0.0,
        ),
        "right_arm_shoulder": ImplicitActuatorCfg(
            joint_names_expr=["r_joint[1-3]"],
            effort_limit=torch.inf,
            velocity_limit=torch.inf,
            stiffness=None,
            damping=None,
            armature=0.0,
        ),
        "right_arm_wrist": ImplicitActuatorCfg(
            joint_names_expr=["r_joint[4-6]"],
            effort_limit=torch.inf,
            velocity_limit=torch.inf,
            stiffness=None,
            damping=None,
            armature=0.0,
        ),
        "grippers": ImplicitActuatorCfg(
            joint_names_expr=[".*_hand_joint.*"],
            effort_limit_sim=200.0,
            stiffness=2e3,
            damping=1e2,
        ),
        # "right-arm": ImplicitActuatorCfg(
        #     joint_names_expr=[
        #         "right_shoulder_.*",
        #         "right_elbow_.*",
        #         "right_wrist_.*",
        #     ],
        #     effort_limit=torch.inf,
        #     velocity_limit=torch.inf,
        #     stiffness=None,
        #     damping=None,
        #     armature=0.0,
        # ),
        # "left-arm": ImplicitActuatorCfg(
        #     joint_names_expr=[
        #         "left_shoulder_.*",
        #         "left_elbow_.*",
        #         "left_wrist_.*",
        #     ],
        #     effort_limit=torch.inf,
        #     velocity_limit=torch.inf,
        #     stiffness=None,
        #     damping=None,
        #     armature=0.0,
        # ),
        # "right-hand": ImplicitActuatorCfg(
        #     joint_names_expr=[
        #         "R_.*",
        #     ],
        #     effort_limit=None,
        #     velocity_limit=None,
        #     stiffness=None,
        #     damping=None,
        # ),
        # "left-hand": ImplicitActuatorCfg(
        #     joint_names_expr=[
        #         "L_.*",
        #     ],
        #     effort_limit=None,
        #     velocity_limit=None,
        #     stiffness=None,
        #     damping=None,
        # ),
    },
)