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)),
'test-cfree-pose-pose': from_test(get_cfree_pose_pose_test()),
'test-cfree-approach-pose': from_test(get_cfree_obj_approach_pose_test()),
'test-cfree-traj-pose': from_test(negate_test(get_movable_collision_test())), #get_cfree_traj_pose_test()),
'TrajCollision': get_movable_collision_test(),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_problem(robot,
movable=[],
teleport=False,
movable_collisions=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 = {}
# initial state
print('Robot:', robot)
conf = BodyConf(robot, get_configuration(robot))
# add a robot configuration
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))
# add the poses of all movable objects
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
# add all placement surfaces with a sampled 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)]
# goal
body = movable[1]
goal = (
'and',
('AtConf', conf), # move the arm to the initial configuration
# ('Holding', body),
# ('On', body, fixed[1]),
# ('On', body, fixed[2]),
# ('Cleaned', movable[1]),
('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