Source

papl / src / PaplRRT.ml

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

module Tree = PaplTree

type 'a pair_t = 'a * 'a

type 'a expand_t = 'a -> 'a -> 'a list * bool

type 'a status_t =
    Advanced of bool * 'a
  | Blocked

module BruteForce = struct

  module type SETUP = sig
    type q_t
    include (PaplTreePlanner.SETUP
             with type q_t := q_t and type value_t = q_t)

    val add_root : t -> q_t -> node_t
    val create : ?callback:(t -> unit) -> unit -> t
    val create_point : q_t -> t
    val create_region : q_t list PaplSampler.t -> t
    val get_nodes : t -> node_t BatEnum.t
  end

  module type SETUP_PAIR = sig
    type q_t
    module Fst : SETUP with type q_t = q_t
    module Snd : SETUP with type q_t = q_t
  end

  module type BF_SETUP = sig
    type q_t
    val search_dist : q_t PaplMetric.option_t
    val search_constr : q_t PaplEdgeConstraint.t option
    val expand : q_t expand_t
    val sampler : q_t option PaplSampler.t
  end

  module type BF_SETUP_PAIR = sig
    type q_t
    val search_dist : (q_t PaplMetric.option_t) pair_t
    val search_constr : (q_t PaplEdgeConstraint.t option) pair_t
    val expand : (q_t expand_t) pair_t
    val sampler : (q_t option PaplSampler.t) pair_t
  end

  module MakeSetup (S : BF_SETUP) = struct

    type q_t = S.q_t
    type node_t = q_t PaplTree.node

    module Index = PaplIndex.BruteForce.Make
      (struct
         type value_t = node_t

         (* This is OK, because we only use functions of the index that don't
            use the RNG. *)
         let rng = None
       end)

    type t = {
      index : Index.t;
      callback : t -> unit;
    }

    type value_t = q_t

    let get_q q = q

    let add_qs s parent qs =
      let add parent q =
        let node = Tree.add parent q in
        let () = Index.add s.index node in
          node
      in
        List.fold_left add parent qs

    let search_constr =
      BatOption.default
        (PaplConstraint.fixed_accept true)
        S.search_constr

    let nearest s q_attractor =
      Index.nearest
        s.index
        Tree.get
        S.search_dist
        search_constr
        q_attractor

    let expand_helper s attractor =
      match nearest s attractor with
          None -> Blocked
        | Some near ->
            match S.expand (Tree.get near) attractor with
                [], _ -> Blocked
              | xs, reached ->
                  let node = add_qs s near xs in
                    Advanced (reached, node)

    let expand s =
      s.callback s;
      match BatEnum.get S.sampler with
          None -> PaplPlanner.fail "RRT extension sampler is empty"
        | Some None -> None
        | Some (Some attractor) ->
            match expand_helper s attractor with
                Blocked -> None
              | Advanced (_, node) -> Some node

    let connect s attractor =
      match expand_helper s attractor with
        | Advanced (true, node) ->
            (* To avoid duplication of the attractor node on the path, the parent
               of the attractor is returned. *)
            Tree.parent node
        | _ -> None

    let add_root s q =
      let node = Tree.add_root q in
      let () = Index.add s.index node in
        node

    let create ?(callback = fun _ -> ()) () = {
      index = Index.create ();
      callback = callback;
    }

    let create_point q =
      let s = create () in
      let _ = add_root s q in
        s

    let create_region sampler =
      let add s q = let _ = add_root s q in () in
      let callback s =
        BatOption.may
          (List.iter (fun q -> add s q))
          (BatEnum.get sampler)
      in create ~callback ()

    let get_nodes s = Index.enum s.index
  end

  module MakeSetupPair (M : BF_SETUP_PAIR) = struct
    module FstSetup = struct
      type q_t = M.q_t
      let search_dist = fst M.search_dist
      let search_constr = fst M.search_constr
      let expand = fst M.expand
      let sampler = fst M.sampler
    end
    module SndSetup = struct
      type q_t = M.q_t
      let search_dist = snd M.search_dist
      let search_constr = snd M.search_constr
      let expand = snd M.expand
      let sampler = snd M.sampler
    end
    type q_t = M.q_t
    module Fst = MakeSetup(FstSetup)
    module Snd = MakeSetup(SndSetup)
  end

  module MakePair
    (A : SETUP)
    (B : SETUP with type q_t = A.q_t) =
  struct
    type q_t = A.q_t
    module Fst = A
    module Snd = B
  end

  module MakePointBidirPairHelper (M : SETUP_PAIR) = struct
    module A = M.Fst
    module B = M.Snd
    module P = PaplTreePlanner.MakeBidir (A)(B)
    type q_t = A.q_t
    type target_t = q_t pair_t

    let planner stop (a, b) =
      P.planner stop (A.create_point a, B.create_point b)
  end

  module MakeSamplerBidirPairHelper (M : SETUP_PAIR) = struct
    module A = M.Fst
    module B = M.Snd
    module P = PaplTreePlanner.MakeBidir (A)(B)
    type q_t = A.q_t
    type target_t = q_t PaplPlanner.region_t pair_t

    let planner stop (a, b) =
      P.planner stop (A.create_region a, B.create_region b)
  end

  module MakePointBidir
    (A : BF_SETUP)
    (B : BF_SETUP with type q_t = A.q_t) =
    MakePointBidirPairHelper(MakePair(MakeSetup(A))(MakeSetup(B)))

  module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)

  module MakeSamplerBidir
    (A : BF_SETUP)
    (B : BF_SETUP with type q_t = A.q_t) =
    MakeSamplerBidirPairHelper(MakePair(MakeSetup(A))(MakeSetup(B)))

  module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)

  module MakePointBidirPair (M : BF_SETUP_PAIR) =
    MakePointBidirPairHelper(MakeSetupPair(M))

  module MakeSamplerBidirPair (M : BF_SETUP_PAIR) =
    MakeSamplerBidirPairHelper(MakeSetupPair(M))
end