Commits

bergsoe  committed fe8b99b

- Upgrade to latest Batteries.

- New PaplInterpolate.map_option utility.

- New PaplRRTExpand.intermediary_option function for
expansion towards a target as far as the interpolation
function allows.

- [get_uniform] micro optimizations.

- New PaplTime.Trajectory.project_interpolate utility.

- New PaplTrajectory.Sampler submodule for sampling of trajectories.

  • Participants
  • Parent commits 49f713e

Comments (0)

Files changed (13)

File src/PaplEdgeConstraint.ml

   See LICENSE file for terms and conditions.
 *)
 
-open BatStd
-
 type 'a t = ('a * 'a) PaplConstraint.t
 
 let mr = PaplConstraint.make_reject
 
 let constrain_both constr = mr (fun (a, b) -> re constr a || re constr b)
 
-let constrain_fst constr = mr (re constr -| fst)
+let constrain_fst constr = mr (fun x -> re constr (fst x))
 
-let constrain_snd constr = mr (re constr -| snd)
+let constrain_snd constr = mr (fun x -> re constr (snd x))
 
 let reject_threshold_by_cmp (metric, eps) cmp =
   mr (fun (x, y) -> cmp (metric x y) eps)

File src/PaplInterpolate.ml

 
 type 'a intermediary_t = 'a -> 'a -> 'a BatEnum.t
 
+let map_option f interpolate =
+  fun a b ->
+    let ip = interpolate a b in
+      fun s -> f (ip s)
+
 let stop () = raise BatEnum.No_more_elements
 
 let intermediary_steps interpolate (metric, eps) = (); fun a b ->

File src/PaplInterpolate.mli

     the stream.
 *)
 
+val map_option : ('a -> 'a option) -> 'a t -> 'a option_t
+(** Map a conversion function that returns an optional value onto the results of
+    an interpolation.
+
+    [(map_option f interpolate) a b s] is equivalent to [f (interpolate a b
+    s)].
+*)
+
 val intermediary_steps :
   'a t -> 'a PaplMetric.threshold_t -> 'a intermediary_t
 (** If [im = intermediary_steps ip (metric, eps)] then [im a b] are the

File src/PaplPlanner.ml

 
 let point_to_region q = BatList.enum [[q]]
 
+let map_pair = BatTuple.Tuple2.mapn
+
 let point_to_region_target target =
-  BatPair.map point_to_region target
+  map_pair point_to_region target
 
 let region_to_point_planner region_planner =
   bind_target point_to_region_target region_planner
         | Some (q :: _) -> q
   in
   let sp stop target =
-    pp stop (BatPair.map (get stop) target)
+    pp stop (map_pair (get stop) target)
   in
     sp
 

File src/PaplRRTExpand.ml

     fun start goal ->
       let (path, reached) = expand start goal in
         split_n ?min_stride ?max_stride n path, reached
+
+let intermediary_option interpolate (metric, eps) constr =
+  let ok a b = PaplConstraint.accept constr (a, b) in
+    fun a b ->
+      let ip = interpolate a b in
+      let step = eps /. metric a b in
+      let rec loop prev pos acc =
+        if pos >= 1.0 then (List.rev acc, true)
+        else
+          match ip pos with
+              None -> (List.rev acc, false)
+            | Some q ->
+                if ok prev q
+                then loop q (pos +. step) (q :: acc)
+                else (List.rev acc, false)
+      in loop a step []

File src/PaplRRTExpand.mli

 
     The optional parameters [min_stride] and [max_stride] can be used to lower
     or raise the number of elements [n] in the resulting path, depending on the
-    length [len] of the path. The length of the resulting path is at least [len
-    / min_stride] (for [len > min_stride]) and at most [len / max_stride + 1]
+    length [len] of the path. The length of the resulting path is at most [len
+    / min_stride] (for [len > min_stride]) and at least [len / max_stride + 1]
     (assuming [min_stride < max_stride]).
 *)
+
+val intermediary_option :
+  'a PaplInterpolate.option_t ->
+  'a PaplMetric.threshold_t ->
+  'a PaplEdgeConstraint.t ->
+  'a expand_t

File src/PaplSBLExpand.ml

   type t = int * int
   type box_t = V2D.t * V2D.t
 
+  let map_pair = BatTuple.Tuple2.mapn
+
   let round_int x = int_of_float (floor (x +. 0.5))
-  let to_int pair = BatPair.map round_int pair
-  let to_float pair = BatPair.map float_of_int pair
+  let to_int pair = map_pair round_int pair
+  let to_float pair = map_pair float_of_int pair
 
   let standard_boxes = V2D.standard_boxes
   let standard_boxes_by_offset n v =

File src/PaplSampler.ml

 
 type 'a pair_t = 'a * 'a
 
-open BatStd
-
 type 'a t = 'a BatEnum.t
 
 type range_t = float * float
     raise (RangeError ("a > b", (a, b)))
   else a +. float_fun (b -. a)
 
-let get_uniform ?rng range =
-  get_uniform_helper (PaplRandom.float ?rng) range
+let get_uniform ?rng =
+  let float_fun = PaplRandom.float ?rng in
+    fun range -> get_uniform_helper float_fun range
 
 let uniform ?rng range = uniform_helper (PaplRandom.enum_float ?rng) range
 

File src/PaplStopCriteria.ml

   (* This procedure uses a lock to register the time-out of a thread. Using a
      simple [bool ref] doesn't work: Ocaml can't see that the variable is
      volatile. *)
-  let module M = BatMutex.Mutex in
-  let lock = M.create () in
+  let lock = Mutex.create () in
   let _ = Thread.create
     (fun () ->
        Thread.delay time;
-       M.lock lock)
+       Mutex.lock lock)
     ()
   in
   let stopped = ref false in
   let stop () =
     if !stopped then true
