def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
initial_poses = {
ROBOT: (0., 15., 0.),
CUP: (7.5, 0., 0.),
'sugar_cup': (-10., 0., 0.),
'cream_cup': (15., 0, 0),
'spoon': (0.5, 0.5, 0),
'stirrer': (20, 0.5, 0),
COASTER: (-20., 0, 0),
}
problem = create_problem(initial_poses)
with Profiler(field='tottime'):
#solution = solve_serialized(problem, planner='ff-eager', unit_costs=args.unit,
# unit_efforts=True, effort_weight=1, debug=False) # max_planner_time=5,
solution = solve(problem,
algorithm=args.algorithm,
unit_costs=args.unit,
planner='ff-eager',
unit_efforts=True,
effort_weight=1,
debug=False) # max_planner_time=5,
print_solution(solution)
def solve_pddlstream(planner='max-astar'):
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = get_problem(optimize=True)
print(problem.constant_map)
print(problem.init)
print(problem.goal)
stream_info = {
't-ge': StreamInfo(eager=True),
'withdraw': StreamInfo(opt_gen_fn=PartialInputs(unique=True)),
}
with Profiler(field='cumtime'):
solution = solve(problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=stream_info,
planner=planner,
initial_complexity=3,
clean=False,
debug=True,
verbose=True)
print_solution(solution)
plan, cost, certificate = solution
print('Certificate:', certificate.preimage_facts)
def main(deterministic=False, observable=False, collisions=True, factor=True):
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
# TODO: AssertionError: degenerate distribution DDist{l0: 0.00000}
# TODO: global search over the state
belief_problem = get_belief_problem(deterministic, observable)
pddlstream_problem = to_pddlstream(belief_problem, collisions)
print('Cost scale:', get_cost_scale())
stream_info = {
'GE': StreamInfo(from_test(ge_fn), eager=False),
'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))),
'MoveCost': FunctionInfo(move_cost_fn),
'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))),
'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)),
}
planner = 'ff-wastar1'
success_cost = 0 # 0 | MAX_COST
with Profiler(field='tottime', num=10):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=stream_info,
planner=planner,
debug=False,
success_cost=success_cost,
max_time=30)
print_solution(solution)
def main(planner='max-astar', defer=False):
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = pddlstream_from_belief()
_, _, _, _, init, goal = problem
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal)
stream_info = {
'motion': StreamInfo(defer_fn=defer_shared if defer else never_defer),
}
replan_actions = set()
#replan_actions = {'phone'}
with Profiler(field='tottime', num=5):
solution = solve(problem,
algorithm=args.algorithm,
unit_costs=args.unit,
planner=planner,
stream_info=stream_info,
replan_actions=replan_actions)
print_solution(solution)
def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
uniform_rooms = UniformDist(['room0', OTHER])
#uniform_tables = UniformDist(['table0', 'table1'])
#uniform_tables = UniformDist(['table0', OTHER])
uniform_tables = UniformDist(['table0', 'table1', OTHER])
#initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0)
initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2)
#initial_belief = get_table_belief(uniform_tables, 1.0)
#initial_belief = get_table_belief(uniform_tables, 0.2)
#initial_belief = get_item_belief()
pddlstream_problem = pddlstream_from_belief(initial_belief)
_, _, _, _, init, goal = pddlstream_problem
print('Init:', sorted(init))
print('Goal:', goal)
planner = 'max-astar'
with Profiler(field='tottime', num=10):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit,
planner=planner)
print_solution(solution)
def solve_pddlstream(debug=False):
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = get_problem()
planner = 'lmcut-astar' # cerberus
solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, planner=planner, debug=debug)
print_solution(solution)
def main(time_step=0.01):
parser = create_parser()
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
# TODO: argument for selecting prior
args = parser.parse_args()
print('Arguments:', args)
# TODO: nonuniform distribution to bias towards other actions
# TODO: closed world and open world
real_world = connect(use_gui=not args.viewer)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.25) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_user()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
wait_for_user()
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state, args)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_user()
disconnect()
def solve_pddlstream():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = get_problem1()
print('Init:', problem.init)
print('Goal:', problem.goal)
solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit)
print_solution(solution)
def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
# TODO: maybe load problems as a domain explicitly
problem = get_pddlstream_problem()
#if args.algorithm not in FOCUSED_ALGORITHMS:
# raise RuntimeError('The {} algorithm does not support fluent streams'.format(args.algorithm))
solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit)
print_solution(solution)
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():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
streams, orders, info = create_problem2() # create_problem1 | create_problem2
opt_solution = opt_from_graph(streams, orders, info)
print(SEPARATOR)
solution = solve_skeleton(opt_solutions=[opt_solution])
print(SEPARATOR)
print(solution) # TODO: print the stream plan
print_solution(solution)
def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def main(replan=True, defer=True):
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = get_pddlstream()
stream_info = {
#'test-pose': StreamInfo(eager=True, p_success=0),
'motion': StreamInfo(defer_fn=defer_unique if defer else never_defer),
}
replan_actions = {'pick'} if replan else set()
with Profiler():
solution = solve(problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=stream_info,
replan_actions=replan_actions)
print_solution(solution)
def main():
parser = create_parser()
#parser.add_argument('-p', '--problem', default='blocked', help='The name of the problem to solve')
args = parser.parse_args()
print('Arguments:', args)
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem # TODO: use --problem
tamp_problem = problem_fn()
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
#solution = solve_serialized(pddlstream_problem, planner='max-astar', unit_costs=args.unit)
solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, debug=False,
#complexity_step=INF, max_complexity=0,
)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def main():
parser = create_parser()
parser.add_argument('-enable', action='store_true', help='Enables rendering during planning')
parser.add_argument('-teleport', action='store_true', help='Teleports between configurations')
parser.add_argument('-simulate', action='store_true', help='Simulates the system')
parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
robot, names, movable = load_world()
print('Objects:', names)
saver = WorldSaver()
problem = pddlstream_from_problem(robot, movable=movable, teleport=args.teleport)
_, _, _, stream_map, init, goal = problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', str_from_object(set(stream_map)))
with Profiler():
with LockRenderer(lock=not args.enable):
solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, success_cost=INF)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
command = postprocess_plan(plan)
if args.simulate:
wait_for_user('Simulate?')
command.control()
else:
wait_for_user('Execute?')
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
wait_for_user('Finish?')
disconnect()
def main():
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
problem = get_problem1()
print('Init:', problem.init)
print('Goal:', problem.goal)
info = {
# Intentionally, misleading the stream
'increment': StreamInfo(p_success=0.01, overhead=1),
'decrement': StreamInfo(p_success=1, overhead=1),
}
solution = solve(problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=info,
planner='max-astar',
effort_weight=1)
print_solution(solution)
def main():
parser = create_parser()
parser.add_argument('-n', '--n_boxes', default=3, type=int, help='The number of boxes')
args = parser.parse_args()
print('Arguments:', args)
problem = get_problem1(n_boxes=args.n_boxes)
print('Init:', sorted(problem.init))
print('Goal:', problem.goal)
print(analyze_goal(problem, debug=True, use_actions=True, blocked_predicates=[
# TODO: make sure to use lowercase
'handempty',
'clear',
# These are conditions that you can always reachieve?
]))
print(SEPARATOR)
with Profiler():
solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit,
planner='ff-wastar1', debug=True)
print_solution(solution)
def main():
parser = create_parser()
parser.add_argument('-attachments', action='store_true')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
tamp_problem, args = initialize(parser)
stream_info = {
't-region': StreamInfo(eager=False, p_success=0),
'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
dump_pddlstream(pddlstream_problem)
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
with Profiler():
solution = solve(pddlstream_problem,
stream_info=stream_info,
planner=planner,
max_planner_time=10,
debug=False,
max_time=args.max_time,
max_iterations=INF,
verbose=True,
unit_costs=args.unit,
success_cost=success_cost,
unit_efforts=False,
effort_weight=0,
visualize=args.visualize)
print_solution(solution)
plan, cost, evaluations = solution
step_plan(tamp_problem, plan)
def main():
parser = create_parser()
parser.add_argument('-problem', default='problem1', help='The name of the problem to solve')
parser.add_argument('-cfree', action='store_true', help='Disables collisions')
parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode')
parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time')
parser.add_argument('-enable', action='store_true', help='Enables rendering during planning')
parser.add_argument('-teleport', action='store_true', help='Teleports between configurations')
parser.add_argument('-simulate', action='store_true', help='Simulates the system')
parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS}
if args.problem not in problem_fn_from_name:
raise ValueError(args.problem)
problem_fn = problem_fn_from_name[args.problem]
connect(use_gui=args.viewer)
with HideOutput():
problem = problem_fn()
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport)
stream_info = {
'inverse-kinematics': StreamInfo(),
'plan-base-motion': StreamInfo(overhead=1e1),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
#print('Streams:', stream_map.keys())
success_cost = 0 if args.optimal else INF
planner = 'ff-astar'
search_sample_ratio = 1
max_planner_time = 10
with Profiler(field='cumtime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem, stream_info=stream_info,
planner=planner, max_planner_time=max_planner_time,
unit_costs=args.unit, success_cost=success_cost,
max_time=args.max_time, verbose=True, debug=False,
unit_efforts=True, effort_weight=1,
search_sample_ratio=search_sample_ratio)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
# Maybe openrave didn't actually sample any joints...
# http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/
with LockRenderer(lock=not args.enable):
commands = post_process(problem, plan, teleport=args.teleport)
saver.restore()
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step)
wait_for_user()
disconnect()
def main():
parser = create_parser(default_algorithm='binding')
parser.add_argument('-cfree',
action='store_true',
help='Disables collisions')
parser.add_argument('-deterministic',
action='store_true',
help='Uses a deterministic sampler')
parser.add_argument('-optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-t',
'--max_time',
default=120,
type=int,
help='The max time')
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
with HideOutput():
problem = problem_fn(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(problem)
stream_info = {
'test-cfree-conf-pose': StreamInfo(p_success=1e-2),
'test-cfree-traj-pose': StreamInfo(p_success=1e-1),
'compute-motion': StreamInfo(eager=True, p_success=0),
'test-reachable': StreamInfo(eager=True),
'Distance': FunctionInfo(eager=True),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
success_cost = 0 if args.optimal else INF
planner = 'ff-wastar1'
search_sample_ratio = 0
max_planner_time = 10
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
debug=False,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
unit_efforts=True,
effort_weight=1,
search_sample_ratio=search_sample_ratio)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
with LockRenderer():
commands = post_process(problem, plan)
saver.restore() # Assumes bodies are ordered the same way
wait_for_user()
if args.simulate:
control_commands(commands)
else:
apply_commands(BeliefState(problem), commands,
time_step=1e-2) # 1e-2 | None
wait_for_user()
disconnect()
def main():
parser = create_parser()
parser.add_argument('-problem',
default='rovers1',
help='The name of the problem to solve')
parser.add_argument('-cfree',
action='store_true',
help='Disables collisions')
parser.add_argument('-deterministic',
action='store_true',
help='Uses a deterministic sampler')
parser.add_argument('-optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-t',
'--max_time',
default=120,
type=int,
help='The max time')
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS}
if args.problem not in problem_fn_from_name:
raise ValueError(args.problem)
problem_fn = problem_fn_from_name[args.problem]
connect(use_gui=args.viewer)
with HideOutput():
rovers_problem = problem_fn()
saver = WorldSaver()
draw_base_limits(rovers_problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(rovers_problem,
collisions=not args.cfree,
teleport=args.teleport,
holonomic=False,
reversible=True,
use_aabb=True)
stream_info = {
'test-cfree-ray-conf': StreamInfo(),
'test-reachable': StreamInfo(p_success=1e-1),
'obj-inv-visible': StreamInfo(),
'com-inv-visible': StreamInfo(),
'sample-above': StreamInfo(),
'sample-motion': StreamInfo(overhead=10),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
#print('Streams:', stream_map.keys())
success_cost = 0 if args.optimal else INF
planner = 'ff-wastar3'
search_sample_ratio = 2
max_planner_time = 10
# TODO: need to accelerate samples here because of the failed test reachable
with Profiler(field='tottime', num=25):
with LockRenderer(lock=not args.enable):
# TODO: option to only consider costs during local optimization
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
debug=False,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
unit_efforts=True,
effort_weight=1,
search_sample_ratio=search_sample_ratio)
for body in get_bodies():
if body not in saver.bodies:
remove_body(body)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
# Maybe OpenRAVE didn't actually sample any joints...
# http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/
with LockRenderer():
commands = post_process(rovers_problem, plan)
saver.restore()
wait_for_user('Begin?')
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(BeliefState(rovers_problem), commands, time_step)
wait_for_user('Finish?')
disconnect()
def main(verbose=True):
# TODO: could work just on postprocessing
# TODO: try the other reachability database
# TODO: option to only consider costs during local optimization
parser = create_parser()
parser.add_argument('-problem',
default='packed',
help='The name of the problem to solve')
parser.add_argument('-n',
'--number',
default=5,
type=int,
help='The number of objects')
parser.add_argument('-cfree',
action='store_true',
help='Disables collisions')
parser.add_argument('-deterministic',
action='store_true',
help='Uses a deterministic sampler')
parser.add_argument('-optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-t',
'--max_time',
default=120,
type=int,
help='The max time')
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS}
if args.problem not in problem_fn_from_name:
raise ValueError(args.problem)
problem_fn = problem_fn_from_name[args.problem]
connect(use_gui=args.viewer)
with HideOutput():
problem = problem_fn(num=args.number)
draw_base_limits(problem.base_limits, color=(1, 0, 0))
saver = WorldSaver()
#handles = []
#for link in get_group_joints(problem.robot, 'left_arm'):
# handles.append(draw_link_name(problem.robot, link))
#wait_for_user()
pddlstream_problem = pddlstream_from_problem(problem,
collisions=not args.cfree,
teleport=args.teleport)
stream_info = {
'inverse-kinematics':
StreamInfo(),
'plan-base-motion':
StreamInfo(overhead=1e1),
'test-cfree-pose-pose':
StreamInfo(p_success=1e-3, verbose=verbose),
'test-cfree-approach-pose':
StreamInfo(p_success=1e-2, verbose=verbose),
'test-cfree-traj-pose':
StreamInfo(p_success=1e-1,
verbose=verbose), # TODO: apply to arm and base trajs
#'test-cfree-traj-grasp-pose': StreamInfo(verbose=verbose),
'Distance':
FunctionInfo(p_success=0.99, opt_fn=lambda q1, q2: BASE_CONSTANT),
#'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT),
}
#stream_info = {}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', str_from_object(set(stream_map)))
success_cost = 0 if args.optimal else INF
planner = 'ff-astar' if args.optimal else 'ff-wastar3'
search_sample_ratio = 2
max_planner_time = 10
# effort_weight = 0 if args.optimal else 1
effort_weight = 1e-3 if args.optimal else 1
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
debug=False,
unit_efforts=True,
effort_weight=effort_weight,
search_sample_ratio=search_sample_ratio)
saver.restore()
cost_over_time = [(s.cost, s.time) for s in SOLUTIONS]
for i, (cost, runtime) in enumerate(cost_over_time):
print('Plan: {} | Cost: {:.3f} | Time: {:.3f}'.format(
i, cost, runtime))
#print(SOLUTIONS)
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
with LockRenderer(lock=not args.enable):
commands = post_process(problem, plan, teleport=args.teleport)
saver.restore()
draw_base_limits(problem.base_limits, color=(1, 0, 0))
wait_for_user()
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(State(), commands, time_step)
wait_for_user()
disconnect()
def main():
# TODO: side grasps (horizontal gripper, one finger, forklift)
parser = create_parser()
parser.add_argument('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-s',
'--skeleton',
action='store_true',
help='Enforces skeleton plan constraints')
tamp_problem, args = initialize(parser)
# TODO: test if placed in the same region
defer_fn = defer_shared # never_defer | defer_unique | defer_shared
stream_info = {
's-region':
StreamInfo(defer_fn=defer_fn),
's-grasp':
StreamInfo(defer_fn=defer_fn),
's-ik':
StreamInfo(defer_fn=get_defer_all_unbound(
inputs='?g')), # defer_fn | defer_unbound
's-motion':
StreamInfo(defer_fn=get_defer_any_unbound()),
't-cfree':
StreamInfo(defer_fn=get_defer_any_unbound(),
eager=False,
verbose=False), # defer_fn | defer_unbound
't-region':
StreamInfo(eager=True, p_success=0), # bound_fn is None
'dist':
FunctionInfo(eager=False,
defer_fn=get_defer_any_unbound(),
opt_fn=lambda q1, q2: MOVE_COST),
'gurobi-cfree':
StreamInfo(
eager=False, negate=True
), # TODO: AttributeError: 'tuple' object has no attribute 'instance'
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
#stream_info = {}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
skeletons = [TIGHT_SKELETON] if args.skeleton else None
assert implies(args.skeleton, args.problem == 'tight')
max_cost = INF # 8*MOVE_COST
constraints = PlanConstraints(
skeletons=skeletons,
#skeletons=[],
#skeletons=[skeleton, []],
exact=True,
max_cost=max_cost)
replan_actions = set()
#replan_actions = {'move', 'pick', 'place'}
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
collisions=not args.cfree,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
dump_pddlstream(pddlstream_problem)
success_cost = 0 if args.optimal else INF
#planner = 'dijkstra'
planner = 'max-astar'
#planner = 'ff-wastar1'
#effort_weight = 1.
effort_weight = 1. / get_cost_scale()
#effort_weight = None
with Profiler(field='cumtime', num=20):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
planner=planner,
max_planner_time=10,
hierarchy=hierarchy,
max_time=args.max_time,
max_iterations=INF,
debug=False,
verbose=True,
unit_costs=args.unit,
success_cost=success_cost,
unit_efforts=True,
effort_weight=effort_weight,
search_sample_ratio=1,
visualize=args.visualize) # TODO: solve_serialized
print_solution(solution)
plan, cost, evaluations = solution
if plan is not None:
display_plan(tamp_problem, retime_plan(plan))
def main(partial=False, defer=False, verbose=True):
parser = create_parser()
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
problem = problem_fn()
#state_id = save_state()
saver = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem,
teleport=args.teleport)
stream_info = {
# 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose),
# 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose),
# 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
stream_info.update({
'sample-pose':
StreamInfo(PartialInputs('?r')),
'inverse-kinematics':
StreamInfo(PartialInputs('?p')),
'plan-base-motion':
StreamInfo(PartialInputs('?q1 ?q2'),
defer_fn=defer_shared if defer else never_defer),
} if partial else {
'sample-pose': StreamInfo(from_fn(opt_pose_fn)),
'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)),
'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)),
})
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', str_from_object(set(stream_map)))
print(SEPARATOR)
with Profiler():
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=stream_info,
success_cost=INF,
verbose=True,
debug=False)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
print(SEPARATOR)
with LockRenderer(lock=not args.enable):
commands = post_process(problem, plan)
problem.remove_gripper()
saver.restore()
#restore_state(state_id)
saver.restore()
wait_if_gui('Execute?')
if args.simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
wait_if_gui('Finish?')
disconnect()
