Source

papl / src / PaplSBL.ml

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
(*
  Copyright (c) 2012 Anders Lau Olsen.
  See LICENSE file for terms and conditions.
*)

module Tree = PaplDLTree
module IC = PaplIncrConstraint

type 'a pair_t = 'a * 'a

type 'a expand_t = 'a -> 'a list

type kind_t = Standard | NonTransfer

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 remove_enum : t -> node_t BatEnum.t -> unit
  val add : t -> node_t -> q_t -> PaplIncrConstraint.t -> node_t

  val get_q : value_t -> q_t
  val get_ec : value_t -> PaplIncrConstraint.t ref
  val make_constr : (q_t * q_t) -> PaplIncrConstraint.t

  val kind : kind_t
end

module type SETUP_PAIR = sig
  type q_t
  module Fst : SETUP with type q_t = q_t
  module Snd : SETUP with type q_t = q_t
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 (SA : SETUP) (SB : SETUP with type q_t = SA.q_t) =
struct
  type q_t = SA.q_t
  type target_t = SA.t * SB.t

  module SA = MakeSetupUtil(SA)
  module SB = MakeSetupUtil(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 delete_tree_helper remove_enum s node =
    remove_enum s (Tree.enum node);
    Tree.detach node

  let delete_tree_a s node = delete_tree_helper SA.remove_enum s node
  let delete_tree_b s node = delete_tree_helper SB.remove_enum s node

  let add_tree_helper add get_q get_ec s parent other =
    let rec build parent other =
      let node =
        add s parent
          (get_q (Tree.get other))
          !(get_ec (Tree.get other))
      in
        BatEnum.iter
          (fun child -> build node child)
          (Tree.children other)
    in build parent other

  let add_tree_a s parent other =
    add_tree_helper SA.add SB.get_q SB.get_ec s parent other

  let add_tree_b s parent other =
    add_tree_helper SB.add SA.get_q SA.get_ec s parent other
  
  let transfer_helper kind delete_tree add_tree =
    match kind with
      | Standard -> begin
          fun na nb sa sb ->
            delete_tree sa na;
            add_tree sb nb na
        end
      | NonTransfer ->
          fun na nb sa sb ->
            delete_tree sa na

  let transfer_a_to_b = transfer_helper SA.kind delete_tree_a add_tree_b
  let transfer_b_to_a = transfer_helper SB.kind delete_tree_b add_tree_a

  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 = SA.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;
              transfer_a_to_b na nb ta tb;
              true
          | Find (B rb) ->
              SB.disconnect_at_rejected nb rb;
              SB.set_node_ec nb ec;
              transfer_b_to_a nb na tb ta;
              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 SA.get_q) @
      (path_to_root nb 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

module MakeBidir1 (S : SETUP) = MakeBidir (S)(S)

module MakeBidirPair (S : SETUP_PAIR) = MakeBidir (S.Fst) (S.Snd)

module BruteForce = struct
  module type SETUP = sig
    include SETUP

    val add_root : t -> q_t -> node_t
    val create : ?callback:(t -> unit) -> unit -> t
    val create_point : q_t -> t
    val create_region : q_t PaplPlanner.region_t -> t

    val get_roots : t -> node_t BatEnum.t
    (** Enumeration of all nodes that have been added with {! add_root}. *)
  end

  module type SETUP_PAIR = sig
    type q_t
    module Fst : SETUP with type q_t = q_t
    module Snd : SETUP with type q_t = q_t
  end

  module MakePointBidirPair (M : SETUP_PAIR) = struct
    module P = MakeBidirPair (M)
    type q_t = M.q_t
    type target_t = q_t pair_t

    let planner stop (a, b) =
      P.planner stop (M.Fst.create_point a, M.Snd.create_point b)
  end

  module MakeSamplerBidirPair (M : SETUP_PAIR) = struct
    module P = MakeBidirPair (M)
    type q_t = M.q_t
    type target_t = q_t PaplPlanner.region_t pair_t

    let planner stop (a, b) =
      P.planner stop (M.Fst.create_region a, M.Snd.create_region b)
  end

  module MakePair
    (A : SETUP)
    (B : SETUP with type q_t = A.q_t) =
  struct
    type q_t = A.q_t
    module Fst = A
    module Snd = B
  end

  module Unbiased = struct

    module type BF_SETUP = sig
      type q_t
      val make_constr : (q_t * q_t) -> PaplIncrConstraint.t
      val connect_dist : q_t PaplMetric.option_t
      val connect_constr : q_t PaplEdgeConstraint.t option
      val expand : q_t -> q_t list
      val kind : kind_t
      val rng : PaplRandom.rng_t
    end

    module MakeSetup (M : BF_SETUP) = struct
      type q_t = M.q_t

      type value_t = {
        q : q_t;
        ec : PaplIncrConstraint.t ref;
        mark : bool ref;
      }

      type node_t = value_t Tree.node

      let get_q value = value.q
      let get_ec value = value.ec
      let get_mark value = value.mark
      let make_constr = M.make_constr

      let kind = M.kind

      module Index = PaplIndex.BruteForce.MakeMark
        (struct
           type value_t = node_t
           let rng = M.rng
           let get_mark node = get_mark (Tree.get node)
         end)

      module A = BatDynArray

      type t = {
        index : Index.t;
        callback : t -> unit;

        (* The roots are stored not for the purposes of the planning, but to
           facilitate later analysis. *)
        roots : node_t A.t;
      }

      let remove_enum s enum =
        BatEnum.iter Index.mark enum;
        Index.delete_marked s.index

      let make_value q ec = {
        q = q;
        ec = ref ec;
        mark = PaplIndex.BruteForce.make_mark ();
      }

      let add s parent q ec =
        let node = Tree.add parent (make_value q ec) in
        let () = Index.add s.index node in
          node

      let ec_dummy = PaplIncrConstraint.fixed_accept

      let add_root s q =
        let node = Tree.add_root (make_value q ec_dummy) in
        let () = Index.add s.index node in
        let () = A.add s.roots node in
          node

      let get_roots s = A.enum s.roots

      let create ?(callback = fun _ -> ()) () = {
        index = Index.create ();
        callback = callback;
        roots = A.create ();
      }

      let create_point q =
        let s = create () in
        let _ = add_root s q in
          s

      let create_region sampler =
        let add s q = let _ = add_root s q in () in
        let callback s =
          BatOption.may
            (List.iter (fun q -> add s q))
            (BatEnum.get sampler)
        in create ~callback ()

      let callback s = s.callback s

      let add_all s parent qs =
        let add_q parent q =
          add s parent q (M.make_constr (get_q (Tree.get parent), q))
        in List.fold_left add_q parent qs

      let expand s =
        callback s;
        match Index.choose_random s.index with
          | None -> None
          | Some n_tree ->
              match M.expand (get_q (Tree.get n_tree)) with
                | [] -> None
                | qs -> Some (add_all s n_tree qs)

      let nearest s dist constr q_attractor =
        let get node = get_q (Tree.get node) in
          Index.nearest s.index get dist constr q_attractor

      let connect_constr =
        match M.connect_constr with
            None -> PaplConstraint.fixed_accept true
          | Some connect_constr -> connect_constr

      let connect s q_attractor =
        nearest s M.connect_dist connect_constr q_attractor
    end

    module MakePointBidir
      (SA : BF_SETUP)
      (SB : BF_SETUP with type q_t = SA.q_t) =
      MakePointBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))

    module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)

    module MakeSamplerBidir
      (SA : BF_SETUP)
      (SB : BF_SETUP with type q_t = SA.q_t) =
      MakeSamplerBidirPair(MakePair(MakeSetup(SA))(MakeSetup(SB)))

    module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)
  end

  module RRT = struct
    module type BF_SETUP = sig
      type q_t
      val make_constr : (q_t * q_t) -> PaplIncrConstraint.t
      val connect_dist : q_t PaplMetric.option_t
      val connect_constr : q_t PaplEdgeConstraint.t option
      val expand_dist : q_t PaplMetric.option_t
      val expand_constr : q_t PaplEdgeConstraint.t option
      val expand : q_t -> q_t -> q_t list
      val sampler : q_t option PaplSampler.t
      val kind : kind_t
    end

    module type BF_SETUP_PAIR = sig
      type q_t
      val make_constr : ((q_t * q_t) -> PaplIncrConstraint.t) pair_t
      val connect_dist : q_t PaplMetric.option_t pair_t
      val connect_constr : q_t PaplEdgeConstraint.t option pair_t
      val expand_dist : q_t PaplMetric.option_t pair_t
      val expand_constr : q_t PaplEdgeConstraint.t option pair_t
      val expand : (q_t -> q_t -> q_t list) pair_t
      val sampler : q_t option PaplSampler.t pair_t
      val kind : kind_t pair_t
    end

    module MakeSetup (M : BF_SETUP) = struct
      (* We shamelessly forward to this EST version. The only place where the
         algoriths differ is in in the [expand] method.
      *)
      module Default = Unbiased.MakeSetup(
        struct
          include M
          let expand _ = []
          let rng = None
        end)

      (* Include the EST version. *)
      include Default

      let expand_constr =
        match M.expand_constr with
            None -> PaplConstraint.fixed_accept true
          | Some expand_constr -> expand_constr

      (* Replace the [expand] method. *)
      let expand s =
        Default.callback s;
        match BatEnum.get M.sampler with
            None -> None
          | Some None -> None
          | Some (Some q_attractor) ->
              match Default.nearest
                s M.expand_dist expand_constr q_attractor with
                  | None -> None
                  | Some n_tree ->
                      let q_tree = get_q (Tree.get n_tree) in
                        match M.expand q_tree q_attractor with
                          | [] -> None
                          | qs -> Some (Default.add_all s n_tree qs)
    end

    module MakeSetupPair (M : BF_SETUP_PAIR) = struct
      module FstSetup = struct
        type q_t = M.q_t
        let make_constr = fst M.make_constr
        let connect_dist = fst M.connect_dist
        let connect_constr = fst M.connect_constr
        let expand_dist = fst M.expand_dist
        let expand_constr = fst M.expand_constr
        let expand = fst M.expand
        let sampler = fst M.sampler
        let kind = fst M.kind
      end
      module SndSetup = struct
        type q_t = M.q_t
        let make_constr = snd M.make_constr
        let connect_dist = snd M.connect_dist
        let connect_constr = snd M.connect_constr
        let expand_dist = snd M.expand_dist
        let expand_constr = snd M.expand_constr
        let expand = snd M.expand
        let sampler = snd M.sampler
        let kind = snd M.kind
      end
      type q_t = M.q_t
      module Fst = MakeSetup(FstSetup)
      module Snd = MakeSetup(SndSetup)
    end

    module MakePointBidir
      (A : BF_SETUP)
      (B : BF_SETUP with type q_t = A.q_t) =
      MakePointBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))

    module MakePointBidir1 (S : BF_SETUP) = MakePointBidir(S)(S)

    module MakeSamplerBidir
      (A : BF_SETUP)
      (B : BF_SETUP with type q_t = A.q_t) =
      MakeSamplerBidirPair(MakePair(MakeSetup(A))(MakeSetup(B)))

    module MakeSamplerBidir1 (S : BF_SETUP) = MakeSamplerBidir(S)(S)

    module MakePointBidirPair (M : BF_SETUP_PAIR) =
      MakePointBidirPair(MakeSetupPair(M))

    module MakeSamplerBidirPair (M : BF_SETUP_PAIR) =
      MakeSamplerBidirPair(MakeSetupPair(M))
  end
end