def main():
parser = argparse.ArgumentParser()
parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning')
args = parser.parse_args()
print(args)
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1)
floor = create_floor()
with HideOutput():
robot = load_pybullet(get_model_path(ROOMBA_URDF), fixed_base=True, scale=2.0)
for link in get_all_links(robot):
set_color(robot, link=link, color=ORANGE)
robot_z = stable_z(robot, floor)
set_point(robot, Point(z=robot_z))
#set_base_conf(robot, rover_confs[i])
data_path = add_data_path()
shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF), fixed_base=True) # TODO: returns a tuple
dump_body(shelf)
#draw_aabb(get_aabb(shelf))
wait_for_user()
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 = 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,
success_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 main(display=True, teleport=False):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
parser.add_argument('-simulate', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=args.viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport)
_, _, _, 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)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, success_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 not args.viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
else:
saved_world.restore()
command = postprocess_plan(plan)
if args.simulate:
user_input('Simulate?')
command.control()
else:
user_input('Execute?')
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
#ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB)
#ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box'))
#print(ycb_path)
#load_pybullet(ycb_path)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False)
#set_euler(table, Euler(yaw=np.pi/2))
with HideOutput(False):
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
#robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf')
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
#robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table)
set_point(block1, Point(z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=+0.25, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, z=block_z))
blocks = [block1, block2, block3]
add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON),
Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED)
set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z))
wait_for_user()
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False)
grasp = y_grasp # fingers won't collide
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user('Finish?')
disconnect()
def main():
parser = argparse.ArgumentParser()
# choreo_brick_demo | choreo_eth-trees_demo
parser.add_argument('-p',
'--problem',
default='choreo_brick_demo',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-t',
'--teleport',
action='store_true',
help='Teleports instead of computing motions')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
robot, brick_from_index, obstacle_from_name = load_pick_and_place(
args.problem)
np.set_printoptions(precision=2)
pr = cProfile.Profile()
pr.enable()
with WorldSaver():
pddlstream_problem = get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=not args.cfree,
teleport=args.teleport)
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, time_step=(np.inf if args.teleport else 0.1))
def plan_commands(state,
viewer=False,
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
sim_world = connect(use_gui=viewer)
#clone_world(client=sim_world)
task = state.task
robot_conf = get_configuration(task.robot)
robot_pose = get_pose(task.robot)
with ClientSaver(sim_world):
with HideOutput():
robot = create_pr2(use_drake=USE_DRAKE_PR2)
set_pose(robot, robot_pose)
set_configuration(robot, robot_conf)
mapping = clone_world(client=sim_world, exclude=[task.robot])
assert all(i1 == i2 for i1, i2 in mapping.items())
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,
success_cost=MAX_COST,
verbose=verbose)
plan, cost, evaluations = solution
if MAX_COST <= cost:
plan = None
print_solution(solution)
print('Finite cost:', cost < MAX_COST)
commands = post_process(state, plan)
pr.disable()
if profile:
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
saved_world.restore()
disconnect()
return commands
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 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():
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 display_trajectories(ground_nodes, trajectories, time_step=0.05):
if trajectories is None:
return
connect(use_gui=True)
floor, robot = load_world()
wait_for_interrupt()
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
connected = set(ground_nodes)
for trajectory in trajectories:
if isinstance(trajectory, PrintTrajectory):
print(trajectory, trajectory.n1 in connected, trajectory.n2
in connected, is_ground(trajectory.element, ground_nodes),
len(trajectory.path))
connected.add(trajectory.n2)
#wait_for_interrupt()
#set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for conf in trajectory.path:
set_joint_positions(robot, movable_joints, conf)
if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_NAME)))
if last_point is not None:
color = (0, 0, 1) if is_ground(trajectory.element,
ground_nodes) else (1, 0, 0)
handles.append(
add_line(last_point, current_point, color=color))
last_point = current_point
wait_for_duration(time_step)
#wait_for_interrupt()
#user_input('Finish?')
wait_for_interrupt()
disconnect()
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
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),
}
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
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)
print('Real cost:', float(cost) / SCALE_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(time_step=0.01):
# TODO: closed world and open world
real_world = connect(use_gui=True)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.5) # 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_interrupt()
# 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)
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state)
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_interrupt()
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)
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():
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(display=True, teleport=False, partial=False, defer=False):
parser = argparse.ArgumentParser()
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
#parser.add_argument('-display', action='store_true', help='displays the solution')
args = parser.parse_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()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'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),
'MoveCost':
FunctionInfo(opt_move_cost_fn),
} 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)),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
pr = cProfile.Profile()
pr.enable()
with LockRenderer():
#solution = solve_incremental(pddlstream_problem, debug=True)
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
success_cost=INF,
debug=False)
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
with LockRenderer():
commands = post_process(problem, plan)
if args.viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
if args.simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(viewer=True):
# TODO: setCollisionFilterGroupMask
# TODO: only produce collisions rather than collision-free
# TODO: return collisions using wild-stream functionality
# TODO: handle movements between selected edges
# TODO: fail if wild stream produces unexpected facts
# TODO: try search at different cost levels (i.e. w/ and w/o abstract)
# https://github.mit.edu/yijiangh/assembly-instances
#read_minizinc_data(os.path.join(root_directory, 'print_data.txt'))
#return
# djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
elements, node_points, ground_nodes = load_extrusion('voronoi')
node_order = list(range(len(node_points)))
#np.random.shuffle(node_order)
node_order = sorted(node_order, key=lambda n: node_points[n][2])
elements = sorted(elements,
key=lambda e: min(node_points[n][2] for n in e))
#node_order = node_order[:100]
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element)
]
print('Nodes: {} | Ground: {} | Elements: {}'.format(
len(node_points), len(ground_nodes), len(elements)))
#plan = plan_sequence_test(node_points, elements, ground_nodes)
connect(use_gui=viewer)
floor, robot = load_world()
obstacles = [floor]
initial_conf = get_joint_positions(robot, get_movable_joints(robot))
#dump_body(robot)
#if has_gui():
# draw_model(elements, node_points, ground_nodes)
# wait_for_interrupt('Continue?')
#joint_weights = compute_joint_weights(robot, num=1000)
#elements = elements[:5]
#elements = elements[:10]
#elements = elements[:25]
#elements = elements[:35]
#elements = elements[:50]
#elements = elements[:75]
#elements = elements[:100]
#elements = elements[:150]
#elements = elements[150:]
# TODO: prune if it collides with any of its supports
# TODO: prioritize choices that don't collide with too many edges
# TODO: accumulate edges along trajectory
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
#for element in elements:
# color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0, 0)
# draw_element(node_points, element, color=color)
#wait_for_interrupt('Continue?')
# TODO: topological sort
#node = node_order[40]
#node_neighbors = get_node_neighbors(elements)
#for element in node_neighbors[node]:
# color = (0, 1, 0) if element_supports(element, node, node_points) else (1, 0, 0)
# draw_element(node_points, element, color)
#element = elements[-1]
#draw_element(node_points, element, (0, 1, 0))
#incoming_edges, _ = neighbors_from_orders(get_supported_orders(elements, node_points))
#supporters = []
#retrace_supporters(element, incoming_edges, supporters)
#for e in supporters:
# draw_element(node_points, e, (1, 0, 0))
#wait_for_interrupt('Continue?')
#for name, args in plan:
# n1, e, n2 = args
# draw_element(node_points, e)
# user_input('Continue?')
#test_ik(robot, node_order, node_points)
element_bodies = dict(zip(elements, create_elements(node_points,
elements)))
precompute = False
if precompute:
pr = cProfile.Profile()
pr.enable()
trajectories = sample_trajectories(robot, obstacles, node_points,
element_bodies, ground_nodes)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
user_input('Continue?')
else:
trajectories = []
motions = False
plan = plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories)
if motions:
plan = compute_motions(robot, obstacles, element_bodies, initial_conf,
plan)
disconnect()
display_trajectories(ground_nodes, plan)
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():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z + 1),
target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1,
tool_pose=Pose(euler=Euler(yaw=np.pi / 2)),
top_offset=0.02,
grasp_length=0.03,
under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
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()
def main(display=True, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm')
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=5*60, type=int, help='The max time')
parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning')
args = parser.parse_args()
print(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(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream, edges = pddlstream_from_problem(problem, teleport=teleport)
_, constant_map, _, stream_map, init, goal = pddlstream
print('Constants:', constant_map)
print('Init:', init)
print('Goal:', goal)
success_cost = 0 if args.optimal else INF
max_planner_time = 10
stream_info = {
'compute-motion': StreamInfo(eager=True, p_success=0),
'ConfConfCollision': FunctionInfo(p_success=1, overhead=0.1),
'TrajConfCollision': FunctionInfo(p_success=1, overhead=1),
'TrajTrajCollision': FunctionInfo(p_success=1, overhead=10),
'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure)
}
pr = cProfile.Profile()
pr.enable()
with LockRenderer(False):
if args.algorithm == 'incremental':
solution = solve_incremental(pddlstream,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
start_complexity=INF,
verbose=True, debug=True)
elif args.algorithm == 'focused':
solution = solve_focused(pddlstream, stream_info=stream_info,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
max_skeletons=None, bind=True, max_failures=INF,
verbose=True, debug=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
if plan is None:
wait_for_user()
return
if (not display) or (plan is None):
disconnect()
return
if not args.viewer:
disconnect()
connect(use_gui=True)
with LockRenderer():
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
saver.restore() # Assumes bodies are ordered the same way
draw_edges(edges)
state = BeliefState(problem)
wait_for_user()
#time_step = None if teleport else 0.01
#with VideoSaver('video.mp4'):
display_plan(problem, state, plan)
wait_for_user()
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():
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(teleport=False):
#parser = create_parser()
parser = argparse.ArgumentParser()
parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm')
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=5*60, type=int, help='The max time')
parser.add_argument('-enable', action='store_true', help='Enables rendering during planning')
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():
namo_problem = problem_fn(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(namo_problem.limits, color=RED)
pddlstream_problem, edges = pddlstream_from_problem(namo_problem, teleport=teleport)
_, constant_map, _, stream_map, init, goal = pddlstream_problem
print('Constants:', constant_map)
print('Init:', init)
print('Goal:', goal)
stream_info = {
'compute-motion': StreamInfo(eager=True, p_success=0),
'ConfConfCollision': PredicateInfo(p_success=1, overhead=0.1),
'TrajConfCollision': PredicateInfo(p_success=1, overhead=1),
'TrajTrajCollision': PredicateInfo(p_success=1, overhead=10),
'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure)
}
success_cost = 0 if args.optimal else INF
max_planner_time = 10
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
# TODO: solution = solve_incremental(pddlstream_problem
if args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
start_complexity=INF,
verbose=True, debug=True)
elif args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem, stream_info=stream_info,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
max_skeletons=None, bind=True, max_failures=INF,
verbose=True, debug=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
saver.restore()
draw_edges(edges)
state = BeliefState(namo_problem)
wait_for_user('Begin?')
#time_step = None if teleport else 0.01
#with VideoSaver('video.mp4'):
display_plan(namo_problem, state, plan)
wait_for_user('Finish?')
disconnect()
def main(precompute=False):
parser = argparse.ArgumentParser()
# djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
parser.add_argument('-p',
'--problem',
default='simple_frame',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-m',
'--motions',
action='store_true',
help='Plans motions between each extrusion')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
# TODO: setCollisionFilterGroupMask
# TODO: fail if wild stream produces unexpected facts
# TODO: try search at different cost levels (i.e. w/ and w/o abstract)
# TODO: submodule in https://github.mit.edu/yijiangh/assembly-instances
elements, node_points, ground_nodes = load_extrusion(args.problem)
node_order = list(range(len(node_points)))
#np.random.shuffle(node_order)
node_order = sorted(node_order, key=lambda n: node_points[n][2])
elements = sorted(elements,
key=lambda e: min(node_points[n][2] for n in e))
#node_order = node_order[:100]
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element)
]
#plan = plan_sequence_test(node_points, elements, ground_nodes)
connect(use_gui=args.viewer)
floor, robot = load_world()
obstacles = [floor]
initial_conf = get_joint_positions(robot, get_movable_joints(robot))
#dump_body(robot)
#if has_gui():
# draw_model(elements, node_points, ground_nodes)
# wait_for_interrupt('Continue?')
#joint_weights = compute_joint_weights(robot, num=1000)
#elements = elements[:50] # 10 | 50 | 100 | 150
#debug_elements(robot, node_points, node_order, elements)
element_bodies = dict(zip(elements, create_elements(node_points,
elements)))
if precompute:
pr = cProfile.Profile()
pr.enable()
trajectories = sample_trajectories(robot, obstacles, node_points,
element_bodies, ground_nodes)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
user_input('Continue?')
else:
trajectories = []
plan = plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories,
collisions=not args.cfree)
if args.motions:
plan = compute_motions(robot, obstacles, element_bodies, initial_conf,
plan)
disconnect()
display_trajectories(ground_nodes, plan)
def main(display=True, teleport=False):
parser = argparse.ArgumentParser()
#parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve')
parser.add_argument('-algorithm',
default='focused',
help='Specifies the algorithm')
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('-unit', action='store_true', help='Uses unit costs')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(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(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'test-cfree-conf-pose': StreamInfo(negate=True, p_success=1e-2),
'test-cfree-traj-pose': StreamInfo(negate=True, 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
pr = cProfile.Profile()
pr.enable()
with LockRenderer(True):
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem,
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,
bind=True,
max_skeletons=None,
search_sample_ratio=search_sample_ratio)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True)
else:
raise ValueError(args.algorithm)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
if plan is None:
wait_for_user()
return
if (not display) or (plan is None):
disconnect()
return
with LockRenderer():
commands = post_process(problem, plan, teleport=teleport)
saver.restore() # Assumes bodies are ordered the same way
if not args.viewer:
disconnect()
connect(use_gui=True)
with LockRenderer():
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
saver.restore() # Assumes bodies are ordered the same way
wait_for_user()
if args.simulate:
control_commands(commands)
else:
time_step = None if teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step=time_step)
wait_for_user()
disconnect()
def main(display=True, simulate=False, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
#parser.add_argument('-display', action='store_true', help='displays the solution')
args = parser.parse_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()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'sample-pose': StreamInfo(PartialInputs('?r')),
'inverse-kinematics': StreamInfo(PartialInputs('?p')),
'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')),
'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,
success_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
commands = post_process(problem, plan)
if args.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(display=True, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-problem',
default='rovers1',
help='The name of the problem to solve')
parser.add_argument('-algorithm',
default='focused',
help='Specifies the algorithm')
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('-unit', action='store_true', help='Uses unit costs')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(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=(1, 0, 0))
pddlstream_problem = pddlstream_from_problem(problem,
collisions=not args.cfree,
teleport=teleport)
stream_info = {
'test-cfree-ray-conf': StreamInfo(negate=True),
'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
pr = cProfile.Profile()
pr.enable()
with LockRenderer(False):
if args.algorithm == 'focused':
# TODO: option to only consider costs during local optimization
solution = solve_focused(
pddlstream_problem,
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,
#bind=True, max_skeletons=None,
search_sample_ratio=search_sample_ratio)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
if plan is None:
return
if (not display) or (plan is None):
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(problem, plan, teleport=teleport)
saver.restore() # Assumes bodies are ordered the same way
if not args.viewer:
disconnect()
connect(use_gui=True)
with LockRenderer():
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
saver.restore() # Assumes bodies are ordered the same way
if args.simulate:
control_commands(commands)
else:
time_step = None if teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step)
wait_for_user()
disconnect()
