papl / src / PaplTime.mli

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
(*
  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
*)
(**
   Time configurations
*)

type 'a pair_t = 'a * 'a
type rng_t = PaplRandom.rng_t

(** {2 Types} *)

type 'a t = {
  time : float;
  q : 'a;
}
(** A time configuration. *)

type direction_t = Forward | Backward
(** The direction of a search.

    The value of this flag signifies whether a search goes forward in time
    (towards greater time values) or backwards in time (towards smaller time
    values).
*)

type range_t = float * float
(** A time range.

    A range [(a, b)] represents the interval [\[a, b)].
*)

(** {2 Constructors} *)

val make : float -> 'a -> 'a t
(** Construct a time configuration.

    [make time q] is equivalent to [{ time = time; q = q; }]
*)

(** {2 Operations} *)

val q : 'a t -> 'a
(** The configuration part of a time configuration. *)

val time : 'a t -> float
(** The time value of a time configuration. *)

val map_q : ('a -> 'b) -> 'a t -> 'b t
(** Map a function onto the configuration part of a time configuration
    and keep the time unchanged.
*)

val map_time : (float -> float) -> 'a t -> 'a t
(** Map a function onto the time part of a time configuration
    and keep the configuration unchanged.
*)

val get_range : ('a t * 'a t) -> range_t
(** The time values of a pair of time configurations.
*)

(** {2 Planning spaces}
*)

val interpolate : 'a PaplInterpolate.t -> ('a t) PaplInterpolate.t
(** Interpolation over time configurations.

    The interpolator [interpolate q_interpolate] interpolates the configuration
    part with [q_interpolate] and the time part with standard linear
    interpolation for floats ({! PaplInterpolate.Float.interpolate}).
*)

(** {2 Constraints} *)

(**
   Edge constraints for time configurations.

   Examples include constraints that check the ordering in time and constraints
   for checking that an edge can be traversed in the time specified by the time
   values.
*)
module EdgeConstraint : sig
  val within_time : 'a PaplMetric.t -> 'a t PaplEdgeConstraint.t
    (** [within_time dist_time] accepts [(x, y)] if the traveral time from [x.q]
        to [y.q] according to [dist_time] is smaller than or equal to the actual
        time difference [PaplMetric.time x y].
    *)

  val inorder : unit -> 'a t PaplEdgeConstraint.t
    (** 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 [x.time >= y.time]. *)

  val order_pair : unit -> 'a t PaplEdgeConstraint.t pair_t
    (** [order_pair ()] is equivalent to [(inorder(), not_inorder ())].
    *)

  val within_time_inorder : 'a PaplMetric.t -> 'a t PaplEdgeConstraint.t
    (** [within_time_inorder dist_time] is equivalent to
        [inorder () <&> within_time dist_time].
    *)

  val within_time_not_inorder : 'a PaplMetric.t -> 'a t PaplEdgeConstraint.t
    (** [within_time_inorder dist_time] is equivalent to
        [not_inorder () <&> within_time dist_time].
    *)

  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_inorder dist_time, within_time_not_inorder dist_time)].
    *)

  val constrain_by_subdivision :
    'a t PaplConstraint.t ->
    'a t PaplInterpolate.t ->
    float ->
    'a t PaplEdgeConstraint.t
(** Subdividing edge constraint for time configurations.

    [constrain_by_subdivision constr interpolate dt] splits an edge into steps
    in time of length (at most) [dt] using [interpolate] and verifies the
    configurations by [constr].

    See {! PaplEdgeConstraint.constrain_by_subdivision} for details.
*)

  val q : 'a PaplEdgeConstraint.t -> 'a t PaplEdgeConstraint.t
(** A constraint on the configuration part of a time configuration.

    [q ec] accepts an edge [(qta, qtb)] if [ec] accepts the edge [(qta.q,
    qtb.q)].
*)

  val time : float PaplEdgeConstraint.t -> 'a t PaplEdgeConstraint.t
(** A constraint on the time part of a time configuration.

    [q ec] accepts an edge [(qta, qtb)] if [ec] accepts the edge [(qta.time,
    qtb.time)].
*)

  (** Constrained metrics *)
  module Metric : sig

    val order_pair : 'a t PaplMetric.t -> 'a t PaplMetric.option_t pair_t
    (** [(fwd, bwd) = order_pair dist] is the metric [dist] constrained by
        [inorder ()] and [not_inorder ()] respectively.
    *)

    val within_time_order_pair :
      'a PaplMetric.t ->
      'a t PaplMetric.t ->
      'a t PaplMetric.option_t pair_t
    (** [(fwd, bwd) = within_time_order_pair dist_time dist] is the metric
        [dist] constrained by [within_time_inorder dist_time] and
        [within_time_not_inorder dist_time] respectively.
    *)
  end
