def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
# _, upper = robot_aabb
# radius = upper[0]
extent = get_aabb_extent(robot_aabb)
radius = max(extent[:2]) / 2.
#draw_aabb(robot_aabb)
center, (diameter, height) = approximate_as_cylinder(body)
distance = radius + diameter / 2. + epsilon
_, _, z = get_point(body) # Assuming already placed stably
for [scale] in unit_generator(d=1, use_halton=use_halton):
#theta = PI # 0 | PI
theta = random.uniform(*theta_interval)
position = np.append(distance * unit_from_theta(theta=theta),
[z]) # TODO: halton
yaw = scale * 2 * PI - PI
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose
if draw:
world_pose = multiply(get_link_pose(robot, link), grasp.value)
set_pose(body, world_pose)
handles = draw_pose(get_pose(body), length=1)
wait_for_user()
remove_handles(handles)
#continue
yield (grasp, )
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(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():
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 apply(self, state, **kwargs):
mesh, _ = get_detection_cone(self.robot,
self.body,
camera_link=self.camera_frame,
depth=self.max_depth)
if mesh is None:
wait_for_user()
assert (mesh is not None)
with LockRenderer():
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration)
remove_body(cone)
wait_for_duration(1e-2)
state.registered.add(self.body)
yield
def test(r, t, b2, p2):
if not problem.collisions:
return True
p2.assign()
state = State()
for _ in t.apply(state):
state.assign()
#for b1 in state.attachments:
# if pairwise_collision(b1, b2):
# return False
if pairwise_collision(r, b2):
set_renderer(True)
color = get_visual_data(b2)[0].rgbaColor
handles = add_line(point_from_conf(t.path[0].values, z=0.02),
point_from_conf(t.path[-1].values, z=0.02), color=color)
wait_for_user()
set_renderer(False)
return False
return True
def apply(self, state, **kwargs):
# TODO: check if actually can register
mesh, _ = get_detection_cone(self.robot,
self.body,
camera_link=self.camera_frame,
depth=self.max_depth)
if mesh is None:
wait_for_user()
assert (
mesh is not None
) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions
with LockRenderer():
cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration)
remove_body(cone)
wait_for_duration(1e-2)
state.registered.add(self.body)
yield
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
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, 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:
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()
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()
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(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():
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(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(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()
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():
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()
