Commits

bergsoe committed 5f72e1a

Rewrite of the RRT module.

The dependency on the TreeIndex module is removed and the entire
TreeIndex module is deleted.

The general RRT functors are removed. All that remains are the
concrete BruteForce implementations.

The attractor sampler is no longer part of the setup module by default,
since in practice the sampler often depends on the target.

The RRT module has overall become simpler and less general. There is
currently too little actual code to reuse to warrant the contorsions of
the more general support.

Comments (0)

Files changed (2)

 *)
 
 module Tree = PaplTree
-module TP = PaplTreePlanner
 
 type 'a pair_t = 'a * 'a
 
-type callback_t = unit -> unit
-
 type 'a expand_t = 'a -> 'a -> 'a list * bool
 
 type 'a status_t =
     Advanced of bool * 'a
   | Blocked
 
-module type BASE = sig
-  type q_t
-  type value_t
-
-  module Index : PaplTreeIndex.S_RRT with type q_t = value_t
-
-  val expand : q_t -> q_t -> q_t list * bool
-  val sampler : q_t option PaplSampler.t
-
-  val make_value : q_t -> value_t
-  val get_q : value_t -> q_t
-end
-
-module type SETUP = sig
-  include BASE
-  type target_t
-
-  val create_callback : target_t -> Index.t -> callback_t
-end
-
-module MakeTPSetup (S : BASE) = struct
-  module SI = S.Index
-
-  type t = {
-    buf : SI.t;
-    callback : callback_t;
-  }
-
-  type q_t = S.q_t
-  type value_t = S.value_t
-  type node_t = value_t PaplTree.node
-
-  let get_q = S.get_q
-
-  let add_qs s parent qs =
-    List.fold_left
-      (fun parent q -> SI.add s.buf parent (S.make_value q))
-      parent
-      qs
-
-  let expand_helper s choose attractor =
-    match choose s.buf (S.make_value attractor) with
-        None -> Blocked
-      | Some near ->
-          match S.expand (S.get_q (Tree.get near)) attractor with
-              [], _ -> Blocked
-            | xs, reached ->
-                let node = add_qs s near xs in
-                  Advanced (reached, node)
-
-  let expand s =
-    s.callback ();
-    match BatEnum.get S.sampler with
-        None -> PaplPlanner.fail "RRT extension sampler is empty"
-      | Some None -> None
-      | Some (Some attractor) ->
-          match expand_helper s SI.nearest attractor with
-              Blocked -> None
-            | Advanced (_, node) -> Some node
-
-  let connect s attractor =
-    match expand_helper s SI.choose_connect 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 buf q = SI.add_root buf (S.make_value q)
-
-  let create buf callback = {
-    buf = buf;
-    callback = callback;
-  }
-
-  let create_callback build =
-    let buf = SI.create () in
-    let callback = build buf in
-      create buf callback
-
-  let create_point q =
-    let buf = SI.create () in
-    let _ = add_root buf q in
-    let callback () = () in
-      create buf callback
-
-  let create_region sampler =
-    let buf = SI.create () in
-    let add q = let _ = add_root buf q in () in
-    let callback () =
-      BatOption.may
-        (List.iter (fun q -> add q))
-        (BatEnum.get sampler)
-    in create buf callback
-end
-
-module MakeBidirHelper
-  (SA0 : BASE)
-  (SB0 : BASE with type q_t = SA0.q_t) =
-struct
-  type q_t = SA0.q_t
-  module SA = MakeTPSetup(SA0)
-  module SB = MakeTPSetup(SB0)
-
-  module Bidir = TP.MakeBidir (SA)(SB)
-end
-
-module MakeBidir
-  (SA0 : SETUP)
-  (SB0 : SETUP with type q_t = SA0.q_t) =
-struct
-  include MakeBidirHelper (SA0) (SB0)
-  type target_t = SA0.target_t * SB0.target_t
-
-  let planner stop (a, b) =
-    let ta = SA.create_callback (SA0.create_callback a) in
-    let tb = SB.create_callback (SB0.create_callback b) in
-      Bidir.planner stop (ta, tb)
-end
-
-module MakePointBidir
-  (SA0 : BASE)
-  (SB0 : BASE with type q_t = SA0.q_t) =
-struct
-  include MakeBidirHelper (SA0) (SB0)
-  type target_t = SA.q_t pair_t
-
-  let planner stop (a, b) =
-    let ta = SA.create_point a in
-    let tb = SB.create_point b in
-      Bidir.planner stop (ta, tb)
-end
-
-module MakeSamplerBidir
-  (SA0 : BASE)
-  (SB0 : BASE with type q_t = SA0.q_t) =
-struct
-  include MakeBidirHelper (SA0) (SB0)
-  type target_t = SA0.q_t PaplPlanner.region_t pair_t
-
-  let planner stop (a, b) =
-    let ta = SA.create_region a in
-    let tb = SB.create_region b in
-      Bidir.planner stop (ta, tb)
-end
-
 module BruteForce = struct
