(PaplPlannerConstraint.constrain_edge_and_snd constr)

(make_intermediaries ip metric steps)

+let quot_rem a b = a / b, a mod b

+ let len = List.length path in

+ let i, r = quot_rem (len - n) n in

+ let skip = if r > 0 then i + 1 else i in

+ match BatList.drop skip path with

+ | q :: path -> q :: loop (r - 1) path

+ | [] -> failwith "PaplRRTExpand.shorten_n: impossible"

+let steps_n constr intermediary n =

+ let msg = Printf.sprintf

+ "PaplRRTExpand.steps_n: n = %d must be greater than 0."

+ let expand = by_intermediaries constr [intermediary] in

+ let (path, reached) = expand start goal in

+ shorten_n n path, reached