Source

papl_robwork / src / PaplRobwork.ml

(*
  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
*)

type workcell_t
type state_t
type device_t
type frame_t
type fk_t
type transform_t = float array
type q_t = float array
type time_state_path_t

type time_state_t = state_t PaplTime.t

external caml_robwork_getWorkCellErrorSize : workcell_t -> int
  = "caml_robwork_getWorkCellErrorSize"
external caml_robwork_getWorkCellError : workcell_t -> string -> unit
  = "caml_robwork_getWorkCellError"
external caml_robwork_isValidDevice : device_t -> bool
  = "caml_robwork_isValidDevice"
external caml_robwork_isValidFrame : frame_t -> bool
  = "caml_robwork_isValidFrame"
external caml_robwork_isValidWorkCell : workcell_t -> bool
  = "caml_robwork_isValidWorkCell"
external caml_robwork_loadWorkCell : string -> workcell_t
  = "caml_robwork_loadWorkCell"
external caml_robwork_getState : workcell_t -> state_t
  = "caml_robwork_getState"
external caml_robwork_getDevice : workcell_t -> string -> device_t
  = "caml_robwork_getDevice"
external caml_robwork_getFrame : workcell_t -> string -> frame_t
  = "caml_robwork_getFrame"
external caml_robwork_getDOF : device_t -> int
  = "caml_robwork_getDOF"
external caml_robwork_getEnd : device_t -> frame_t
  = "caml_robwork_getEnd"
external caml_robwork_getLower : device_t -> q_t -> unit
  = "caml_robwork_getLower"
external caml_robwork_getUpper : device_t -> q_t -> unit
  = "caml_robwork_getUpper"
external caml_robwork_getVelocityLimits : device_t -> q_t -> unit
  = "caml_robwork_getVelocityLimits"
external caml_robwork_setQ : state_t -> device_t -> q_t -> unit
  = "caml_robwork_setQ"
external caml_robwork_getQ : state_t -> device_t -> q_t -> unit
  = "caml_robwork_getQ"
external caml_robwork_gripFrame : frame_t -> frame_t -> state_t -> bool
  = "caml_robwork_gripFrame"
external caml_robwork_inCollision : workcell_t -> state_t -> bool
  = "caml_robwork_inCollision"
external caml_robwork_makeTimedStatePath : unit -> time_state_path_t
  = "caml_robwork_makeTimedStatePath"
external caml_robwork_addTimedState :
  time_state_path_t -> float -> state_t -> unit
  = "caml_robwork_addTimedState"
external caml_robwork_writeTimedStatePath :
  workcell_t -> time_state_path_t -> string -> bool
  = "caml_robwork_writeTimedStatePath"
external caml_robwork_copyState : state_t -> state_t
  = "caml_robwork_copyState"
external caml_robwork_getFKTable : state_t -> fk_t
  = "caml_robwork_getFKTable"
external caml_robwork_getTransform : fk_t -> frame_t -> transform_t -> unit
  = "caml_robwork_getTransform"

exception Exception of string

let die msg = raise (Exception msg)

let load_workcell name =
  let workcell = caml_robwork_loadWorkCell name in
  let len = caml_robwork_getWorkCellErrorSize workcell in
    if len <> 0 then
      let error = String.make len ' ' in
      let () = caml_robwork_getWorkCellError workcell error in
        die ("PaplRobwork.load_workcell: " ^ error)
    else
      workcell

let get_state workcell = caml_robwork_getState workcell

let get_device workcell name =
  let device = caml_robwork_getDevice workcell name in
    if caml_robwork_isValidDevice device
    then device
    else die ("PaplRobwork.get_device: No device named " ^ name)

let get_frame workcell name =
  let frame = caml_robwork_getFrame workcell name in
    if caml_robwork_isValidFrame frame
    then frame
    else die ("PaplRobwork.get_frame: No frame named " ^ name)

let get_world_frame workcell = get_frame workcell "WORLD"

let get_dof device = caml_robwork_getDOF device