-  module type BASE = sig
-    type q_t
-    include PaplTreeIndex.BruteForce.RRT.SETUP with type q_t := q_t
 
-    val expand : q_t -> q_t -> q_t list * bool
-    val sampler : q_t option PaplSampler.t
+  module type 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
   end
 
-  module type SETUP = sig
-    include BASE
-    type target_t
+  module MakeTreePlannerSetup (S : SETUP) = struct
 
-    val create_callback : target_t -> (q_t -> unit) -> callback_t
-  end
+    type q_t = S.q_t
+    type node_t = q_t PaplTree.node
 
-  module MakeDefault (M : sig type q_t end) = struct
-    type q_t = M.q_t
+    module Index = PaplIndex.BruteForce.Make
+      (struct
+         type value_t = node_t
 
-    let expand_constr = None
-    let connect_constr = None
-    let rng = None
-  end
+         (* This is OK, because we only use functions of the index that don't
+            use the RNG. *)
+         let rng = None
+       end)
+
+    type attractor_t = q_t option PaplSampler.t
+
+    type t = {
+      index : Index.t;
+      callback : t -> unit;
+      attractor : attractor_t;
+    }
 
-  module MakeRRTSetupBase (S : BASE) = struct
-    include S
     type value_t = q_t
 
-    let make_value q = q
     let get_q q = q
 
-    module Index = PaplTreeIndex.BruteForce.RRT.Make(S)
+    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.attractor 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 _ -> ()) attractor = {
+      index = Index.create ();
+      callback = callback;
+      attractor = attractor;
+    }
+
+    let create_point attractor q =
+      let s = create attractor in
+      let _ = add_root s q in
+        s
+
+    let create_region attractor 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 attractor
+
+    let get_nodes s = Index.enum s.index
   end
 
-  module MakeRRTSetup (S : SETUP) = struct
-    include MakeRRTSetupBase (S)
-    type target_t = S.target_t
-
-    let create_callback target buf =
-      let add_root q = let _ = Index.add_root buf (make_value q) in ()
-      in S.create_callback target add_root
+  module WithAttractor = struct
+    module type SETUP = sig
+      include SETUP
+      val attractor_sampler : q_t option PaplSampler.t
+    end
+
+    module MakeTreePlannerSetup (A : SETUP) = struct
+      include MakeTreePlannerSetup(A)
+
+      let create ?callback () = create ?callback A.attractor_sampler
+      let create_point q = create_point A.attractor_sampler q
+      let create_region sampler = create_region A.attractor_sampler sampler
+    end
+
+    module MakePointBidir
+      (SA : SETUP)
+      (SB : SETUP with type q_t = SA.q_t) =
+    struct
+      module A = MakeTreePlannerSetup(SA)
+      module B = MakeTreePlannerSetup(SB)
+      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 MakePointBidir1 (S : SETUP) = MakePointBidir(S)(S)
+
+    module MakeSamplerBidir
+      (SA : SETUP)
+      (SB : SETUP with type q_t = SA.q_t) =
+    struct
+      module A = MakeTreePlannerSetup(SA)
+      module B = MakeTreePlannerSetup(SB)
+      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 MakeSamplerBidir1 (S : SETUP) = MakeSamplerBidir(S)(S)
   end
-
-  module MakeBidir
-    (SA : SETUP)
-    (SB : SETUP with type q_t = SA.q_t) =
-    MakeBidir
-      (MakeRRTSetup(SA))
-      (MakeRRTSetup(SB))
-
-  module MakePointBidir
-    (SA : BASE)
-    (SB : BASE with type q_t = SA.q_t) =
-    MakePointBidir
-      (MakeRRTSetupBase(SA))
-      (MakeRRTSetupBase(SB))
-
-  module MakeSamplerBidir
-    (SA : BASE)
-    (SB : BASE with type q_t = SA.q_t) =
-    MakeSamplerBidir
-      (MakeRRTSetupBase(SA))
-      (MakeRRTSetupBase(SB))
 end
 
 let expand_by_intermediaries constr intermediaries = ();
 *)
 (** RRT planners *)
 
