Commits

bergsoe  committed e4cff8d

Rename SBLExpandSampler to SBLExpand.

  • Participants
  • Parent commits b58bfc3

Comments (0)

Files changed (4)

     PaplDLTreePlanner,
     PaplTreePlanner,
     PaplSBL,
-    PaplSBLExpandSampler,
+    PaplSBLExpand,
     PaplTree,
     PaplIndex,
     PaplTreeIndex,
 module QTime = PaplQTime
 module RRT = PaplRRT
 module Sampler = PaplSampler
-module SBLExpandSampler = PaplSBLExpandSampler
+module SBLExpand = PaplSBLExpand
 module SBL = PaplSBL
 module SpatialGroup = PaplSpatialGroup
 module StopCriteria = PaplStopCriteria

File src/PaplSBLExpandSampler.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

File src/PaplSBLExpandSampler.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