Commits

bergsoe committed d14bfe7

Another iteration of the SBL interface.

- RRT.BF_SETUP_PAIR is introduced to ease the construction of
asymmetric search trees (e.g. for time planning).

- The attractor sampler is moved into the default RRT setup. It
turns out that there wasn't much point in practice in passing
the sampler as an argument to the search tree constructor.

- Refactoring of the various MakeBidir functors. The code is much
more compact now.

  • Participants
  • Parent commits de0c559

Comments (0)

Files changed (2)

   val kind : kind_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 MakeSetupUtil (S : SETUP) = struct
   include S
 
 
 module MakeBidir1 (S : SETUP) = MakeBidir (S)(S)
 
+module MakeBidirPair (S : SETUP_PAIR) = MakeBidir (S.Fst) (S.Snd)
+
 module BruteForce = struct
+  module type SETUP = sig
+    include SETUP
+
+    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 PaplPlanner.region_t -> t
+
+    val get_roots : t -> node_t BatEnum.t
+    (** Enumeration of all nodes that have been added with {! add_root}. *)
+  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 MakePointBidirPair (M : SETUP_PAIR) = struct
+    module P = MakeBidirPair (M)
+    type q_t = M.q_t
+    type target_t = q_t pair_t
+
+    let planner stop (a, b) =
+      P.planner stop (M.Fst.create_point a, M.Snd.create_point b)
+  end
+
+  module MakeSamplerBidirPair (M : SETUP_PAIR) = struct
+    module P = MakeBidirPair (M)
+    type q_t = M.q_t
+    type target_t = q_t PaplPlanner.region_t pair_t
+
+    let planner stop (a, b) =
+      P.planner stop (M.Fst.create_region a, M.Snd.create_region b)
+  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 Unbiased = struct
 
     module type BF_SETUP = sig
     module MakePointBidir
       (SA : BF_SETUP)
       (SB : BF_SETUP with type q_t = SA.q_t) =
-    struct
-      module A = MakeSetup(SA)
-      module B = MakeSetup(SB)
-      module P = 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
+      MakePointBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))
 
     module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)
 
     module MakeSamplerBidir
       (SA : BF_SETUP)
       (SB : BF_SETUP with type q_t = SA.q_t) =
-    struct
-      module A = MakeSetup(SA)
-      module B = MakeSetup(SB)
-      module P = 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
+      MakeSamplerBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))
 
     module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
   end
     module type BF_SETUP = sig
       type q_t
       val make_constr : (q_t * q_t) -> PaplIncrConstraint.t
-
       val connect_dist : q_t PaplMetric.option_t
       val connect_constr : q_t PaplEdgeConstraint.t option
-
       val expand_dist : q_t PaplMetric.option_t
       val expand_constr : q_t PaplEdgeConstraint.t option
-
       val expand : q_t -> q_t -> q_t list
-
+      val sampler : q_t option PaplSampler.t
       val kind : kind_t
     end
 
+    module type BF_SETUP_PAIR = sig
+      type q_t
+      val make_constr : ((q_t * q_t) -> PaplIncrConstraint.t) pair_t
+      val connect_dist : q_t PaplMetric.option_t pair_t
+      val connect_constr : q_t PaplEdgeConstraint.t option pair_t
+      val expand_dist : q_t PaplMetric.option_t pair_t
+      val expand_constr : q_t PaplEdgeConstraint.t option pair_t
+      val expand : (q_t -> q_t -> q_t list) pair_t
+      val sampler : q_t option PaplSampler.t pair_t
+      val kind : kind_t pair_t
+    end
+
     module MakeSetup (M : BF_SETUP) = struct
       (* We shamelessly forward to this EST version. The only place where the
          algoriths differ is in in the [expand] method.
           let rng = None
         end)
 
-      type q_t = Default.q_t
-      type value_t = Default.value_t
-      type node_t = Default.node_t
-      type t = {
-        attractor : q_t option PaplSampler.t;
-        index : Default.t;
-        callback : t -> unit;
-      }
-
-      type attractor_t = q_t option PaplSampler.t
-
-      let connect s q = Default.connect s.index q
-      let remove_enum s enum = Default.remove_enum s.index enum
-      let add s q ec = Default.add s.index q ec
-      let get_q = Default.get_q
-      let get_ec = Default.get_ec
-      let make_constr = Default.make_constr
-      let kind = Default.kind
+      (* Include the EST version. *)
+      include Default
 
       let expand_constr =
         match M.expand_constr with
             None -> PaplConstraint.fixed_accept true
           | Some expand_constr -> expand_constr
 