let get_end device = caml_robwork_getEnd device

let zero_q device = Array.make (get_dof device) 0.

let get_q_by get device =
  let q = zero_q device in
  let () = get device q in
    q

let get_lower device = get_q_by caml_robwork_getLower device
let get_upper device = get_q_by caml_robwork_getUpper device
let get_bounds device = (get_lower device, get_upper device)

let get_velocity_limits device =
  get_q_by caml_robwork_getVelocityLimits device

let set_q state device q =
  let n = get_dof device in
  let n' = Array.length q in
  if n = n'
  then caml_robwork_setQ state device q
  else
    let msg = Printf.sprintf
      "PaplRobwork.set_q: device and configuration (length %d and %d) do not match."
      n n'
    in
      die msg

let get_q state device =
  let q = zero_q device in
  let () = caml_robwork_getQ state device q in
    q

let copy_state state = caml_robwork_copyState state

let grip_frame item frame state =
  if caml_robwork_gripFrame item frame state then ()
  else die "PaplRobwork.grip_frame: item must be a movable DAF."

let in_collision workcell state =
  caml_robwork_inCollision workcell state

let write_time_state_path workcell path file =
  let buf = caml_robwork_makeTimedStatePath () in
  let () =
    let open PaplTime in
      List.iter
        (fun s ->
           caml_robwork_addTimedState
             buf s.time s.q)
        path
  in
  let ok = caml_robwork_writeTimedStatePath workcell buf file in
    if ok
    then ()
    else die
      ("PaplRobwork.write_time_state_path: error writing path to " ^ file)

let update_q state device q =
  let state = copy_state state in
  let () = set_q state device q in
    state

let to_time_state_path state device qts =
  List.map
    (fun qt -> PaplTime.map_q (fun q -> update_q state device q) qt)
    qts

let to_state_path state device qs =
  List.map (fun q -> update_q state device q) qs

let set_device_trajectory_qs state device_traj_pairs t =
  List.iter
    (fun (device, trajectory) ->
       let q = PaplTrajectory.get trajectory t in
         set_q state device q)
    device_traj_pairs

module FK = struct
  let make state = caml_robwork_getFKTable state

  let get fk frame =
    let buf = Array.make 12 0. in
    let () = caml_robwork_getTransform fk frame buf in
      buf
end

module Trajectory = struct
  let of_device_trajectories state device_traj_pairs =
    let get_bounds_by get =
      BatList.map (fun (_, traj) -> get traj) device_traj_pairs
    in
    let t0 = BatList.max (get_bounds_by PaplTrajectory.t0) in
    let t1 = BatList.min (get_bounds_by PaplTrajectory.t1) in
      PaplTrajectory.make
        (t0, t1)
        (fun t ->
           let state = copy_state state in
           let () = set_device_trajectory_qs state device_traj_pairs t in
             state)
end

module Constraint = struct

  let state_in_collision workcell =
    PaplConstraint.make_reject
      (fun state -> in_collision workcell state)

  let q_in_collision ?state workcell device =
    let state =
      match state with
          None -> get_state workcell
        | Some state -> copy_state state
    in
      PaplConstraint.make_reject
        (fun q ->
           let () = set_q state device q in
             in_collision workcell state)

  let q_time state constr obstacles device =
    let state = copy_state state in
    let reject qt =
      let open PaplTime in
      let () = set_q state device qt.q in
      let () = set_device_trajectory_qs state obstacles qt.time in
        PaplConstraint.reject constr state
    in
      PaplConstraint.make_reject reject

  let q_time_in_collision ?state workcell obstacles device =
    let state =
      match state with
          None -> get_state workcell
        | Some state -> state in
    let constr = state_in_collision workcell in
      q_time state constr obstacles device
end

module Metric = struct
  let q_time_dist device =
    PaplQ.dist_inf_size_normalized (get_velocity_limits device)

  let state_time_dist device =
    let dist = q_time_dist device in
      fun sa sb ->
        let qa = get_q sa device in
        let qb = get_q sb device in
          dist qa qb
end