end

(**
   Planner constraints for time configurations.
*)
module PlannerConstraint : sig
  val q : 'a PaplPlannerConstraint.t -> 'a t PaplPlannerConstraint.t
(** Planner constraint on the configuration part of a time configuration.
*)

  val time : float PaplPlannerConstraint.t -> 'a t PaplPlannerConstraint.t
(** Planner constraint on the time part of a time configuration.
*)

  val constrain_by_subdivision :
    'a t PaplConstraint.t ->
    'a t PaplInterpolate.t ->
    float ->
    'a t PaplPlannerConstraint.t
(** Subdividing constraint for time configurations.

    See {! EdgeConstraint.constrain_by_subdivision} and {!
    PaplEdgeConstraint.constrain_by_subdivision} for details.
*)
end

(**
   Constraints for time configurations.
*)
module Constraint : sig
  val q : 'a PaplConstraint.t -> 'a t PaplConstraint.t
(** The constraint [q constr] accepts a time configuration [qt] if [constr]
    accepts [qt.q].

    In other words, the constraint ignores the time value and checks the
    configuration value only.
*)

  val time : float PaplConstraint.t -> 'a t PaplConstraint.t
(** The constraint [time constr] accepts a time configuration [qt] if [constr]
    accepts [qt.time].

    In other words, the constraint ignores the configuration value and checks
    the time value only.
*)

  val within_time : 'a PaplMetric.t -> 'a t -> 'a t PaplConstraint.t
(** Time distance constraint.

    The constraint [within_time dist_time qta] accepts configurations [qtb] for
    which the distance between [qta.q] and [qtb.q] measured by [dist_time] is
    less than or equal to the absolute distance between [qta.time] and
    [qtb.time].
*)

  val all_within_time :
    'a PaplMetric.t -> 'a t list -> 'a t PaplConstraint.t
(** Time distance constraint for a set of time configurations.

    The constraint [all_within_time dist_time qts] accepts a configuration [qtb]
    if [within_time dist_time qta] accepts [qtb] for all [qta] in [qts].
*)

  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]
    if [(qt.q, PaplTrajectory.get traj qt.time)] is accepted by [constr].

    In other words, the constraint verifies a configuration in the context of a
    world moving as specified by [traj].
*)
end

(** {2 Metrics} *)

(** Metrics and metric options on time configurations. *)
module Metric : sig
  val time : ('a t) PaplMetric.t
(** The absolute distance in time between a pair of time configurations.

    [time x y == abs_float (x.time -. y.time)]
*)

  val q : 'a PaplMetric.t -> ('a t) PaplMetric.t
(** The distance between the configuration parts.

    The metric [q metric] return the distance between the configuration parts of
    a pair of configurations according to [metric].
*)

  val dist2_sqr : 'a PaplMetric.t -> ('a t) PaplMetric.t
