Commits

bergsoe committed 7c953cb

Initial commit.

Comments (0)

Files changed (67)

+OASISFormat: 0.2
+OCamlVersion: >= 3.12
+Name: Papl
+Version: 0.0.0
+Synopsis: Sampling based path planning library
+Description:
+
+  Papl is a library for sampling based path planning. Unlike most path
+  planning systems Papl does not define the robots or mechanisms for
+  which the planning is done. The user provides collision detection
+  and other operations for the configuration space, and the library
+  constructs the planners.
+
+Authors: Anders Lau Olsen
+Copyrights: (C) 2012 Anders Lau Olsen
+License: LGPL-2.1 with OCaml linking exception
+LicenseFile: LICENSE
+BuildTools: ocamlbuild, camlp4o
+Plugins:
+  META (0.2),
+  StdFiles (0.2)
+
+XStdFilesREADME: false
+XStdFilesREADMEFilename: README.txt
+XStdFilesINSTALL: true
+XStdFilesINSTALLFilename: INSTALL.txt
+XStdFilesAUTHORS: false
+XStdFilesAUTHORSFilename: AUTHORS.txt
+
+#----------------------------------------------------------------------
+# Papl
+
+# Papl library
+Library papl
+  Path: src
+  FindlibName: papl
+  Install:  true
+  CompiledObject: native
+  NativeOpt: -inline 3
+  Modules:
+    Papl,
+    PaplLog,
+    PaplPair,
+    PaplPath,
+    PaplSubdivision,
+    PaplInterpolate,
+    PaplStopCriteria,
+    PaplPlanner,
+    PaplMetric,
+    PaplConstraint,
+    PaplEdge,
+    PaplIncrConstraint,
+    PaplEdgeConstraint,
+    PaplPlannerConstraint,
+    PaplIncrEdgeConstraint,
+    PaplSampler,
+    PaplStraightPlanner,
+    PaplSpatialGroup,
+    PaplTransform,
+    PaplTrajectory,
+    PaplVectorMetric,
+    PaplVector,
+    PaplTime,
+    PaplDLTreePlanner,
+    PaplTreePlanner,
+    PaplSBL,
+    PaplSBLExpandSampler,
+    PaplTree,
+    PaplIndex,
+    PaplTreeIndex,
+    PaplDLTree,
+    PaplRRT,
+    PaplQ,
+    PaplQTime
+  BuildDepends:
+    threads,
+    str,
+    bigarray,
+    camomile,
+    batteries
+
+module Constraint = PaplConstraint
+module DLTree = PaplDLTree
+module DLTreePlanner = PaplDLTreePlanner
+module EdgeConstraint = PaplEdgeConstraint
+module Edge = PaplEdge
+module IncrConstraint = PaplIncrConstraint
+module IncrEdgeConstraint = PaplIncrEdgeConstraint
+module Index = PaplIndex
+module Interpolate = PaplInterpolate
+module Log = PaplLog
+module Metric = PaplMetric
+module Pair = PaplPair
+module Path = PaplPath
+module PlannerConstraint = PaplPlannerConstraint
+module Planner = PaplPlanner
+module Q = PaplQ
+module QTime = PaplQTime
+module RRT = PaplRRT
+module Sampler = PaplSampler
+module SBLExpandSampler = PaplSBLExpandSampler
+module SBL = PaplSBL
+module SpatialGroup = PaplSpatialGroup
+module StopCriteria = PaplStopCriteria
+module StraightPlanner = PaplStraightPlanner
+module Subdivision = PaplSubdivision
+module Time = PaplTime
+module Trajectory = PaplTrajectory
+module Transform = PaplTransform
+module TreeIndex = PaplTreeIndex
+module Tree = PaplTree
+module TreePlanner = PaplTreePlanner
+module VectorMetric = PaplVectorMetric
+module Vector = PaplVector

src/PaplConstraint.ml

+
+type 'a t = 'a -> bool
+
+let accept f x = not (f x)
+
+let reject f x = f x
+
+let make_reject f = f
+
+let make_accept f = (); fun x -> not (f x)
+
+let (<&>) ca cb = (); fun x -> ca x || cb x
+
+let merge cs = (); fun x -> BatEnum.exists (fun c -> c x) cs
+
+let fixed_reject res = (); fun _ -> res
+
+let fixed_accept res = (); fun _ -> not res
+
+let invert constr = (); fun x -> not (constr x)
+
+let bind get cs = (); fun x -> cs (get x)
+
+let constrain_path c =
+  let reject = List.exists (reject c) in
+    make_reject reject
+
+(*
+  Other constraints:
+
+  - [novel metric minDist values] checks if a value is at least a distance
+    [minDist] away from all elements of [values].
+
+    A version of [novel] may collect the values it is given to produce an
+    increasingly stronger constraint.
+
+  - A constraint that counts the number of times it has been invoked.
+*)

src/PaplConstraint.mli

