##
## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
##
## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
## property and proprietary rights in and to this material, related
## documentation and any modifications thereto. Any use, reproduction,
## disclosure or distribution of this material and related documentation
## without an express license agreement from NVIDIA CORPORATION or
## its affiliates is strictly prohibited.
##

robot_cfg:
  kinematics:
    usd_path: "robot/ur_description/ur5e_robotiq_2f_140.usd"
    usd_robot_root: "/robot"
    isaac_usd_path: ""
    usd_flip_joints: {}
    usd_flip_joint_limits: []

    urdf_path: "robot/ur_description/ur5e_robotiq_2f_140.urdf"
    asset_root_path: "robot/ur_description"

    base_link: "base_link"
    ee_link: "grasp_frame"
    link_names: null
    lock_joints: {'finger_joint': 0.0}

    extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" ,
    #"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
    #"joint_name": "attach_joint" }}
    extra_collision_spheres: null #{"attached_object": 4}

    collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
    'wrist_2_link' ,'wrist_3_link', 'tool0', 'robotiq_arg2f_base_link',
    'left_outer_knuckle',
    'left_inner_knuckle',
    'left_outer_finger',
    'left_inner_finger',
    'left_inner_finger_pad',
    'right_outer_knuckle',
    'right_inner_knuckle',
    'right_outer_finger' ,
    'right_inner_finger',
    'right_inner_finger_pad',
    ] # List[str]
    collision_spheres:
      shoulder_link:
        - "center": [0.0, 0.0, 0.0]
          "radius": 0.1
      upper_arm_link:
        - "center": [-0.416, -0.0, 0.143]
          "radius": 0.078
        - "center": [-0.015, 0.0, 0.134]
          "radius": 0.077
        - "center": [-0.14, 0.0, 0.138]
          "radius": 0.062
        - "center": [-0.285, -0.001, 0.139]
          "radius": 0.061
        - "center": [-0.376, 0.001, 0.138]
          "radius": 0.077
        - "center": [-0.222, 0.001, 0.139]
          "radius": 0.061
        - "center": [-0.055, 0.008, 0.14]
          "radius": 0.07
        - "center": [-0.001, -0.002, 0.143]
          "radius": 0.08
      forearm_link:
        - "center": [-0.01, 0.002, 0.031]
          "radius": 0.072
        - "center": [-0.387, 0.0, 0.014]
          "radius": 0.057
        - "center": [-0.121, -0.0, 0.006]
          "radius": 0.057
        - "center": [-0.206, 0.001, 0.007]
          "radius": 0.057
        - "center": [-0.312, -0.001, 0.006]
          "radius": 0.056
        - "center": [-0.057, 0.003, 0.008]
          "radius": 0.065
        - "center": [-0.266, 0.0, 0.006]
          "radius": 0.057
        - "center": [-0.397, -0.001, -0.018]
          "radius": 0.052
        - "center": [-0.164, -0.0, 0.007]
          "radius": 0.057
      wrist_1_link:
        - "center": [-0.0, 0.0, -0.009]
          "radius": 0.047
        - "center": [-0.0, 0.0, -0.052]
          "radius": 0.047
        - "center": [-0.002, 0.027, -0.001]
          "radius": 0.045
        - "center": [0.001, -0.01, 0.0]
          "radius": 0.046
      wrist_2_link:
        - "center": [0.0, -0.01, -0.001]
          "radius": 0.047
        - "center": [0.0, 0.008, -0.001]
          "radius": 0.047
        - "center": [0.001, -0.001, -0.036]
          "radius": 0.047
        - "center": [0.001, -0.03, -0.0]
          "radius": 0.047
      wrist_3_link:
        - "center": [0.001, 0.001, -0.029]
          "radius": 0.043
      tool0:
        - "center": [0.001, 0.001, 0.05]
          "radius": -0.01 #0.05
      robotiq_arg2f_base_link:
        - "center": [0.0, -0.0, 0.04]
          "radius": 0.035
        - "center": [0.0, -0.0, 0.02]
          "radius": 0.035
      left_outer_finger:
          - "center": [0.0, -0.01, 0.0]
            "radius": 0.01
          - "center": [0.0, 0.015, -0.01]
            "radius": 0.01

      left_inner_finger_pad:
          - "center": [0.0, -0.0, 0.005]
            "radius": 0.01
          - "center": [0.0, 0.02, 0.005]
            "radius": 0.01
          - "center": [0.0, -0.02, 0.005]
            "radius": 0.01
      left_inner_knuckle:
          - "center": [0.0, 0.04, -0.0]
            "radius": 0.01
          - "center": [0.0, 0.06, -0.0]
            "radius": 0.01
          - "center": [0.0, 0.08, -0.0]
            "radius": 0.01
      left_inner_finger:
          - "center": [0.0, -0.0, -0.0]
            "radius": 0.01

      left_outer_knuckle:
          - "center": [0.0, 0.055, 0.01]
            "radius": 0.01
          - "center": [0.0, 0.08, 0.005]
            "radius": 0.01


      right_outer_finger:
          - "center": [0.0, -0.01, 0.0]
            "radius": 0.01
          - "center": [0.0, 0.015, -0.01]
            "radius": 0.01

      right_inner_finger_pad:
          - "center": [0.0, -0.0, 0.005]
            "radius": 0.01
          - "center": [0.0, 0.02, 0.005]
            "radius": 0.01
          - "center": [0.0, -0.02, 0.005]
            "radius": 0.01
      right_inner_knuckle:
          - "center": [0.0, 0.04, -0.0]
            "radius": 0.01
          - "center": [0.0, 0.06, -0.0]
            "radius": 0.01
          - "center": [0.0, 0.08, -0.0]
            "radius": 0.01
      right_inner_finger:
          - "center": [0.0, -0.0, -0.0]
            "radius": 0.01

      right_outer_knuckle:
          - "center": [0.0, 0.055, 0.01]
            "radius": 0.01
          - "center": [0.0, 0.08, 0.005]
            "radius": 0.01


    collision_sphere_buffer: 0.005

    self_collision_ignore: {
      "upper_arm_link": ["forearm_link", "shoulder_link"],
      "forearm_link": ["wrist_1_link", "wrist_2_link", "wrist_3_link"],
      "wrist_1_link": ["wrist_2_link","wrist_3_link","tool0", "robotiq_arg2f_base_link"],
      "wrist_2_link": ["wrist_3_link", "tool0",  "robotiq_arg2f_base_link"],
      "wrist_3_link": ["tool0", "robotiq_arg2f_base_link"],

      "tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad',
      'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad',
      'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "left_outer_finger": ['left_inner_finger_pad',
      'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "left_inner_finger_pad": [
      'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "left_inner_knuckle": ['left_inner_finger', 'left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "left_inner_finger": ['left_outer_knuckle',
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "left_outer_knuckle": [
      'right_outer_finger', 'right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],



      "right_outer_finger": ['right_inner_finger_pad',
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "right_inner_finger_pad": [
      'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],

      "right_inner_knuckle": [ 'right_inner_finger', 'right_outer_knuckle'],

      "right_inner_finger": [ 'right_outer_knuckle'],



      }
    self_collision_buffer: {
      'shoulder_link': 0.01,
      'upper_arm_link': 0,
      'forearm_link': 0,
      'wrist_1_link': 0,
      'wrist_2_link': 0,
      'wrist_3_link' : 0,
      'tool0': 0.02,
      }

    use_global_cumul: True
    mesh_link_names:  ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
    'wrist_2_link' ,'wrist_3_link', 'robotiq_arg2f_base_link',
    'left_outer_knuckle',
    'left_inner_knuckle',
    'left_outer_finger',
    'left_inner_finger',
    'right_outer_knuckle',
    'right_inner_knuckle',
    'right_outer_finger' ,
    'right_inner_finger',
    ] # List[str]

    cspace:
      joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                    'wrist_2_joint', 'wrist_3_joint',
                    'finger_joint']
      retract_config:  [0.0, -2.2, 1.0, -1.383, -1.57, 0.00, 0.6]
      null_space_weight:  [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,1.0]
      cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
      max_jerk: 500.0
      max_acceleration: 12.0
      position_limit_clip: 0.0
