# Copyright (c) 2024, MindRobot Project
# SPDX-License-Identifier: BSD-3-Clause

"""Configuration for MindRobot dual-arm robot.

Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach).
Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/

The following configurations are available:

* :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose).
* :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control.
"""

import isaaclab.sim as sim_utils
from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg

from mindbot.utils.assets import MINDBOT_ASSETS_DIR

MINDBOT_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        # usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot_2i/mindrobot_2i.usd",
        usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot_zed/mindbot_mini.usd",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            max_depenetration_velocity=5.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            fix_root_link=False,
            enabled_self_collisions=False,  # TODO: re-enable after fixing trunk self-collision
            solver_position_iteration_count=8,
            solver_velocity_iteration_count=0,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        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.2,
            "head_pitch_Joint": 0.0,
            "head_yaw_Joint": 0.0,
        },
        pos=(0.001, 0.001, 0.7),  # z = 0.44 (base clearance) + 0.26 (PrismaticJoint)
    ),
    actuators={
        # RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s
        "left_arm_shoulder": ImplicitActuatorCfg(
            joint_names_expr=["l_joint[1-3]"],
            effort_limit_sim=50.0,
            velocity_limit_sim=3.14,
            stiffness=80.0,
            damping=4.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_sim=20.0,
            velocity_limit_sim=3.93,
            stiffness=80.0,
            damping=4.0,
        ),
        "right_arm_shoulder": ImplicitActuatorCfg(
            joint_names_expr=["r_joint[1-3]"],
            effort_limit_sim=50.0,
            velocity_limit_sim=3.14,
            stiffness=80.0,
            damping=4.0,
        ),
        "right_arm_wrist": ImplicitActuatorCfg(
            joint_names_expr=["r_joint[4-6]"],
            effort_limit_sim=20.0,
            velocity_limit_sim=3.93,
            stiffness=80.0,
            damping=4.0,
        ),
        "head": ImplicitActuatorCfg(
            joint_names_expr=[
                "head_yaw_Joint",
                "head_pitch_Joint"
                ],
            effort_limit_sim=12.0,
            stiffness=80.0,
            damping=4.0,
        ),
        # IdealPDActuator: explicitly computes F = k_p*(target-pos) + k_d*(target_vel-vel)
        # and sends via set_dof_actuation_forces(), bypassing PhysX drive which
        # doesn't work correctly for prismatic joints with set_dof_position_targets().
        # NOTE: trunk gravity disabled by startup event. High stiffness to resist
        # arm reaction forces (~150N) — deflection < 2mm at 100k stiffness.
        "trunk": IdealPDActuatorCfg(
            joint_names_expr=["PrismaticJoint"],
            effort_limit=2000.0,
            velocity_limit=0.5,
            stiffness=100000.0,
            damping=5000.0,
        ),
        "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_sim=200.0,
            stiffness=0.0,
            damping=100.0,
        ),
        "grippers": ImplicitActuatorCfg(
            joint_names_expr=[".*_hand_joint.*"],
            effort_limit_sim=200.0,
            stiffness=2e3,
            damping=1e2,
        ),
    },
    soft_joint_pos_limit_factor=1.0,
)
"""Base configuration for MindRobot. Gravity enabled, low PD gains."""


MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy()
MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].damping = 80.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].stiffness = 400.0
MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0
"""IK task-space control configuration. Gravity disabled, stiffer PD for tracking."""


# =====================================================================
# Robot Body / Joint Name Constants
# Keep all robot-specific strings here so env cfgs stay robot-agnostic.
# =====================================================================

# Joint name patterns (regex, for use in action/actuator configs)
MINDBOT_LEFT_ARM_JOINTS = "l_joint[1-6]"
MINDBOT_RIGHT_ARM_JOINTS = "r_joint[1-6]"
MINDBOT_LEFT_GRIPPER_JOINTS = ["left_hand_joint_left", "left_hand_joint_right"]
MINDBOT_RIGHT_GRIPPER_JOINTS = ["right_hand_joint_left", "right_hand_joint_right"]
MINDBOT_WHEEL_JOINTS = [
    "right_b_revolute_Joint",
    "left_b_revolute_Joint",
    "left_f_revolute_Joint",
    "right_f_revolute_Joint",
]
MINDBOT_HEAD_JOIONTS =[
    "head_yaw_Joint",    #
    "head_pitch_Joint"   
]

MINDBOT_TRUNK_JOINT = "PrismaticJoint"

# EEF body names (as defined in the USD asset)
MINDBOT_LEFT_EEF_BODY = "left_hand_body"
MINDBOT_RIGHT_EEF_BODY = "right_hand_body"

# Prim paths relative to the Robot prim (i.e. after "{ENV_REGEX_NS}/Robot/")
MINDBOT_LEFT_ARM_PRIM_ROOT = "rm_65_fb_left"
MINDBOT_RIGHT_ARM_PRIM_ROOT = "rm_65_b_right"
MINDBOT_LEFT_EEF_PRIM = "rm_65_fb_left/l_hand_01/left_hand_body"
MINDBOT_RIGHT_EEF_PRIM = "rm_65_b_right/r_hand/right_hand_body"
