def main(focused=False, deterministic=False, unit_costs=True):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
stream_info = {
#'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'plan-motion': StreamInfo(p_success=1), # bound_fn is None
#'trajcollision': StreamInfo(p_success=1), # bound_fn is None
#'cfree': StreamInfo(eager=True),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
max_time=10,
max_cost=INF,
debug=False,
effort_weight=None,
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
unit_costs=unit_costs,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
for i, (action, args) in enumerate(plan):
user_input('Continue?')
print(i, action, args)
s2 = args[-1]
state = TAMPState(
s2[R], s2[H],
{b: s2[b]
for b in state.block_poses if s2[b] is not None})
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main(deterministic=False, observable=False, collisions=True, focused=True, factor=True):
# TODO: global search over the state
belief_problem = get_belief_problem(deterministic, observable)
pddlstream_problem = to_pddlstream(belief_problem, collisions)
pr = cProfile.Profile()
pr.enable()
planner = 'ff-wastar1'
if focused:
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)),
}
solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, debug=False,
max_cost=0, unit_costs=False, max_time=30)
else:
solution = solve_incremental(pddlstream_problem, planner=planner, debug=True,
max_cost=MAX_COST, unit_costs=False, max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, init = solution
print('Real cost:', float(cost)/SCALE_COST)
def main():
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(sorted(init))
print(goal)
pr = cProfile.Profile()
pr.enable()
planner = 'max-astar'
#solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=False)
solution = solve_focused(pddlstream_problem,
planner=planner,
unit_costs=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def plan_trajectories(task,
context,
collisions=True,
use_impedance=False,
max_time=180):
stream_info = {
'TrajPoseCollision': FunctionInfo(p_success=1e-3),
'TrajConfCollision': FunctionInfo(p_success=1e-3),
}
pr = cProfile.Profile()
pr.enable()
problem = get_pddlstream_problem(task,
context,
collisions=collisions,
use_impedance=use_impedance)
solution = solve_focused(problem,
stream_info=stream_info,
planner='ff-wastar2',
max_time=max_time,
search_sampling_ratio=0)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(5)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
print('Unable to find a solution in under {} seconds'.format(max_time))
return None
return postprocess_plan(task.mbp, task.gripper, plan)
def main(focused=True, unit_costs=False):
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)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=unit_costs)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs)
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
print(evaluations)
colors = dict(zip(tamp_problem.initial.block_poses, COLORS))
viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial')
state = tamp_problem.initial
print(state)
draw_state(viewer, state, colors)
for action in plan:
user_input('Continue?')
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
problem_fn = holding_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
problem = problem_fn()
state_id = save_state()
#saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
#StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1,
# 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0},
# from_fn(get_base_motion_synth(problem, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
if viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
problem = problem_fn() # TODO: way of doing this without reloading?
user_input('Execute?')
commands = post_process(problem, plan)
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
def solve_pddlstream(focused=True):
pddlstream_problem = get_problem()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=True)
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=[]):
if trajectories is None:
return None
pddlstream_fn = get_pddlstream2 # get_pddlstream | get_pddlstream2
# TODO: Iterated search
# http://www.fast-downward.org/Doc/Heuristic#h.5Em_heuristic
# randomize_successors=True
pr = cProfile.Profile()
pr.enable()
pddlstream_problem = pddlstream_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories)
#solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True)
#solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600,
# max_planner_time=300, debug=True)
stream_info = {
#'sample-print': StreamInfo(PartialInputs(unique=True)),
}
planner = 'ff-ehc'
#planner = 'ff-wastar1000' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper
#planner = 'ff-eager-wastar1000'
#planner = 'ff-wastar5'
solution = solve_focused(
pddlstream_problem,
stream_info=stream_info,
max_time=600,
effort_weight=1,
unit_efforts=True,
use_skeleton=False, #unit_costs=True,
planner=planner,
max_planner_time=15,
debug=True)
# solve_exhaustive | solve_incremental
# Reachability heuristics good for detecting dead-ends
# Infeasibility from the start means disconnected or collision
# Random restart based strategy here
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
plan, _, _ = solution
if plan is None:
return None
if pddlstream_fn == get_pddlstream:
return [t for _, (n1, e, t, n2) in plan]
elif pddlstream_fn == get_pddlstream2:
return [t for _, (n1, e, t) in reversed(plan)]
else:
raise ValueError(pddlstream_fn)
def main():
# TODO: maybe load problems as a domain explicitly
pddlstream_problem = get_pddlstream_problem()
stream_info = {
#'test-feasible': StreamInfo(negate=True),
}
solution = solve_focused(pddlstream_problem, stream_info=stream_info)
#solution = solve_incremental(pddlstream_problem) # Should throw an error
print_solution(solution)
def solve_pddlstream(focused=True):
problem_fn = get_problem1 # get_problem1 | get_problem2
pddlstream_problem = problem_fn()
print('Init:', pddlstream_problem.init)
print('Goal:', pddlstream_problem.goal)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def plan_commands(state, teleport=False, profile=False, verbose=True):
# TODO: could make indices into set of bodies to ensure the same...
# TODO: populate the bodies here from state and not the real world
task = state.task
robot_conf = get_configuration(task.robot)
robot_pose = get_pose(task.robot)
sim_world = connect(use_gui=False)
with ClientSaver(sim_world):
with HideOutput():
robot = create_pr2(use_drake=USE_DRAKE_PR2)
#robot = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_pose(robot, robot_pose)
set_configuration(robot, robot_conf)
clone_world(client=sim_world, exclude=[task.robot])
set_client(sim_world)
saved_world = WorldSaver() # StateSaver()
pddlstream_problem = pddlstream_from_state(state, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
if verbose:
print('Goal:', goal)
print('Streams:', stream_map.keys())
stream_info = {
'test-vis-base': StreamInfo(eager=True, p_success=0),
'test-reg-base': StreamInfo(eager=True, p_success=0),
}
hierarchy = [
ABSTRIPSLayer(pos_pre=['AtBConf']),
]
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
hierarchy=hierarchy,
debug=False,
max_cost=MAX_COST,
verbose=verbose)
pr.disable()
plan, cost, evaluations = solution
if MAX_COST <= cost:
plan = None
print_solution(solution)
print('Finite cost:', cost < MAX_COST)
if profile:
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
saved_world.restore()
commands = post_process(state, plan)
disconnect()
return commands
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(focused=True, unit_costs=False):
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)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=unit_costs)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def plan_sequence_test(node_points, elements, ground_nodes):
pr = cProfile.Profile()
pr.enable()
pddlstream_problem = get_pddlstream_test(node_points, elements,
ground_nodes)
#solution = solve_focused(pddlstream_problem, planner='goal-lazy', max_time=10, debug=False)
solution = solve_exhaustive(pddlstream_problem,
planner='goal-lazy',
max_time=10,
debug=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
plan, _, _ = solution
return plan
def main(focused=True):
# TODO: maybe load problems as a domain explicitly
pddlstream_problem = pddlstream_from_belief()
_, _, _, _, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=False)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=False)
solution = solve_incremental(pddlstream_problem, unit_costs=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(5)
print_solution(solution)
def solve_pddlstream():
pddlstream_problem, namo = get_problem()
namo.env.SetViewer('qtcoin')
stime = time.time()
#solution = solve_incremental(pddlstream_problem, unit_costs=True, max_time=500)
solution = solve_focused(pddlstream_problem, unit_costs=True, max_time=500)
search_time = time.time()-stime
plan, cost, evaluations = solution
print "Search time", search_time
if solution[0] is None:
print "No Solution"
sys.exit(-1)
print_solution(solution)
process_plan(plan, namo)
namo.problem_config['env'].Destroy()
openravepy.RaveDestroy()
return plan, search_time
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?')
def main():
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)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(problem,
unit_costs=True,
planner='ff-eager',
debug=True) # max_planner_time=5,
pr.disable()
print_solution(solution)
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main(viewer=True):
connect(use_gui=viewer)
robot, brick_from_index, obstacle_from_name = load_pick_and_place(
'choreo_brick_demo') # choreo_brick_demo | choreo_eth-trees_demo
np.set_printoptions(precision=2)
pr = cProfile.Profile()
pr.enable()
with WorldSaver():
pddlstream_problem = get_pddlstream(robot, brick_from_index,
obstacle_from_name)
solution = solve_focused(pddlstream_problem,
planner='ff-wastar1',
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
print_solution(solution)
plan, _, _ = solution
if plan is None:
return
step_plan(plan)
def main(focused=True, unit_costs=False):
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
stream_info = {
'test-cfree': StreamInfo(negate=True),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if focused:
#solution = solve_execution(pddlstream_problem, unit_costs=unit_costs, stream_info=stream_info)
solution = solve_focused(pddlstream_problem,
unit_costs=unit_costs,
stream_info=stream_info,
debug=False)
else:
solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def main():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
# starting objects
start_conf = np.array([0, 5])
p0 = np.array([0, 0])
goal_region = np.array([5, 10])
b0 = 'block0'
ground = np.array([-5, 15])
initial_atoms = [('CanMove', ), ('Conf', start_conf),
('AtConf', start_conf), ('HandEmpty', ),
('Region', goal_region), ('Block', b0), ('Pose', b0, p0),
('AtPose', b0, p0), ('Region', ground),
('Placeable', b0, ground), ('Placeable', b0, goal_region)]
goal_literals = [('HandEmpty', ), ('In', b0, goal_region),
('AtConf', start_conf)]
goal_formula = And(*goal_literals)
## environments
# The pose of block is the center of bottom line
# The pose of the gripper is the center of mass
BLOCK_WIDTH = 2
BLOCK_HEIGHT = BLOCK_WIDTH
GRIPPER_WIDTH = 2
GRIPPER_HEIGHT = GRIPPER_WIDTH
GRASP = -np.array([0, BLOCK_HEIGHT + GRIPPER_HEIGHT / 2])
def distance(q1, q2):
return int(np.linalg.norm(q1 - q2))
def posecollision(blk1, pose1, blk2, pose2):
i1 = pose1[0] * np.ones(2) + np.array([-BLOCK_WIDTH, +BLOCK_WIDTH
]) / 2.
i2 = pose2[0] * np.ones(2) + np.array([-BLOCK_WIDTH, +BLOCK_WIDTH
]) / 2.
return (i2[0] <= i1[1]) and (i1[0] <= i2[1])
def sample_pose(blk, region):
return ((np.random.uniform(region[0] + BLOCK_WIDTH / 2,
region[1] - BLOCK_WIDTH / 2), 0), )
def inverse_kinematics(blk, pose):
return (pose - GRASP, )
def plan_motion(conf1, conf2):
traj = [conf1, conf2]
return (traj, )
constant_map = {}
# Upper case does not matter at all
stream_map = {
'distance': distance,
'posecollision': posecollision,
'sample-pose': from_fn(sample_pose),
'inverse-kinematics': from_fn(inverse_kinematics),
'plan-motion': from_fn(plan_motion)
}
# A pddlstream problem
problem = domain_pddl, constant_map, stream_pddl, stream_map, initial_atoms, goal_formula
# solution = solve_incremental(problem, unit_costs=False)
solution = solve_incremental(problem, unit_costs=False)
# plan, cost, evaluations = solution
print_solution(solution)
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
#StreamSynthesizer('safe-free-motion', {'plan-free-motion': 1, 'trajcollision': 0},
# from_fn(get_free_motion_synth(robot, movable, teleport))),
#StreamSynthesizer('safe-holding-motion', {'plan-holding-motion': 1, 'trajcollision': 0},
# from_fn(get_holding_motion_synth(robot, movable, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', stream_map.keys())
print(names)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
paths = []
for name, args in plan:
if name == 'place':
paths += args[-1].reverse().body_paths
elif name in ['move', 'move_free', 'move_holding', 'pick']:
paths += args[-1].body_paths
print(paths)
command = Command(paths)
if not viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
else:
saved_world.restore()
user_input('Execute?')
if simulate:
command.control()
else:
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=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()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'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)),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
synthesizers = [
StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1,
'TrajPoseCollision': 0,
'TrajGraspCollision': 0,
'TrajArmCollision': 0},
from_fn(get_base_motion_synth(problem, teleport))),
] if USE_SYNTHESIZERS else []
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem, stream_info=stream_info,
synthesizers=synthesizers, max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
print('Real cost:', float(cost)/SCALE_COST)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
commands = post_process(problem, plan)
if viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(focused=True, deterministic=False, unit_costs=False):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
#'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'plan-motion': StreamInfo(p_success=1), # bound_fn is None
#'trajcollision': StreamInfo(p_success=1), # bound_fn is None
#'cfree': StreamInfo(eager=True),
}
dynamic = [
StreamSynthesizer('cfree-motion', {
'plan-motion': 1,
'trajcollision': 0
},
gen_fn=from_fn(cfree_motion_fn)),
#StreamSynthesizer('optimize', {'sample-pose': 1, 'inverse-kinematics': 1,
# 'posecollision': 0, 'distance': 0},
# gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))),
]
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
action_info=action_info,
stream_info=stream_info,
synthesizers=dynamic,
max_time=10,
max_cost=INF,
debug=False,
effort_weight=None,
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
for i, action in enumerate(plan):
user_input('Continue?')
print(i, *action)
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main(deterministic=True):
# TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects
# TODO: cost-sensitive planning to avoid large kuka moves
# get_contact_results_output_port
# TODO: gripper closing via collision information
time_step = 0.0002 # TODO: context.get_continuous_state_vector() fails
if deterministic:
random.seed(0)
np.random.seed(0)
parser = argparse.ArgumentParser()
parser.add_argument('-p', '--problem', default='load_manipulation', help='The name of the problem to solve.')
parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions')
parser.add_argument('-v', '--visualizer', action='store_true', help='Use the drake visualizer')
parser.add_argument('-s', '--simulate', action='store_true', help='Simulate')
args = parser.parse_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)
print('Problem:', args.problem)
problem_fn = problem_fn_from_name[args.problem]
meshcat_vis = None
if not args.visualizer:
import meshcat
# Important that variable is saved
meshcat_vis = meshcat.Visualizer() # vis.set_object
# http://127.0.0.1:7000/static/
mbp, scene_graph, task = problem_fn(time_step=time_step)
#station, mbp, scene_graph = load_station(time_step=time_step)
#builder.AddSystem(station)
#dump_plant(mbp)
#dump_models(mbp)
print(task)
#print(sorted(body.name() for body in task.movable_bodies()))
#print(sorted(body.name() for body in task.fixed_bodies()))
##################################################
builder, _ = build_diagram(mbp, scene_graph, not args.visualizer)
if args.simulate:
state_machine = connect_controllers(builder, mbp, task.robot, task.gripper)
else:
state_machine = None
diagram = builder.Build()
RenderSystemWithGraphviz(diagram) # Useful for getting port names
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
task.diagram = diagram
task.diagram_context = diagram_context
#context = mbp.CreateDefaultContext() # scene_graph.CreateDefaultContext()
for joint, position in task.initial_positions.items():
set_joint_position(joint, context, position)
for model, pose in task.initial_poses.items():
set_world_pose(mbp, context, model, pose)
open_wsg50_gripper(mbp, context, task.gripper)
#close_wsg50_gripper(mbp, context, task.gripper)
#set_configuration(mbp, context, task.gripper, [-0.05, 0.05])
# from underactuated.meshcat_visualizer import MeshcatVisualizer
# #add_meshcat_visualizer(scene_graph)
# viz = MeshcatVisualizer(scene_graph, draw_timestep=0.033333)
# viz.load()
# viz.draw(context)
diagram.Publish(diagram_context)
initial_state = get_state(mbp, context)
##################################################
problem = get_pddlstream_problem(mbp, context, scene_graph, task, collisions=not args.cfree)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(problem, planner='ff-wastar2', max_cost=INF, max_time=120, debug=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
trajectories = postprocess_plan(mbp, task.gripper, plan)
##################################################
set_state(mbp, context, initial_state)
if args.simulate:
splines, gripper_setpoints = convert_splines(mbp, task.robot, task.gripper, context, trajectories)
sim_duration = compute_duration(splines)
print('Splines: {}\nDuration: {:.3f} seconds'.format(len(splines), sim_duration))
set_state(mbp, context, initial_state)
if True:
state_machine.Load(splines, gripper_setpoints)
simulate_splines(diagram, diagram_context, sim_duration)
else:
# NOTE: there is a plan that moves home initially for 15 seconds
from .lab_1.robot_plans import JointSpacePlan
plan_list = [JointSpacePlan(spline) for spline in splines]
#meshcat_vis.delete()
user_input('Simulate?')
test_manipulation(plan_list, gripper_setpoints)
else:
#time_step = None
#time_step = 0.001
time_step = 0.02
step_trajectories(diagram, diagram_context, context, trajectories, time_step=time_step) #, teleport=True)
def main(focused=True,
deterministic=True,
unit_costs=False,
use_synthesizers=False):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
if use_synthesizers and not has_gurobi():
use_synthesizers = False
print(
'Warning! use_synthesizers=True requires gurobipy. Setting use_synthesizers=False.'
)
print('Focused: {} | Costs: {} | Synthesizers: {}'.format(
focused, not unit_costs, use_synthesizers))
problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
't-cfree': StreamInfo(eager=False, negate=True),
#'distance': FunctionInfo(opt_fn=lambda *args: 1),
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
synthesizers = [
#StreamSynthesizer('cfree-motion', {'s-motion': 1, 'trajcollision': 0},
# gen_fn=from_fn(cfree_motion_fn)),
StreamSynthesizer('optimize', {
's-region': 1,
's-ik': 1,
'posecollision': 0,
't-cfree': 0,
'distance': 0
},
gen_fn=from_fn(get_optimize_fn(
tamp_problem.regions))),
] if use_synthesizers else []
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
print('Initial:', str_from_object(pddlstream_problem.init))
print('Goal:', str_from_object(pddlstream_problem.goal))
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(
pddlstream_problem,
action_info=action_info,
stream_info=stream_info,
planner='ff-wastar1',
max_planner_time=10,
synthesizers=synthesizers,
verbose=True,
max_time=300,
max_cost=INF,
debug=False,
hierarchy=hierarchy,
effort_weight=1,
search_sampling_ratio=0, # TODO: run without to see difference
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
hierarchy=hierarchy,
unit_costs=unit_costs,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is not None:
display_plan(tamp_problem, plan)
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
# parser = argparse.ArgumentParser() # Automatically includes help
# parser.add_argument('-viewer', action='store_true', help='enable viewer.')
# parser.add_argument('-display', action='store_true', help='enable viewer.')
# args = parser.parse_args()
# TODO: getopt
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
# dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
StreamSynthesizer(
'safe-free-motion', {
'plan-free-motion': 1,
'trajcollision': 0
}, from_fn(get_free_motion_synth(robot, movable, teleport))),
StreamSynthesizer(
'safe-holding-motion', {
'plan-holding-motion': 1,
'trajcollision': 0
}, from_fn(get_holding_motion_synth(robot, movable, teleport))),
] if USE_SYNTHESIZERS else []
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', stream_map.keys())
print('Names:', names)
# initialize and measure performance
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
# print stats
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
if not viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
else:
saved_world.restore()
command = postprocess_plan(plan)
#user_input('Execute?')
if simulate:
command.control()
else:
# command.step()
command.refine(num_steps=10).execute(time_step=0.001)
# wait_for_interrupt()
#user_input('Finish?')
disconnect()
