bergsoe avatar bergsoe committed de0c559

Update of PaplTime to match the conventions of PaplRRT and
PaplSBL.

The argument order of the various [order] functions has been swapped.

New [Trajectory.discretize] utility.

Comments (0)

Files changed (2)

 
 let map_time f qt = make (f qt.time) qt.q
 
+let q qt = qt.q
+
+let time qt = qt.time
+
 let get_range (qa, qb) = qa.time, qb.time
-let in_range (a, b) t = a <= t && t <= b
 
 let in_range (a, b) t = a <= t && t <= b
 
 
 let reject_direction direction =
   match direction with
-      Forward -> (fun (this, other) -> other.time > this.time)
-    | Backward -> (fun (this, other) -> other.time < this.time)
+      Forward -> (fun (this, other) -> this.time > other.time)
+    | Backward -> (fun (this, other) -> other.time > this.time)
 
 module Metric = struct
   let time x y = abs_float (x.time -. y.time)
       path in
     let t0 = (List.hd path).time in
       PaplTrajectory.of_list ~t0:t0 trajectories
+
+  let discretize ?t0 ?t1 traj step =
+    let module T = PaplTrajectory in
+    let t0 = BatOption.default (T.t0 traj) t0 in
+    let t1 = BatOption.default (T.t1 traj) t1 in
+      if t0 == neg_infinity then
+        failwith "PaplTime.Trajectory.discretize: t0 == neg_infinity";
+      if t1 == infinity then
+        failwith "PaplTime.Trajectory.discretize: t1 == infinity";
+
+      if t0 > t1 then []
+      else if t0 == t1 then
+        let q = T.get traj t0 in [make t0 q; make t0 q]
+      else
+        let rec loop t =
+          if t > t1 then
+            [make t1 (T.get traj t1)]
+          else
+            let qt = make t (T.get traj t) in
+              qt :: loop (t +. step)
+        in loop t0
 end
 
 module Planner = struct
     let reject y = reject_within_time dist_time (x, y) in
       PaplConstraint.make_reject reject
 
+  let all_within_time dist_time qts =
+    PaplConstraint.merge
+      (List.map (within_time dist_time) qts)
+
   let by_trajectory constr traj =
     PaplConstraint.make_reject
       (fun { time = t; q = q } ->
   let order direction =
     PaplConstraint.make_reject (reject_direction direction)
 
-  let inorder () = order Backward
+  let inorder () = order Forward
 
-  let not_inorder () = order Forward
+  let not_inorder () = order Backward
 
   let order_pair () = (order Forward, order Backward)
 
       (within_time dist_time)
 
   let within_time_order_pair dist_time =
-    (within_time_not_inorder dist_time,
-     within_time_inorder dist_time)
+    (within_time_inorder dist_time,
+     within_time_not_inorder dist_time)
 
   let q ec =
     let reject (a, b) = PaplConstraint.reject ec (a.q, b.q) in
 
 (** {2 Operations} *)
 
+val q : 'a t -> 'a
+
+val time : 'a t -> float
+
 val map_q : ('a -> 'b) -> 'a t -> 'b t
 
 val map_time : (float -> float) -> 'a t -> 'a t
 
+val get_range : ('a t * 'a t) -> range_t
+
 (** {2 Planning spaces}
 *)
 
     (** The [inorder ()] constraint accepts [(x, y)] iff [x.time <= y.time]. *)
 
   val not_inorder : unit -> 'a t PaplEdgeConstraint.t
-    (** The [not_inorder ()] constraint accepts [(x, y)] iff [y.time <= x.time]. *)
+    (** The [not_inorder ()] constraint accepts [(x, y)] iff [x.time >= y.time]. *)
 
   val order_pair : unit -> 'a t PaplEdgeConstraint.t pair_t
-    (** [order_pair ()] is equivalent to [(not_inorder(), inorder ())].
+    (** [order_pair ()] is equivalent to [(inorder(), not_inorder ())].
     *)
 
   val within_time_inorder : 'a PaplMetric.t -> 'a t PaplEdgeConstraint.t
 
   val within_time_order_pair : 'a PaplMetric.t -> 'a t PaplEdgeConstraint.t pair_t
     (** [within_time_order_pair dist_time] is equivalent to
-        [(within_time_not_inorder dist_time, within_time_inorder dist_time)].
+        [(within_time_inorder dist_time, within_time_not_inorder dist_time)].
     *)
 
   val constrain_by_subdivision :
     the time value only.
 *)
 
+  val within_time : 'a PaplMetric.t -> 'a t -> 'a t PaplConstraint.t
+
+  val all_within_time :
+    'a PaplMetric.t -> 'a t list -> 'a t PaplConstraint.t
+
   val by_trajectory :
     ('a * 'b) PaplConstraint.t -> 'b PaplTrajectory.t -> 'a t PaplConstraint.t
 (** The constraint [by_trajectory constr traj] accepts a time configuration [qt]
   val dist2_sqr : 'a PaplMetric.t -> ('a t) PaplMetric.t
 (** Squared Euclidean distance between time configurations.
 
-    [dist2 q_metric x y] returns [dt**2 +. dq**2] where [dt] is the difference
-    in time between [x] and [y], and [dq] is the difference between [x] and [y]
-    according to [q_metric].
+    [dist2_sqr q_metric x y] returns [dt**2 +. dq**2] where [dt] is the
+    difference in time between [x] and [y], and [dq] is the difference between
+    [x] and [y] according to [q_metric].
 *)
 
   val dist2 : 'a PaplMetric.t -> ('a t) PaplMetric.t
     [dist_time_order_option direction dist_time] returns [Some (dist_time a b)]
     if the this distance is smaller than or equal to the distance in time
     between [a] and [b] {i and} [a] is in the direction [direction] relative to
-    [b]. If [direction] is [Forward] then [a.time >= b.time] should be true; if
-    [direction] is [Backward] then [b.time >= a.time] should be true. Otherwise
+    [b]. If [direction] is [Forward] then [a.time <= b.time] should be true; if
+    [direction] is [Backward] then [a.time >= b.time] should be true. Otherwise
     [None] is returned.
 *)
 
 
     The elements of [path] must have increasing time values.
 *)
+
+  val discretize :
+    ?t0:float ->
+    ?t1:float ->
+    'a PaplTrajectory.t ->
+    float ->
+    'a t list
+(** Discretize a trajectory into a time path.
+
+    The path [discretize t0 t1 traj step] returns the values of [traj] at the
+    positions [t0, t0 + step, t0 + step + step, ..., t1].
+
+    If [t0 > t1] then the empty path is returned.
+
+    Otherwise a path with at least 2 elements is always returned. The first
+    element of the path is always [get traj t0] and the last element is always
+    [get traj t1].
+
+    The distance in time between the last two elements can be smaller than
+    [step] (and generally is).
+
+    The default values for the optional parameters [t0] and [t1] are [traj.t0]
+    and [traj.t1].
+
+    If [t0 == neg_infinity] or [t1 = infinity], an exception is raised.
+*)
+
 end
 
 (** {2 Planners} *)
    General planner utilities for time planning.
 *)
 module Planner : sig
+
+  val fail_if_unordered : 'a t * 'b t -> unit
+
+  val fail_if_too_far : 'a PaplMetric.t -> 'a t * 'a t -> unit
+
   val to_trajectory :
     'q t PaplInterpolate.t ->
     ('target, 'q t) PaplPlanner.path_t ->
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.