def pddlstream_from_problem(robot, movable=[], teleport=False, grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
print('Robot:', robot)
conf = BodyConf(robot, get_configuration(robot))
init = [('CanMove',),
('Conf', conf),
('AtConf', conf),
('HandEmpty',)]
fixed = get_fixed(robot, movable)
print('Movable:', movable)
print('Fixed:', fixed)
for body in movable:
pose = BodyPose(body, get_pose(body))
init += [('Graspable', body),
('Pose', body, pose),
('AtPose', body, pose)]
for surface in fixed:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
init += [('Sink', body)]
if 'stove' in name:
init += [('Stove', body)]
body = movable[0]
goal = ('and',
('AtConf', conf),
#('Holding', body),
#('On', body, fixed[1]),
#('On', body, fixed[2]),
#('Cleaned', body),
('Cooked', body),
)
stream_map = {
'sample-pose': from_gen_fn(get_stable_gen(fixed)),
'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)),
'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)),
'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)),
'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)),
'TrajCollision': get_movable_collision_test(),
}
if USE_SYNTHESIZERS:
stream_map.update({
'plan-free-motion': empty_gen(),
'plan-holding-motion': empty_gen(),
})
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), scale_cost(1)),
Equal(('PlaceCost',), scale_cost(1)),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = [AND]
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
'MoveCost': move_cost_fn,
'TrajPoseCollision': fn_from_constant(False),
'TrajArmCollision': fn_from_constant(False),
'TrajGraspCollision': fn_from_constant(False),
}
if USE_SYNTHESIZERS:
stream_map['plan-base-motion'] = empty_gen(),
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal