1. bergsoe
  2. papl


papl / src / PaplTime.ml

  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
type 'a pair_t = 'a * 'a
type rng_t = PaplRandom.rng_t

type 'a t = {
  time : float;
  q : 'a;

type 'a my_t = 'a t

type range_t = float * float

type direction_t = Forward | Backward

let (<&>) = PaplConstraint.(<&>)

let map_pair = BatTuple.Tuple2.mapn

let stop () = raise BatEnum.No_more_elements

let zip_pair f (x0, x1) (y0, y1) = (f x0 y0, f x1 y1)

let make time q = { time = time; q = q; }

let map_q f qt = make qt.time (f qt.q)

let map_time f qt = make (f qt.time) qt.q

let q qt = qt.q

let time qt = qt.time

let get_range (qa, qb) = qa.time, qb.time

let in_range (a, b) t = a <= t && t <= b

let in_option_range (a, b) t =
  match !a, !b with
      Some lower, Some upper -> in_range (lower, upper) t
    | _ -> false

let in_range_constr range =
  let accept qt = in_range range qt.time in
    PaplConstraint.make_accept accept

let in_option_range_constr range =
  let accept qt = in_option_range range qt.time in
    PaplConstraint.make_accept accept

let interpolate q_interpolate x y =
  let ip = q_interpolate x.q y.q in
    fun s -> make
      (PaplInterpolate.Float.interpolate x.time y.time s)
      (ip s)

let reject_direction direction =
  match direction with
      Forward -> (fun (this, other) -> this.time > other.time)
    | Backward -> (fun (this, other) -> other.time > this.time)

module Metric = struct
  let time x y = abs_float (x.time -. y.time)

  let q q_metric = (); fun x y -> q_metric x.q y.q

  let dist2_sqr q_metric = ();
    fun x y ->
      let dt = abs_float (x.time -. y.time) in
      let dq = q_metric x.q y.q
      in dt *. dt +. dq *. dq

  let dist2 q_metric =
    let dist = dist2_sqr q_metric in
      fun x y -> sqrt (dist x y)

  let order_option direction =
    let reject_direction = reject_direction direction in
      fun a b ->
        if reject_direction (a, b) then None
        else Some (time a b)

  let order_option_pair () =
    order_option Forward,
    order_option Backward

  let dist_time_option dist_time = ();
    fun a b ->
      let dt = abs_float (a.time -. b.time) in
      let dtq = dist_time a.q b.q in
        if dtq > dt then None
        else Some dtq

  let dist_time_order_option direction dist_time =
    let reject_direction = reject_direction direction in
    let dist_time_option = dist_time_option dist_time in
      fun a b ->
        if reject_direction (a, b) then None
        else dist_time_option a b

  let dist_time_order_option_pair dist_time =
    dist_time_order_option Forward dist_time,
    dist_time_order_option Backward dist_time

let reject_within_time dist_time (x, y) =
  let dt = abs_float (x.time -. y.time) in
  let dtq = dist_time x.q y.q in
    dtq > dt

let accept_within_time dist_time (x, y) =
  not (reject_within_time dist_time (x, y))

module Path = struct
  let of_metric ?(t0 = 0.) qs metric =
    let rec loop t qs =
      match qs with
        | [] -> invalid_arg "PaplTime.Path.of_metric: empty path."
        | [a] -> [make t a]
        | a :: b :: rest ->
            let d = metric a b in
              make t a :: loop (t +. d) (b :: rest)
      loop t0 qs

  let scale_to path len =
    let x0 = BatList.first path in
    let x1 = BatList.last path in
    let t0 = x0.time in
    let t1 = x1.time in
    let d = t1 -. t0 in
      if d = 0.
      then [x0; make len x1.q]
        let scale = len /. d in
            (fun qt ->
               let t = qt.time in
               let t' = t0 +. scale *. (t -. t0) in
                 make t' qt.q)

  let add_time qt dt = make (qt.time +. dt) qt.q

  let rec shift_helper path time =
    match path with
        [] -> ([], time)
      | [qt] ->
          let qt = add_time qt time in
            ([qt], qt.time)
      | qt :: path ->
          let qt = add_time qt time in
          let (result, result_time) = shift_helper path time in
            (qt :: result, result_time)

  let shift path dt = List.map (fun qt -> add_time qt dt) path

  let concat ?(t0 = 0.) paths =
    let rec loop paths time =
      match paths with
          [] -> []
        | path :: paths ->
            let (path, time) = shift_helper path time in
              path :: loop paths time
      List.concat (loop paths 0.)

module Trajectory = struct
  let of_interpolate interpolate a b =
    let ip = interpolate a b in
      if b.time < a.time then
        invalid_arg "PaplTime.Trajectory.of_interpolate: b.time < a.time";
        (a.time, b.time)
        (fun s -> (ip s).q)

  let of_path interpolate path =
    let trajectories = PaplPath.map_adjacent
      (fun x0 x1 -> of_interpolate interpolate x0 x1)
      path in
    let t0 = (List.hd path).time in
      PaplTrajectory.of_list ~t0:t0 trajectories

  let discretize ?t0 ?t1 traj step =
    let module T = PaplTrajectory in
    let t0 = BatOption.default (T.t0 traj) t0 in
    let t1 = BatOption.default (T.t1 traj) t1 in
      if t0 == neg_infinity then
        failwith "PaplTime.Trajectory.discretize: t0 == neg_infinity";
      if t1 == infinity then
        failwith "PaplTime.Trajectory.discretize: t1 == infinity";

      if t0 > t1 then []
      else if t0 == t1 then
        let q = T.get traj t0 in [make t0 q; make t0 q]
        let rec loop t =
          if t > t1 then
            [make t1 (T.get traj t1)]
            let qt = make t (T.get traj t) in
              qt :: loop (t +. step)
        in loop t0

  let project_interpolate trajectory project q_interpolate =
      (fun qt ->
         match project qt.q (PaplTrajectory.get trajectory qt.time) with
             None -> None
           | Some q -> Some (make qt.time q))
      (interpolate q_interpolate)

  module Sampler = struct
    let uniform ?rng traj =
      if PaplTrajectory.is_empty traj then BatEnum.empty ()
        let range = PaplTrajectory.range traj in
        let get_uniform = PaplSampler.get_uniform ?rng in
        let next () =
          let t = get_uniform range in
            make t (PaplTrajectory.get traj t)
          BatEnum.from next

    let get_uniform ?rng =
      let get_uniform = PaplSampler.get_uniform ?rng in
        fun traj ->
          let t = get_uniform (PaplTrajectory.range traj) in
            make t (PaplTrajectory.get traj t)

module Planner = struct
  let points_are_unordered (start, goal) = goal.time < start.time

  let fail_if_unordered target =
    if points_are_unordered target then
      PaplPlanner.fail "Goal time greater than start time"

  let fail_if_too_far dist_time target =
    if reject_within_time dist_time target then
      PaplPlanner.fail "Distance in time from start to goal too great"

  let fail_if_bad_point_target constr dist_time target =
    fail_if_unordered target;
    fail_if_too_far dist_time target;
    if PaplConstraint.reject constr (fst target) then
      PaplPlanner.fail "Start point is rejected";
    if PaplConstraint.reject constr (snd target) then
      PaplPlanner.fail "Goal point is rejected"

  let to_trajectory interpolate path_planner =
      (fun path -> Trajectory.of_path interpolate path)

module Sampler = struct
  let time_space_make = make (* outer [make] *)

  let make st sq =
    let next () =
      match (BatEnum.get st, BatEnum.get sq) with
          (Some t, Some q) -> make t q
        | _ -> stop ()
    in BatEnum.from next

  let to_option s = BatEnum.map (fun x -> Some x) s

  let uniform ?rng range sq = make (PaplSampler.uniform ?rng range) sq

  let get_uniform ?rng =
    let get_uniform = PaplSampler.get_uniform ?rng in
      fun range sq ->
        match BatEnum.get sq with
            None -> None
          | Some q ->
              let t = get_uniform range in
                Some (time_space_make t q)

  let fold_time op init qts =
    List.fold_right (fun qt acc -> op qt.time acc) qts init

  let observe_range sampler t op =
    let update qts =
      match qts, !t with
          [], _ -> ()
        | qt :: qts, None ->
            t := Some (fold_time op qt.time qts)
        | _, Some t' ->
            t := Some (fold_time op t' qts)
      BatEnum.map (fun qts -> update qts; qts) sampler

  let observe_range_pair (forw_sampler, back_sampler) =
    let t_lower, t_upper = ref None, ref None in
    let forw_sampler =
      observe_range forw_sampler t_lower Pervasives.min in
    let back_sampler =
      observe_range back_sampler t_upper Pervasives.max
      ((forw_sampler, back_sampler),
       (t_lower, t_upper))

module Constraint = struct
  let q constr = PaplConstraint.bind (fun qt -> qt.q) constr
  let time constr = PaplConstraint.bind (fun qt -> qt.time) constr

  let within_time dist_time x =
    let reject y = reject_within_time dist_time (x, y) in
      PaplConstraint.make_reject reject

  let all_within_time dist_time qts =
      (List.map (within_time dist_time) qts)

  let by_trajectory constr traj =
      (fun { time = t; q = q } ->
         let q' = PaplTrajectory.get traj t in
           PaplConstraint.reject constr (q, q'))

module EdgeConstraint = struct
  let within_time dist_time =
    let reject target = reject_within_time dist_time target in
      PaplConstraint.make_reject reject

  let constrain_by_subdivision qt_constr qt_interpolate dt =
      qt_constr qt_interpolate (Metric.time, dt)

  let order direction =
    PaplConstraint.make_reject (reject_direction direction)

  let inorder () = order Forward

  let not_inorder () = order Backward

  let order_pair () = (order Forward, order Backward)

  let order_pair_with_constraint constr =
    let open PaplConstraint in
      map_pair (fun c -> c <&> constr) (order_pair ())

  let within_time_inorder dist_time =
      (inorder ())
      (within_time dist_time)

  let within_time_not_inorder dist_time =
      (not_inorder ())
      (within_time dist_time)

  let within_time_order_pair dist_time =
    (within_time_inorder dist_time,
     within_time_not_inorder dist_time)

  let q ec =
    let reject (a, b) = PaplConstraint.reject ec (a.q, b.q) in
      PaplConstraint.make_reject reject

  let time ec =
    let reject (a, b) = PaplConstraint.reject ec (a.time, b.time) in
      PaplConstraint.make_reject reject

  module Metric = struct

    let to_option_pair dist pair = 
        (fun constr -> PaplEdgeConstraint.Metric.to_option
           constr dist)

    let order_pair dist =
      to_option_pair dist (order_pair ())

    let within_time_order_pair dist_time dist =
      to_option_pair dist
        (within_time_order_pair dist_time)

module PlannerConstraint = struct
  let q (qc, ec) = (Constraint.q qc, EdgeConstraint.q ec)

  let time (qc, ec) = (Constraint.time qc, EdgeConstraint.time ec)

  let constrain_by_subdivision qt_constr qt_interpolate dt =
    (qt_constr, EdgeConstraint.constrain_by_subdivision
       qt_constr qt_interpolate dt)