1 # This file is part of NIT ( http://www.nitlanguage.org ).
3 # Licensed under the Apache License, Version 2.0 (the "License");
4 # you may not use this file except in compliance with the License.
5 # You may obtain a copy of the License at
7 # http://www.apache.org/licenses/LICENSE-2.0
9 # Unless required by applicable law or agreed to in writing, software
10 # distributed under the License is distributed on an "AS IS" BASIS,
11 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 # See the License for the specific language governing permissions and
13 # limitations under the License.
15 # A simple planning problem solver using A* for a robot that delivers parcels.
17 # A map is made of `Location`s (with cartesian coordinates) connected with `Road`s.
18 # Some parcels and a robot are placed on the map.
20 # The goal of the robot is to plan the delivery of the parcels to their destination in the most
21 # efficient way knowing that:
23 # * the robot has a given speed to drive
24 # * the loading and unloading of parcels take time
25 # * the capacity of the robot is limited
30 private import simplan_lexer
31 private import simplan_parser
33 # The description of the planing problem to solve
35 super SearchProblem[State, Action]
37 # Some parameters of the problem
39 # The speed on the road.
40 # Used for the action `Drive`
43 # Cost of loading a parcel in a robot
44 # Used for the action `Load`
45 var duration_loading
= 30.0
47 # Cost of unloading a parcel in a robot
48 # Used for the action `Unload`
49 var duration_unloading
= 30.0
51 # How many parcels can a robot transport at the same time?
54 # The name of the robot
57 # Locations on the map, by their names
58 var locations
= new HashMap[String, Location]
60 # All the parcels, by their `id`
61 var parcels
= new Array[Parcel]
63 # All the parcels, by their names
64 var parcel_by_name
= new HashMap[String, Parcel]
66 # +infinite, used to initialize some floats with the maximum value.
67 private var infinite
: Float = 1.0 / 0.0
70 redef var initial_state
: State is noinit
72 # Parse and initialize a problem from description `text`
73 fun parse
(text
: String)
75 var l
= new Lexer_simplan(text
)
76 var p
= new Parser_simplan
77 p
.tokens
.add_all l
.lex
83 n
= n
.children
.first
.as(not null).children
.first
.as(not null)
85 print
"Error: expected a problem, got a plan."
91 for n2
in n
.n_locations
.n_list
.as(not null).children
do
92 var e
= new Location(locations
.length
, n2
.n_name
.text
, n2
.n_x
.text
.to_f
, n2
.n_y
.text
.to_f
)
93 assert not locations
.has_key
(e
.name
)
96 print
"# {locations.length} locations"
100 for n2
in n
.n_roads
.n_list
.as(not null).children
do
101 var o
= locations
.get_or_null
(n2
.n_orig
.text
)
102 var d
= locations
.get_or_null
(n2
.n_dest
.text
)
103 assert o
!= null and d
!= null
104 var r
= new Road(o
,d
)
110 print
"# {nbr} roads"
112 # Compute all road durations
113 for e
in locations
.values
do
114 e
.durations
= new Array[Float].filled_with
(infinite
, locations
.length
)
115 e
.durations
[e
.id
] = 0.0
117 e
.durations
[r
.dest
.id
] = r
.distance
/ speed
121 # Close the duration relation between each pair of locations
122 for k
in locations
.values
do
123 for i
in locations
.values
do
124 for j
in locations
.values
do
125 var d
= i
.durations
[k
.id
] + k
.durations
[j
.id
]
126 if i
.durations
[j
.id
] > d
then
127 i
.durations
[j
.id
] = d
135 for n2
in n
.n_robots
.n_list
.as(not null).children
do
136 var name
= n2
.n_name
.text
137 robot
= locations
.get_or_null
(n2
.n_emplacement
.text
)
138 assert name
== robot_name
and robot
!= null
144 var parcel_locations
= new Array[nullable Location]
145 for n2
in n
.n_parcels
.n_list
.as(not null).children
do
146 var name
= n2
.n_name
.text
147 var e
= locations
.get_or_null
(n2
.n_emplacement
.text
)
149 var parcel
= new Parcel(parcels
.length
, name
, e
, e
)
151 parcel_locations
.add e
152 parcel_by_name
[name
] = parcel
155 print
"# {parcels.length} parcels"
157 # Load the goal of parcels
158 for n2
in n
.n_goal
.n_list
.as(not null).children
do
159 var parcel
= parcel_by_name
.get_or_null
(n2
.n_name
.text
)
160 var e
= locations
.get_or_null
(n2
.n_emplacement
.text
)
161 assert parcel
!= null and e
!= null
166 print
"# size of the problem: {locations.length.to_f.pow(parcels.length.to_f+1.0).to_precision(0)}"
168 initial_state
= new State(self, robot
, parcel_locations
, 0)
171 # Parse a plan for a given problem
172 fun parse_plan
(text
: String): Plan
174 var l
= new Lexer_simplan(text
)
175 var p
= new Parser_simplan
176 p
.tokens
.add_all l
.lex
182 n
= n
.children
.first
.as(not null).children
.first
.as(not null)
183 if n
isa Nproblem then
184 print
"Error: expected a plan, got a problem."
189 var res
= new Plan(self)
190 var e
= initial_state
192 for n2
in n
.n_actions
.as(not null).children
do
193 if n2
isa Naction_load then
194 var parcel
= parcel_by_name
.get_or_null
(n2
.n_parcel
.text
)
195 assert parcel
!= null
196 var a
= new Load(self, parcel
)
200 else if n2
isa Naction_unload then
201 var parcel
= parcel_by_name
.get_or_null
(n2
.n_parcel
.text
)
202 assert parcel
!= null
203 var a
= new Unload(self, parcel
)
207 else if n2
isa Naction_drive then
208 var o
= locations
.get_or_null
(n2
.n_orig
.text
)
209 var d
= locations
.get_or_null
(n2
.n_dest
.text
)
210 assert o
!= null and d
!= null
211 var road
: nullable Road = null
212 for r
in o
.roads
do if r
.dest
== d
then
217 var a
= new Drive(self, road
)
223 print
"# loaded plan in {res.actions.length} steps; cost = {cost}"
227 redef fun actions
(state
)
229 var res
= new Array[Action]
232 for road
in state
.robot
.roads
do
233 res
.add
(new Drive(self, road
))
237 for i
in [0..state
.parcels
.length
[ do
238 if state
.parcels
[i
] == state
.robot
and state
.nb_parcels
< self.capacity
and self.parcels
[i
].goal
!= state
.robot
then
239 res
.add
(new Load(self, parcels
[i
]))
244 for i
in [0..state
.parcels
.length
[ do
245 # Unloading is always a valid action,
246 # even if dropping a parcel outside of its goal is unlikely (but possible)
247 # to be a part of an optimal plan.
248 if state
.parcels
[i
] == null then #and self.parcels[i].goal == state.robot then
249 res
.add
(new Unload(self, parcels
[i
]))
256 redef fun apply_action
(state
, action
) do return action
.trans
(state
)
258 redef fun cost
(state1
, action
, state2
) do return action
.cost
260 redef fun is_goal
(state
)
262 for i
in [0..parcels
.length
[ do
263 if state
.parcels
[i
] != parcels
[i
].goal
then return false
268 redef fun heuristic
(state
)
270 # Combination (maximum) of two heuristic:
271 # Heuristic 1: maximum driving time to take and deliver a single parcel
272 var max_for_one
= 0.0
273 # Heuristic 2: driving time to take the nearest parcel, then deliver all parcels
274 var total_deliver_drive
= 0.0
275 var min_take_one
= infinite
276 # Total loading/unloading time (incompressible cost, added to both heuristics)
279 for i
in [0..parcels
.length
[ do
280 # The parcel location
281 var c
= state
.parcels
[i
]
284 var b
= parcels
[i
].goal
286 # Parcel is fine, nothing to do.
287 if c
== b
then continue
289 # Driving time to take and deliver this parcel
292 # Current position of the parcel (somewhere or in the robot)
293 var current
: Location
296 # The parcel is in the robot
297 current
= state
.robot
300 # The parcel is somewhere
303 var tt
= state
.robot
.duration
(c
)
304 total_load
+= duration_loading
307 if tt
< min_take_one
then min_take_one
= tt
310 # Drive and unload the parcel
311 var td
= current
.duration
(b
)
312 total_load
+= duration_unloading
315 if t
> max_for_one
then max_for_one
= t
316 total_deliver_drive
+= td
319 # Since one robot can transport several parcels at the same time,
320 # the best optimistic scenario is to deliver at full capacity
321 if min_take_one
== infinite
then min_take_one
= 0.0
322 var res
= min_take_one
+ total_deliver_drive
/ capacity
.to_f
324 # Get best of both heuristics
325 if res
< max_for_one
then res
= max_for_one
327 # Add incompressible (un)loading costs
333 redef fun make_memory
336 res
.add
new RBTreeMap[State, SearchNode[State, Action]]
337 res
.add
new BinTreeMap[State, SearchNode[State, Action]]
344 # Indexed identifier, starting at 0
345 # Used to access locations in arrays.
348 # The name of the location, from the problem description
351 # The x-coordinate of the location
354 # The y-coordinate of the location
357 # All roads from this location
358 var roads
= new Array[Road]
360 # Drive duration to any other location on the map
361 fun duration
(dest
: Location): Float
363 return durations
[dest
.id
]
366 private var durations
= new Array[Float]
368 redef fun to_s
do return "{id}({x}, {y})"
371 # A directed road segment between two locations
379 # The distance according to the coordinates of the locations.
380 var distance
: Float is noautoinit
384 var dx
= orig
.x-dest
.x
385 var dy
= orig
.y-dest
.y
386 distance
= (dx
*dx
+ dy
*dy
).sqrt
392 # Indexed identifier, starting by 0
393 # Used to access parcels in arrays.
395 # The name of the parcel according to the problem description
398 # The starting location
405 # A plan on a problem
407 # The original problem
408 var problem
: PlanProblem
410 # The sequence of actions in the plan
411 var actions
= new Array[Action]
413 # Check and write the plan
414 fun dump
(verbose
: Bool)
416 var e
= problem
.initial_state
417 print
"SimplePlan \{"
419 if verbose
then print
"#{e} {cost}"
427 print
"#{a.road.orig} -- {a.road.dest}: {a.road.distance}"
433 if not problem
.is_goal
(e
) then
434 print
"# /!\\ Goal Failed"
436 print
"# plan in {actions.length} steps; cost={cost}\n"
440 # A primitive movement
441 abstract class Action
442 # The unitary cost of the action
443 fun cost
: Float is abstract
445 # The state resulting to the application of the action
446 fun trans
(e
: State): State is abstract
448 # The original problem
449 var problem
: PlanProblem
452 # A loading of a parcel
459 redef fun cost
do return problem
.duration_loading
461 redef fun to_s
do return "Load({problem.robot_name},{parcel.name});"
466 assert res
.parcels
[parcel
.id
] == res
.robot
467 res
.parcels
= res
.parcels
.clone
468 res
.parcels
[parcel
.id
] = null
469 assert res
.nb_parcels
< problem
.capacity
475 # A unloading of a parcel
479 # The unloaded parcel
482 redef fun cost
do return problem
.duration_unloading
484 redef fun to_s
do return "Unload({problem.robot_name},{parcel.name});"
489 assert res
.parcels
[parcel
.id
] == null
490 res
.parcels
= res
.parcels
.clone
491 res
.parcels
[parcel
.id
] = res
.robot
492 assert res
.nb_parcels
> 0
505 redef fun cost
do return road
.distance
/ problem
.speed
507 redef fun to_s
do return "Drive({problem.robot_name},{road.orig.name},{road.dest.name});"
512 assert res
.robot
== road
.orig
513 res
.robot
= road
.dest
518 # Where each robot and parcel are?
521 redef type OTHER: State
523 # The original problem
524 var problem
: PlanProblem
529 # Where each parcel is
530 # `null` if the parcel is loaded
531 var parcels
: Array[nullable Location]
533 # Number of loaded parcels
534 # Must be consistent with `parcels`
538 # Used by `Action::trans`
540 # Warning: the `parcels` array is not cloned for efficiency.
541 # You need to clone it if you mutate it
542 private fun clone
: State
544 var res
= new State(problem
, robot
, parcels
, nb_parcels
)
551 if res
!= 0 then return res
552 res
= robot
.hash
* 23 + parcels
.hash
* 7
557 private var hash_cache
= 0
561 if not o
isa State then return false
562 return robot
== o
.robot
and parcels
== o
.parcels
565 redef fun <(o
) do return (self <=> o
) < 0
568 var res
= robot
.id
<=> o
.robot
.id
569 if res
!= 0 then return res
570 for i
in [0..parcels
.length
[ do
572 var oc
= o
.parcels
[i
]
573 if c
== oc
then continue
574 if c
== null then return -1
575 if oc
== null then return 1
577 if res
!= 0 then return res
584 var res
= "{robot.name}"
597 if not args
.is_empty
and args
.first
== "--configs" then
603 if args
.is_empty
then
604 print
"Usage: plan [--configs] problem.txt [plan.txt]\nSearch, or apply, a plan to a problem."
608 var problem
= new PlanProblem
611 var problem_text
= args
.first
.to_path
.read_all
612 problem
.parse
(problem_text
)
615 if args
.length
== 2 then
616 var plan_text
= args
[1].to_path
.read_all
617 var plan
= problem
.parse_plan
(plan_text
)
624 problem
.run_configs
(100000)
629 var solver
= problem
.astar
630 solver
.memorize
= true
633 print
"No plan found :("
636 var plan
= new Plan(problem
)
637 plan
.actions
.add_all res
.plan
639 print
"# solver infos: {solver}"