Commits

bergsoe committed 4c384c1

- Everything Sampler Planner is renamed to Region Planner.

- New PaplPlanner.make_buffered_region utility.

Comments (0)

Files changed (8)

src/PaplPlanner.ml

 
 type ('target, 'q) path_t = ('target, 'q list) t
 
-type 'a point_t = ('a * 'a, 'a) path_t
+type 'a point_path_t = ('a * 'a, 'a) path_t
 
-type 'a sampler_t = ('a region_pair_t, 'a) path_t
+type 'a region_path_t = ('a region_pair_t, 'a) path_t
 
 type ('target, 'q) trajectory_t = ('target, 'q PaplTrajectory.t) t
 
 type 'q point_trajectory_t = ('q * 'q, 'q) trajectory_t
 
-type 'a sampler_trajectory_t = ('a region_pair_t, 'a) trajectory_t
+type 'a region_trajectory_t = ('a region_pair_t, 'a) trajectory_t
 
 let fail msg = raise (Path_not_found msg)
 
 let map_result f planner = ();
   fun stop target -> f (planner stop target)
 
-let point_to_sampler_target target =
-  BatPair.map (fun q -> BatList.enum [[q]]) target
+let point_to_region q = BatList.enum [[q]]
 
-let sampler_to_point_planner sampler_planner =
-  bind_target point_to_sampler_target sampler_planner
+let point_to_region_target target =
+  BatPair.map point_to_region target
 
-let point_to_sampler_planner pp =
+let region_to_point_planner region_planner =
+  bind_target point_to_region_target region_planner
+
+let point_to_region_planner pp =
   let rec get stop region =
     if PaplStopCriteria.stop stop then
       fail "Stop criteria said stop before region point was found"
             and type target_t = q_t * q_t
 end
 
-module type SAMPLER = sig
+module type REGION = sig
   type q_t
   include S with type q_t := q_t
             and type target_t = q_t region_t pair_t
 end
+
+let make_buffered_region
+    ?(region_name = "")
+    ?(max_region_tries = max_int)
+    ?(max_constr_tries = max_int)
+    ?(max_buffer_len = max_int)
+    ?(retrieve = (fun _ _ -> true))
+    constr
+    region
+    =
+  let incr var = var := !var + 1 in
+  let returned_cnt = ref 0 in
+  let region_tries = ref 0 in
+  let region_found = ref false in
+  let constr_tries = ref 0 in
+  let constr_found = ref false in
+  let module A = BatDynArray in
+  let buffer = A.make 50 in
+  let empty_region_msg = Printf.sprintf
+    "PaplPlanner.make_buffered_region: region \"%s\" seems empty. %d attempts of retrieving a configuration were made."
+    region_name
+    max_region_tries
+  in
+  let rejected_region_msg = Printf.sprintf
+    "PaplPlanner.make_buffered_region: region \"%s\" seems empty. %d configurations were rejected."
+    region_name
+    max_constr_tries
+  in
+  let retrieve () =
+    let bad found max tries = not !found && !tries > max in
+      if A.empty buffer then begin
+        if bad region_found max_region_tries region_tries then
+          fail empty_region_msg;
+        if bad constr_found max_constr_tries constr_tries then
+          fail rejected_region_msg;
+        Some []
+      end
+      else if retrieve !region_tries !returned_cnt then begin
+        let q = A.last buffer in
+          A.delete_last buffer;
+          incr returned_cnt;
+          Some [q]
+      end
+      else
+        Some []
+  in
+  let next () =
+    incr region_tries;
+    if A.length buffer < max_buffer_len then
+      match BatEnum.get region with
+          None -> retrieve ()
+        | Some [] -> retrieve ()
+        | Some qs ->
+            region_found := true;
+            List.iter
+              (fun q ->
+                 incr constr_tries;
+                 if PaplConstraint.accept constr q then begin
+                   constr_found := true;
+                   A.add buffer q
+                 end)
+              qs;
+            retrieve ()
+    else
+      retrieve ()
+  in
+    BatEnum.from_while next

src/PaplPlanner.mli

 
 type ('target, 'q) path_t = ('target, 'q list) t
 
-type 'a point_t = ('a * 'a, 'a) path_t
+type 'a point_path_t = ('a * 'a, 'a) path_t
 
-type 'a sampler_t = ('a region_pair_t, 'a) path_t
+type 'a region_path_t = ('a region_pair_t, 'a) path_t
 
 (** {3 Trajectory planner types} *)
 
 
 type 'q point_trajectory_t = ('q * 'q, 'q) trajectory_t
 
