language grl 0.1
module W2FullSpecExample
  persistent tool gripper = tool {
    tcp: pose(0 mm, 0 mm, 120 mm, 0 deg, 0 deg, 0 deg),
    mass: 1.5 kg
  }
  persistent frame fixture = frame {
    origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg)
  }
  const speed v_fast = joint(55 %)
  const speed v_pick = linear(180 mm/s)
  const zone z_pick = z(10 mm)
  target home = joint_target {
    joints: [0 deg, -20 deg, 40 deg, 0 deg, 30 deg, 0 deg]
  }
  target pick = pose_target {
    pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg),
    config: robot_config(0, 0, 1),
    tool: gripper,
    frame: fixture
  }
  target place = pose_target {
    pose: pose(650 mm, -100 mm, 320 mm, 180 deg, 0 deg, 90 deg),
    config: robot_config(0, 0, 1),
    tool: gripper,
    frame: fixture
  }
  path pick_path {
    source {
      type: "generated_example",
      id: "pick_path"
    }
    defaults {
      tool: gripper,
      frame: fixture,
      speed: v_pick,
      zone: z_pick
    }
    point approach movej home speed v_fast zone fine
    point above_pick movel pick offset z 100 mm
    point at_pick movel pick zone fine
    event after at_pick io.do[1] = true
    event after at_pick wait io.di[1] == true timeout 2 s on_timeout alarm "Clamp close timeout"
    point leave_pick movel pick offset z 100 mm
  }
  path place_path {
    defaults {
      tool: gripper,
      frame: fixture,
      speed: v_pick,
      zone: z_pick
    }
    point above_place movel place offset z 100 mm
    point at_place movel place zone fine
    event after at_place io.do[1] = false
    event after at_place wait io.di[2] == true timeout 2 s on_timeout alarm "Clamp open timeout"
    point leave_place movel place offset z 100 mm
  }
  operation pick_op {
    kind: handling
    path: pick_path
    process {
      gripper: "main_clamp"
    }
  }
  operation place_op {
    kind: handling
    path: place_path
    process {
      gripper: "main_clamp"
    }
  }
  proc main()
    set_tool gripper
    set_frame fixture
    run_operation pick_op
    run_operation place_op
    movej home speed v_fast zone fine
  end
end
