15. Task and Motion Planning via UP SpiderPlan
Note: this will not run in docker or on Colab since up-spiderplan requires docker.
15.1. Setup
Install jupyter in a python 3.7 or 3.8. For more details on up-spiderplan see https://github.com/aiplan4eu/up-spiderplan.
15.2. Install Requirements
[ ]:
%pip install unified-planning
%pip install up-spiderplan==0.6.4
[2]:
import os
import math
import unified_planning.engines.results as results
from unified_planning.shortcuts import *
15.3. Creating Movable Robot
We declare a type for robots using the MovableType.
[3]:
t_robot = MovableType("robot")
15.4. Using a Map
Next we add an occupancy map by using a reference to a YAML file in the format used by the Robot Operating System (ROS). See http://wiki.ros.org/map_server.
[4]:
occ_map = OccupancyMap("./maps/office-map-1.yaml", (0, 0))
15.5. Adding Configuration Types
A configuration specifies a location and orientation in our map. The last value is the number of dimensions of a configuration (here we have three for X, Y coordnates and angle).
[5]:
t_robot_config = ConfigurationType("robot_config", occ_map, 3)
15.6. Adding Fluents
Fluents are added as ususal, but we use our previously added ConfigurationType in place of a regular location.
[6]:
t_parcel = UserType("parcel")
robot_at = Fluent("robot_at", BoolType(), robot=t_robot, configuration=t_robot_config)
parcel_at = Fluent("parcel_at", BoolType(), parcel=t_parcel, configuration=t_robot_config)
carries = Fluent("carries", BoolType(), robot=t_robot, parcel=t_parcel)
15.7. Creating Configuration Objects
Configuration objects combine a symbolic name like parkin-1 with a pose. Each pose is a tuple with the number of elements specified by the configuration type (so three in this case).
[7]:
park1 = ConfigurationObject("parking-1", t_robot_config, (46.0, 26.0, 3*math.pi/2))
park2 = ConfigurationObject("parking-2", t_robot_config, (40.0, 26.0, 3*math.pi/2))
office1 = ConfigurationObject("office-1", t_robot_config, (4.0, 4.0, 3*math.pi/2))
office2 = ConfigurationObject("office-2", t_robot_config, (14.0, 4.0, math.pi/2))
office3 = ConfigurationObject("office-3", t_robot_config, (24.0, 4.0, 3*math.pi/2))
office4 = ConfigurationObject("office-4", t_robot_config, (32.0, 4.0, 3*math.pi/2))
office5 = ConfigurationObject("office-5", t_robot_config, (4.0, 24.0, 3*math.pi/2))
office6 = ConfigurationObject("office-6", t_robot_config, (14.0, 24.0, math.pi/2))
office7 = ConfigurationObject("office-7", t_robot_config, (24.0, 24.0, math.pi/2))
office8 = ConfigurationObject("office-8", t_robot_config, (32.0, 24.0, math.pi/2))
15.8. Adding Movable Objects
Motion planning requires objects that can move in our map. For this purpose we add two MovableObject instances for our robots names r1 and r2.
The footprint specifies the 2-dimensional outline of the robot. The motion model specifies how the robot moves. Here we use the Reeds-Shepp car model (see, e.g., https://lavalle.pl/planning/ch13.pdf) which allows movement forward and backwards, and turning at a specific turning_radius.
[8]:
r1 = MovableObject(
"r1",
t_robot,
footprint=[(-1.0, 0.5), (1.0, 0.5), (1.0, -0.5), (-1.0, -0.5)],
motion_model=MotionModels.REEDSSHEPP,
parameters={"turning_radius": 2.0},
)
r2 = MovableObject(
"r2",
t_robot,
footprint=[(-1.0, 0.5), (1.0, 0.5), (1.0, -0.5), (-1.0, -0.5)],
motion_model=MotionModels.REEDSSHEPP,
parameters={"turning_radius": 2.0},
)
15.9. Some Additional Objects
Creating some parcels in the regular Unified Planning style.
[9]:
nothing = Object("nothing", t_parcel)
p1 = Object("parcel-1", t_parcel)
p2 = Object("parcel-2", t_parcel)
15.10. Operators
Now we can create operators that can use motion constraints.
15.10.1. Move
The move operators has a motion constraint that requires the robot to move from its current configuration to the goal configuration. Apart from the last line that adds this constraints and the type InstantaneousMotionAction, this looks just like classical planning operators defined in Unified Planning.
[10]:
move = InstantaneousMotionAction(
"move", robot=t_robot, c_from=t_robot_config, c_to=t_robot_config
)
robot = move.parameter("robot")
c_from = move.parameter("c_from")
c_to = move.parameter("c_to")
move.add_precondition(robot_at(robot, c_from))
move.add_effect(robot_at(robot, c_from), False)
move.add_effect(robot_at(robot, c_to), True)
move.add_motion_constraint(Waypoints(robot, c_from, [c_to]))
15.10.2. Pick
Robots can pick parcels at their current location if they carry nothing. This operator does not use a motion planning constraint.
[11]:
pick = InstantaneousMotionAction(
"pick", robot=t_robot, loc=t_robot_config, parcel=t_parcel
)
pick_robot = pick.parameter("robot")
pick_loc = pick.parameter("loc")
pick_parcel = pick.parameter("parcel")
pick.add_precondition(robot_at(pick_robot, pick_loc))
pick.add_precondition(parcel_at(pick_parcel, pick_loc))
pick.add_precondition(carries(pick_robot, nothing))
pick.add_precondition(Not(carries(pick_robot, pick_parcel)))
pick.add_effect(carries(pick_robot, pick_parcel), True)
pick.add_effect(parcel_at(pick_parcel, pick_loc), False)
pick.add_effect(carries(pick_robot, nothing), False)
15.10.3. Place
Robots can place objects they carry at their current location. Again, no special motion planning constraint is used.
[12]:
place = InstantaneousMotionAction(
"place", robot=t_robot, loc=t_robot_config, parcel=t_parcel
)
place_robot = place.parameter("robot")
place_loc = place.parameter("loc")
place_parcel = place.parameter("parcel")
place.add_precondition(robot_at(place_robot, place_loc))
place.add_precondition(carries(place_robot, place_parcel))
place.add_precondition(Not(parcel_at(place_parcel, place_loc)))
place.add_precondition(Not(carries(place_robot, nothing)))
place.add_effect(carries(place_robot, place_parcel), False)
place.add_effect(carries(place_robot, nothing), True)
place.add_effect(parcel_at(place_parcel, place_loc), True)
Finally, we can assemble the problem. There is nothing specific to motion planning here. We use the regular UP syntax.
[13]:
problem = Problem("TAMP")
problem.add_fluent(robot_at, default_initial_value=False)
problem.add_fluent(parcel_at, default_initial_value=False)
problem.add_fluent(carries, default_initial_value=False)
problem.add_action(move)
problem.add_action(pick)
problem.add_action(place)
problem.add_object(park1)
problem.add_object(park2)
problem.add_object(office1)
problem.add_object(office2)
problem.add_object(office3)
problem.add_object(office4)
problem.add_object(office5)
problem.add_object(office6)
problem.add_object(office7)
problem.add_object(office8)
problem.add_object(r1)
problem.add_object(r2)
problem.add_object(nothing)
problem.add_object(p1)
problem.add_object(p2)
problem.set_initial_value(carries(r1, nothing), True)
problem.set_initial_value(carries(r2, nothing), True)
problem.set_initial_value(parcel_at(p1, office1), True)
problem.set_initial_value(parcel_at(p2, office6), True)
problem.set_initial_value(robot_at(r1, park1), True)
problem.add_goal(robot_at(r1, park1))
problem.add_goal(parcel_at(p1, office2))
problem.add_goal(parcel_at(p2, office3))
15.11. Solve the Problem and Show the Result
Now we can solve the problem and print the result. If a path exists, we plot it.
Note that the following will not run in a cloud environment due to the requirements of up_spiderplan
[14]:
from up_spiderplan.solver import EngineImpl
from up_spiderplan.util import plot_path
solver = EngineImpl(run_docker=True)
result = solver.solve(problem)
if result.status in results.POSITIVE_OUTCOMES:
for a in result.plan.actions:
print(a)
print(a.motion_paths)
else:
print("NO-PLAN-FOUND")
Container up-spiderplan-server-web Created
Container up-spiderplan-server-web Starting
Container up-spiderplan-server-web Started
Container up-spiderplan-server-web Stopping
move(r1, parking-1, office-6)
{waypoints(r1, parking-1, [office-6]): [((46.0, 26.0, -1.570796326794897), 0.0), ((45.93725792336355, 25.50297712490758, -1.821939505650915), 0.0), ((45.75296826163486, 25.03713849713428, -2.073082684506933), 0.0), ((45.45869373089342, 24.631711799564062, -2.3242258633629502), 0.0), ((45.07331233257161, 24.311507688712815, -2.5479571445910896), 0.0), ((44.65696087359254, 24.030539634034664, -2.5479571445910896), 0.0), ((44.24060941461347, 23.74957157935651, -2.5479571445910896), 0.0), ((43.82425795563441, 23.468603524678358, -2.5479571445910896), 0.0), ((43.40790649665534, 23.187635470000203, -2.5479571445910896), 0.0), ((42.99155503767628, 22.90666741532205, -2.5479571445910896), 0.0), ((42.57520357869721, 22.625699360643896, -2.5479571445910896), 0.0), ((42.15885211971814, 22.344731305965745, -2.5479571445910896), 0.0), ((41.742500660739076, 22.06376325128759, -2.5479571445910896), 0.0), ((41.32614920176001, 21.782795196609435, -2.5479571445910896), 0.0), ((40.909797742780945, 21.501827141931283, -2.5479571445910896), 0.0), ((40.49344628380188, 21.22085908725313, -2.5479571445910896), 0.0), ((40.07709482482281, 20.939891032574977, -2.5479571445910896), 0.0), ((39.66074336584374, 20.658922977896825, -2.5479571445910896), 0.0), ((39.24439190686468, 20.37795492321867, -2.5479571445910896), 0.0), ((38.82804044788561, 20.09698686854052, -2.5479571445910896), 0.0), ((38.41065410146332, 19.817601354114696, -2.5914425321141192), 0.0), ((37.99140911579281, 19.620593007155787, -2.774524112120613), 0.0), ((37.558262319545285, 19.45405043704998, -2.774524112120613), 0.0), ((37.125115523297765, 19.287507866944175, -2.774524112120613), 0.0), ((36.69274204396115, 19.119041717542192, -2.7289903469211083), 0.0), ((36.26188037999589, 18.861707456408183, -2.4773976886837996), 0.0), ((35.90864600210454, 18.505213179169388, -2.225805030446491), 0.0), ((35.655280550920516, 18.072005788218338, -1.9742123722091822), 0.0), ((35.51773735280972, 17.589362478846134, -1.7226197139718735), 0.0), ((35.48622515725194, 17.378783492268347, -1.4710270557345648), 0.0), ((35.37398048919628, 17.867929513330946, -1.2194343974972564), 0.0), ((35.14349826936187, 18.313732880039304, -0.9678417392599474), 0.0), ((34.80929096603267, 18.688123283000866, -0.7162490810226387), 0.0), ((34.39240216727075, 18.967526978207548, -0.46465642278533004), 0.0), ((33.91908155721363, 19.134351123712495, -0.21306376454802134), 0.0), ((33.419127881596005, 19.178228335769884, 0.02682948635701843), 0.0), ((32.916123656022165, 19.16472975175062, 0.02682948635701843), 0.0), ((32.413119430448326, 19.151231167731357, 0.02682948635701843), 0.0), ((31.91011520487449, 19.137732583712094, 0.02682948635701843), 0.0), ((31.407110979300654, 19.12423399969283, 0.02682948635701843), 0.0), ((30.904106753726815, 19.110735415673567, 0.02682948635701843), 0.0), ((30.40110252815298, 19.097236831654303, 0.02682948635701843), 0.0), ((29.898098302579143, 19.083738247635036, 0.02682948635701843), 0.0), ((29.395094077005304, 19.070239663615773, 0.02682948635701843), 0.0), ((28.892089851431468, 19.05674107959651, 0.02682948635701843), 0.0), ((28.38908562585763, 19.043242495577246, 0.02682948635701843), 0.0), ((27.886081400283793, 19.029743911557983, 0.02682948635701843), 0.0), ((27.383077174709953, 19.01624532753872, 0.02682948635701843), 0.0), ((26.88007294913612, 19.002746743519456, 0.02682948635701843), 0.0), ((26.37706872356228, 18.989248159500193, 0.02682948635701843), 0.0), ((25.874064497988442, 18.975749575480926, 0.02682948635701843), 0.0), ((25.371060272414606, 18.962250991461662, 0.02682948635701843), 0.0), ((24.86805604684077, 18.9487524074424, 0.02682948635701843), 0.0), ((24.36505182126693, 18.935253823423135, 0.02682948635701843), 0.0), ((23.86204759569309, 18.921755239403872, 0.02682948635701843), 0.0), ((23.35904337011926, 18.90825665538461, 0.02682948635701843), 0.0), ((22.85603914454542, 18.894758071365345, 0.02682948635701843), 0.0), ((22.35303491897158, 18.88125948734608, 0.02682948635701843), 0.0), ((21.850030693397745, 18.867760903326815, 0.02682948635701843), 0.0), ((21.34702646782391, 18.85426231930755, 0.02682948635701843), 0.0), ((20.84402224225007, 18.840763735288288, 0.02682948635701843), 0.0), ((20.341018016676234, 18.827265151269025, 0.02682948635701843), 0.0), ((19.838013791102398, 18.81376656724976, 0.02682948635701843), 0.0), ((19.33500956552856, 18.800267983230498, 0.02682948635701843), 0.0), ((18.832005339954723, 18.786769399211234, 0.02682948635701843), 0.0), ((18.329001114380887, 18.77327081519197, 0.02682948635701843), 0.0), ((17.825996888807047, 18.759772231172708, 0.02682948635701843), 0.0), ((17.322992663233208, 18.74627364715344, 0.02682948635701843), 0.0), ((16.819988437659376, 18.732775063134177, 0.02682948635701843), 0.0), ((16.316984212085536, 18.719276479114914, 0.02682948635701843), 0.0), ((15.813979986511697, 18.70577789509565, 0.02682948635701843), 0.0), ((15.314632395106008, 18.653074132098716, 0.2253703587058815), 0.0), ((14.81311557172982, 18.46445783892078, 0.49408443302643207), 0.0), ((15.062210064359029, 18.673743714278746, 0.7627985073469826), 0.0), ((15.396469084474557, 19.092511117084808, 1.031512581667533), 0.0), ((15.607553238769189, 19.58499330964333, 1.3002266559880837), 0.0), ((15.680312213776695, 20.11584296984609, 1.5689407303086342), 0.0), ((15.609523820827693, 20.64695899634616, 1.8376548046291847), 0.0), ((15.400268812414149, 21.14022116967326, 2.106368878949735), 0.0), ((15.120754101503913, 21.59924185982504, 2.1180136049713516), 0.0), ((14.841123319239742, 22.058192446012843, 2.1180136049713516), 0.0), ((14.561492536975571, 22.517143032200647, 2.1180136049713516), 0.0), ((14.281943750731966, 22.976143211014815, 2.108224475435998), 0.0), ((14.07177380754538, 23.469016242497034, 1.8395104011154473), 0.0), ((14.0, 24.0, 1.5707963267948966), 0.0)]}
pick(r1, office-6, parcel-2)
None
move(r1, office-6, office-3)
{waypoints(r1, office-6, [office-3]): [((14.0, 24.0, 1.5707963267948966), 0.0), ((14.06332623771857, 23.500705759605932, 1.8231121463816684), 0.0), ((14.237285348638517, 23.027550925924544, 1.9605147989741347), 0.0), ((14.42900904431806, 22.560758556910272, 1.9605147989741347), 0.0), ((14.620732739997601, 22.093966187896, 1.9605147989741347), 0.0), ((14.812456435677143, 21.627173818881726, 1.9605147989741347), 0.0), ((15.004180131356685, 21.160381449867455, 1.9605147989741347), 0.0), ((15.195903827036227, 20.693589080853183, 1.9605147989741347), 0.0), ((15.387627522715768, 20.22679671183891, 1.9605147989741347), 0.0), ((15.57935121839531, 19.76000434282464, 1.9605147989741347), 0.0), ((15.771074914074852, 19.29321197381037, 1.9605147989741347), 0.0), ((15.962798609754394, 18.826419604796097, 1.9605147989741347), 0.0), ((16.154522305433936, 18.359627235781822, 1.9605147989741347), 0.0), ((16.346246001113478, 17.89283486676755, 1.9605147989741347), 0.0), ((16.534904654921718, 17.42485155309309, 1.9031685931840001), 0.0), ((16.637957110754652, 16.932220715425494, 1.650852773597228), 0.0), ((16.612923915917563, 16.419055310055132, 1.3932533155616582), 0.0), ((16.457982713311715, 15.929199598801569, 1.1356538575260884), 0.0), ((16.183358281839652, 15.494979820634654, 0.8780543994905188), 0.0), ((16.492319618577717, 15.766015215031077, 0.620454941454949), 0.0), ((16.94523989082278, 16.008561336628887, 0.36285548341937934), 0.0), ((17.445006792005042, 16.127718537818563, 0.10525602538380952), 0.0), ((17.95864003036575, 16.115623474206753, -0.15234343265175998), 0.0), ((18.452244255856584, 15.973074315351733, -0.40994289068733025), 0.0), ((18.893245859998203, 15.709478072477904, -0.6675423487228996), 0.0), ((19.282718044700076, 15.372280345748909, -0.7186550207613479), 0.0), ((19.67050409961618, 15.033087334716132, -0.7186550207613479), 0.0), ((20.058290154532287, 14.693894323683356, -0.7186550207613479), 0.0), ((20.446076209448393, 14.354701312650583, -0.7186550207613479), 0.0), ((20.833862264364498, 14.015508301617807, -0.7186550207613479), 0.0), ((21.221648319280604, 13.67631529058503, -0.7186550207613479), 0.0), ((21.60943437419671, 13.337122279552254, -0.7186550207613479), 0.0), ((21.992942214525602, 12.993291880164987, -0.7980936960112046), 0.0), ((22.30621314913975, 12.573795008916608, -1.0606275880349878), 0.0), ((22.53307725049504, 12.100383300807707, -1.134233819554011), 0.0), ((22.755090022034633, 11.624561372857837, -1.134233819554011), 0.0), ((22.977102793574225, 11.148739444907967, -1.134233819554011), 0.0), ((23.199115565113818, 10.672917516958098, -1.134233819554011), 0.0), ((23.41376801213085, 10.193925227432729, -1.2237652128118113), 0.0), ((23.52585928681664, 9.682503826237477, -1.4862991048355947), 0.0), ((23.569610900874807, 9.16594910633662, -1.4862991048355951), 0.0), ((23.613362514932977, 8.649394386435764, -1.4862991048355951), 0.0), ((23.657114128991143, 8.132839666534908, -1.4862991048355951), 0.0), ((23.70086574304931, 7.616284946634051, -1.4862991048355951), 0.0), ((23.74461735710748, 7.099730226733195, -1.4862991048355951), 0.0), ((23.788368971165646, 6.583175506832339, -1.4862991048355951), 0.0), ((23.832120585223812, 6.066620786931483, -1.4862991048355951), 0.0), ((23.875872199281982, 5.550066067030626, -1.4862991048355951), 0.0), ((23.91962381334015, 5.033511347129769, -1.4862991048355951), 0.0), ((23.963375427398315, 4.516956627228914, -1.4862991048355951), 0.0), ((24.0, 4.0, -1.570796326794897), 0.0)]}
place(r1, office-3, parcel-2)
None
move(r1, office-3, office-1)
{waypoints(r1, office-3, [office-1]): [((24.0, 4.0, -1.570796326794897), 0.0), ((23.94993098152358, 4.505304905655232, -1.4366259118949467), 0.0), ((23.881941197129063, 5.009002511016412, -1.4366259118949467), 0.0), ((23.813951412734546, 5.512700116377593, -1.4366259118949467), 0.0), ((23.74596162834003, 6.016397721738773, -1.4366259118949467), 0.0), ((23.677971843945514, 6.520095327099952, -1.4366259118949467), 0.0), ((23.609982059550997, 7.023792932461133, -1.4366259118949467), 0.0), ((23.54199227515648, 7.527490537822313, -1.4366259118949467), 0.0), ((23.474002490761965, 8.031188143183494, -1.4366259118949467), 0.0), ((23.40601270636745, 8.534885748544674, -1.4366259118949467), 0.0), ((23.338022921972932, 9.038583353905853, -1.4366259118949467), 0.0), ((23.27003313757842, 9.542280959267035, -1.4366259118949467), 0.0), ((23.202043353183903, 10.045978564628214, -1.4366259118949467), 0.0), ((23.133887843556845, 10.549653072144665, -1.4236903836144805), 0.0), ((22.999842515613555, 11.030038583494393, -1.173671304628905), 0.0), ((22.75110680724659, 11.462321982251911, -0.9236522256433292), 0.0), ((22.40314824833094, 11.819621899318104, -0.6736331466577534), 0.0), ((21.977604501752314, 12.079719783211829, -0.4236140676721778), 0.0), ((21.500937834052706, 12.226441551747781, -0.17359498868660195), 0.0), ((21.002769101540654, 12.25098003388849, 0.058610424376439596), 0.0), ((20.50358955873686, 12.221689361766195, 0.058610424376439596), 0.0), ((20.004410015933065, 12.1923986896439, 0.058610424376439596), 0.0), ((19.505230473129266, 12.163108017521605, 0.058610424376439596), 0.0), ((19.006050930325472, 12.13381734539931, 0.058610424376439596), 0.0), ((18.506871387521677, 12.104526673277014, 0.058610424376439596), 0.0), ((18.007691844717883, 12.075236001154721, 0.058610424376439596), 0.0), ((17.508512301914088, 12.045945329032426, 0.058610424376439596), 0.0), ((17.00933275911029, 12.01665465691013, 0.058610424376439596), 0.0), ((16.510153216306495, 11.987363984787835, 0.058610424376439596), 0.0), ((16.010973673502697, 11.95807331266554, 0.058610424376439596), 0.0), ((15.511794130698902, 11.928782640543245, 0.058610424376439596), 0.0), ((15.012614587895108, 11.89949196842095, 0.058610424376439596), 0.0), ((14.513435045091313, 11.870201296298655, 0.058610424376439596), 0.0), ((14.014255502287517, 11.84091062417636, 0.058610424376439596), 0.0), ((13.515075959483722, 11.811619952054064, 0.058610424376439596), 0.0), ((13.015896416679926, 11.78232927993177, 0.058610424376439596), 0.0), ((12.516716873876131, 11.753038607809476, 0.058610424376439596), 0.0), ((12.017537331072335, 11.72374793568718, 0.058610424376439596), 0.0), ((11.51835778826854, 11.694457263564885, 0.058610424376439596), 0.0), ((11.019178245464742, 11.66516659144259, 0.058610424376439596), 0.0), ((10.519998702660947, 11.635875919320295, 0.058610424376439596), 0.0), ((10.020819159857151, 11.606585247198, 0.058610424376439596), 0.0), ((9.521639617053358, 11.577294575075705, 0.058610424376439596), 0.0), ((9.022460074249562, 11.54800390295341, 0.058610424376439596), 0.0), ((8.523280531445765, 11.518713230831114, 0.058610424376439596), 0.0), ((8.02410098864197, 11.489422558708819, 0.058610424376439596), 0.0), ((7.524921445838176, 11.460131886586524, 0.058610424376439596), 0.0), ((7.025636081159256, 11.433368653275501, 0.008312999446406621), 0.0), ((6.5302913547055645, 11.491437504070792, -0.24170607953916878), 0.0), ((6.064715571483966, 11.670260264187503, -0.4917251585247442), 0.0), ((5.657860374116771, 11.958716912288157, -0.7417442375103191), 0.0), ((5.349231023316989, 12.317444342140742, -0.9789098253203039), 0.0), ((5.410965535215843, 12.144379856799516, -1.2160754131302887), 0.0), ((5.521675267520461, 11.684292038794325, -1.4532410009402734), 0.0), ((5.521189048874822, 11.211071996218818, -1.6904065887502582), 0.0), ((5.46119954431436, 10.741043791384316, -1.6026949766710832), 0.0), ((5.369380681559214, 10.174364590689018, -1.7971127029889453), 0.0), ((5.240294699884539, 9.613757655676844, -1.7971127029889453), 0.0), ((5.111208718209864, 9.05315072066467, -1.7971127029889453), 0.0), ((4.98212273653519, 8.492543785652497, -1.7971127029889453), 0.0), ((4.853036754860515, 7.931936850640322, -1.7971127029889453), 0.0), ((4.723950773185841, 7.371329915628149, -1.7971127029889453), 0.0), ((4.594864791511166, 6.810722980615974, -1.7971127029889453), 0.0), ((4.4657788098364914, 6.250116045603801, -1.7971127029889453), 0.0), ((4.336692828161817, 5.689509110591628, -1.7971127029889453), 0.0), ((4.207606846487142, 5.128902175579453, -1.7971127029889453), 0.0), ((4.078520864812467, 4.56829524056728, -1.7971127029889453), 0.0), ((4.0, 4.0, -1.570796326794897), 0.0)]}
pick(r1, office-1, parcel-1)
None
move(r1, office-1, office-2)
{waypoints(r1, office-1, [office-2]): [((4.0, 4.0, -1.570796326794897), 0.0), ((4.0490951493745015, 4.488240861573623, -1.7110435491217957), 0.0), ((4.117762967580191, 4.974646296298211, -1.7110435491217957), 0.0), ((4.18643078578588, 5.461051731022798, -1.7110435491217957), 0.0), ((4.255098603991568, 5.947457165747386, -1.7110435491217957), 0.0), ((4.323766422197258, 6.433862600471973, -1.7110435491217957), 0.0), ((4.392434240402946, 6.92026803519656, -1.7110435491217957), 0.0), ((4.461102058608635, 7.406673469921148, -1.7110435491217957), 0.0), ((4.529769876814324, 7.893078904645735, -1.7110435491217957), 0.0), ((4.598437695020013, 8.379484339370322, -1.7110435491217957), 0.0), ((4.667105513225701, 8.86588977409491, -1.7110435491217957), 0.0), ((4.735773331431391, 9.352295208819495, -1.7110435491217957), 0.0), ((4.781757343890984, 9.840746115043585, -1.5600790949781227), 0.0), ((4.708515737343361, 10.356010009500348, -1.299117594011677), 0.0), ((4.504810879120678, 10.83493123955484, -1.0381560930452314), 0.0), ((4.184436705514802, 11.245079508778032, -0.7771945920787857), 0.0), ((4.208515578396222, 11.269501608437592, -0.5162330911123401), 0.0), ((4.690714413172965, 11.073681544091828, -0.2552715901458944), 0.0), ((5.207110587698058, 11.008903424357786, 0.005689910820551258), 0.0), ((5.722736178305315, 11.079553719271159, 0.2666514117869969), 0.0), ((6.2112929405854755, 11.26182721752568, 0.32316079782152474), 0.0), ((6.698837651397482, 11.360545689183704, 0.09917749112464125), 0.0), ((7.195046957358909, 11.40992047582049, 0.09917749112464125), 0.0), ((7.691256263320337, 11.459295262457275, 0.09917749112464125), 0.0), ((8.187465569281764, 11.50867004909406, 0.09917749112464125), 0.0), ((8.683674875243192, 11.558044835730847, 0.09917749112464125), 0.0), ((9.17988418120462, 11.607419622367631, 0.09917749112464125), 0.0), ((9.676093487166048, 11.656794409004418, 0.09917749112464125), 0.0), ((10.172302793127475, 11.706169195641204, 0.09917749112464125), 0.0), ((10.668512099088904, 11.755543982277988, 0.09917749112464125), 0.0), ((11.164721405050331, 11.804918768914774, 0.09917749112464125), 0.0), ((11.660930711011758, 11.85429355555156, 0.09917749112464125), 0.0), ((12.157140016973187, 11.903668342188345, 0.09917749112464125), 0.0), ((12.653349322934613, 11.953043128825131, 0.09917749112464125), 0.0), ((13.149362603994515, 12.004144157198173, 0.14085985940303236), 0.0), ((13.62930137442114, 12.134661625303796, 0.3901897337052056), 0.0), ((14.062193689194437, 12.379570336152428, 0.6395196080073788), 0.0), ((14.421267752445598, 12.723724153419687, 0.8888494823095522), 0.0), ((14.136626239647612, 12.253174632806784, 1.164694857421645), 0.0), ((13.990904594594236, 11.722889007582136, 1.4405402325337378), 0.0), ((13.974966564042772, 11.171936049618903, 1.5742884841701321), 0.0), ((13.97689315104917, 10.62024866337065, 1.5742884841701321), 0.0), ((13.97881973805557, 10.068561277122393, 1.5742884841701321), 0.0), ((13.980746325061968, 9.516873890874137, 1.5742884841701321), 0.0), ((13.982672912068367, 8.965186504625883, 1.5742884841701321), 0.0), ((13.984599499074765, 8.413499118377628, 1.5742884841701321), 0.0), ((13.986526086081163, 7.861811732129373, 1.5742884841701321), 0.0), ((13.988452673087561, 7.310124345881117, 1.5742884841701321), 0.0), ((13.990379260093961, 6.758436959632862, 1.5742884841701321), 0.0), ((13.99230584710036, 6.206749573384607, 1.5742884841701321), 0.0), ((13.994232434106758, 5.655062187136352, 1.5742884841701321), 0.0), ((13.996159021113156, 5.103374800888097, 1.5742884841701321), 0.0), ((13.998085608119554, 4.551687414639842, 1.5742884841701321), 0.0), ((14.0, 4.0, 1.5707963267948966), 0.0)]}
place(r1, office-2, parcel-1)
None
move(r1, office-2, parking-1)
{waypoints(r1, office-2, [parking-1]): [((14.0, 4.0, 1.5707963267948966), 0.0), ((14.063131803390862, 4.4985394557746305, 1.3188702200951394), 0.0), ((14.20752620263333, 4.981235117162517, 1.2765775252117173), 0.0), ((14.353639469475372, 5.46343628493416, 1.2765775252117173), 0.0), ((14.499752736317415, 5.945637452705803, 1.2765775252117173), 0.0), ((14.645866003159458, 6.427838620477445, 1.2765775252117173), 0.0), ((14.7919792700015, 6.9100397882490885, 1.2765775252117173), 0.0), ((14.938092536843543, 7.39224095602073, 1.2765775252117173), 0.0), ((15.084205803685585, 7.874442123792373, 1.2765775252117173), 0.0), ((15.230319070527628, 8.356643291564016, 1.2765775252117173), 0.0), ((15.376432337369671, 8.83884445933566, 1.2765775252117173), 0.0), ((15.522545604211713, 9.321045627107303, 1.2765775252117173), 0.0), ((15.668658871053756, 9.803246794878945, 1.2765775252117173), 0.0), ((15.814772137895797, 10.285447962650586, 1.2765775252117173), 0.0), ((16.011119729770027, 10.748069072333942, 1.044206484167902), 0.0), ((16.318359899421967, 11.276567687212985, 1.0442064841679004), 0.0), ((16.625600069073908, 11.805066302092026, 1.0442064841678997), 0.0), ((16.819972838148477, 12.262121010307744, 1.293183408590884), 0.0), ((16.89572810784342, 12.752978340673012, 1.5421603330138682), 0.0), ((16.848194054849632, 13.247367148663095, 1.7911372574368523), 0.0), ((16.838738198731672, 13.360794791249052, 2.0401141818598365), 0.0), ((17.11663416473473, 12.949147355038042, 2.289091106282821), 0.0), ((17.487396314572543, 12.618670112391115, 2.538068030705805), 0.0), ((17.92815976688805, 12.389743576657956, 2.7870449551287897), 0.0), ((18.411742663856433, 12.276485634686807, 3.0360218795517735), 0.0), ((18.908322474859823, 12.28588089752021, -2.9981865032048285), 0.0), ((19.387424089125023, 12.41697598808689, -2.7692730649116415), 0.0), ((19.851261147445026, 12.598120181782296, -2.7692730649116415), 0.0), ((20.315098205765032, 12.7792643754777, -2.7692730649116415), 0.0), ((20.778935264085035, 12.960408569173106, -2.7692730649116415), 0.0), ((21.242772322405038, 13.141552762868512, -2.7692730649116415), 0.0), ((21.70660938072504, 13.322696956563918, -2.7692730649116415), 0.0), ((22.170446439045048, 13.503841150259325, -2.7692730649116415), 0.0), ((22.634283497365054, 13.68498534395473, -2.7692730649116415), 0.0), ((23.098120555685057, 13.866129537650135, -2.7692730649116415), 0.0), ((23.56195761400506, 14.047273731345541, -2.7692730649116415), 0.0), ((24.025794672325063, 14.228417925040947, -2.7692730649116415), 0.0), ((24.489631730645065, 14.409562118736353, -2.7692730649116415), 0.0), ((24.953468788965072, 14.59070631243176, -2.7692730649116415), 0.0), ((25.417305847285075, 14.771850506127166, -2.7692730649116415), 0.0), ((25.88114290560508, 14.952994699822572, -2.7692730649116415), 0.0), ((26.344979963925084, 15.134138893517978, -2.7692730649116415), 0.0), ((26.808817022245087, 15.315283087213382, -2.7692730649116415), 0.0), ((27.27265408056509, 15.496427280908788, -2.7692730649116415), 0.0), ((27.7364911388851, 15.677571474604195, -2.7692730649116415), 0.0), ((28.2003281972051, 15.858715668299599, -2.7692730649116415), 0.0), ((28.664165255525102, 16.039859861995005, -2.7692730649116415), 0.0), ((29.12800231384511, 16.22100405569041, -2.7692730649116415), 0.0), ((29.59183937216511, 16.402148249385817, -2.7692730649116415), 0.0), ((30.055676430485118, 16.583292443081223, -2.7692730649116415), 0.0), ((30.519513488805124, 16.76443663677663, -2.7692730649116415), 0.0), ((30.98335054712512, 16.945580830472032, -2.7692730649116415), 0.0), ((31.44718760544513, 17.126725024167442, -2.7692730649116415), 0.0), ((31.911024663765133, 17.307869217862848, -2.7692730649116415), 0.0), ((32.374861722085136, 17.489013411558254, -2.7692730649116415), 0.0), ((32.83869878040514, 17.67015760525366, -2.7692730649116415), 0.0), ((33.30253583872515, 17.851301798949066, -2.7692730649116415), 0.0), ((33.766372897045144, 18.03244599264447, -2.7692730649116415), 0.0), ((34.230209955365154, 18.21359018633988, -2.7692730649116415), 0.0), ((34.694047013685164, 18.39473438003528, -2.7692730649116415), 0.0), ((35.15788407200516, 18.575878573730687, -2.7692730649116415), 0.0), ((35.62172113032517, 18.757022767426093, -2.7692730649116415), 0.0), ((36.085558188645166, 18.9381669611215, -2.7692730649116415), 0.0), ((36.549395246965176, 19.119311154816906, -2.7692730649116415), 0.0), ((37.01323230528518, 19.300455348512312, -2.7692730649116415), 0.0), ((37.47706936360518, 19.481599542207718, -2.7692730649116415), 0.0), ((37.93088021379691, 19.68493547229953, -2.613170404594171), 0.0), ((38.373379146934134, 19.96917888814043, -2.5664593721967073), 0.0), ((38.81471995203804, 20.255271341480984, -2.5664593721967073), 0.0), ((39.25606075714194, 20.541363794821535, -2.5664593721967073), 0.0), ((39.69740156224584, 20.82745624816209, -2.5664593721967073), 0.0), ((40.138742367349735, 21.11354870150264, -2.5664593721967073), 0.0), ((40.580083172453634, 21.399641154843195, -2.5664593721967073), 0.0), ((41.02142397755753, 21.685733608183746, -2.5664593721967073), 0.0), ((41.46276478266144, 21.9718260615243, -2.5664593721967073), 0.0), ((41.904105587765336, 22.25791851486485, -2.5664593721967073), 0.0), ((42.345446392869235, 22.544010968205406, -2.5664593721967073), 0.0), ((42.78678719797313, 22.830103421545957, -2.5664593721967073), 0.0), ((43.22812800307703, 23.11619587488651, -2.5664593721967073), 0.0), ((43.66946880818093, 23.402288328227062, -2.5664593721967073), 0.0), ((44.11080961328483, 23.688380781567616, -2.5664593721967073), 0.0), ((44.552150418388734, 23.974473234908167, -2.5664593721967073), 0.0), ((44.99349122349263, 24.26056568824872, -2.5664593721967073), 0.0), ((45.409202515031865, 24.580793083575244, -2.359731587601127), 0.0), ((45.729687929308746, 24.995918495736717, -2.096753167332383), 0.0), ((45.93123999785338, 25.480084554286705, -1.8337747470636412), 0.0), ((46.0, 26.0, -1.570796326794897), 0.0)]}
Container up-spiderplan-server-web Stopped
15.12. Plotting the Solution
Assuming there is a solution, we can use the plot_path function to inspect it:
[15]:
plot_path(result)
