nit: Added link to `CONTRIBUTING.md` from the README

[nit.git] / contrib / simplan / simplan.nit

# This file is part of NIT ( http://www.nitlanguage.org ).
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# A simple planning problem solver using A* for a robot that delivers parcels.
#
# A map is made of `Location`s (with cartesian coordinates) connected with `Road`s.
# Some parcels and a robot are placed on the map.
#
# The goal of the robot is to plan the delivery of the parcels to their destination in the most
# efficient way knowing that:
#
# * the robot has a given speed to drive
# * the loading and unloading of parcels take time
# * the capacity of the robot is limited
module simplan

import ai::search

private import simplan_lexer
private import simplan_parser

# The description of the planing problem to solve
class PlanProblem
        super SearchProblem[State, Action]

        # Some parameters of the problem

        # The speed on the road.
        # Used for the action `Drive`
        var speed = 1.0

        # Cost of loading a parcel in a robot
        # Used for the action `Load`
        var duration_loading = 30.0

        # Cost of unloading a parcel in a robot
        # Used for the action `Unload`
        var duration_unloading = 30.0

        # How many parcels can a robot transport at the same time?
        var capacity = 2

        # The name of the robot
        var robot_name = "r0"

        # Locations on the map, by their names
        var locations = new HashMap[String, Location]

        # All the parcels, by their `id`
        var parcels = new Array[Parcel]

        # All the parcels, by their names
        var parcel_by_name = new HashMap[String, Parcel]

        # +infinite, used to initialize some floats with the maximum value.
        private var infinite: Float = 1.0 / 0.0

        # The starting state
        redef var initial_state: State is noinit

        # Parse and initialize a problem from description `text`
        fun parse(text: String)
        do
                var l = new Lexer_simplan(text)
                var p = new Parser_simplan
                p.tokens.add_all l.lex
                var n = p.parse
                if n isa NError then
                        print n.message
                        exit 1
                end
                n = n.children.first.children.first.as(not null)
                if n isa Nplan then
                        print "Error: expected a problem, got a plan."
                        exit 1
                end
                assert n isa Nproblem

                # Load all locations
                for n2 in n.n_locations.n_list.children do
                        var e = new Location(locations.length, n2.n_name.text, n2.n_x.text.to_f, n2.n_y.text.to_f)
                        assert not locations.has_key(e.name)
                        locations[e.name] = e
                end
                print "# {locations.length} locations"

                # Load all roads
                var nbr = 0
                for n2 in n.n_roads.n_list.children do
                        var o = locations.get_or_null(n2.n_orig.text)
                        var d = locations.get_or_null(n2.n_dest.text)
                        assert o != null and d != null
                        var r = new Road(o,d)
                        o.roads.add(r)
                        r = new Road(d,o)
                        d.roads.add(r)
                        nbr += 2
                end
                print "# {nbr} roads"

                # Compute all road durations
                for e in locations.values do
                        e.durations = new Array[Float].filled_with(infinite, locations.length)
                        e.durations[e.id] = 0.0
                        for r in e.roads do
                                e.durations[r.dest.id] = r.distance / speed
                        end
                end

                # Close the duration relation between each pair of locations
                for k in locations.values do
                        for i in locations.values do
                                for j in locations.values do
                                        var d = i.durations[k.id] + k.durations[j.id]
                                        if i.durations[j.id] > d then
                                                i.durations[j.id] = d
                                        end
                                end
                        end
                end

                # Load the robot
                var robot = null
                for n2 in n.n_robots.n_list.children do
                        var name = n2.n_name.text
                        robot = locations.get_or_null(n2.n_emplacement.text)
                        assert name == robot_name and robot != null
                end
                assert robot != null
                print "# 1 robot"

                # Load the parcels
                var parcel_locations = new Array[nullable Location]
                for n2 in n.n_parcels.n_list.children do
                        var name = n2.n_name.text
                        var e = locations.get_or_null(n2.n_emplacement.text)
                        assert e != null
                        var parcel = new Parcel(parcels.length, name, e, e)
                        parcels.add parcel
                        parcel_locations.add e
                        parcel_by_name[name] = parcel

                end
                print "# {parcels.length} parcels"

                # Load the goal of parcels
                for n2 in n.n_goal.n_list.children do
                        var parcel = parcel_by_name.get_or_null(n2.n_name.text)
                        var e = locations.get_or_null(n2.n_emplacement.text)
                        assert parcel != null and e != null
                        parcel.goal = e
                end
                print "# 1 goal"

                print "# size of the problem: {locations.length.to_f.pow(parcels.length.to_f+1.0).to_precision(0)}"

                initial_state = new State(self, robot, parcel_locations, 0)
        end

        # Parse a plan for a given problem
        fun parse_plan(text: String): Plan
        do
                var l = new Lexer_simplan(text)
                var p = new Parser_simplan
                p.tokens.add_all l.lex
                var n = p.parse
                if n isa NError then
                        print n.message
                        exit 1
                end
                n = n.children.first.children.first.as(not null)
                if n isa Nproblem then
                        print "Error: expected a plan, got a problem."
                        exit 1
                end
                assert n isa Nplan

                var res = new Plan(self)
                var e = initial_state
                var cost = 0.0
                for n2 in n.n_actions.children do
                        if n2 isa Naction_load then
                                var parcel = parcel_by_name.get_or_null(n2.n_parcel.text)
                                assert parcel != null
                                var a = new Load(self, parcel)
                                res.actions.add(a)
                                e = a.trans(e)
                                cost += a.cost
                        else if n2 isa Naction_unload then
                                var parcel = parcel_by_name.get_or_null(n2.n_parcel.text)
                                assert parcel != null
                                var a = new Unload(self, parcel)
                                res.actions.add(a)
                                e = a.trans(e)
                                cost += a.cost
                        else if n2 isa Naction_drive then
                                var o = locations.get_or_null(n2.n_orig.text)
                                var d = locations.get_or_null(n2.n_dest.text)
                                assert o != null and d != null
                                var road: nullable Road = null
                                for r in o.roads do if r.dest == d then
                                        road = r
                                        break
                                end
                                assert road != null
                                var a = new Drive(self, road)
                                res.actions.add(a)
                                e = a.trans(e)
                                cost += a.cost
                        else abort
                end
                print "# loaded plan in {res.actions.length} steps; cost = {cost}"
                return res
        end

        redef fun actions(state)
        do
                var res = new Array[Action]

                # Driving?
                for road in state.robot.roads do
                        res.add(new Drive(self, road))
                end

                # Loading?
                for i in [0..state.parcels.length[ do
                        if state.parcels[i] == state.robot and state.nb_parcels < self.capacity and self.parcels[i].goal != state.robot then
                                res.add(new Load(self, parcels[i]))
                        end
                end

                # Unloading?
                for i in [0..state.parcels.length[ do
                        # Unloading is always a valid action,
                        # even if dropping a parcel outside of its goal is unlikely (but possible)
                        # to be a part of an optimal plan.
                        if state.parcels[i] == null then #and self.parcels[i].goal == state.robot then
                                res.add(new Unload(self, parcels[i]))
                        end
                end

                return res
        end

        redef fun apply_action(state, action) do return action.trans(state)

        redef fun cost(state1, action, state2) do return action.cost

        redef fun is_goal(state)
        do
                for i in [0..parcels.length[ do
                        if state.parcels[i] != parcels[i].goal then return false
                end
                return true
        end

        redef fun heuristic(state)
        do
                # Combination (maximum) of two heuristic:
                # Heuristic 1: maximum driving time to take and deliver a single parcel
                var max_for_one = 0.0
                # Heuristic 2: driving time to take the nearest parcel, then deliver all parcels
                var total_deliver_drive = 0.0
                var min_take_one = infinite
                # Total loading/unloading time (incompressible cost, added to both heuristics)
                var total_load = 0.0

                for i in [0..parcels.length[ do
                        # The parcel location
                        var c = state.parcels[i]

                        # Its goal location
                        var b = parcels[i].goal

                        # Parcel is fine, nothing to do.
                        if c == b then continue

                        # Driving time to take and deliver this parcel
                        var t = 0.0

                        # Current position of the parcel (somewhere or in the robot)
                        var current: Location

                        if c == null then
                                # The parcel is in the robot
                                current = state.robot
                                min_take_one = 0.0
                        else
                                # The parcel is somewhere
                                current = c
                                # So go get it.
                                var tt = state.robot.duration(c)
                                total_load += duration_loading

                                t += tt
                                if tt < min_take_one then min_take_one = tt
                        end

                        # Drive and unload the parcel
                        var td = current.duration(b)
                        total_load += duration_unloading

                        t += td
                        if t > max_for_one then max_for_one = t
                        total_deliver_drive += td
                end

                # Since one robot can transport several parcels at the same time,
                # the best optimistic scenario is to deliver at full capacity
                if min_take_one == infinite then min_take_one = 0.0
                var res = min_take_one + total_deliver_drive / capacity.to_f

                # Get best of both heuristics
                if res < max_for_one then res = max_for_one

                # Add incompressible (un)loading costs
                res += total_load

                return res
        end

        redef fun make_memory
        do
                var res = super
                res.add new RBTreeMap[State, SearchNode[State, Action]]
                res.add new BinTreeMap[State, SearchNode[State, Action]]
                return res
        end
end

# A node on the map
class Location
        # Indexed identifier, starting at 0
        # Used to access locations in arrays.
        var id: Int

        # The name of the location, from the problem description
        var name: String

        # The x-coordinate of the location
        var x: Float

        # The y-coordinate of the location
        var y: Float

        # All roads from this location
        var roads = new Array[Road]

        # Drive duration to any other location on the map
        fun duration(dest: Location): Float
        do
                return durations[dest.id]
        end

        private var durations = new Array[Float]

        redef fun to_s do return "{id}({x}, {y})"
end

# A directed road segment between two locations
class Road
        # The origin
        var orig: Location

        # The destination
        var dest: Location

        # The distance according to the coordinates of the locations.
        var distance: Float is noautoinit

        init
        do
                var dx = orig.x-dest.x
                var dy = orig.y-dest.y
                distance = (dx*dx + dy*dy).sqrt
        end
end

# A parcel to move
class Parcel
        # Indexed identifier, starting by 0
        # Used to access parcels in arrays.
        var id: Int
        # The name of the parcel according to the problem description
        # Used for output
        var name: String
        # The starting location
        var start: Location
        # The goal location
        var goal: Location
end


# A plan on a problem
class Plan
        # The original problem
        var problem: PlanProblem

        # The sequence of actions in the plan
        var actions = new Array[Action]

        # Check and write the plan
        fun dump(verbose: Bool)
        do
                var e = problem.initial_state
                print "SimplePlan \{"
                var cost = 0.0
                if verbose then print "#{e} {cost}"
                for a in actions do
                        print "{a}"
                        e = a.trans(e)
                        cost += a.cost
                        if verbose then
                                print "#{e} {cost}"
                                if a isa Drive then
                                        print "#{a.road.orig} -- {a.road.dest}: {a.road.distance}"
                                end
                        end

                end
                print "\}"
                if not problem.is_goal(e) then
                        print "# /!\\ Goal Failed"
                end
                print "# plan in {actions.length} steps; cost={cost}\n"
        end
end

# A primitive movement
abstract class Action
        # The unitary cost of the action
        fun cost: Float is abstract

        # The state resulting to the application of the action
        fun trans(e: State): State is abstract

        # The original problem
        var problem: PlanProblem
end

# A loading of a parcel
class Load
        super Action

        # The loaded parcel
        var parcel: Parcel

        redef fun cost do return problem.duration_loading

        redef fun to_s do return "Load({problem.robot_name},{parcel.name});"

        redef fun trans(e)
        do
                var res = e.clone
                assert res.parcels[parcel.id] == res.robot
                res.parcels = res.parcels.clone
                res.parcels[parcel.id] = null
                assert res.nb_parcels < problem.capacity
                res.nb_parcels += 1
                return res
        end
end

# A unloading of a parcel
class Unload
        super Action

        # The unloaded parcel
        var parcel: Parcel

        redef fun cost do return problem.duration_unloading

        redef fun to_s do return "Unload({problem.robot_name},{parcel.name});"

        redef fun trans(e)
        do
                var res = e.clone
                assert res.parcels[parcel.id] == null
                res.parcels = res.parcels.clone
                res.parcels[parcel.id] = res.robot
                assert res.nb_parcels > 0
                res.nb_parcels -= 1
                return res
        end
end

# A road driving
class Drive
        super Action

        # The road drove on
        var road: Road

        redef fun cost do return road.distance / problem.speed

        redef fun to_s do return "Drive({problem.robot_name},{road.orig.name},{road.dest.name});"

        redef fun trans(e)
        do
                var res = e.clone
                assert res.robot == road.orig
                res.robot = road.dest
                return res
        end
end

# Where each robot and parcel are?
class State
        super Comparable
        redef type OTHER: State

        # The original problem
        var problem: PlanProblem

        # Where is the robot
        var robot: Location

        # Where each parcel is
        # `null` if the parcel is loaded
        var parcels: Array[nullable Location]

        # Number of loaded parcels
        # Must be consistent with `parcels`
        var nb_parcels: Int

        # Clone of the state
        # Used by `Action::trans`
        #
        # Warning: the `parcels` array is not cloned for efficiency.
        # You need to clone it if you mutate it
        private fun clone: State
        do
                var res = new State(problem, robot, parcels, nb_parcels)
                return res
        end

        redef fun hash
        do
                var res = hash_cache
                if res != 0 then return res
                res = robot.hash * 23 + parcels.hash * 7
                hash_cache = res
                return res
        end

        private var hash_cache = 0

        redef fun ==(o)
        do
                if not o isa State then return false
                return robot == o.robot and parcels == o.parcels
        end

        redef fun <(o) do return (self <=> o) < 0

        redef fun <=>(o) do
                var res = robot.id <=> o.robot.id
                if res != 0 then return res
                for i in [0..parcels.length[ do
                        var c = parcels[i]
                        var oc = o.parcels[i]
                        if c == oc then continue
                        if c == null then return -1
                        if oc == null then return 1
                        res = c.id <=> oc.id
                        if res != 0 then return res
                end
                return 0
        end

        redef fun to_s
        do
                var res = "{robot.name}"
                for c in parcels do
                        res += ","
                        if c != null then
                                res += c.name
                        end
                end
                return res
        end
end

# --configs
var configs = false
if not args.is_empty and args.first == "--configs" then
        configs = true
        args.shift
end

# Usage
if args.is_empty then
        print "Usage: plan [--configs] problem.txt [plan.txt]\nSearch, or apply, a plan to a problem."
        exit 0
end

var problem = new PlanProblem

# Load
var problem_text = args.first.to_path.read_all
problem.parse(problem_text)

# Apply a plan
if args.length == 2 then
        var plan_text = args[1].to_path.read_all
        var plan = problem.parse_plan(plan_text)
        plan.dump(false)
        exit 0
end

# run --configs?
if configs then
        problem.run_configs(100000)
        exit 0
end

# Solve the plan
var solver = problem.astar
solver.memorize = true
var res = solver.run
if res == null then
        print "No plan found :("
        return
end
var plan = new Plan(problem)
plan.actions.add_all res.plan
plan.dump(false)
print "# solver infos: {solver}"