-type callback_t = unit -> unit
-
 type 'a expand_t = 'a -> 'a -> 'a list * bool
 
-module type BASE = sig
-  type q_t
-  type value_t
-
-  module Index : PaplTreeIndex.S_RRT with type q_t = value_t
-
-  val expand : q_t expand_t
-  val sampler : q_t option PaplSampler.t
-
-  val make_value : q_t -> value_t
-  val get_q : value_t -> q_t
-end
-
-module type SETUP = sig
-  include BASE
-  type target_t
-
-  val create_callback : target_t -> Index.t -> callback_t
-end
-
-module MakeBidir
-  (SA : SETUP)
-  (SB : SETUP with type q_t = SA.q_t) :
-  PaplPlanner.S
-  with type q_t = SA.q_t
-  and type target_t = SA.target_t * SB.target_t
-
-module MakePointBidir
-  (SA : BASE)
-  (SB : BASE with type q_t = SA.q_t) :
-  PaplPlanner.POINT with type q_t = SA.q_t
-
-module MakeSamplerBidir
-  (SA : BASE)
-  (SB : BASE with type q_t = SA.q_t) :
-  PaplPlanner.SAMPLER with type q_t = SA.q_t
-
 module BruteForce : sig
-  module type BASE = sig
-    type q_t
-    include PaplTreeIndex.BruteForce.RRT.SETUP with type q_t := q_t
 
+  module type 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 SETUP = sig
-    include BASE
-    type target_t
+  module MakeTreePlannerSetup (A : SETUP) : sig
+    include (PaplTreePlanner.SETUP
+             with type q_t = A.q_t
+             and type value_t = A.q_t)
 
-    val create_callback : target_t -> (q_t -> unit) -> callback_t
+    type attractor_t = q_t option PaplSampler.t
+    val add_root : t -> q_t -> node_t
+    val create : ?callback:(t -> unit) -> attractor_t -> t
+    val create_point : attractor_t -> q_t -> t
+    val create_region : attractor_t -> q_t list PaplSampler.t -> t
   end
 
-  module MakeDefault (M : sig type q_t end) : sig
-    type q_t = M.q_t
-
-    val expand_constr : q_t PaplEdgeConstraint.t option
-    val connect_constr : q_t PaplEdgeConstraint.t option
-    val rng : PaplRandom.rng_t
+  module WithAttractor : sig
+    module type SETUP = sig
+      include SETUP
+      val attractor_sampler : q_t option PaplSampler.t
+    end
+
+    module MakeTreePlannerSetup (A : SETUP) : sig
+      include (PaplTreePlanner.SETUP
+               with type q_t = A.q_t
+               and type value_t = A.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
+    end
+
+    module MakePointBidir
+      (SA : SETUP)
+      (SB : SETUP with type q_t = SA.q_t) :
+      PaplPlanner.POINT with type q_t = SA.q_t
+
+    module MakePointBidir1 (S : SETUP) :
+      PaplPlanner.POINT with type q_t = S.q_t
+
+    module MakeSamplerBidir
+      (SA : SETUP)
+      (SB : SETUP with type q_t = SA.q_t) :
+      PaplPlanner.SAMPLER with type q_t = SA.q_t
+
+    module MakeSamplerBidir1 (S : SETUP) :
+      PaplPlanner.SAMPLER with type q_t = S.q_t
   end
-
-  module MakeBidir
-    (SA : SETUP)
-    (SB : SETUP with type q_t = SA.q_t) :
-    PaplPlanner.S
-    with type q_t = SA.q_t
-    and type target_t = SA.target_t * SB.target_t
-
-  module MakePointBidir
-    (SA : BASE)
-    (SB : BASE with type q_t = SA.q_t) :
-    PaplPlanner.POINT with type q_t = SA.q_t
-
-  module MakeSamplerBidir
-    (SA : BASE)
-    (SB : BASE with type q_t = SA.q_t) :
-    PaplPlanner.SAMPLER with type q_t = SA.q_t
 end
 
 val expand_by_intermediaries :