def get_kitchen_task(arm='left', grasp_type='top'):
with HideOutput():
pr2 = create_pr2(use_drake=USE_DRAKE_PR2)
set_arm_conf(pr2, arm, get_carry_conf(arm, grasp_type))
open_arm(pr2, arm)
other_arm = get_other_arm(arm)
set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
close_arm(pr2, other_arm)
table, cabbage, sink, stove = create_kitchen()
floor = get_bodies()[1]
class_from_body = {
table: 'table',
cabbage: 'cabbage',
sink: 'sink',
stove: 'stove',
} # TODO: use for debug
movable = [cabbage]
surfaces = [table, sink, stove]
rooms = [floor]
return BeliefTask(robot=pr2, arms=[arm], grasp_types=[grasp_type],
class_from_body=class_from_body,
movable=movable, surfaces=surfaces, rooms=rooms,
#goal_localized=[cabbage],
#goal_registered=[cabbage],
#goal_holding=[(arm, cabbage)],
#goal_on=[(cabbage, table)],
goal_on=[(cabbage, sink)],
)
def __init__(self,
robots,
limits,
movable=[],
collisions=True,
goal_holding={},
goal_confs={}):
self.robots = tuple(robots)
self.limits = limits
self.movable = tuple(movable)
self.collisions = collisions
self.goal_holding = goal_holding
self.goal_confs = goal_confs
self.obstacles = tuple(body for body in get_bodies()
if body not in self.robots + self.movable)
self.custom_limits = {}
if self.limits is not None:
for robot in self.robots:
self.custom_limits.update(get_custom_limits(
robot, self.limits))
self.initial_confs = {
robot: Conf(robot, get_base_joints(robot))
for robot in self.robots
}
self.initial_poses = {body: Pose(body) for body in self.movable}
def __init__(self,
body_from_name,
robots,
limits,
collisions=True,
goal_confs={}):
self.body_from_name = dict(body_from_name)
self.robots = tuple(robots)
self.limits = limits
self.collisions = collisions
self.goal_confs = dict(goal_confs)
robot_bodies = set(map(self.get_body, self.robots))
self.obstacles = tuple(body for body in get_bodies()
if body not in robot_bodies)
self.custom_limits = {}
if self.limits is not None:
for body in robot_bodies:
self.custom_limits.update(
get_custom_limits(body, self.limits, yaw_limit=(0, 0)))
self.initial_confs = {
name: Conf(self.get_body(name),
get_base_joints(self.get_body(name)))
for name in self.robots
}
def sample_placements(body_surfaces, obstacles=None, min_distances={}):
if obstacles is None:
obstacles = [
body for body in get_bodies() if body not in body_surfaces
]
obstacles = list(obstacles)
# TODO: max attempts here
for body, surface in body_surfaces.items():
min_distance = min_distances.get(body, 0.01)
while True:
pose = sample_placement(body, surface)
if pose is None:
return False
if not any(
pairwise_collision(body, obst, max_distance=min_distance)
for obst in obstacles if obst not in [body, surface]):
obstacles.append(body)
break
return True
def __init__(self,
rovers=[],
landers=[],
objectives=[],
rocks=[],
soils=[],
stores=[],
limits=[],
body_types=[]):
self.rovers = rovers
self.landers = landers
self.objectives = objectives
self.rocks = rocks
self.soils = soils
self.stores = stores
self.limits = limits
no_collisions = self.rovers + self.rocks
self.fixed = set(get_bodies()) - set(no_collisions)
self.body_types = body_types
self.costs = False
def get_fixed(robot, movable):
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
return fixed
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 fixed(self):
movable = [self.robot] + list(self.movable)
return list(filter(lambda b: b not in movable, get_bodies()))
def fixed(self):
movable = [self.robot] + list(self.movable)
if self.gripper is not None:
movable.append(self.gripper)
return list(filter(lambda b: b not in movable, get_bodies()))
def get_fixed_bodies(problems):
return set(
get_bodies()) - set(problems.rovers + problems.rocks + problems.soils)
