Source

papl / src / PaplTime.mli

Full commit
(*
  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

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

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

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

  val q : 'a PaplEdgeConstraint.t -> 'a t PaplEdgeConstraint.t
  val time : float PaplEdgeConstraint.t -> 'a t PaplEdgeConstraint.t

  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

  val time : float PaplPlannerConstraint.t -> 'a t PaplPlannerConstraint.t

  val constrain_by_subdivision :
    'a t PaplConstraint.t ->
    'a t PaplInterpolate.t ->
    float ->
    'a t PaplPlannerConstraint.t
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

  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]
    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
end

(** {2 Paths} *)

module Path : sig
  val of_metric : ?t0:float -> 'a list -> 'a PaplMetric.t -> 'a t list

  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 is equal to [len].

     If the duration of [path] is zero, then [path] should be of length 2.
  *)
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.
*)

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