-      let callback s = s.callback s
-
+      (* Replace the [expand] method. *)
       let expand s =
-        callback s;
-        match BatEnum.get s.attractor with
+        Default.callback s;
+        match BatEnum.get M.sampler with
             None -> None
           | Some None -> None
           | Some (Some q_attractor) ->
               match Default.nearest
-                s.index M.expand_dist expand_constr q_attractor with
+                s M.expand_dist expand_constr q_attractor with
                   | None -> None
                   | Some n_tree ->
                       let q_tree = get_q (Tree.get n_tree) in
                         match M.expand q_tree q_attractor with
                           | [] -> None
-                          | qs -> Some (Default.add_all s.index n_tree qs)
+                          | qs -> Some (Default.add_all s n_tree qs)
+    end
 
-      let add_root s q = Default.add_root s.index q
+    module MakeSetupPair (M : BF_SETUP_PAIR) = struct
+      module FstSetup = struct
+        type q_t = M.q_t
+        let make_constr = fst M.make_constr
+        let connect_dist = fst M.connect_dist
+        let connect_constr = fst M.connect_constr
+        let expand_dist = fst M.expand_dist
+        let expand_constr = fst M.expand_constr
+        let expand = fst M.expand
+        let sampler = fst M.sampler
+        let kind = fst M.kind
+      end
+      module SndSetup = struct
+        type q_t = M.q_t
+        let make_constr = snd M.make_constr
+        let connect_dist = snd M.connect_dist
+        let connect_constr = snd M.connect_constr
+        let expand_dist = snd M.expand_dist
+        let expand_constr = snd M.expand_constr
+        let expand = snd M.expand
+        let sampler = snd M.sampler
+        let kind = snd M.kind
+      end
+      type q_t = M.q_t
+      module Fst = MakeSetup(FstSetup)
+      module Snd = MakeSetup(SndSetup)
+    end
 
-      let get_roots s = Default.get_roots s.index
+    module MakePointBidir
+      (A : BF_SETUP)
+      (B : BF_SETUP with type q_t = A.q_t) =
+      MakePointBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))
 
-      let create ?(callback = fun _ -> ()) attractor = {
-        attractor = attractor;
-        index = Default.create ();
-        callback = callback;
-      }
+    module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)
 
-      let create_point attractor q =
-        let s = create attractor in
-        let _ = add_root s q in
-          s
+    module MakeSamplerBidir
+      (A : BF_SETUP)
+      (B : BF_SETUP with type q_t = A.q_t) =
+      MakeSamplerBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))
 
-      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
-    end
+    module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
 
-    module MakeSetupWithAttractor (M :
-      sig
-        include BF_SETUP
-        val attractor_sampler : q_t option PaplSampler.t
-      end) =
-    struct
-      include MakeSetup(M)
-
-      let arg = M.attractor_sampler
-      let create ?callback () = create ?callback arg
-      let create_point q = create_point arg q
-      let create_region r = create_region arg r
-    end
+    module MakePointBidirPair (M : BF_SETUP_PAIR) =
+      MakePointBidirPair(MakeSetupPair(M))
+
+    module MakeSamplerBidirPair (M : BF_SETUP_PAIR) =
+      MakeSamplerBidirPair(MakeSetupPair(M))
   end
 end
 (** SBL planners
 *)
 
+type 'a pair_t = 'a * 'a
+
 type kind_t = Standard | NonTransfer
 
 module type SETUP = sig
   val kind : kind_t
 end
 
-module MakeBidir (SA : SETUP) (SB : SETUP with type q_t = SA.q_t) :
+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 MakeBidir (A : SETUP) (B : SETUP with type q_t = A.q_t) :
   PaplPlanner.S
-  with type q_t = SA.q_t
-  and type target_t = SA.t * SB.t
+  with type q_t = A.q_t
+  and type target_t = A.t * B.t
 
 module MakeBidir1 (S : SETUP) :
   PaplPlanner.S
   with type q_t = S.q_t
   and type target_t = S.t * S.t
 