+(** Constraints on values
+
+    A constraint is effectively a boolean function that either {i accepts} or {i
+    rejects} a value.
+
+    If [c] is a constraint and [x] is a value, then [x] is accepted if [accept c
+    x] and rejected if [reject c x]. [accept c x] is equivalent to [not (reject
+    c x)].
+*)
+
+(** {2 Constraints} *)
+
+(** {3 Types} *)
+
+type 'a t
+
+(** {3 Operations} *)
+
+val accept : 'a t -> 'a -> bool
+
+val reject : 'a t -> 'a -> bool
+
+(** {3 Constructors} *)
+
+val make_accept : ('a -> bool) -> 'a t
+
+val make_reject : ('a -> bool) -> 'a t
+
+val fixed_reject : bool -> 'a t
+(** A fixed constraint.
+
+    [reject (fixed_reject b) x] returns [b] for all [x].
+*)
+
+val fixed_accept : bool -> 'a t
+(** A fixed constraint.
+
+    [accept (fixed_accept b) [x]] returns [b] for all [x].
+*)
+
+val (<&>) : 'a t -> 'a t -> 'a t
+(** Composition of constraints.
+
+    The constraint [ca <&> cb] accepts a value [x] if [ca] and [cb] both accept
+    [x].
+*)
+
+val merge : ('a t) BatEnum.t -> 'a t
+
+val invert : 'a t -> 'a t
+(** Logical inversion of a constraint.
+
+    The constraint [c' = invert c] returns [not b] where [b] is the value
+    returned by [c].
+*)
+
+val bind : ('b -> 'a) -> 'a t -> 'b t
+
+val constrain_path : 'a t -> 'a list t
+
+module A = BatDynArray
+
+type 'a node = {
+  value : 'a;
+  mutable parent : 'a node option;
+  children : 'a node A.t
+}
+
+let make_node q parent =
+  let node = {
+    value = q;
+    parent = Some parent;
+    children = A.create ();
+  } in
+    A.add parent.children node;
+    node
+
+let make_root_node q = {
+  value = q;
+  parent = None;
+  children = A.create ();
+}
+
+let add_root q = make_root_node q
+
+let is_root node = BatOption.is_none node.parent
+
+let add parent q = make_node q parent
+
+let get node = node.value
+
+let parent node = node.parent
+
+let children node = A.enum node.children
+
+let rec iter f node =
+  f node;
+  A.iter (fun node -> iter f node) node.children 
+
+let rec fold_node f acc node =
+  let acc = f acc node in
+    A.fold_left
+      (fun node acc -> fold_node f node acc)
+      acc
+      node.children
+
+let to_single_linked node =
+  let rec collect parent cs all_nodes =
+    match BatEnum.get cs with
+        None -> parent :: all_nodes
+      | Some child ->
+          let node = PaplTree.add parent (get child) in
+          let all_nodes = collect node (children child) all_nodes in
+            collect parent cs all_nodes
+  in
+  let all_nodes = collect (PaplTree.add_root (get node)) (children node) [] in
+    all_nodes
+
+let map_node f node =
+  let rec collect parent cs =
+    match BatEnum.get cs with
+        None -> ()
+      | Some child ->
+          let node = add parent (f child) in
+          let () = collect node (children child) in
+            collect parent cs
+  in
+  let root = add_root (f node) in
+  let () = collect root (children node) in
+    root
+
+let count node =
+  fold_node (fun acc node -> acc + 1) 0 node
+
+let delete_child child parent =
+  let i = A.index_of ((==) child) parent.children in
+    A.delete parent.children i
+
+let detach node =
+  match node.parent with
+      Some parent ->
+        delete_child node parent;
+        node.parent <- None
+    | None -> ()
+
+let reattach node parent =
+  detach node;
+  node.parent <- Some parent;
+  A.add parent.children node
+
+let rec reverse_chain node =
+  match node.parent with
+      None -> ()
+    | Some parent ->
+        reverse_chain parent;
+        delete_child node parent;
+        parent.parent <- Some node;
+        A.add node.children parent
+
+let reattach_rotate node prev_root new_root =
+  detach prev_root;
+  reverse_chain node;
+  node.parent <- Some new_root
+
+let rec node_path_to_root node =
+  match node.parent with
+    | None -> [node]
+    | Some parent -> node :: node_path_to_root parent
+
+let rec path_to_root node = List.map get (node_path_to_root node)
+
+let fold_nodes_to_root f acc node =
+  let rec loop acc node =
+    let acc = f acc node in
+      match node.parent with
+          None -> acc
+        | Some parent ->
+            loop acc parent
+  in
+    loop acc node
+
+let filter_nodes_to_root pred node =
+  let rec loop node =
+    if pred node then
+      node :: rest node
+    else
+      rest node
+  and rest node =
+    match node.parent with
+        None -> []
+      | Some parent ->
+          loop parent
+  in
+    loop node
+
+let constr cs =
+  let cs' node = PaplConstraint.reject cs (get node)
+  in PaplConstraint.make_reject cs'
+
+let edge_constr cs =
+  let cs' (na, nb) = PaplConstraint.reject cs (get na, get nb)
+  in PaplConstraint.make_reject cs'
+
+let constr cs =
+  let reject node = PaplConstraint.reject cs (get node)
+  in PaplConstraint.make_reject reject
+
+let edge_constr cs =
+  let reject (na, nb) = PaplConstraint.reject cs (get na, get nb)
+  in PaplConstraint.make_reject reject
+(** Tree with links from child to parent and parent to child
+*)
+
+type 'a node
+  (** The type of a node of the tree.
+
+      The node stores a value of type ['a].
+  *)
+
+val add_root : 'a -> 'a node
+  (** Add an element as the root of a new tree and return the new root
+      node.
+  *)
+
+val is_root : 'a node -> bool
+
+val add : 'a node -> 'a -> 'a node
+  (** Add a new element as a child node of a node and return the new node.
+
+      The node [n' = add n q] is a new child node of [n] and stores the element
+      [q].
+  *)
+
+val get : 'a node -> 'a
+  (** The element value stored in a node. *)
+
+val parent : 'a node -> 'a node option
+  (** The parent of a node or [None] if the node is a root node. *)
+
+val children : 'a node -> 'a node BatEnum.t
+(** The children of a node. *)
+
+val iter : ('a node -> unit) -> 'a node -> unit
+(** [iter f root] calls [f node] for all nodes in the subtree of [root].
+
+    The order of traversal is unspecified.
+*)
+
+val fold_node : ('acc -> 'a node -> 'acc) -> 'acc -> 'a node -> 'acc
+
+val to_single_linked : 'a node -> 'a PaplTree.node list
+(** [to_single_linked node] returns the nodes of a single listed tree with the
+    same shape and contents as [node].
+
+    The order of the nodes is unspecified.
+*)
+
+val map_node : ('a node -> 'b) -> 'a node -> 'b node
+(** [map_node f node] returns a tree with the same shape as [node] but with
+    values equal to [f nx] for every node [nx] of the subtree of [node].
+*)
+
+val detach : 'a node -> unit
+(** Detach a node from its parent.
+
+    The node becomes a root node. The children of the node are the same.
+*)
+
+val reattach : 'a node -> 'a node -> unit
+(** [reattach node parent] detachs [node] from its current parent and attachs it
+    to [parent] instead.
+
+    The children of [node] are the same.
+*)
+
+val count : 'a node -> int
+(** Count the number of nodes in a subtree.
+*)
+
+val filter_nodes_to_root : 
+  ('a node -> bool) -> 'a node -> 'a node list
+
+val fold_nodes_to_root :
+  ('acc -> 'a node -> 'acc) ->
+  'acc ->
+  'a node ->
+  'acc
+
+val node_path_to_root : 'a node -> 'a node list
+  (** [node_path_to_root node] is the list of nodes from [node] up to and
+      including its root.
+
+      If [node] is a root node then the result is the single-element list
+      [[node]].
+  *)
+
+val path_to_root : 'a node -> 'a list
+  (** [path_to_root node] is the list of elements values on the path from
+      node up to and including its root.
+
+      If [node] is a root node then the result is [get node].
+
+      [path_to_root node] is equivalent to
+      [List.map get (node_path_to_root node)].
+  *)
+
+val constr : 'a PaplConstraint.t -> 'a node PaplConstraint.t
+(** Convert a constraint for a value to a constraint for a node.
+*)
+
+val edge_constr : 'a PaplEdgeConstraint.t -> 'a node PaplEdgeConstraint.t
+(** Convert an edge constraint for values to an edge constraint for nodes.
+*)

src/PaplDLTreePlanner.ml

+
+module Tree = PaplDLTree
+module IC = PaplIncrConstraint
+
+module type SETUP = sig
+  type t
+  type q_t
+  type value_t
+  type node_t = value_t PaplDLTree.node
+
+  val expand : t -> node_t option
+  val connect : t -> q_t -> node_t option
+
+  val get_q : value_t -> q_t
+  val get_ec : value_t -> PaplIncrConstraint.t ref
+end
+
+module type CONNECT = sig
+  module SA : SETUP
+  module SB : SETUP with type q_t = SA.q_t
+
+  val make_constr : SA.q_t * SA.q_t -> PaplIncrConstraint.t
+  val transfer_a_to_b : SA.node_t -> SB.node_t -> SA.t -> SB.t -> unit
+  val transfer_b_to_a : SA.node_t -> SB.node_t -> SA.t -> SB.t -> unit
+end
+
+module MakeSetupUtil (S : SETUP) = struct
+  include S
+
+  let get = Tree.get
+  let get_node_q node = S.get_q (get node)
+  let get_node_ec node = !(S.get_ec (get node))
+  let set_node_ec node ec = S.get_ec (get node) := ec
+
+  module CompareNode = struct
+    type t = S.node_t
+    let compare a b =
+      (* High cost elements move to the top. *)
+      IC.compare_cost (get_node_ec b) (get_node_ec a)
+  end
+  module Heap = BatHeap.Make (CompareNode)
+
+  let disconnect_at_rejected node rejected_node =
+    let rec loop node =
+      if node == rejected_node then Tree.detach node
+      else
+        let next = BatOption.get (Tree.parent node) in
+          loop next;
+          Tree.reattach next node;
+          set_node_ec next (get_node_ec node);
+          Tree.detach node;
+    in
+      loop node
+
+  let add_heap_if heap node =
+    let ec = get_node_ec node in
+    let fully = IC.is_fully_evaluated ec in
+      if not fully || (fully && IC.reject ec) then
+        Heap.add node heap
+      else
+        heap
+
+  let add_root_path_to_heap node heap =
+    Tree.fold_nodes_to_root add_heap_if heap node
+
+  let make_root_path_heap node =
+    add_root_path_to_heap node Heap.empty
+end
+
+module MakeBidir (C : CONNECT) = struct
+  type q_t = C.SA.q_t
+  type a_t = C.SA.t
+  type b_t = C.SB.t
+
+  module SA = MakeSetupUtil(C.SA)
+  module SB = MakeSetupUtil(C.SB)
+  module HA = SA.Heap
+  module HB = SB.Heap
+
+  type find_t =
+      Bridge
+    | A of SA.node_t
+    | B of SB.node_t
+
+  exception Find of find_t
+
+  let raise_find x = raise (Find x)
+
+  let check_bridge_while_greater ec ca cb =
+    let rec loop () =
+      let c = IC.cost ec in
+        if c > ca && c > cb then begin
+          if IC.reject ec then raise_find Bridge
+          else loop ()
+        end
+        else ()
+    in loop ()
+
+  let rec find_ab heap_a heap_b ec =
+    if HA.size heap_a == 0 then find_b heap_b ec
+    else if HB.size heap_b == 0 then find_a heap_a ec
+    else
+      let na = HA.find_min heap_a in
+      let nb = HB.find_min heap_b in
+      let eca = SA.get_node_ec na in
+      let ecb = SB.get_node_ec nb in
+      let ca = IC.cost eca in
+      let cb = IC.cost ecb in
+        check_bridge_while_greater ec ca cb;
+        if ca > cb then
+          let heap_a = HA.del_min heap_a in
+            if IC.reject eca then raise_find (A na)
+            else find_ab (SA.add_heap_if heap_a na) heap_b ec
+        else
+          let heap_b = HB.del_min heap_b in
+            if IC.reject ecb then raise_find (B nb)
+            else find_ab heap_a (SB.add_heap_if heap_b nb) ec
+  and find_a heap_a ec =
+    if HA.size heap_a == 0 then check_bridge_while_greater ec 0. 0.
+    else
+      let na = HA.find_min heap_a in
+      let eca = SA.get_node_ec na in
+      let ca = IC.cost eca in
+        check_bridge_while_greater ec ca 0.;
+        let heap_a = HA.del_min heap_a in
+          if IC.reject eca then raise_find (A na)
+          else find_a (SA.add_heap_if heap_a na) ec
+  and find_b heap_b ec =
+    if HB.size heap_b == 0 then check_bridge_while_greater ec 0. 0.
+    else
+      let nb = HB.find_min heap_b in
+      let ecb = SB.get_node_ec nb in
+      let ca = IC.cost ecb in
+        check_bridge_while_greater ec ca 0.;
+        let heap_b = HB.del_min heap_b in
+          if IC.reject ecb then raise_find (B nb)
+          else find_b (SB.add_heap_if heap_b nb) ec
+
+  let reject_connection na nb ta tb =
+    let heap_a = SA.make_root_path_heap na in
+    let heap_b = SB.make_root_path_heap nb in
+    let ec = C.make_constr (SA.get_node_q na, SB.get_node_q nb) in
+      (* This check doesn't cost anything in terms of performance or
+         correctness, but gives the user a convenient way of changing his mind
+         about the connection.
+      *)
+      if PaplIncrConstraint.is_fully_evaluated ec &&
+        PaplIncrConstraint.reject ec
+      then true
+      else
+        try find_ab heap_a heap_b ec; false with
+            Find Bridge -> true
+          | Find (A ra) ->
+              SA.disconnect_at_rejected na ra;
+              SA.set_node_ec na ec;
+              C.transfer_a_to_b na nb ta tb;
+              true
+          | Find (B rb) ->
+              SB.disconnect_at_rejected nb rb;
+              SB.set_node_ec nb ec;
+              C.transfer_b_to_a na nb ta tb;
+              true
+
+  let path_to_root node get =
+    List.map get (Tree.path_to_root node)
+
+  let get_path na nb =
+    List.rev (path_to_root na C.SA.get_q) @
+      (path_to_root nb C.SB.get_q)
+
+  let planner stop (ta, tb) =
+    let rec loop_a ta tb =
+      let continue () = loop_b ta tb in
+        if PaplStopCriteria.stop stop then
+          PaplPlanner.fail "Stop criteria says stop."
+        else
+          match SA.expand ta with
+              None -> continue ()
+            | Some na ->
+                match SB.connect tb (SA.get_node_q na) with
+                    None -> continue ()
+                  | Some nb ->
+                      if reject_connection na nb ta tb then
+                        continue ()
+                      else
+                        get_path na nb
+    and loop_b ta tb =
+      let continue () = loop_a ta tb in
+        if PaplStopCriteria.stop stop then
+          PaplPlanner.fail "Stop criteria says stop."
+        else
+          match SB.expand tb with
+              None -> continue ()
+            | Some nb ->
+                match SA.connect ta (SB.get_node_q nb) with
+                    None -> continue ()
+                  | Some na ->
+                      if reject_connection na nb ta tb then
+                        continue ()
+                      else
+                        get_path na nb
+    in loop_a ta tb
+end

src/PaplDLTreePlanner.mli

+
+module type SETUP = sig
+  type t
+  type q_t
+  type value_t
+  type node_t = value_t PaplDLTree.node
+
+  val expand : t -> node_t option
+  val connect : t -> q_t -> node_t option
+
+  val get_q : value_t -> q_t
+  val get_ec : value_t -> PaplIncrConstraint.t ref
+end
+
+module type CONNECT = sig
+  module SA : SETUP
+  module SB : SETUP with type q_t = SA.q_t
+
+  val make_constr : SA.q_t * SA.q_t -> PaplIncrConstraint.t
+  val transfer_a_to_b : SA.node_t -> SB.node_t -> SA.t -> SB.t -> unit
+  val transfer_b_to_a : SA.node_t -> SB.node_t -> SA.t -> SB.t -> unit
+end
+
+module MakeBidir (C : CONNECT) :
+  PaplPlanner.BIDIR
+  with type q_t = C.SA.q_t
+  and type a_t = C.SA.t
+  and type b_t = C.SB.t
+
+type 'a t = 'a * 'a
+(** Edges
+
+   An edge is a tuple [(a, b)] that represents a motion from a start point [a]
+   to an end point [b].
+*)
+
+type 'a t = 'a * 'a

src/PaplEdgeConstraint.ml

+
+open BatStd
+
+type 'a t = ('a * 'a) PaplConstraint.t
+
+let mr = PaplConstraint.make_reject
+let ma = PaplConstraint.make_accept
+let re = PaplConstraint.reject
+let ac = PaplConstraint.accept
+
+let constrain_both constr = mr (fun (a, b) -> re constr a || re constr b)
+
+let constrain_fst constr = mr (re constr -| fst)
+
+let constrain_snd constr = mr (re constr -| snd)
+
+let reject_threshold_by_cmp (metric, eps) cmp =
+  mr (fun (x, y) -> cmp (metric x y) eps)
+
+let accept_threshold_by_cmp (metric, eps) cmp =
+  ma (fun (x, y) -> cmp (metric x y) eps)
+
+let greater_than threshold =
+  reject_threshold_by_cmp threshold (<=)
+
+let constrain_intermediary constr intermediary =
+  let reject (a, b) = BatEnum.exists (re constr) (intermediary a b)
+  in mr reject
+
+let less_than threshold =
+  reject_threshold_by_cmp threshold (>=)
+
+let constrain_steps constr interpolate threshold =
+  constrain_intermediary
+    constr
+    (PaplInterpolate.intermediary_steps interpolate threshold)
+
+let constrain_by_subdivision q_constr interpolate threshold =
+  PaplIncrConstraint.to_constraint
+    (PaplIncrEdgeConstraint.constrain_by_subdivision
+       q_constr interpolate threshold)
+
+let constrain_path ec =
+  let reject xs =
+    match xs with
+        [] -> invalid_arg "constrain_path: empty path."
+      | [_] -> invalid_arg "constrain_path: one-element path."
+      | _ ->
+          let rec loop xs =
+            match xs with
+                x0 :: x1 :: rest ->
+                  PaplConstraint.reject ec (x0, x1) || loop (x1 :: rest)
+              | _ -> false
+          in loop xs
+  in PaplConstraint.make_reject reject
+
+let to_incremental metric ec ((a, b) as edge) =
+  let len = metric a b in
+  let cost () = len in
+  let stop = ref false in
+  let reject () =
+    if !stop then None
+    else
+      let r = Some (PaplConstraint.reject ec edge) in
+        stop := true;
+        r
+  in PaplIncrConstraint.make_reject reject cost
+
+module Metric = struct
+  let to_option constr m = ();
+    fun a b ->
+      if PaplConstraint.reject constr (a, b) then None
+      else Some (m a b)
+
+  let pre_constrain_option constr m = ();
+    fun a b ->
+      if PaplConstraint.reject constr (a, b) then None
+      else m a b
+
+  let post_constrain_option constr m = ();
+    fun a b ->
+      match m a b with
+          None -> None
+        | Some dist as result ->
+            if PaplConstraint.reject constr (a, b) then None
+            else result
+end

src/PaplEdgeConstraint.mli

+(**
+   Edge constraints
+
+   A constraint (see {! PaplConstraint}) for an edge (see {!PaplEdge}) checks if certain
+   properties of the edge are satisfied.
+*)
+
+(** {2 Edge constraints} *)
+
+(** {3 Types} *)
+
+type 'a t = 'a PaplEdge.t PaplConstraint.t
+(** An edge constraint.
+*)
+
+(** {3 Constructors} *)
+
+val constrain_both : 'a PaplConstraint.t -> 'a t
+(** A constraint on both elements of a pair.
+
+    The [constrain_both constr] accepts a pair [(a, b)] if [constr] accepts both [a]
+    and [b]. [a] is checked before [b].
+*)
+
+val constrain_fst : 'a PaplConstraint.t -> 'a t
+(** A constraint on the first element of a pair.
+
+    The [constrain_both constr] accepts a pair [(a, b)] if [constr] accepts the
+    first element [b].
+*)
+
+val constrain_snd : 'a PaplConstraint.t -> 'a t
+(** A constraint on the second element of a pair.
+
+    The [constrain_both constr] accepts a pair [(a, b)] if [constr] accepts the
+    second element [b].
+*)
+
+val reject_threshold_by_cmp :
+  'a PaplMetric.threshold_t -> (float -> float -> bool) -> 'a t
+(**
+   A general distance threshold constraint.
+
+   The edge constraint [c = reject_threshold_by_cmp (metric, eps) cmp] rejects
+   an edge [(a, b)] if [cmp (metric a b) eps].
+*)
+
+val accept_threshold_by_cmp :
+  'a PaplMetric.threshold_t -> (float -> float -> bool) -> 'a t
+(**
+   A general distance threshold constraint.
+
+   The edge constraint [c = accept_threshold_by_cmp (metric, eps) cmp] accepts
+   an edge [(a, b)] if [cmp (metric a b) eps].
+*)
+
+val less_than : 'a PaplMetric.threshold_t -> 'a t
+(** Max-distance constraint.
+
+    The constraint [less_than (metric, eps)] accepts [(a, b)] if [metric (a, b)
+    < eps].
+*)
+
+val greater_than : 'a PaplMetric.threshold_t -> 'a t
+(** Min-distance constraint.
+
+    The constraint [greater_than (metric, eps)] accepts [(a, b)] if [metric (a,
+    b) > eps].
+*)
+
+val constrain_intermediary :
+  'a PaplConstraint.t -> 'a PaplInterpolate.intermediary_t -> 'a t
+
+val constrain_steps :
+  'a PaplConstraint.t ->
+  'a PaplInterpolate.t ->
+  'a PaplMetric.threshold_t ->
+  'a t
+(** Checking of discrete points between start and end points
+
+    The constraint [c = constrain_steps constr interpolate (metric, eps)]
+    checks that [constr] is satisfied for the intermediary points [x1, ...,
+    xN-1] on the segment for the edge [(x0, xn)], where [eps = metric xi xi+1]
+    for [i = 0, ..., N-2] and [metric xN-1 xN <= eps].
+
+    The constraint [c] checks the points in order.
+
+    The constraint neither checks the start point [x0] nor the end-point [xN].
+    To include the end-point in the constraint, use [constrain_snd constr <&> c]
+    (see {! PaplConstraint.(<&>)}).
+*)
+
+val constrain_by_subdivision :
+  'a PaplConstraint.t ->
+  'a PaplInterpolate.t ->
+  'a PaplMetric.threshold_t ->
+  'a t
+
+val constrain_path : 'a t -> 'a list PaplConstraint.t
+(**
+   A constraint for a path constructed from a constraint for an edge.
+
+   Paths must have at least 2 elements or an exception is raised.
+*)
+
+val to_incremental :
+  'a PaplMetric.t -> 'a t -> 'a PaplEdge.t PaplIncrConstraint.create_t
+(** [iec = of_edge_constraint metric ec edge] is an incremental edge constraint
+    that verifies [edge] by a single call of [ec] and whose initial cost is
+    equal to the length of [edge] measured by [metric].
+*)
+
+module Metric : sig
+  val to_option : 'a t -> 'a PaplMetric.t -> 'a PaplMetric.option_t
+(** Convert a metric to a metric option by applying a constraint to the
+    arguments.
+*)
+
+  val pre_constrain_option :
+    'a t -> 'a PaplMetric.option_t -> 'a PaplMetric.option_t
+(** Constrain an option metric by applying a constraint to the arguments.
+
+    The constraint is applied to the arguments {i before} the metric option is
+    evaluated.
+*)
+
+  val post_constrain_option :
+    'a t -> 'a PaplMetric.option_t -> 'a PaplMetric.option_t
+(** Constrain an option metric by applying a constraint to the arguments.
+
+    The constraint is applied to the arguments {i after} the metric option has
+    been evaluated.
+*)
+end

src/PaplIncrConstraint.ml

+
+type t = {
+  reject : unit -> bool option;
+  cost : unit -> float;
+  mutable prev : bool;
+  mutable is_fully_evaluated : bool;
+}
+
+type 'a create_t = 'a -> t
+
+let make_reject ~reject ~cost =
+  { reject = reject;
+    cost = cost;
+    prev = false;
+    is_fully_evaluated = false;
+  }
+
+let make_fixed reject = {
+  reject = (fun () -> None);
+  cost = (fun () -> failwith "PaplIncrConstraint: Internal error");
+  prev = reject;
+  is_fully_evaluated = true;
+}
+
+let fixed_accept = make_fixed false
+
+let fixed_reject = make_fixed true
+
+let is_fully_evaluated c = c.is_fully_evaluated
+
+let reject c =
+  if c.is_fully_evaluated then
+    c.prev
+  else
+    match c.reject () with
+        None ->
+          c.is_fully_evaluated <- true;
+          c.prev
+      | Some b ->
+          c.prev <- b;
+          c.is_fully_evaluated <- b;
+          b
+
+let cost c =
+  if c.is_fully_evaluated
+  then if c.prev then infinity else 0.
+  else c.cost ()
+
+let rec full_reject c =
+  let b = reject c in
+    if is_fully_evaluated c then
+      b
+    else
+      full_reject c
+
+let to_constraint create =
+  PaplConstraint.make_reject (fun x -> full_reject (create x))
+
+let compare_cost ca cb = Pervasives.compare (cost ca) (cost cb)
+
+type my_t = t
+
+module Heap =
+  BatHeap.Make
+    (struct
+       type t = my_t
+       let compare = compare_cost
+     end)
+
+let prioritized cs =
+  let heap = ref (Heap.of_enum cs) in
+  let cost () =
+    if Heap.size !heap == 0 then 0.
+    else cost (Heap.find_min !heap) in
+  let my_reject = reject in
+  let rec reject () =
+    if Heap.size !heap == 0 then
+      None
+    else
+      let top = Heap.find_min !heap in
+        if is_fully_evaluated top then begin
+          heap := Heap.del_min !heap;
+          reject ()
+        end else
+          Some (my_reject top)
+  in
+    make_reject ~reject ~cost

src/PaplIncrConstraint.mli

+(**
+   Incremental constraints
+
+   An {b incremental constraint} is a constraint (see also {! PaplConstraint}) that
+   is evaluated piecemeally. The constraint has a cost that changes over time as
+   the constraint is being checked. The costs are used for prioritized checking
+   of a set of incremental constraints. The constraints with the greatest costs
+   are given the highest priority. Constraints that are cheap to verify and
+   likely be rejected should therefore be given the greatest costs.
+
+   Incremental are used for example in tree and roadmap planners with lazily
+   verified nodes or edges.
+*)
+
+(** {2 Types} *)
+
+type t
+(** An incremental constraint for values of type ['a]. *)
+
+type 'a create_t = 'a -> t
+(** Constructor of an incremental constraint for a value of type ['a].
+*)
+
+(** {2 Constructors} *)
+
+val make_reject :
+  reject:(unit -> bool option) ->
+  cost:(unit -> float) ->
+  t
+(** Construct an incremental constraint.
+
+    [make ~reject:reject ~cost:cost] returns an incremental constraint.
+    [~reject] incrementally checks the constraint, and [~cost] returns its cost.
+*)
+
+val fixed_reject : t
+(** A fully evaluated constraint that rejects. *)
+
+val fixed_accept : t
+(** A fully evaluated constraint that accepts. *)
+
+(** {2 Operations} *)
+
+val cost : t -> float
+(** Cost function for constraint checking.
+
+    A fully checked constraint has a cost of [0.0] if it is accepted (but a
+    constraint with cost [0.0] is not necessarily fully checked).
+
+    A rejected constraint has a cost of [infinity].
+*)
+
+val is_fully_evaluated : t -> bool
+(** True if no more constraint checks are needed for the value.
+
+    Use [reject] to see if the constraint was accepted or violated.
+*)
+
+val reject : t -> bool
+(** Incrementally check the constraint.
+
+    [reject c] returns [true] if the constraint is violated and [false]
+    otherwise. If [reject c] is [true], then [reject c] will be [true] for all
+    later calls. If [reject c] is [false] and the constraint is fully checked
+    (see {! is_fully_evaluated}), then [reject c] will be [false] for all later
+    calls.
+*)
+
+val full_reject : t -> bool
+(** Fully evaluate the constraint.
+
+    [full_reject c] returns the result of repeatedly calling [reject c] until
+    either a violation is found or nothing further remains to be checked.
+*)
+
+val compare_cost : t -> t -> int
+(** Comparison function for the costs of constraints.
+
+    [compare_cost ca cb] is equivalent to [Pervasives.compare (cost ca) (cost
+    cb)].
+*)
+
+(** {2 Conversions} *)
+
+val to_constraint : 'a create_t -> 'a PaplConstraint.t
+(** Conversion from an incremental constraint creator to a standard constraint.
+
+    The constraint [cs' = to_constraint cs] checked for the value [x] is
+    equivalent to [full_reject (cs x)].
+*)
+
+val prioritized : t BatEnum.t -> t
+(** Construct an incremental constraint for enumerations from an incremental
+    constraint for a single element.
+
+    The constraint [cp = prioritized c] constructs an incremental constraint for
+    each element of its sequence. These constraints are sorted by cost.
+
+    A single check of [cp] performs a single check for the element with the
+    greatest cost. [cp] is fully checked when as soon as a single element has
+    been rejected or otherwise when all elements have been accepted.
+
+    The cost of [cp] is equal to the greatest cost of any of its elements or
+    [0.] if there are no elements.
+*)

src/PaplIncrEdgeConstraint.ml

+
+type 'a create_t = 'a PaplEdge.t PaplIncrConstraint.create_t
+
+type state_t = {
+  mutable cost : float;
+  mutable level : int;
+}
+
+let make_state cost level = { cost = cost; level = level }
+
+(* The number of levels for an edge of length 'len' and segments of len 'eps'.
+
+   (This computation should be moved to PaplSubdivision.)
+*)
+let levels len eps =
+  let rec loop len eps acc =
+    if len <= eps
+    then acc
+    else loop (len /. 2.0) eps (acc + 1)
+  in loop len eps 0
+
+let constrain_by_subdivision constr interpolate (metric, eps) =
+  if eps <= 0.
+  then invalid_arg "eps <= 0"
+  else
+    let create (a, b) =
+      let ip = interpolate a b in
+      let len = metric a b in
+      let last_level = levels len eps in
+      let state = make_state len 1 in
+      let reject () =
+        if state.level > last_level
+        then None (* OK - and nothing more to check *)
+        else
+          let positions = PaplSubdivision.standard_cached state.level in
+          let bad pos =
+            let q = ip pos in
+              PaplConstraint.reject constr q
+          in let result = Some (List.exists bad positions) in
+            state.level <- state.level + 1;
+            state.cost <- state.cost /. 2.;
+            result
+      in
+      let cost () = state.cost
+      in PaplIncrConstraint.make_reject ~reject ~cost
+    in
+      create
+
+let constrain_path create path =
+  match path with
+      [] -> invalid_arg "constrain_path: empty path."
+      | [_] -> invalid_arg "constrain_path: one-element path."
+      | _ ->
+          let rec build xs =
+            match xs with
+                x0 :: x1 :: rest ->
+                  create (x0, x1) :: build (x1 :: rest)
+              | _ -> []
+          in
+            PaplIncrConstraint.prioritized (BatList.enum (build path))

src/PaplIncrEdgeConstraint.mli

+(** Incremental edge constraints
+
+    An incremental edge constraints is an incremental constraint (see
+    {! PaplIncrConstraint}) for an edge (see {! PaplEdge}).
+*)
+
+type 'a create_t = 'a PaplEdge.t PaplIncrConstraint.create_t
+
+val constrain_by_subdivision :
+  'a PaplConstraint.t ->
+  'a PaplInterpolate.t ->
+  'a PaplMetric.threshold_t ->
+  'a create_t
+(** Edge verification by repeated subdivision of edges.
+
+    The constraint [subdivide constr interpolate (metric, eps) edge] repeatedly
+    splits an edge into subsegments (using [interpolate]) until every segment
+    has a length shorter than [eps] when measured by [metric]. The constraint
+    splits every segment at its center and checks the centers by [constr].
+
+    The constraint assumes that [interpolate] splits a segment into subsegments
+    of equal length. In other words, if [b = interpolate a c 0.5] then [metric a
+    b = metric b c].
+
+    The constraint does not check the start and end points of the input edge.
+
+    The cost of the constraint equals current length of the subsegments.
+*)
+
+val constrain_path : 'a create_t -> 'a list -> PaplIncrConstraint.t
+(**
+   Prioritized checking of a path.
+
+   The path must be at least 2 elements long.
+*)
+
+module type BASE = sig
+  type value_t
+  type t
+  val add : t -> value_t -> unit
+end
+
+module BruteForce = struct
+
+  module A = BatDynArray
+
+  module type S = sig
+    include BASE
+
+    val option_shortest :
+      t -> value_t PaplMetric.norm_option_t -> value_t option
+
+    val shortest :
+      t -> value_t PaplMetric.norm_t -> value_t option
+
+    val choose : t -> value_t option
+  end
+
+  type mark_t = bool ref
+
+  let make_mark () = ref false
+
+  module type S_MARK = sig
+    include S
+
+    val mark : value_t -> unit
+    val delete_marked : t -> unit
+  end
+
+  module type SETUP = sig
+    type value_t
+  end
+
+  module type SETUP_MARK = sig
+    type value_t
+    val get_mark : value_t -> mark_t
+  end
+
+  module Make (M : SETUP) = struct
+
+    type value_t = M.value_t
+    type t = value_t A.t
+
+    let create () = A.create ()
+
+    let add s x = A.add s x
+
+    let option_shortest buffer norm_option =
+      if A.empty buffer then None
+      else
+        let min_dist = ref (-1.) in
+        let min_item = ref (A.get buffer 0) in
+        let has_item = ref false in
+        let last = A.length buffer - 1 in
+          for i = 0 to last do
+            let item = A.get buffer i in
+              match norm_option item, !has_item with
+                  None, _ -> ()
+                | Some dist, false ->
+                    min_item := item;
+                    min_dist := dist;
+                    has_item := true
+                | Some dist, true ->
+                    if dist < !min_dist then begin
+                      min_item := item;
+                      min_dist := dist
+                    end
+          done;
+          if !has_item then Some !min_item else None
+
+    let shortest buffer norm =
+      if A.empty buffer then None
+      else begin
+        let min_item = ref (A.get buffer 0) in
+        let min_dist = ref (norm !min_item) in
+          for i = 1 to A.length buffer - 1 do
+            let item' = A.get buffer i in
+            let dist = norm item' in
+              if dist < !min_dist then begin
+                min_dist := dist;
+                min_item := item'
+              end
+          done;
+          Some !min_item
+      end
+
+    let choose buffer =
+      if A.empty buffer then None
+      else
+        let n = A.length buffer in
+        let i = BatRandom.int n in
+          Some (A.get buffer i)
+  end
+
+  module MakeMark (M : SETUP_MARK) = struct
+    include Make (M)
+
+    let mark x = M.get_mark x := true
+
+    let swap buf i j =
+      let tmp = A.get buf i in
+        A.set buf i (A.get buf j);
+        A.set buf j tmp
+
+    let delete_marked s =
+      let buf = s in
+      let get = M.get_mark in
+      let is_marked x = !(get x) in
+      let j = ref (A.length buf) in
+      let i = ref 0 in
+        while !i < !j do
+          let x = A.get buf !i in
+            if is_marked x then begin
+              j := !j - 1;
+              swap buf !i !j;
+          end
+            else
+              i := !i + 1
+        done;
+        A.delete_range buf !j (A.length buf - !j)
+  end
+end
+
+module type BASE = sig
+  type value_t
+  type t
+  val add : t -> value_t -> unit
+end
+
+module BruteForce : sig
+  module type S = sig
+    include BASE
+
+    val option_shortest :
+      t -> value_t PaplMetric.norm_option_t -> value_t option
+
+    val shortest :
+      t -> value_t PaplMetric.norm_t -> value_t option
+
+    val choose : t -> value_t option
+  end
+
+  type mark_t = bool ref
+  val make_mark : unit -> mark_t
+
+  module type S_MARK = sig
+    include S
+
+    val mark : value_t -> unit
+    val delete_marked : t -> unit
+  end
+
+  module type SETUP = sig
+    type value_t
+  end
+
+  module type SETUP_MARK = sig
+    type value_t
+    val get_mark : value_t -> mark_t
+  end
+
+  module Make (M : SETUP) : sig
+    include S with type value_t = M.value_t
+    val create : unit -> t
+  end
+
+  module MakeMark (M : SETUP_MARK) : sig
+    include S_MARK with type value_t = M.value_t
+    val create : unit -> t
+  end
+end

src/PaplInterpolate.ml

+
+type 'a t = 'a -> 'a -> float -> 'a
+
+type 'a intermediary_t = 'a -> 'a -> 'a BatEnum.t
+
+let flip_interpolate ip s a b = ip a b s
+
+let intermediary_steps interpolate (metric, eps) a b =
+  let ip = interpolate a b in
+  let len = metric a b in
+  let step = eps /. len in
+  let pos = ref step in
+  let next () =
+    if !pos < 1.0 then
+      let q = ip !pos in
+        pos := !pos +. step;
+        Some q
+    else
+      None
+  in
+    BatEnum.from_while next
+
+(* See also this alternative implementation that accepts a looser connection
+   between the metric and the interpolation.
+
+let intermediary_steps interpolate (metric, eps) start goal =
+  let pos = ref start in
+  let next () =
+    let d = metric !pos goal in
+      if d < eps then
+        None
+      else
+        let pos' = interpolate (eps /. d) !pos goal in
+          pos := pos';
+          Some pos'
+  in
+    BatEnum.from_while next
+*)
+
+module Float = struct
+  let interpolate a b s = a +. (b -. a) *. s
+  let flip_interpolate s a b = interpolate a b s
+end
+
+module Int = struct
+  let round v = int_of_float (v +. 0.5)
+
+  let interpolate x y s =
+    round (Float.interpolate (float_of_int x) (float_of_int y) s)
+end
+
+module Tuple2 = struct
+  let interpolate ipa ipb (a0, b0) (a1, b1) =
+    let ip1 = ipa a0 a1 in
+    let ip2 = ipb b0 b1 in
+      fun s -> ip1 s, ip2 s
+end
+
+module Tuple3 = struct
+  let interpolate ipa ipb ipc (a0, b0, c0) (a1, b1, c1) =
+    let ip1 = ipa a0 a1 in
+    let ip2 = ipb b0 b1 in
+    let ip3 = ipc c0 c1 in
+      fun s -> ip1 s, ip2 s, ip3 s
+end
+
+module List = struct
+  let interpolate ip xs ys s =
+    List.map2 (flip_interpolate ip s) xs ys
+end
+
+module Array = struct
+  let interpolate ip xs ys s =
+    BatArray.map2 (flip_interpolate ip s) xs ys
+end

src/PaplInterpolate.mli

+(**
+   Planning spaces
+
+   A motion planner searches for a path in a planning space. The type of the
+   planning space is generally user defined and depends on the robot and the
+   planning problem. This library does not distinguish between configuration
+   space and state space planning. A member of a planning space is called a {b
+   configuration} regardless of the nature of the planning.
+
+   Planning spaces may support a number of operations discussed in modules such
+   as {! PaplMetric}, {! PaplSampler}, {! PaplConstraint}, and more. The {! PaplInterpolate}
+   module is concerned with the property of being able to move between a pair of
+   discrete configurations. This is called {b interpolation} and is accomplished
+   by a function of type {! t}.
+*)
+
+(** {2 Types} *)
+
+type 'a t = 'a -> 'a -> float -> 'a
+(** Interpolation function for a planning space.
+
+    An interpolation function [interpolate] returns a configuration [x =
+    interpolate s a b] on the path connecting [a] to [b]. The interpolation
+    parameter [s] must be in the range [[0, 1]].
+*)
+
+type 'a intermediary_t = 'a -> 'a -> 'a BatEnum.t
+
+val intermediary_steps :
+  'a t -> 'a PaplMetric.threshold_t -> 'a intermediary_t
+(**
+   [intermediary_steps ip (metric, eps) a b] are the intermediary steps of
+   length [eps] connecting [a] to [b].
+
+   [a] and [b] are not explicitly included in the stream (but they can be if the
+   interpolation function [ip] happens to return them).
+*)
+
+(** {2 Float planning space} *)
+
+(** Planning space for floats.
+
+    Configurations are floats.
+
+    Floats are interpolated linearly.
+*)
+module Float : sig
+  val interpolate : float t
+  val flip_interpolate : float -> float -> float -> float
+end
+
+module Int : sig
+  val interpolate : int t
+end
+
+(** {2 Tuple planning space} *)
+
+(** Planning space for tuples.
+
+    Tuples can contain values of seperate types. Each coordinate is interpolated
+    using a seperate interpolation function.
+*)
+
+module Tuple2 : sig
+  val interpolate : 'a t -> 'b t -> ('a * 'b) t
+end
+
+module Tuple3 : sig
+  val interpolate : 'a t -> 'b t -> 'c t -> ('a * 'b * 'c) t
+end
+
+(** {2 List planning space} *)
+
+(** Planning space for lists.
+
+    Configurations are lists.
+
+    Each coordinate is interpolated independently.
+
+    The lists must be of the same length when interpolated.
+*)
+
+module List : sig
+  val interpolate : 'a t -> 'a list t
+end
+
+(** {2 Array planning space} *)
+
+(** Planning space for arrays.
+
+    Configurations are arrays.
+
+    Each coordinate is interpolated independently.
+
+    Arrays being interpolated must be of the same length.
+*)
+
+module Array : sig
+  val interpolate : 'a t -> 'a array t
+end
+
+(* A printf with flush.
+
+   Or add a "%!" to your string format to flush.
+*)
+let printf x =
+  Printf.kprintf (fun s -> print_string s; flush stdout) x
+
+type 'a pair_t = 'a * 'a
+
+type 'a t = 'a -> 'a -> float
+
+type 'a norm_t = 'a -> float
+
+type 'a option_t = 'a -> 'a -> float option
+
+type 'a norm_option_t = 'a -> float option
+
+type 'a threshold_t = 'a t * float
+
+let to_option m = (); fun a b -> Some (m a b)
+
+let to_option_less_than m eps = ();
+  fun a b ->
+    let d = m a b in
+      if d < eps then Some d
+      else None
+
+let bind f m a b = f (m a b)
+
+let scale s = bind (( *. ) s)
+
+let add m n a b = m a b +. n a b
+
+let pow m p = bind (fun v -> v**p) m
+
+let path_length m xs =
+  match xs with
+      [] -> invalid_arg "path_length: empty path."
+    | [_] -> invalid_arg "path_length: one-element path."
+    | x :: xs -> fst
+        (List.fold_left
+           (fun (len, prev) next -> (len +. m prev next, next))
+           (0., x)
+           xs)
+
+module Float = struct
+  let dist a b = abs_float (b -. a)
+end
+
+module Int = struct
+  let dist a b = float_of_int (abs (b - a))
+end
+
+module Tuple2 = struct
+  let dist_helper f g m0 m1 (a0, a1) (b0, b1) =
+    let d0 = m0 a0 b0 in
+    let d1 = m1 a1 b1 in
+      g (f d0) (f d1)
+
+  let id x = x
+
+  let dist1 m0 m1 = dist_helper id (+.) m0 m1
+
+  let sq x = x *. x
+  let dist2_sqr m0 m1 = dist_helper sq (+.) m0 m1
+
+  let dist2 m0 m1 = dist_helper sq (fun a b -> sqrt (a +. b)) m0 m1
+
+  let dist_inf m0 m1 = dist_helper id max m0 m1
+end
+
+module type OPTION = sig
+  type t
+  val dist : t option_t
+end
+(** Distance measures
+
+   A metric is a function that measures the distance between a pair of values.
+   Mathematically speaking, a metric must satisfy properties of
+
+   - non-negativity
+   - symmetry
+   - reflexivity
+   - triangle inequality
+
+   but many algorithms on metric implementations are correct even if none of
+   those properties are satisfied.
+
+   Consult the documentation for each algorithm to know the requirements for the
+   metric.
+*)
+
+(** {2 Types} *)
+
+type 'a pair_t = 'a * 'a
+
+type 'a t = 'a -> 'a -> float
+(** The type of a metric.
+
+    A metric measures the distance between a pair of values.
+*)
+
+type 'a norm_t = 'a -> float
+(** The type of a norm.
+
+    A norm measures the magnitude of a value.
+*)
+
+type 'a option_t = 'a -> 'a -> float option
+
+type 'a norm_option_t = 'a -> float option
+
+type 'a threshold_t = 'a t * float
+(** A threshold is a tuple [(metric, eps)] containing a metric [metric] for
+    measuring the distance between points and a threshold value [eps] for the
+    distance.
+
+    Whether [eps] is a lower, upper, or equality boundary for values returned by
+    [metric] depends on the user of the threshold.
+*)
+
+(** {2 Constructors} *)
+
+val to_option : 'a t -> 'a option_t
+
+val to_option_less_than : 'a t -> float -> 'a option_t
+
+val bind : (float -> float) -> 'a t -> 'a t
+(**
+   If [m' = bind f m] then [m' a b] is equivalent to [f (m a b)].
+*)
+
+val scale : float -> 'a t -> 'a t
+(**
+   [scale s] is equivalent to [bind (( *. ) s)].
+*)
+
+val add : 'a t -> 'a t -> 'a t
+(**
+   If [m = add m1 m2] then [m a b] is equivalent to [m1 a b +. m2 a b].
+*)
+
+val pow : 'a t -> float -> 'a t
+(**
+   [pow m p] is equivalent to [bind (fun v -> v**p) m].
+*)
+
+val path_length : 'a t -> 'a list -> float
+(**
+   The length of a path.
+
+   The length of an empty path or a one-element path is undefined and raises an
+   exception.
+*)
+
+module Float : sig
+  val dist : float t
+    (** [Float.dist a b] returns the absolute value of [b -. a].
+    *)
+end
+
+module Int : sig
+  val dist : int t
+    (** [Int.dist a b] returns the absolute value of [b - a] converted to a
+        [float].
+    *)
+end
+
+module Tuple2 : sig
+  val dist1 : 'a t -> 'b t -> ('a * 'b) t
+
+  val dist2 : 'a t -> 'b t -> ('a * 'b) t
+
+  val dist2_sqr : 'a t -> 'b t -> ('a * 'b) t
+
+  val dist_inf : 'a t -> 'b t -> ('a * 'b) t
+end
+
+module type OPTION = sig
+  type t
+  val dist : t option_t
+end
+type 'a pair_t = 'a * 'a
+
+let interpolate ip = PaplInterpolate.Tuple2.interpolate ip ip
+
+module Int = struct
+  let ip = PaplInterpolate.Int.interpolate
+
+  let interpolate = interpolate ip
+end
+
+module Metric = struct
+  module Int = PaplVectorMetric.MakePair
+    (struct
+       type elt = int
+       let diff x0 x1 = float_of_int (x1 - x0)
+       let value x = float_of_int x
+     end)
+
+  module Float = PaplVectorMetric.MakePair
+    (struct
+       type elt = float
+       let diff x0 x1 = x1 -. x0
+       let value x = x
+     end)
+end
+
+module Tuple2_PlannerConstraint = struct
+  let re = PaplConstraint.reject
+  let mr = PaplConstraint.make_reject
+
+  let couple ca cb cab =
+    let reject_node ((a, b) as node) =
+      re (fst ca) a ||
+        re (fst cb) b ||
+        re (fst cab) node
+    in
+    let reject_edge (((a0, b0), (a1, b1)) as edge) =
+      re (snd ca) (a0, a1) ||
+        re (snd cb) (b0, b1) ||
+        re (snd cab) edge
+
+    in (mr reject_node, mr reject_edge)
+end
+
+module PlannerConstraint = struct
+  let couple = Tuple2_PlannerConstraint.couple
+end
+(**
+   Pair configurations
+*)
+
+type 'a pair_t = 'a * 'a
+
+(** {2 Planning spaces} *)
+
+(** Each coordinate is interpolated independently using the same interpolation
+    function.
+*)
+
+val interpolate : 
+  'a PaplInterpolate.t ->
+  'a pair_t PaplInterpolate.t
+
+module Int : sig
+  val interpolate : 
+    int pair_t PaplInterpolate.t
+(** The interpolation function interpolates the coordinates as if they were
+    floats and then rounds the values to the nearest integer.
+*)
+end
+
+(** {2 Metrics} *)
+
+module Metric : sig
+  module Int : PaplVectorMetric.S
+    with
+      type t = int pair_t
+    and
+      type weight_t = float pair_t
+
+  module Float : PaplVectorMetric.S
+    with
+      type t = float pair_t
+    and
+      type weight_t = float pair_t
+end
+
+(** {2 Planner constraints} *)
+
+module PlannerConstraint : sig
+  val couple :
+    'a PaplPlannerConstraint.t ->
+    'a PaplPlannerConstraint.t ->
+    'a pair_t PaplPlannerConstraint.t ->
+    'a pair_t PaplPlannerConstraint.t
+(** Combination of constraints for coupled planning.
+
+    Let [A] and [B] be robots in an environment E. Let [ca] and [cb] be
+    constraints for [A] in [E] and [B] in [E], and let [cab] be a constraint for
+    collisions between [A] and [E].
+
+    The constraint [c = combine ca cb cab] combines these constraints into a
+    single constraint for the movement of the coupled robot [(A, B)].
+*)
+end
+type 'a t = 'a list
+
+let fold_adjacent f xs acc =
+  match xs with
+      [] -> invalid_arg "Papl_path.fold_adjacent: empty path."
+    | [_] -> invalid_arg "Papl_path.fold_adjacent: one-element path."
+    | _ ->
+        let rec loop xs acc =
+          match xs with
+              x0 :: x1 :: rest ->
+                loop (x1 :: rest) (f x0 x1 acc)
+            | _ -> acc
+        in loop xs acc
+(** Paths
+
+    A path is a list of configurations.
+
+    A path must have length at least 2.
+*)
+
+type 'a t = 'a list
+
+val fold_adjacent : ('a -> 'a -> 'acc -> 'acc) -> 'a t -> 'acc -> 'acc
+
+type 'a pair_t = 'a * 'a
+
+type 'a region_t = 'a list PaplSampler.t
+
+type 'a region_pair_t = 'a region_t pair_t
+
+(* path *)
+
+type ('target, 'q) t =
+    PaplStopCriteria.t -> 'target -> 'q list
+
+type ('target, 'q) option_t =
+    PaplStopCriteria.t -> 'target -> 'q list option
+
+type 'a point_t = ('a * 'a, 'a) t
+
+type 'a sampler_t = ('a region_pair_t, 'a) t
+
+(* trajectory *)
+
+type ('target, 'q) trajectory_t =
+    PaplStopCriteria.t -> 'target -> 'q PaplTrajectory.t
+
+type 'q point_trajectory_t =
+    ('q * 'q, 'q) trajectory_t
+
+type 'a sampler_trajectory_t =
+    ('a list PaplSampler.t pair_t, 'a) trajectory_t
+
+exception Path_not_found of string
+
+let fail msg = raise (Path_not_found msg)
+
+let to_option planner =
+  let planner stop target =
+    try Some (planner stop target)
+    with Path_not_found msg ->
+      Printf.printf "Planner message: %s\n" msg;
+      None
+  in
+    planner
+
+let to_trajectory convert path_planner =
+  let planner stop target =
+    convert (path_planner stop target)
+  in
+    planner
+
+let bind_target f planner = fun stop target -> planner stop (f target)
+
+let map_path f planner =
+  fun stop target -> List.map f (planner stop target)
+
+let point_to_sampler_target target =
+  BatPair.map (fun q -> BatList.enum [[q]]) target
+
+let sampler_to_point_planner sampler_planner =
+  bind_target point_to_sampler_target sampler_planner
+
+let point_to_sampler_planner pp =
+  let rec get stop region =
+    if PaplStopCriteria.stop stop then
+      fail "Stop criteria said stop before region point was found"
+    else
+      match BatEnum.get region with
+          None -> fail "Region sampler is empty"
+        | Some [] -> get stop region
+        | Some (q :: _) -> q
+  in
+  let sp stop target =
+    pp stop (BatPair.map (get stop) target)
+  in
+    sp
+
+module Trajectory = struct
+  let of_metric ?t0 interpolate metric path_planner =
+    to_trajectory
+      (PaplTrajectory.of_path ?t0 interpolate metric)
+      path_planner
+end
+
+module type BIDIR = sig
+  type q_t
+  type a_t
+  type b_t
+
+  val planner : (a_t * b_t, q_t) t
+end
+
+module type POINT_BIDIR = sig
+  type q_t
+  include BIDIR with type q_t := q_t
+                and type a_t = q_t
+                and type b_t = q_t
+end
+
+module type SAMPLER_BIDIR = sig
+  type q_t
+  include BIDIR with type q_t := q_t
+                and type a_t = q_t region_t
+                and type b_t = q_t region_t
+end
+
+(** Motion planners *)
+
+(** {2 Types} *)
+
+(** The type of a planner.
+
+    A call [planner stop (from, to)] of a planner [planner] searches for a path
+    connecting a start region [from] to a goal region [end]. The planner
+    regularly calls [stop] to decide if the search should continue. The planner
+    returns [None] either if [stop] says stop or the planner has decided or
+    determined that it can't find a solution path.
+*)
+
+type 'a pair_t = 'a * 'a
+
+type 'a region_t = 'a list PaplSampler.t
+
+type 'a region_pair_t = 'a region_t pair_t
+
+(* path *)
+
+type ('target, 'q) t =
+    PaplStopCriteria.t -> 'target -> 'q list
+
+type ('target, 'q) option_t =
+    PaplStopCriteria.t -> 'target -> 'q list option
+
+type 'a point_t = ('a * 'a, 'a) t
+
+type 'a sampler_t = ('a region_pair_t, 'a) t
+
+(* trajectory *)
+
+type ('target, 'q) trajectory_t =
+    PaplStopCriteria.t -> 'target -> 'q PaplTrajectory.t
+
+type 'q point_trajectory_t =
+    ('q * 'q, 'q) trajectory_t