Commits

bergsoe committed b58bfc3

Rename SBLExpandSampler to SBLExpand.

Comments (0)

Files changed (2)

src/PaplSBLExpand.ml

+(*
+  Copyright (c) 2012 Anders Lau Olsen.
+  See LICENSE file for terms and conditions.
+*)
+type rng_t = PaplRandom.rng_t
+
+let random_range ?rng =
+  let float_fun = PaplRandom.float ?rng in
+    fun (a, b) -> a +. float_fun (b -. a)
+
+let stop () = raise BatEnum.No_more_elements
+
+module EST = struct
+  type 'a expand_sampler_t = 'a PaplSBL.EST.expand_sampler_t
+
+  let make_standard_boxes scale n (lower, upper) =
+    BatArray.of_enum
+      (BatEnum.take n
+         (BatEnum.from_loop 1.0
+            (fun n ->
+               let s = 1.0 /. n in
+                 (scale s lower, scale s upper), n +. 1.)))
+
+  module Float = struct
+    type t = float
+    type box_t = t * t
+
+    let standard_boxes n box = make_standard_boxes ( *. ) n box
+
+    let standard_boxes_by_offset n d = standard_boxes n (-.d, d)
+
+    let uniform_within_boxes ?rng outer inner_boxes =
+      let (ol, ou) = outer in
+      let rnd = random_range ?rng in
+      let box_cnt = Array.length inner_boxes in
+        fun q ->
+          let box = ref 0 in
+          let next () =
+            if !box >= box_cnt then stop ()
+            else
+              let (il, iu) = inner_boxes.(!box) in
+              let () = box := !box + 1 in
+              let lower = max ol (q +. il) in
+              let upper = min ou (q +. iu) in
+                if lower <= upper then
+                  rnd (lower, upper)
+                else
+                  stop ()
+          in
+            BatEnum.from next
+  end
+
+  module Q = struct
+    type t = PaplQ.t
+    type box_t = t * t
+
+    let standard_boxes n box =
+      make_standard_boxes PaplQ.( *: ) n box
+
+    let standard_boxes_by_offset n v = standard_boxes n (PaplQ.neg v, v)
+
+    let uniform_within_boxes ?rng (ol, ou) inner_boxes =
+      let rnd = random_range ?rng in
+      let box_cnt = Array.length inner_boxes in
+        fun q ->
+          let n = Array.length q in
+          let box = ref 0 in
+          let next () =
+            if !box >= box_cnt then stop ()
+            else begin
+              let r = Array.make n 0. in
+              let (il, iu) = inner_boxes.(!box) in
+              let () = box := !box + 1 in
+              let rec loop i =
+                if i >= n then r
+                else
+                  let lower = max ol.(i) (q.(i) +. il.(i)) in
+                  let upper = min ou.(i) (q.(i) +. iu.(i)) in
+                    if lower <= upper then begin
+                      r.(i) <- rnd (lower, upper);
+                      loop (i + 1)
+                    end
+                    else
+                      stop ()
+              in
+                loop 0
+            end
+          in BatEnum.from next
+  end
+
+  module V2D = struct
+    type t = PaplVector.V2D.t
+    type box_t = t * t
+
+    module V = PaplVector.V2D
+
+    let standard_boxes n box =
+      make_standard_boxes V.( *: ) n box
+
+    let standard_boxes_by_offset n v = standard_boxes n (V.neg v, v)
+
+    let uniform_within_boxes ?rng outer inner_boxes =
+      let ((ol0, ol1), (ou0, ou1)) = outer in
+      let rnd = random_range ?rng in
+      let box_cnt = Array.length inner_boxes in
+        fun (q0, q1) ->
+          let box = ref 0 in
+          let next () =
+            if !box >= box_cnt then stop ()
+            else
+              let ((il0, il1), (iu0, iu1)) = inner_boxes.(!box) in
+              let () = box := !box + 1 in
+              let l0 = max ol0 (q0 +. il0) in
+              let l1 = max ol1 (q1 +. il1) in
+              let u0 = min ou0 (q0 +. iu0) in
+              let u1 = min ou1 (q1 +. iu1) in
+                if l0 <= u0 && l1 <= u1 then
+                  (rnd (l0, u0), rnd (l1, u1))
+                else
+                  stop ()
+          in
+            BatEnum.from next
+
+    let uniform_standard ?rng n outer_box offset =
+      let inner_boxes = standard_boxes_by_offset n offset in
+        uniform_within_boxes ?rng outer_box inner_boxes
+  end
+
+  module V3D = struct
+    type t = PaplVector.V3D.t
+    type box_t = t * t
+
+    module V = PaplVector.V3D
+
+    let standard_boxes n box =
+      make_standard_boxes V.( *: ) n box
+
+    let standard_boxes_by_offset n v = standard_boxes n (V.neg v, v)
+
+    let uniform_within_boxes ?rng outer inner_boxes =
+      let ((ol0, ol1, ol2), (ou0, ou1, ou2)) = outer in
+      let rnd = random_range ?rng in
+      let box_cnt = Array.length inner_boxes in
+        fun (q0, q1, q2) ->
+          let box = ref 0 in
+          let next () =
+            if !box >= box_cnt then stop ()
+            else
+              let ((il0, il1, il2), (iu0, iu1, iu2)) = inner_boxes.(!box) in
+              let () = box := !box + 1 in
+              let l0 = max ol0 (q0 +. il0) in
+              let l1 = max ol1 (q1 +. il1) in
+              let l2 = max ol2 (q2 +. il2) in
+              let u0 = min ou0 (q0 +. iu0) in
+              let u1 = min ou1 (q1 +. iu1) in
+              let u2 = min ou2 (q2 +. iu2) in
+                if l0 <= u0 && l1 <= u1 && l2 <= u2 then
+                  (rnd (l0, u0), rnd (l1, u1), rnd (l2, u2))
+                else
+                  stop ()
+          in
+            BatEnum.from next
+
+    let uniform_standard ?rng n outer_box offset =
+      let inner_boxes = standard_boxes_by_offset n offset in
+        uniform_within_boxes ?rng outer_box inner_boxes
+  end
+
+  module I2D = struct
+    type t = int * int
+    type box_t = V2D.t * V2D.t
+
+    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 standard_boxes = V2D.standard_boxes
+    let standard_boxes_by_offset n v =
+      V2D.standard_boxes_by_offset n (to_float v)
+
+    let uniform_within_boxes ?rng outer inner_boxes =
+      let make = V2D.uniform_within_boxes ?rng outer inner_boxes in
+        fun q -> BatEnum.map to_int (make (to_float q))
+  end
+
+  module SO2 = struct
+    type t = PaplTransform.SO2.t
+    type box_t = float * float
+
+    module SO = PaplTransform.SO2
+
+    let standard_boxes n box = make_standard_boxes ( *. ) n box
+
+    let standard_boxes_by_offset n v = standard_boxes n (-. v, v)
+
+    let uniform_within_boxes ?rng accept inner_boxes =
+      let rnd = random_range ?rng in
+      let box_cnt = Array.length inner_boxes in
+        fun rot ->
+          let q = SO.angle rot in
+          let box = ref 0 in
+          let next () =
+            if !box >= box_cnt then stop ()
+            else
+              let (il, iu) = inner_boxes.(!box) in
+              let () = box := !box + 1 in
+                match accept (rnd (q +. il, q +. iu)) with
+                    None -> stop ()
+                  | Some q -> q
+          in
+            BatEnum.from next
+
+    let uniform_standard ?rng n offset =
+      uniform_within_boxes
+        ?rng
+        (fun a -> Some (PaplTransform.SO2.rotate a))
+        (standard_boxes_by_offset n offset)
+  end
+
+  let make_se get make make_pos make_rot = (); fun q ->
+    let (pos, rot) = get q in
+      BatEnum.map
+        (fun (t, r) -> make t r)
+        (BatEnum.combine
+           (make_pos pos, make_rot rot))
+
+  module SE2 = struct
+    type t = PaplTransform.SE2.t
+    type pos_box_t = PaplVector.V2D.t * PaplVector.V2D.t
+    type rot_box_t = float * float
+
+    module SE = PaplTransform.SE2
+
+    let make make_pos make_rot =
+      make_se SE.get SE.make make_pos make_rot
+  end
+
+  module SO3 = struct
+    type t = PaplTransform.SO3.t
+    type box_t = float * float
+
+    module SO = PaplTransform.SO3
+
+    let standard_offsets ?rng n offset =
+      BatArray.of_enum
+        (BatEnum.take n
+           (BatEnum.from_loop 1.0
+              (fun n ->
+                 let s = 1.0 /. n in
+                   (s *. offset, n +. 1.))))
+
+    let uniform_within_boxes ?rng accept offsets =
+      let generators =
+        BatArray.map
+          (fun offset ->
+             SO.Sampler.uniform_offset ?rng offset)
+          offsets
+      in
+        fun rot ->
+          BatEnum.map
+            (fun e ->
+               let step =
+                 match BatEnum.get e with
+                     Some step ->
+                       if accept step
+                       then step
+                       else stop ()
+                   | None -> stop ()
+               in
+                 SO.mul rot step)
+            (BatArray.enum generators)
+
+    let uniform_standard ?rng n offset =
+      uniform_within_boxes
+        ?rng
+        (fun _ -> true)
+        (standard_offsets n offset)
+  end
+
+  module SE3 = struct
+    type t = PaplTransform.SE3.t
+    type pos_box_t = PaplVector.V3D.t * PaplVector.V3D.t
+
+    module SE = PaplTransform.SE3
+
+    let make make_pos make_rot =
+      make_se SE.get SE.make make_pos make_rot
+  end
+
+  module Time = struct
+    type 'a t = 'a PaplTime.t
+
+    let make expand_time expand_q =
+      let make qt =
+        let time_sampler = expand_time qt.PaplTime.time in
+        let q_sampler = expand_q qt.PaplTime.q in
+          BatEnum.map
+            (fun (t, q) -> PaplTime.make t q)
+            (BatEnum.combine (time_sampler, q_sampler))
+            (* Why doesn't BatEnum have a [map2]? *)
+      in
+        make
+
+    let scale_offset expand interpolate dist_time =
+      let dist_time = PaplTime.Metric.q dist_time in
+      let make qat =
+        let adjust qbt =
+          let dt = PaplTime.Metric.time qat qbt in
+          let dtq = dist_time qat qbt in
+            if dtq > dt then
+              let q = interpolate qat.PaplTime.q qbt.PaplTime.q (dt /. dtq) in
+                PaplTime.make qbt.PaplTime.time q
+            else
+              qbt
+        in
+          BatEnum.map adjust (expand qat)
+      in
+        make
+
+    let scale_speed expand dist_time =
+      let make qat =
+        let adjust qbt =
+          let dt = PaplTime.Metric.time qat qbt in
+          let dtq = PaplTime.Metric.q dist_time qat qbt in
+            if dtq > dt then
+              let sign =
+                if qbt.PaplTime.time < qat.PaplTime.time
+                then -1.
+                else 1.
+              in
+              let t = qat.PaplTime.time +. sign *. dtq in
+                PaplTime.make t qbt.PaplTime.q
+            else
+              qbt
+        in
+          BatEnum.map adjust (expand qat)
+      in
+        make
+  end
+end
+
+module RRT = struct
+  type 'a expand_sampler_t = 'a PaplSBL.RRT.expand_sampler_t
+  type box_t = float * float
+
+  let random_subdivide_n_helper
+      float_fun n interpolate dist (lower, upper) =
+    let min_pos = lower /. upper in
+    fun qa qb ->
+      let ip = interpolate qa qb in
+      let len = dist qa qb in
+        if len < lower then
+          BatEnum.singleton qb
+        else
+          BatEnum.unfold
+            (n, min len upper /. len)
+            (fun (n, s) ->
+                 if n == 0 then None
+                 else
+                   let s = float_fun s in
+                     if s < min_pos then
+                       let q = ip min_pos in
+                         Some (q, (0, 0.))
+                     else
+                       let q = ip s in
+                         Some (q, (n - 1, 0.5 *. s)))
+
+  let subdivide_n n interpolate dist range =
+    random_subdivide_n_helper
+      (fun s -> s) n interpolate dist range
+
+  let random_subdivide_n ?rng n interpolate dist range =
+    random_subdivide_n_helper
+      (PaplRandom.float ?rng) n interpolate dist range
+
+  let fixed_steps interpolate dist steps =
+    fun qa qb ->
+      let ip = interpolate qa qb in
+      let len = dist qa qb in
+      let rec get xs =
+        match xs with
+            [] -> None
+          | pos :: xs ->
+              if pos > len then get xs
+              else
+                let s = pos /. len in
+                  Some (ip s, xs)
+      in
+        BatEnum.unfold steps get
+
+  let uniform_within_boxes ?rng interpolate boxes = ();
+    let rnd = random_range ?rng in
+    let make_sampler () = BatEnum.map rnd (BatArray.enum boxes)
+    in
+      fun qa qb ->
+        let sampler = make_sampler () in
+          BatEnum.map (fun s -> interpolate qa qb s) sampler
+
+  let concat es = ();
+    fun qa qb ->
+      BatEnum.concat
+        (BatList.enum
+           (BatList.map (fun e -> e qa qb) es))
+end

src/PaplSBLExpand.mli

+(*
+  Copyright (c) 2012 Anders Lau Olsen.
+  See LICENSE file for terms and conditions.
+*)
+(** SBL expansion samplers
+
+    This module constructs samplers that return configurations within the
+    vicinity of some configuration.
+*)
+
+type rng_t = PaplRandom.rng_t
+
+module EST : sig
+  type 'a expand_sampler_t = 'a PaplSBL.EST.expand_sampler_t
+
+  module Float : sig
+    type t = float
+    type box_t = t * t
+
+    val standard_boxes : int -> box_t -> box_t array
+    val standard_boxes_by_offset : int -> t -> box_t array
+
+    val uniform_within_boxes :
+      ?rng:rng_t -> box_t -> box_t array -> t expand_sampler_t
+  end
+
+  module Q : sig
+    type t = PaplQ.t
+    type box_t = t * t
+
+    val standard_boxes : int -> box_t -> box_t array
+    val standard_boxes_by_offset : int -> t -> box_t array
+
+    val uniform_within_boxes :
+      ?rng:rng_t -> box_t -> box_t array -> t expand_sampler_t
+  end
+
+  module V2D : sig
+    type t = PaplVector.V2D.t
+    type box_t = t * t
+
+    val standard_boxes : int -> box_t -> box_t array
+      (** [standard_boxes n box] is an array of [n] boxes [\[ box(1); box(2); ...;
+          box(n)\]] where [box(i)] is equal to [box] scaled by [1 / i].
+      *)
+
+    val standard_boxes_by_offset : int -> t -> box_t array
+      (** [standard_boxes_by_offset n vec] is equivalent to
+          [standard_boxes n (neg v, v)].
+      *)
+
+    val uniform_within_boxes :
+      ?rng:rng_t -> box_t -> box_t array -> t expand_sampler_t
+
+    val uniform_standard :
+      ?rng:rng_t -> int -> box_t -> t -> t expand_sampler_t
+  end
+
+  module V3D : sig
+    type t = PaplVector.V3D.t
+    type box_t = t * t
+
+    val standard_boxes : int -> box_t -> box_t array
+      (** [standard_boxes n box] is an array of [n] boxes [\[ box(1); box(2); ...;
+          box(n)\]] where [box(i)] is equal to [box] scaled by [1 / i].
+      *)
+
+    val standard_boxes_by_offset : int -> t -> box_t array
+      (** [standard_boxes_by_offset n vec] is equivalent to
+          [standard_boxes n (neg v, v)].
+      *)
+
+    val uniform_within_boxes :
+      ?rng:rng_t -> box_t -> box_t array -> t expand_sampler_t
+
+    val uniform_standard :
+      ?rng:rng_t -> int -> box_t -> t -> t expand_sampler_t
+  end
+
+  module I2D : sig
+    type t = int * int
+    type box_t = V2D.t * V2D.t
+
+    val to_int : V2D.t -> t
+    val to_float : t -> V2D.t
+
+    val standard_boxes : int -> box_t -> box_t array
+    val standard_boxes_by_offset : int -> t -> box_t array
+
+    val uniform_within_boxes :
+      ?rng:rng_t -> box_t -> box_t array -> t expand_sampler_t
+  end
+
+  module SO2 : sig
+    type t = PaplTransform.SO2.t
+    type box_t = float * float
+
+    val standard_boxes : int -> box_t -> box_t array
+    val standard_boxes_by_offset : int -> float -> box_t array
+
+    val uniform_within_boxes :
+      ?rng:rng_t ->
+      (float -> 'a option) ->
+      box_t array ->
+      t -> 'a BatEnum.t
+
+    val uniform_standard : ?rng:rng_t -> int -> float -> t expand_sampler_t
+  end
+
+  module SO3 : sig
+    type t = PaplTransform.SO3.t
+    type box_t = float * float
+
+    val standard_offsets : ?rng:rng_t -> int -> float -> float array
+
+    val uniform_within_boxes :
+      ?rng:rng_t ->
+      (t -> bool) ->
+      float array ->
+      t expand_sampler_t
+
+    val uniform_standard : ?rng:rng_t -> int -> float -> t expand_sampler_t
+  end
+
+  module SE2 : sig
+    type t = PaplTransform.SE2.t
+    type pos_box_t = PaplVector.V2D.t * PaplVector.V2D.t
+    type rot_box_t = float * float
+
+    val make :
+      V2D.t expand_sampler_t ->
+      SO2.t expand_sampler_t ->
+      t expand_sampler_t
+  end
+
+  module SE3 : sig
+    type t = PaplTransform.SE3.t
+    type pos_box_t = PaplVector.V3D.t * PaplVector.V3D.t
+
+    val make :
+      V3D.t expand_sampler_t ->
+      SO3.t expand_sampler_t ->
+      t expand_sampler_t
+  end
+
+  module Time : sig
+    type 'a t = 'a PaplTime.t
+
+    val make : float expand_sampler_t -> 'a expand_sampler_t -> 'a t expand_sampler_t
+  (** Construct an extension function for time values.
+
+      The extension function [make time_expand q_expand] collects time values from
+      [time_expand] and configuration values from [q_expand] to produce a stream
+      of time configurations.
+  *)
+
+    val scale_offset :
+      'a t expand_sampler_t ->
+      'a PaplInterpolate.t ->
+      'a PaplMetric.t ->
+      'a t expand_sampler_t
+  (** The sampler of [scale_offset expand interpolate dist_time q] retrieves
+      samples [q'] from [expand q]. If the distance in time between [q] and [q']
+      is greater or equal to the traversal time according to [dist_time] then [q']
+      is returned. Otherwise the path from [q] to [q'] is shortened using
+      [interpolate] such that the traversal time equals the time distance.
+
+      The metric and interpolation function must be compatible.
+  *)
+
+    val scale_speed :
+      'a t expand_sampler_t ->
+      'a PaplMetric.t ->
+      'a t expand_sampler_t
+  (** The sampler of [scale_speed expand dist_time q] retrieves samples [q'] from
+      [expand q]. If the distance in time between [q] and [q'] is greater or equal
+      to the traversal time according to [dist_time] then [q'] is returned.
+      Otherwise the time value of [q'] is set according to the traversal time.
+  *)
+  end
+end
+
+module RRT : sig
+  type box_t = float * float
+  type 'a expand_sampler_t = 'a PaplSBL.RRT.expand_sampler_t
+
+  val subdivide_n :
+    int -> (* n : if negative then continue to [lower]. *)
+    'a PaplInterpolate.t ->
+    'a PaplMetric.t ->
+    (float * float) -> (* (lower, upper) *)
+    'a expand_sampler_t
+
+  val random_subdivide_n :
+    ?rng:rng_t ->
+    int -> (* n : if negative then continue to [lower]. *)
+    'a PaplInterpolate.t ->
+    'a PaplMetric.t ->
+    (float * float) -> (* (lower, upper) *)
+    'a expand_sampler_t
+
+  val fixed_steps :
+    'a PaplInterpolate.t ->
+    'a PaplMetric.t ->
+    float list ->
+    'a expand_sampler_t
+
+  val uniform_within_boxes :
+    ?rng:rng_t ->
+    'a PaplInterpolate.t -> box_t array -> 'a expand_sampler_t
+
+  val concat :
+    'a expand_sampler_t list ->
+    'a expand_sampler_t
+end