Commits

bergsoe committed e4c8f94

- New PaplTime.Trajectory.Sampler module similar to
PaplTrajectory.Sampler.

- New PaplTime.Path utilities for concatenation and time shifting of
paths of time stamped values.

  • Participants
  • Parent commits 572fe74

Comments (0)

Files changed (2)

                let t' = t0 +. scale *. (t -. t0) in
                  make t' qt.q)
             path
+
+  let add_time qt dt = make (qt.time +. dt) qt.q
+
+  let rec shift_helper path time =
+    match path with
+        [] -> ([], time)
+      | [qt] ->
+          let qt = add_time qt time in
+            ([qt], qt.time)
+      | qt :: path ->
+          let qt = add_time qt time in
+          let (result, result_time) = shift_helper path time in
+            (qt :: result, result_time)
+
+  let shift path dt = List.map (fun qt -> add_time qt dt) path
+
+  let concat paths =
+    let rec loop paths time =
+      match paths with
+          [] -> []
+        | [] :: paths -> loop paths time
+        | (qt :: _) as path :: paths ->
+            let t0 = qt.time in
+            let (path, time) = shift_helper path (time -. t0) in
+              path :: loop paths time
+    in
+      List.concat (loop paths 0.)
 end
 
 module Trajectory = struct
              None -> None
            | Some q -> Some (make qt.time q))
       (interpolate q_interpolate)
+
+  module Sampler = struct
+    let uniform ?rng traj =
+      if PaplTrajectory.is_empty traj then BatEnum.empty ()
+      else
+        let range = PaplTrajectory.range traj in
+        let get_uniform = PaplSampler.get_uniform ?rng in
+        let next () =
+          let t = get_uniform range in
+            make t (PaplTrajectory.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 (PaplTrajectory.range traj) in
+            make t (PaplTrajectory.get traj t)
+  end
 end
 
 module Planner = struct
 
       If the duration of [path] is zero, then [path] should be of length 2.
   *)
+
+  val shift : 'a t list -> float -> 'a t list
+(** [shift path dt] shifts [path] by adding [dt] to all time values.
+*)
+
+  val concat : 'a t list list -> 'a t list
+(** [concat ~t0 paths] concatenates [paths].
+*)
 end
 
 (** {2 Trajectories} *)
     end-effector trajectories.
 *)
 
+  module Sampler : sig
+    val uniform : ?rng:rng_t -> 'q PaplTrajectory.t -> 'q t PaplSampler.t
+(** Uniform sampling of a trajectory.
+
+    The sampler [uniform traj] returns values [{ time = t; q = 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 -> 'q PaplTrajectory.t -> 'q t
+(** A single configuration sampled uniformly at random from a trajectory.
+
+    [get_uniform traj] returns [{ time = t; q = 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
 end
 
 (** {2 Planners} *)