-type 'a sampler_trajectory_t = ('a region_pair_t, 'a) trajectory_t
+type 'a region_trajectory_t = ('a region_pair_t, 'a) trajectory_t
 
 (** {2 General planner operations} *)
 
   ('target, 'result1) t ->
   ('target, 'result2) t
 
-val sampler_to_point_planner :
+val region_to_point_planner :
   ('a region_t pair_t, 'result) t ->
   ('a pair_t, 'result) t
 
-val point_to_sampler_planner :
+val point_to_region : 'a -> 'a region_t
+(** Convert a point to a region containing that point and nothing else.
+*)
+
+val point_to_region_planner :
   ('a pair_t, 'result) t ->
   ('a region_t pair_t, 'result) t
 
             and type target_t = q_t * q_t
 end
 
-module type SAMPLER = sig
+module type REGION = sig
   type q_t
   include S with type q_t := q_t
             and type target_t = q_t region_t pair_t
 end
+
+(** {2 Utilities} *)
+
+val make_buffered_region :
+  ?region_name : string ->
+  ?max_region_tries : int ->
+  ?max_constr_tries : int ->
+  ?max_buffer_len : int ->
+  ?retrieve : (int -> int -> bool) ->
+  'q PaplConstraint.t ->
+  'q region_t ->
+  'q region_t
+(** A buffered region that returns at most one configuration at a time.
+
+    The region [buffer = make_buffered_region ~region_name ~max_region_tries
+    ~max_constr_tries ~max_buffer_len ~retrieve constr region] maintains a
+    buffer of configurations sampled from [region]. Only the configurations of
+    [region] that are accepted by [constr] are added to the buffer.
+
+    [region] is sampled each time that [buffer] is sampled, but only if the
+    current number of elements in the buffer is less than [~max_buffer_len].
+
+    If the buffer is non-empty and [~retrieve m n] returns [true] then [buffer]
+    removes an element [q] from the buffer and returns the single-element list
+    [\[q\]]. [m] is the number of times that [buffer] has been sampled and [n]
+    is the number of times that a single-element list was returned (the
+    remaining [m - n] times an empty list was returned).
+
+    If [m > ~max_region_tries] and [region] has never returned a non-empty list,
+    then [buffer] fails with an error message.
+
+    If [constr] has been invoked more that [~max_constr_tries] and an acceptable
+    configuration is yet to be found, then [buffer] fails with an error message.
+
+    [~region_name] is used as the name of the region in error messages.
+*)
       P.planner stop (A.create_point a, B.create_point b)
   end
 
-  module MakeSamplerBidirPairHelper (M : SETUP_PAIR) = struct
+  module MakeRegionBidirPairHelper (M : SETUP_PAIR) = struct
     module A = M.Fst
     module B = M.Snd
     module P = PaplTreePlanner.MakeBidir (A)(B)
 
   module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)
 
-  module MakeSamplerBidir
+  module MakeRegionBidir
     (A : BF_SETUP)
     (B : BF_SETUP with type q_t = A.q_t) =
-    MakeSamplerBidirPairHelper(MakePair(MakeSetup(A))(MakeSetup(B)))
+    MakeRegionBidirPairHelper(MakePair(MakeSetup(A))(MakeSetup(B)))
 
-  module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
+  module MakeRegionBidir1 (S : BF_SETUP) = MakeRegionBidir(S)(S)
 
   module MakePointBidirPair (M : BF_SETUP_PAIR) =
     MakePointBidirPairHelper(MakeSetupPair(M))
 
-  module MakeSamplerBidirPair (M : BF_SETUP_PAIR) =
-    MakeSamplerBidirPairHelper(MakeSetupPair(M))
+  module MakeRegionBidirPair (M : BF_SETUP_PAIR) =
+    MakeRegionBidirPairHelper(MakeSetupPair(M))
 end
     (M : BF_SETUP_PAIR) :
     PaplPlanner.POINT with type q_t = M.q_t
 
-  module MakeSamplerBidir
+  module MakeRegionBidir
     (SA : BF_SETUP)
     (SB : BF_SETUP with type q_t = SA.q_t) :
-    PaplPlanner.SAMPLER with type q_t = SA.q_t
+    PaplPlanner.REGION with type q_t = SA.q_t
 
-  module MakeSamplerBidir1 (S : BF_SETUP) :
-    PaplPlanner.SAMPLER with type q_t = S.q_t
+  module MakeRegionBidir1 (S : BF_SETUP) :
+    PaplPlanner.REGION with type q_t = S.q_t
 
-  module MakeSamplerBidirPair
+  module MakeRegionBidirPair
     (M : BF_SETUP_PAIR) :
-    PaplPlanner.SAMPLER with type q_t = M.q_t
+    PaplPlanner.REGION with type q_t = M.q_t
 end
       P.planner stop (M.Fst.create_point a, M.Snd.create_point b)
   end
 
-  module MakeSamplerBidirPair (M : SETUP_PAIR) = struct
+  module MakeRegionBidirPair (M : SETUP_PAIR) = struct
     module P = MakeBidirPair (M)
     type q_t = M.q_t
     type target_t = q_t PaplPlanner.region_t pair_t
 
     module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)
 
-    module MakeSamplerBidir
+    module MakeRegionBidir
       (SA : BF_SETUP)
       (SB : BF_SETUP with type q_t = SA.q_t) =
-      MakeSamplerBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))
+      MakeRegionBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))
 
-    module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
+    module MakeRegionBidir1 (S : BF_SETUP) = MakeRegionBidir(S)(S)
   end
 
   module RRT = struct
 
     module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)
 
-    module MakeSamplerBidir
+    module MakeRegionBidir
       (A : BF_SETUP)
       (B : BF_SETUP with type q_t = A.q_t) =
-      MakeSamplerBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))
+      MakeRegionBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))
 
-    module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
+    module MakeRegionBidir1 (S : BF_SETUP) = MakeRegionBidir(S)(S)
 
     module MakePointBidirPair (M : BF_SETUP_PAIR) =
       MakePointBidirPair(MakeSetupPair(M))
 
-    module MakeSamplerBidirPair (M : BF_SETUP_PAIR) =
-      MakeSamplerBidirPair(MakeSetupPair(M))
+    module MakeRegionBidirPair (M : BF_SETUP_PAIR) =
+      MakeRegionBidirPair(MakeSetupPair(M))
   end
 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 MakeRegionBidirPair (M : SETUP_PAIR) :
+    PaplPlanner.REGION with type q_t = M.q_t
 
   module Unbiased : sig
     module type BF_SETUP = sig
     module MakePointBidir1 (S : BF_SETUP) :
       PaplPlanner.POINT with type q_t = S.q_t
 
-    module MakeSamplerBidir
+    module MakeRegionBidir
       (SA : BF_SETUP)
       (SB : BF_SETUP with type q_t = SA.q_t) :
-      PaplPlanner.SAMPLER with type q_t = SA.q_t
+      PaplPlanner.REGION with type q_t = SA.q_t
 
-    module MakeSamplerBidir1 (S : BF_SETUP) :
-      PaplPlanner.SAMPLER with type q_t = S.q_t
+    module MakeRegionBidir1 (S : BF_SETUP) :
+      PaplPlanner.REGION with type q_t = S.q_t
   end
 
   module RRT : sig
     module MakePointBidir1 (S : BF_SETUP) :
       PaplPlanner.POINT with type q_t = S.q_t
 
-    module MakeSamplerBidir
+    module MakeRegionBidir
       (A : BF_SETUP)
       (B : BF_SETUP with type q_t = A.q_t) :
-      PaplPlanner.SAMPLER with type q_t = A.q_t
+      PaplPlanner.REGION with type q_t = A.q_t
 
-    module MakeSamplerBidir1 (S : BF_SETUP) :
-      PaplPlanner.SAMPLER with type q_t = S.q_t
+    module MakeRegionBidir1 (S : BF_SETUP) :
+      PaplPlanner.REGION 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
+    module MakeRegionBidirPair (M : BF_SETUP_PAIR) :
+      PaplPlanner.REGION with type q_t = M.q_t
   end
 end

src/PaplStraightPlanner.ml

 
 let dummy_point_planner stop (a, b) = [a; b]
 
-let dummy_sampler_planner stop (a, b) =
+let dummy_region_planner stop (a, b) =
   let rec loop s =
     if PaplStopCriteria.stop stop
     then PaplPlanner.fail "Stopping criteria says stop."

src/PaplStraightPlanner.mli

     committing to a planner with a more complex search behaviour.
 *)
 
-val point_planner : 'a PaplPlannerConstraint.t -> 'a PaplPlanner.point_t
+val point_planner : 'a PaplPlannerConstraint.t -> 'a PaplPlanner.point_path_t
 (**
    Straight connection point planner.
 
    [\[start; goal\]].
 *)
 
-val dummy_point_planner : 'a PaplPlanner.point_t
+val dummy_point_planner : 'a PaplPlanner.point_path_t
 (** Straight line dummy planner.
 
     The dummy planner immediately returns the path [\[a; b\]] as a solution to a
     targets.
 *)
 
-val dummy_sampler_planner : 'a PaplPlanner.sampler_t
+val dummy_region_planner : 'a PaplPlanner.region_path_t
 (** Straight line dummy planner for regions.
 
     The planner samples the start and goal regions until an element [a] and [b]