Commits

bergsoe committed 9e6d103

New [steps_n] expansion method.

  • Participants
  • Parent commits d14bfe7

Comments (0)

Files changed (2)

File src/PaplRRTExpand.ml

     ?accept_first
     (PaplPlannerConstraint.constrain_edge_and_snd constr)
     (make_intermediaries ip metric steps)
+
+let quot_rem a b = a / b, a mod b
+
+let shorten_n n path =
+  let len = List.length path in
+  if len <= n then path
+  else
+    let i, r = quot_rem (len - n) n in
+    let rec loop r path =
+      let skip = if r > 0 then i + 1 else i in
+        match BatList.drop skip path with
+            [q] -> [q]
+          | q :: path -> q :: loop (r - 1) path
+          | [] -> failwith "PaplRRTExpand.shorten_n: impossible"
+    in loop r path
+
+let steps_n constr intermediary n =
+  let () =
+    if n <= 0 then
+      let msg = Printf.sprintf
+        "PaplRRTExpand.steps_n: n = %d must be greater than 0."
+        n
+      in invalid_arg msg
+  in
+  let expand = by_intermediaries constr [intermediary] in
+    fun start goal ->
+      let (path, reached) = expand start goal in
+        shorten_n n path, reached

File src/PaplRRTExpand.mli

   Copyright (c) 2012 Anders Lau Olsen.
   See LICENSE file for terms and conditions.
 *)
-(** RRT expansions *)
+(** RRT expansions 
+
+    The functions of this module construct expansion methods for RRT planners
+    (see {! PaplRRT}). An RRT expansion method returns configurations on a path
+    leading from a given start configuration towards a goal configuration.
+*)
 
 type 'a expand_t = 'a PaplRRT.expand_t
 
   'a PaplMetric.t ->
   float list ->
   'a expand_t
+
+val steps_n :
+  'a PaplEdgeConstraint.t ->
+  'a PaplInterpolate.intermediary_t ->
+  int ->
+  'a expand_t
+(** Expansions with at most [n] steps.
+
+    The expansion method [steps_n constr intermediary n] splits a segment into
+    steps using [intermediary] and verifies the steps by [constr]. If the number
+    of accepted steps was less than or equal to [n], the steps are returned.
+    Otherwise the method shortens the list of steps to produce a list with
+    exactly [n] elements. 
+*)