def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
'green': create_box((.8, .8), (.4, .4)),
}
goal = 'green'
if goal not in regions:
goal = ARRAY([1, 1])
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, samples, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
print('Initial:', str_from_object(problem.init))
print('Goal:', str_from_object(problem.goal))
constraints = PlanConstraints(max_cost=1.25) # max_cost=INF)
with Profiler(field='tottime', num=10):
solution = solve_incremental(problem,
constraints=constraints,
unit_costs=args.unit,
success_cost=0,
max_time=max_time,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
#viewer = draw_environment(obstacles, regions)
#for sample in samples:
# viewer.draw_point(sample)
#user_input('Continue?')
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
if plan is None:
return
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
'green': create_box((.8, .8), (.4, .4)),
}
goal = 'green'
if goal not in regions:
goal = array([1, 1])
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, samples, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
print('Initial:', str_from_object(problem.init))
print('Goal:', str_from_object(problem.goal))
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(problem,
unit_costs=False,
max_cost=0,
max_time=max_time,
verbose=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, evaluations = solution
#viewer = draw_environment(obstacles, regions)
#for sample in samples:
# viewer.draw_point(sample)
#user_input('Continue?')
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
if plan is None:
return
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
#'goal': create_box((.8, .8), (.4, .4)),
}
goal = np.array([1, 1])
#goal = 'goal'
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(problem,
unit_costs=False,
max_cost=0,
max_time=max_time,
verbose=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, evaluations = solution
print('Plan:', plan)
if plan is None:
return
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')