1. bergsoe
  2. papl

Commits

bergsoe  committed d0f4ee6

- Planner.fail_if_bad_point_target utility.

- EdgeConstraint.Metric submodule for constrained
metrics.

  • Participants
  • Parent commits 08359e1
  • Branches master

Comments (0)

Files changed (2)

File src/PaplTime.ml

View file
     if reject_within_time dist_time target then
       PaplPlanner.fail "Distance in time from start to goal too great"
 
+  let fail_if_bad_point_target constr dist_time target =
+    fail_if_unordered target;
+    fail_if_too_far dist_time target;
+    if PaplConstraint.reject constr (fst target) then
+      PaplPlanner.fail "Start point is rejected";
+    if PaplConstraint.reject constr (snd target) then
+      PaplPlanner.fail "Goal point is rejected"
+
   let to_trajectory interpolate path_planner =
     PaplPlanner.map_result
       (fun path -> Trajectory.of_path interpolate path)
   let time ec =
     let reject (a, b) = PaplConstraint.reject ec (a.time, b.time) in
       PaplConstraint.make_reject reject
+
+  module Metric = struct
+
+    let to_option_pair dist pair = 
+      BatPair.map
+        (fun constr -> PaplEdgeConstraint.Metric.to_option
+           constr dist)
+        pair
+
+    let order_pair dist =
+      to_option_pair dist (order_pair ())
+
+    let within_time_order_pair dist_time dist =
+      to_option_pair dist
+        (within_time_order_pair dist_time)
+  end
 end
 
 module PlannerConstraint = struct

File src/PaplTime.mli

View file
 
   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
 
 (**
 
   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 ->