(** Squared Euclidean distance between time configurations.

    [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
(** Euclidean distance between time configurations.

    [dist2 q_metric x y = sqrt (dist2_sqr q_metric x y)]
*)

  val order_option : direction_t -> 'a t PaplMetric.option_t
(** [order_option direction] returns [(Some (time a b))] for a pair [(a, b)] if
    [a] is in the direction [direction] of [b]. Otherwise [None] is returned.
*)

  val order_option_pair : unit -> 'a t PaplMetric.option_t pair_t
(** [order_option_pair ()] is equivalent to [(order_option Forward, order_option
    Backward)].
*)

  val dist_time_option : 'a PaplMetric.t -> 'a t PaplMetric.option_t
(** Given a pair of time points [(a, b)] the metric option [dist_time_option
    dist_time] returns [Some (dist_time a b)] given [(a, b)] if this distance is
    smaller than equal to the distance in time between [a] and [b]. Otherwise
    [None] is returned.
*)

  val dist_time_order_option :
    direction_t -> 'a PaplMetric.t -> 'a t PaplMetric.option_t
(** Given a pair of time points [(a, b)] the metric option
    [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 [a.time >= b.time] should be true. Otherwise
    [None] is returned.
*)

  val dist_time_order_option_pair :
    'a PaplMetric.t -> 'a t PaplMetric.option_t pair_t
    (** [dist_time_order_option_pair dist_time] is equivalent to [m_forward,
        m_backward] where

        [m_forward = dist_time_order_option Forward dist_time]

        and

        [m_backward = dist_time_order_option Backward dist_time].
    *)

(* Some other names and some other functions will be inserted in this module
   when we understand our needs better.
*)
end

(** {2 Sampling} *)

(**
   Sampling of time configurations.
*)
module Sampler : sig
  val make : float PaplSampler.t -> 'a PaplSampler.t -> ('a t) PaplSampler.t
(** Sampling of time and configuration.

    The sampler [s = make st sq] returns time configuration [(t, q)] where [t]
    is sampled by [st] and [q] is sampled by [sq]. If either [st] or [sq]
    becomes empty, then so does [s].
*)

  val uniform :
    ?rng:rng_t -> range_t -> 'a PaplSampler.t -> ('a t) PaplSampler.t
(** Uniform sampling of a time range.

    The sampler [s = uniform range sq] returns time configurations [(t, q)]
    where [t] is sampled uniformly at random from the range [range] and [q] is
    sampled by the sampler [sq].

    [s] is empty if the range is empty or if [sq] becomes empty.
*)

  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 [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

(** {2 Paths} *)

module Path : sig
  val of_metric : ?t0:float -> 'a list -> 'a PaplMetric.t -> 'a t list
(** Time-stamp a list of configuration by a metric.

    [qts = of_metric ~t0 qs dist] is a list of time-stamped configurations
    containing the configurations of [qs]. The time stamp of the first
    configuration [qt(0)] is [t0] (or [0.] if [t0] is omitted) and time stamp of
    all other [qt(i)] is [qt(i-1).time +. dist_time qt(i-1) qt(i)].
*)

  val scale_to : 'a t list -> float -> 'a t list
  (** [scale_to path len] scales the time stamps of [path] such that the
      duration of the path (the difference in time value between the first and
      the last element) is equal to [len].

      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 : ?t0:float -> 'a t list list -> 'a t list
(** Concatenation of time-stamped paths.

    [concat ~t0 paths] concatenates [paths] as if they were ordinary lists but
    time shifts each path to align with the previous path.

    The first path is time shifted by [~t0] ([0.0] by default). The next path is
    time shifted by the time of the last configuration of the previously shifted
    path.
*)
end

(** {2 Trajectories} *)

module Trajectory : sig
  val of_interpolate :
    'a t PaplInterpolate.t ->
    'a t ->
    'a t ->
    'a PaplTrajectory.t
  (** [of_interpolate interpolate a b] constructs a trajectory on the range
      [(a.time, b.time)] using [interpolate] to interpolate from [a] to [b].
  *)

  val of_path : 'a t PaplInterpolate.t -> 'a t list -> 'a PaplTrajectory.t
(** The trajectory [of_path interpolate path] passes through the elements of
    [path], reaching each element [x] of [path] at time [x.time].

    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.
*)

  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.
*)

  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} *)

(**
   General planner utilities for time planning.
*)
module Planner : sig

  val fail_if_unordered : 'a t * 'b t -> unit
(** Fail with an error message if a pair of targets aren't in causal order.

    [fail_if_unordered (qta, qtb)] fails if [qta.time > qtb.time].

    The error is raised by calling {! PaplPlanner.fail}.
*)

  val fail_if_too_far : 'a PaplMetric.t -> 'a t * 'a t -> unit
(** Fail with an error message if the configurations of a pair of time values
    are too far apart that the movement can executed in the allotted time.

    [fail_if_too_far dist_time (qta, qtb)] fails if [dist_time qta.q qtb.q] is
    greater than the absolute distance between [qta.time] and [qtb.time].

    The error is raised by calling {! PaplPlanner.fail}.
*)

  val fail_if_bad_point_target :
    'a t PaplConstraint.t ->
    'a PaplMetric.t ->
    'a t * 'a t ->
    unit
(** [fail_if_bad_point_target constr time_dist target] checks [target] for some
    common errors and calls {! PaplPlanner.fail} with an error message if an
    error is found.

    The procedure checks the following issues:

    - The time of the start point must be less than or equal the time of the
      goal point.

    - The minimum traversal time computed by [time_dist] must be less than or
      equal to the distance between the start time and the goal time.

    - The start and goal points must be valid according to [constr].
*)

  val to_trajectory :
    'q t PaplInterpolate.t ->
    ('target, 'q t) PaplPlanner.path_t ->
    ('target, 'q) PaplPlanner.trajectory_t
(** Path planner to trajectory planner conversion.

    Convert a planner that finds paths of time configurations to a planner that
    finds a trajectory mapping time values to configurations. An interpolator
    for the path must be passed as argument.
*)
end
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.