export const ABB_IRB120_URDF_SOURCE = {
  repository: "https://github.com/ros-industrial/abb",
  branch: "noetic-devel",
  entrypoint: "abb_irb120_support/urdf/irb120_3_58.xacro",
  macro: "abb_irb120_support/urdf/irb120_3_58_macro.xacro"
} as const;

export const ABB_IRB120_3_58_URDF = `
<robot name="abb_irb120_3_58">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="link_2"/>
  <link name="link_3"/>
  <link name="link_4"/>
  <link name="link_5"/>
  <link name="link_6"/>
  <link name="base"/>
  <link name="flange"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute"><origin rpy="0 0 0" xyz="0 0 0"/><parent link="base_link"/><child link="link_1"/><limit effort="0" lower="-2.87979" upper="2.87979" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 0 1"/></joint>
  <joint name="joint_2" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.29"/><parent link="link_1"/><child link="link_2"/><limit effort="0" lower="-1.91986" upper="1.91986" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 1 0"/></joint>
  <joint name="joint_3" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.27"/><parent link="link_2"/><child link="link_3"/><limit effort="0" lower="-1.91986" upper="1.22173" velocity="4.36332" acceleration="8.72664"/><axis xyz="0 1 0"/></joint>
  <joint name="joint_4" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.07"/><parent link="link_3"/><child link="link_4"/><limit effort="0" lower="-2.79253" upper="2.79253" velocity="5.58505" acceleration="11.1701"/><axis xyz="1 0 0"/></joint>
  <joint name="joint_5" type="revolute"><origin rpy="0 0 0" xyz="0.302 0 0"/><parent link="link_4"/><child link="link_5"/><limit effort="0" lower="-2.094395" upper="2.094395" velocity="5.58505" acceleration="11.1701"/><axis xyz="0 1 0"/></joint>
  <joint name="joint_6" type="revolute"><origin rpy="0 0 0" xyz="0.072 0 0"/><parent link="link_5"/><child link="link_6"/><limit effort="0" lower="-6.98132" upper="6.98132" velocity="7.33038" acceleration="14.66076"/><axis xyz="1 0 0"/></joint>
  <joint name="base_link-base" type="fixed"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="base_link"/><child link="base"/></joint>
  <joint name="joint_6-flange" type="fixed"><origin xyz="0 0 0" rpy="0 0 0"/><parent link="link_6"/><child link="flange"/></joint>
  <joint name="link_6-tool0" type="fixed"><origin xyz="0 0 0" rpy="0 1.5707963267948966 0"/><parent link="flange"/><child link="tool0"/></joint>
</robot>
`;

export const ABB_IRB120_LOAD_OPTIONS = {
  robotId: "abb_irb120_3_58",
  baseLink: "base_link",
  tipLink: "tool0"
} as const;

export const ABB_IRB120_ZERO_JOINTS = [0, 0, 0, 0, 0, 0] as const;
export const ABB_IRB120_PICK_JOINTS = [0.2, -0.35, 0.45, 0.1, -0.2, 0.3] as const;
export const ABB_IRB120_PLACE_JOINTS = [-0.35, -0.25, 0.35, -0.25, 0.15, -0.4] as const;
export const ABB_IRB120_APPROACH_JOINTS = [0, -0.4, 0.5, 0, 0.2, 0] as const;

export const ABB120_ROBOT_FIXTURE = {
  robotId: ABB_IRB120_LOAD_OPTIONS.robotId,
  urdf: ABB_IRB120_3_58_URDF,
  loadOptions: ABB_IRB120_LOAD_OPTIONS,
  source: ABB_IRB120_URDF_SOURCE,
  standardJoints: {
    home: ABB_IRB120_ZERO_JOINTS,
    approach: ABB_IRB120_APPROACH_JOINTS,
    pick: ABB_IRB120_PICK_JOINTS,
    place: ABB_IRB120_PLACE_JOINTS
  }
} as const;
