From 9259bb7379f735d5bd885c0c60bcefbcee961c88 Mon Sep 17 00:00:00 2001 From: Kandai Watanabe Date: Fri, 9 Aug 2024 16:55:08 -0600 Subject: [PATCH] temp --- examples/demo/integrated_example1.py | 75 ++++++++++++++++ examples/demo/learning_example1.py | 20 ++--- examples/demo/planning_example1.py | 22 +++-- examples/demo/planning_example2.py | 2 +- examples/demo/planning_example3.py | 7 ++ examples/demo/planning_example4.py | 6 ++ examples/demo/planning_example4_5.py | 123 +++++++++++++++++++++++++++ examples/demo/planning_example9.py | 4 +- examples/demo/planning_utils.py | 52 ++++++++--- specless/tsp/solver/milp.py | 6 ++ 10 files changed, 287 insertions(+), 30 deletions(-) create mode 100644 examples/demo/integrated_example1.py create mode 100644 examples/demo/planning_example4_5.py diff --git a/examples/demo/integrated_example1.py b/examples/demo/integrated_example1.py new file mode 100644 index 0000000..4da81de --- /dev/null +++ b/examples/demo/integrated_example1.py @@ -0,0 +1,75 @@ +import specless as sl # or load from specless.inference import TPOInference +from examples.demo.planning_utils import get_location_assignments + + +def main(): + + ### Timed Partial Order Inference + + # Manually prepare a list of demonstrations + demonstrations: list = [ + [[1, "Room B"], [16, "Room C"], [31, "Room J"], [46, "Room I"]], + [[10, "Room J"], [20, "Room I"], [30, "Room B"], [40, "Room C"]], + ] + + # Timed Partial Order Inference + inference = sl.TPOInferenceAlgorithm() + timed_partial_order: sl.Specification = inference.infer(demonstrations) + + print(timed_partial_order) + + ##################### + # Given # + ##################### + # A floormap + floormap = { + "Room A": (0, 5), + "Room B": (0, 15), + "Room C": (3, 0), + "Room D": (4, 5), + "Room E": (4, 10), + "Room F": (6, 5), + "Room G": (6, 10), + "Room H": (7, 0), + "Room I": (10, 5), + "Room J": (10, 15), + } + # Distance cost between two locations + # We can similarly use A* to compute the "actual" distance + dist = lambda v1, v2: ((v1[0] - v2[0]) ** 2 + (v1[1] - v2[1]) ** 2) ** 0.5 + # For now, let's just use the euclidean distance + costs = [[dist(v1, v2) for v2 in floormap.values()] for v1 in floormap.values()] + + ##################### + # Define # + ##################### + # Define a task (rooms to visit) + rooms_to_visit = ["Room B", "Room C", "Room J", "Room I"] + # Define initial locations of the robot + robot_initial_locations = ["Room A", "Room D"] + rooms_of_interest = rooms_to_visit + robot_initial_locations + + ##################### + # Main # + ##################### + # Recreate the cost matrix + rooms = list(floormap.keys()) + costs = [ + [costs[rooms.index(r1)][rooms.index(r2)] for r2 in rooms_of_interest] + for r1 in rooms_of_interest + ] + + tours, cost, timestamps = get_location_assignments( + rooms_of_interest, + robot_initial_locations, + costs, + timed_partial_order=timed_partial_order, + ) + + print(tours) + print(cost) + print(timestamps) + + +if __name__ == "__main__": + main() diff --git a/examples/demo/learning_example1.py b/examples/demo/learning_example1.py index 580fbc9..231130c 100644 --- a/examples/demo/learning_example1.py +++ b/examples/demo/learning_example1.py @@ -7,22 +7,22 @@ def main(): # Manually prepare a list of demonstrations demonstrations = [ - ["e1", "e2", "e3", "e4", "e5"], # trace 1 - ["e1", "e4", "e2", "e3", "e5"], # trace 2 - ["e1", "e2", "e4", "e3", "e5"], # trace 3 + ["e1", "e2", "e3", "e4", "e5"], # trace 1 + ["e1", "e4", "e2", "e3", "e5"], # trace 2 + ["e1", "e2", "e4", "e3", "e5"], # trace 3 ] # Run the inference inference = sl.POInferenceAlgorithm() - specification = inference.infer(demonstrations) # returns a Specification + specification = inference.infer(demonstrations) # returns a Specification # prints the specification - print(specification) # doctest: +ELLIPSIS + print(specification) # doctest: +ELLIPSIS # exports the specification to a file # drawws the specification to a file - sl.draw_graph(specification, filepath='spec') + sl.draw_graph(specification, filepath="spec") ### Timed Partial Order Inference @@ -31,15 +31,13 @@ def main(): [[1, "a"], [2, "b"], [3, "c"]], [[4, "d"], [5, "e"], [6, "f"]], ] - columns: list = ["timestamp", "symbol"] - - timedtrace_dataset = sl.ArrayDataset(demonstrations, columns) # Timed Partial Order Inference inference = sl.TPOInferenceAlgorithm() - specification: sl.Specification = inference.infer(timedtrace_dataset) + specification: sl.Specification = inference.infer(demonstrations) + + # TODO: Plan with the TPO. if __name__ == "__main__": main() - diff --git a/examples/demo/planning_example1.py b/examples/demo/planning_example1.py index 10a6c59..e526a0a 100644 --- a/examples/demo/planning_example1.py +++ b/examples/demo/planning_example1.py @@ -1,24 +1,33 @@ """ Goal ==== -In this script, we will show how to solve/implement for Example 1,2,3,4 +In this script, we will show how to solve/implement the examples Examples ======== 1. Given number of rooms, solve a TSP for a single agent. 2. Multiple Agent - 3. Global Time Constraints - 4. Local Time Constraints - #! Note: Add an example with TPOs & Add a flag to export MILP formulation to a file + 3. Global Time Constraints (and with the TPO representation) + 4. Local Time Constraints (and with the TPO representation) + 4.5. How to export the MILP formulation to a file for debugging? 5. What if we don't need to come back to its home depot? 6. What if robots are in a new location (not in the designated area) -> Run A* to get the cost matrix 7. What if a path between two locations are blocked? -> Set the cost to a big number (not infinity) - 8. Can we visit the same place multiple times? + 8.1. Can we visit the same place N times? + -> Yes, specless can easily deal with the problem + 8.2. Can we visit the same place arbitrary many times? -> There are multiple ways: (1) Reformulate MILP, (2) Use OR-Tools -> https://developers.google.com/optimization/routing/penalties + + #! NOTE: The following constraints are difficult to model in our MILP formulation, + # because we formulate the TSP as a flow constraining problem whic makes it difficult + # to add additional "dimension" constraints, e.g., pickup, resource, and capacity. + 9. Pick and Delivery constraints + #! NOTE: Pick and Delivery constraints are difficult to model in MILP, + because it constraining the same vehicle to pickup and deliver is difficult. -> Not Possible. Use OR-Tools -> https://developers.google.com/optimization/routing/pickup_delivery 10. Resource constraints, etc. Battery @@ -73,8 +82,9 @@ def main(): } # Compute the distance cost between two locations - # For now, let's just use the euclidean distance + # For now, let's just use the euclidean distance for demo. # (In a continuous environment, we can similarly use A* to compute the distance) + #! NOTE: We need to use A* for modeling more accurate distance of the environment dist = lambda v1, v2: ((v1[0] - v2[0]) ** 2 + (v1[1] - v2[1]) ** 2) ** 0.5 costs = [[dist(v1, v2) for v2 in floormap.values()] for v1 in floormap.values()] diff --git a/examples/demo/planning_example2.py b/examples/demo/planning_example2.py index 7744aa9..4c0ec51 100644 --- a/examples/demo/planning_example2.py +++ b/examples/demo/planning_example2.py @@ -80,7 +80,7 @@ def main(): # Define # ##################### # Define a task (rooms to visit) - rooms_to_visit = ["Room B", "Room C", "Room J", "Room I"] + rooms_to_visit = ["Room B", "Room C", "Room J", "Room I1", "Room I2"] # NOTE: Define initial locations of the robot robot_initial_locations = ["Room A", "Room D"] rooms_of_interest = rooms_to_visit + robot_initial_locations diff --git a/examples/demo/planning_example3.py b/examples/demo/planning_example3.py index a05a97d..b89741e 100644 --- a/examples/demo/planning_example3.py +++ b/examples/demo/planning_example3.py @@ -51,6 +51,7 @@ """ from examples.demo.planning_utils import get_location_assignments +import specless as sl def main(): @@ -90,6 +91,11 @@ def main(): "Room B": (0, 15), "Room J": (21, 30), } + # OR + # timed_partial_order: sl.TimedPartialOrder = ( + # sl.TimedPartialOrder.from_constraints(global_constraints, {}) + # ) + ##################### # Main # ##################### @@ -105,6 +111,7 @@ def main(): robot_initial_locations, costs, global_constraints=global_constraints, + # timed_partial_order=timed_partial_order, ) print(tours) diff --git a/examples/demo/planning_example4.py b/examples/demo/planning_example4.py index d9b9da4..7f420ef 100644 --- a/examples/demo/planning_example4.py +++ b/examples/demo/planning_example4.py @@ -90,6 +90,11 @@ def main(): ("Room B", "Room C"): (0, 15), ("Room J", "Room I"): (20, 30), } + # OR + # timed_partial_order: sl.TimedPartialOrder = ( + # sl.TimedPartialOrder.from_constraints({}, local_constraints) + # ) + ##################### # Main # ##################### @@ -105,6 +110,7 @@ def main(): robot_initial_locations, costs, local_constraints=local_constraints, + # timed_partial_order=timed_partial_order, ) print(tours) diff --git a/examples/demo/planning_example4_5.py b/examples/demo/planning_example4_5.py new file mode 100644 index 0000000..fe5b708 --- /dev/null +++ b/examples/demo/planning_example4_5.py @@ -0,0 +1,123 @@ +""" +Goal +==== +In this script, we will show how to solve/implement for Example 1,2,3,4 + +Examples +======== + 1. Given number of rooms, solve a TSP for a single agent. + 2. Multiple Agent + 3. Global Time Constraints + 4. Local Time Constraints + 5. What if we don't need to come back to its home depot? + 6. What if robots are in a new location (not in the designated area) + -> Run A* to get the cost matrix + 7. What if a path between two locations are blocked? + -> Set the cost to a big number (not infinity) + 8. Can we visit the same place multiple times?' + -> There are multiple ways: (1) Reformulate MILP, (2) Use OR-Tools + -> https://developers.google.com/optimization/routing/penalties + 9. Pick and Delivery constraints? + -> Not Possible. Use OR-Tools + -> https://developers.google.com/optimization/routing/pickup_delivery + 10. Resource constraints, etc. Battery + -> Use OR-Tools + -> https://developers.google.com/optimization/routing/cvrptw_resources + 11. Carriege Capacity constraints, etc, weight of the package. + -> Use OR-Tools + -> https://developers.google.com/optimization/routing/cvrp + +Environment +=========== +Consider the following environment: + +######################################################## +# # # # +# # A # B # +# ############ ######################### #### +# C # +# # ####### ############ ##### # +# # # D # E # # +# # # # # # +###### ################################### # +# # # # # # +# # # F # G # # +# # ####### ############ ##### # +# H # +# ############ ######################### #### +# # I # J # +# # # # +######################################################## +""" + +from examples.demo.planning_utils import get_location_assignments + + +def main(): + ##################### + # Given # + ##################### + # A floormap + floormap = { + "Room A": (0, 5), + "Room B": (0, 15), + "Room C": (3, 0), + "Room D": (4, 5), + "Room E": (4, 10), + "Room F": (6, 5), + "Room G": (6, 10), + "Room H": (7, 0), + "Room I": (10, 5), + "Room J": (10, 15), + } + # Distance cost between two locations + # We can similarly use A* to compute the "actual" distance + dist = lambda v1, v2: ((v1[0] - v2[0]) ** 2 + (v1[1] - v2[1]) ** 2) ** 0.5 + # For now, let's just use the euclidean distance + costs = [[dist(v1, v2) for v2 in floormap.values()] for v1 in floormap.values()] + + ##################### + # Define # + ##################### + # Define a task (rooms to visit) + rooms_to_visit = ["Room B", "Room C", "Room J", "Room I"] + # Define initial locations of the robot + robot_initial_locations = ["Room A", "Room D"] + rooms_of_interest = rooms_to_visit + robot_initial_locations + + #! NOTE: Adding Local Constraints + local_constraints = { + ("Room B", "Room C"): (0, 15), + ("Room J", "Room I"): (20, 30), + } + # OR + # timed_partial_order: sl.TimedPartialOrder = ( + # sl.TimedPartialOrder.from_constraints({}, local_constraints) + # ) + + ##################### + # Main # + ##################### + # Recreate the cost matrix + rooms = list(floormap.keys()) + costs = [ + [costs[rooms.index(r1)][rooms.index(r2)] for r2 in rooms_of_interest] + for r1 in rooms_of_interest + ] + + tours, cost, timestamps = get_location_assignments( + rooms_of_interest, + robot_initial_locations, + costs, + local_constraints=local_constraints, + # timed_partial_order=timed_partial_order, + export_filename="examples/demo/planning_example4_5.lp", + ) + + print(tours) + print(cost) + print(timestamps) + + +if __name__ == "__main__": + main() diff --git a/examples/demo/planning_example9.py b/examples/demo/planning_example9.py index d4578d2..7ec7501 100644 --- a/examples/demo/planning_example9.py +++ b/examples/demo/planning_example9.py @@ -50,7 +50,9 @@ ######################################################## """ -"""Capacited Vehicles Routing Problem (CVRP).""" +""" +Capacited Vehicles Routing Problem (CVRP). +""" from ortools.constraint_solver import routing_enums_pb2 from ortools.constraint_solver import pywrapcp diff --git a/examples/demo/planning_utils.py b/examples/demo/planning_utils.py index b72c365..3bf4227 100644 --- a/examples/demo/planning_utils.py +++ b/examples/demo/planning_utils.py @@ -47,23 +47,41 @@ TimeBound = Tuple[float, float] +def from_str_to_id_tpo(timed_partial_order, loc_to_node): + global_constraints = {} + local_constraints = {} + for loc, (lb, ub) in timed_partial_order.global_constraints.items(): + node = loc_to_node[loc] + global_constraints[node] = (lb, ub) + + for loc1, srd_dict in timed_partial_order.local_constraints.items(): + for loc2, (lb, ub) in srd_dict.items(): + node1, node2 = loc_to_node[loc1], loc_to_node[loc2] + local_constraints[(node1, node2)] = (lb, ub) + + return sl.TimedPartialOrder.from_constraints(global_constraints, local_constraints) + + def get_node_assignments( nodes: List[int], costs: List[List[float]], initial_nodes: List[int], global_constraints: Dict[int, Tuple[float, float]] = {}, local_constraints: Dict[Tuple[int, int], Tuple[float, float]] = {}, + timed_partial_order: Optional[sl.TimedPartialOrder] = None, come_back_home: bool = True, + export_filename: Optional[str] = None, ): num_agent: int = len(initial_nodes) # Define Time Specification - tpo: sl.TimedPartialOrder = sl.TimedPartialOrder.from_constraints( - global_constraints, local_constraints - ) + if not timed_partial_order: + timed_partial_order: sl.TimedPartialOrder = ( + sl.TimedPartialOrder.from_constraints(global_constraints, local_constraints) + ) # Construct a TSP instance - tsp_with_tpo = sl.TSPWithTPO(nodes, costs, tpo) + tsp_with_tpo = sl.TSPWithTPO(nodes, costs, timed_partial_order) # Instantiate a solver tspsolver = sl.MILPTSPWithTPOSolver() # Solve TSP -> Tours @@ -72,6 +90,7 @@ def get_node_assignments( num_agent=num_agent, init_nodes=initial_nodes, come_back_home=come_back_home, + export_filename=export_filename, ) return tours, cost, timestamps @@ -83,7 +102,9 @@ def get_location_assignments( costs: Optional[List[List[float]]] = None, global_constraints: Dict[Location, TimeBound] = {}, local_constraints: Dict[Tuple[Location, Location], TimeBound] = {}, + timed_partial_order: Optional[sl.TimedPartialOrder] = None, come_back_home: bool = True, + export_filename: Optional[str] = None, ): # Convert locations to nodes n: int = len(locations) @@ -91,13 +112,20 @@ def get_location_assignments( node_to_loc = {i: l for i, l in enumerate(locations)} loc_to_node = {l: i for i, l in enumerate(locations)} initial_nodes = [loc_to_node[l] for l in initial_locations] - node_global_constraints: Dict[int, TimeBound] = { - loc_to_node[loc]: bound for loc, bound in global_constraints.items() - } - node_to_node_local_constraints: Dict[Tuple[int, int], TimeBound] = { - (loc_to_node[l1], loc_to_node[l2]): bound - for (l1, l2), bound in local_constraints.items() - } + + if timed_partial_order: + # Change the keys from strings to node IDs + timed_partial_order = from_str_to_id_tpo(timed_partial_order, loc_to_node) + node_global_constraints = {} + node_to_node_local_constraints = {} + else: + node_global_constraints: Dict[int, TimeBound] = { + loc_to_node[loc]: bound for loc, bound in global_constraints.items() + } + node_to_node_local_constraints: Dict[Tuple[int, int], TimeBound] = { + (loc_to_node[l1], loc_to_node[l2]): bound + for (l1, l2), bound in local_constraints.items() + } if costs is None: costs = [ @@ -112,7 +140,9 @@ def get_location_assignments( initial_nodes, node_global_constraints, node_to_node_local_constraints, + timed_partial_order, come_back_home, + export_filename, ) # Convert node assignments to location assignments location_assignments = [list(map(lambda n: node_to_loc[n], tour)) for tour in tours] diff --git a/specless/tsp/solver/milp.py b/specless/tsp/solver/milp.py index c2d9563..3154ccc 100644 --- a/specless/tsp/solver/milp.py +++ b/specless/tsp/solver/milp.py @@ -108,6 +108,7 @@ def solve( num_agent: int = 1, init_nodes: Optional[List[Node]] = None, come_back_home: bool = True, + export_filename: Optional[str] = None, ) -> Tuple[List, float]: tsp = copy.deepcopy(tsp) @@ -173,6 +174,8 @@ def solve( # Users can add a new objective by replacing get_edge_cost_objective m.addGenConstrMax(tf, tT) self.optimize(m, variables, tf) + if export_filename: + m.write(export_filename) if m.status != GRB.OPTIMAL: print("Tour: [], Cost: n/a") @@ -267,6 +270,7 @@ def solve( num_agent: int = 1, init_nodes: Optional[List[Node]] = None, come_back_home: bool = True, + export_filename: Optional[str] = None, ) -> Tuple[List, float]: tsp = copy.deepcopy(tsp) if len(tsp.nodes) < num_agent: @@ -373,6 +377,8 @@ def solve( # tf = max([tT1, tT2, ..., tT_|init_nodes|]) m.addGenConstrMax(tf, tT) self.optimize(m, variables, tf) + if export_filename: + m.write(export_filename) if m.status != GRB.OPTIMAL: print("Tour: [], Cost: n/a")