+module MakeBidirPair (S : SETUP_PAIR) :
+  PaplPlanner.S
+  with type q_t = S.q_t
+  and type target_t = S.Fst.t * S.Snd.t
+
 module BruteForce : sig
 
+  module type SETUP = sig
+    include SETUP
+
+    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 PaplPlanner.region_t -> t
+
+    val get_roots : t -> node_t BatEnum.t
+    (** Enumeration of all nodes that have been added with {! add_root}. *)
+  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 MakePointBidirPair (M : SETUP_PAIR) :
+    PaplPlanner.POINT with type q_t = M.q_t
+
+  module MakeSamplerBidirPair (M : SETUP_PAIR) :
+    PaplPlanner.SAMPLER with type q_t = M.q_t
+
   module Unbiased : sig
     module type BF_SETUP = sig
       type q_t
       val rng : PaplRandom.rng_t
     end
 
-    module MakeSetup (M : BF_SETUP) : sig
-      include SETUP with type q_t = M.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 PaplPlanner.region_t -> t
-
-      val get_roots : t -> node_t BatEnum.t
-      (** Enumeration of all nodes that have been added with {! add_root}. *)
-    end
+    module MakeSetup (M : BF_SETUP) : SETUP with type q_t = M.q_t
 
     module MakePointBidir
       (SA : BF_SETUP)
       val expand_dist : q_t PaplMetric.option_t
       val expand_constr : q_t PaplEdgeConstraint.t option
       val expand : q_t -> q_t -> q_t list
+      val sampler : q_t option PaplSampler.t
       val kind : kind_t
     end
 
-    module MakeSetup (M : BF_SETUP) : sig
-      include SETUP with type q_t = M.q_t
+    module type BF_SETUP_PAIR = sig
+      type q_t
+      val make_constr : ((q_t * q_t) -> PaplIncrConstraint.t) pair_t
+      val connect_dist : q_t PaplMetric.option_t pair_t
+      val connect_constr : q_t PaplEdgeConstraint.t option pair_t
+      val expand_dist : q_t PaplMetric.option_t pair_t
+      val expand_constr : q_t PaplEdgeConstraint.t option pair_t
+      val expand : (q_t -> q_t -> q_t list) pair_t
+      val sampler : q_t option PaplSampler.t pair_t
+      val kind : kind_t pair_t
+    end
 
-      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 PaplPlanner.region_t -> t
+    module MakeSetup (M : BF_SETUP) : SETUP with type q_t = M.q_t
 
-      val get_roots : t -> node_t BatEnum.t
-      (** Enumeration of all nodes that have been added with {! add_root}. *)
-    end
+    module MakeSetupPair (M : BF_SETUP_PAIR) :
+      SETUP_PAIR with type q_t = M.q_t
+      
+    module MakePointBidir
+      (A : BF_SETUP)
+      (B : BF_SETUP with type q_t = A.q_t) :
+      PaplPlanner.POINT with type q_t = A.q_t
 
-    module MakeSetupWithAttractor (M :
-      sig
-        include BF_SETUP
-        val attractor_sampler : q_t option PaplSampler.t
-      end) : sig
-      include SETUP with type q_t = M.q_t
+    module MakePointBidir1 (S : BF_SETUP) :
+      PaplPlanner.POINT with type q_t = S.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 PaplPlanner.region_t -> t
+    module MakeSamplerBidir
+      (A : BF_SETUP)
+      (B : BF_SETUP with type q_t = A.q_t) :
+      PaplPlanner.SAMPLER with type q_t = A.q_t
 
-      val get_roots : t -> node_t BatEnum.t
-      (** Enumeration of all nodes that have been added with {! add_root}. *)
-    end
+    module MakeSamplerBidir1 (S : BF_SETUP) :
+      PaplPlanner.SAMPLER with type q_t = S.q_t
+
+    module MakePointBidirPair (M : BF_SETUP_PAIR) :
+      PaplPlanner.POINT with type q_t = M.q_t
+
+    module MakeSamplerBidirPair (M : BF_SETUP_PAIR) :
+      PaplPlanner.SAMPLER with type q_t = M.q_t
   end
 end