# 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 the I2RT YAM arm.

I2RT publishes YAM as a single fixed-base 6-DoF arm with a two-finger linear
gripper. The URDF (and its mesh assets) is bundled under
``custom_assets/robots/yam`` and imported directly via Isaac Lab's URDF
converter.

The following configuration parameters are available:

* :obj:`YAM_CFG`: The I2RT YAM 6-DoF arm with a parallel-jaw gripper.

Reference: https://github.com/i2rt-robotics/i2rt
"""

import os

import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg

CUSTOM_ASSETS_DIR = os.path.join(os.path.dirname(__file__), "..", "..", "..", "..", "..", "custom_assets")

##
# Configuration
##

ASSET_URL = "https://github.com/ARISE-Initiative/yamlab/raw/refs/heads/main/yamlab/robot/yam/arm/yam.usd"

GRIPPER_OPEN = -0.04695
"""The upstream prismatic gripper joints are -46.95 mm open and 0 mm closed."""


YAM_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=ASSET_URL,
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=True,
            max_depenetration_velocity=5.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False,
            solver_position_iteration_count=32,
            solver_velocity_iteration_count=0,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        # Previous home pose (deg): joint1=0, joint2=90, joint3=45, joint4=45, joint5=0, joint6=0
        #   joint_pos = {joint1:0.0, joint2:1.5708, joint3:1.5708/2, joint4:1.5708/2, joint5:0.0, joint6:0.0}
        # New home pose requested by user (deg): [-1, 94, 85, -84, -5, 0]  -> TCP world [0.076,-0.190,0.599]
        joint_pos={
            "joint1": -0.017453,   # -1 deg
            "joint2": 1.640610,    # 94 deg
            "joint3": 1.483530,    # 85 deg
            "joint4": -1.466077,   # -84 deg
            "joint5": -0.087266,   # -5 deg
            "joint6": 0.0,         # 0 deg
            "left_finger": GRIPPER_OPEN,
            "right_finger": GRIPPER_OPEN,
        },
    ),
    soft_joint_pos_limit_factor=1.0,
    actuators={
        # "root": ImplicitActuatorCfg(
        #     joint_names_expr=["joint0"],
        #     effort_limit_sim=1.0,
        #     velocity_limit_sim=1.0,
        #     stiffness=100.0,
        #     damping=10.0,
        # ),
        "arm": ImplicitActuatorCfg(
            joint_names_expr=[f"joint{i}" for i in range(1, 7)],
            effort_limit_sim=20.0,
            velocity_limit_sim=2.0,
            stiffness=120.0,
            damping=12.0,
        ),
        "gripper": ImplicitActuatorCfg(
            joint_names_expr=["left_finger", "right_finger"],
            # Was stiffness=0, damping=1 -> stall clamp force ~= damping*vel_cmd ~= 0.05 N (way too weak
            # to hold an object). Raise damping + effort so the velocity-driven jaw clamps firmly.
            # Clamp force is task-dependent, so it is settable per run: heavy solid objects
            # (the scaled-up fruit) slip out below ~70, while thin-walled vessels (cup, mug)
            # get crushed through above ~50 and drop out of the jaws.
            effort_limit_sim=float(os.environ.get("YAM_GRIP_EFFORT", 75.0)),
            velocity_limit_sim=1.0,
            stiffness=0.0,
            damping=float(os.environ.get("YAM_GRIP_DAMPING", 85.0)),
        ),
    },
)
"""Configuration of the I2RT YAM 6-DoF arm with a parallel-jaw gripper."""