-    else if not (M.try_lock lock)
+    else if not (Mutex.try_lock lock)
     then begin
       stopped := true;
       true
     end
     else begin
-      M.unlock lock;
+      Mutex.unlock lock;
       false
     end
   in make stop

File src/PaplTime.ml

 
 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 qt = make t (T.get traj t) in
               qt :: loop (t +. step)
         in loop t0
+
+  let project_interpolate trajectory project q_interpolate =
+    PaplInterpolate.map_option
+      (fun qt ->
+         match project qt.q (PaplTrajectory.get trajectory qt.time) with
+             None -> None
+           | Some q -> Some (make qt.time q))
+      (interpolate q_interpolate)
 end
 
 module Planner = struct
 
   let uniform ?rng range sq = make (PaplSampler.uniform ?rng range) sq
 
-  let get_uniform ?rng range sq =
-    match BatEnum.get sq with
-        None -> None
-      | Some q ->
-          let t = PaplSampler.get_uniform ?rng range in
-            Some (time_space_make t q)
+  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 order_pair_with_constraint constr =
     let open PaplConstraint in
-      BatPair.map (fun c -> c <&> constr) (order_pair ())
+      map_pair (fun c -> c <&> constr) (order_pair ())
 
   let within_time_inorder dist_time =
     PaplConstraint.(<&>)
   module Metric = struct
 
     let to_option_pair dist pair = 
-      BatPair.map
+      map_pair
         (fun constr -> PaplEdgeConstraint.Metric.to_option
            constr dist)
         pair

File src/PaplTime.mli

   val get_uniform : ?rng:rng_t -> range_t -> 'a PaplSampler.t -> 'a t option
 (** A single random configuration sampled from a range and a sampler.
 
-    [get_uniform range sampler] returns a time configuration [{time; q}] where
-    [time] is sampled uniformly at random from [range] and [q] is a sample
-    extracted from [sampler].
-
-    If the range is empty an exception is raised.
+    [get_uniform range sampler] returns a time configuration [Some {time; q}]
+    where [time] is sampled uniformly at random from [range] and [q] is a sample
+    extracted from [sampler]. If [sampler] is empty then [None] is returned.
+    Otherwise if the range is empty an exception is raised.
 *)
 end
 
     If [t0 == neg_infinity] or [t1 = infinity], an exception is raised.
 *)
 
+  val project_interpolate :
+    'target PaplTrajectory.t ->
+    ('q -> 'target -> 'q option) ->
+    'q PaplInterpolate.t ->
+    'q t PaplInterpolate.option_t
+(** Interpolation of time configurations by adjustment of configurations onto a
+    trajectory.
+
+    Let [ip = project_interpolate trajectory project q_interpolate qat qbt]
+    where [(qat, qbt)] is a pair of time configuration. Given [0. <= s <= 1.]
+    the interpolation [ip s] operates as follows:
+
+    - Let [{time = t; q = q} = interpolate q_interpolate qat qbt s].
+    - Let [target] be the value of [trajectory] at position [t].
+    - If [project q target] returns [Some q'] then return [Some {time =
+      t; q = q'}]; otherwise return [None].
+
+    The intended use case of [project_interpolate] is robot path planning for
+    end-effector trajectories.
+*)
+
 end
 
 (** {2 Planners} *)

File src/PaplTrajectory.ml

   See LICENSE file for terms and conditions.
 *)
 
+type rng_t = PaplRandom.rng_t
+
 type 'a t = {
   t0 : float;
   t1 : float;
 
 let empty () = make (0., -1.) (fun _ -> failwith "Internal error: empty.")
 
-let make_fixed range x = make range (BatStd.const x)
+let make_fixed range x = make range (BatPervasives.const x)
 
 let in_range traj t = traj.t0 <= t && t <= traj.t1
 
   in
     loop (traj.t0, x0 traj)
 
+module Sampler = struct
+  let uniform ?rng traj =
+    if is_empty traj then BatEnum.empty ()
+    else
+      let range = range traj in
+      let get_uniform = PaplSampler.get_uniform ?rng in
+      let next () =
+        let t = get_uniform range in
+          get traj t
+      in
+        BatEnum.from next
+      
+  let get_uniform ?rng =
+    let get_uniform = PaplSampler.get_uniform ?rng in
+      fun traj ->
+        let t = get_uniform (range traj) in
+          get traj t
+end
+
 module Tuple2 = struct
   let combine (ta, tb) =
     let t0 = max (t0 ta) (t0 tb) in

File src/PaplTrajectory.mli

     values of some type.
 *)
 
+type rng_t = PaplRandom.rng_t
+
 (** {2 Types} *)
 
 type 'a t
     traj (t +. dt)) > eps]. If no such [t] is found then [None] is returned.
 *)
 
+(** {2 Sampling of trajectories} *)
+
+module Sampler : sig
+  val uniform : ?rng:rng_t -> 'a t -> 'a PaplSampler.t
+(** Uniform sampling of a trajectory.
+
+    The sampler [uniform traj] returns values [get traj t] where [t] is selected
+    uniformly at random from the range of [traj]. If the range is empty then the
+    sampler is empty.
+*)
+
+  val get_uniform : ?rng:rng_t -> 'a t -> 'a
+(** A single configuration sampled uniformly at random from a trajectory.
+
+    [get_uniform traj] returns [get traj t] for a value [t] selected uniformly
+    at random from the range of [traj].
+
+    If the range is empty then an exception is raised.
+*)
+end
+
 (** {2 Tuples} *)
 
 module Tuple2 : sig