コード例 #1
0
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
コード例 #2
0
def pddlstream_from_tamp(tamp_problem):
    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    initial = tamp_problem.initial
    mode = {b: Mode(p, None) for b, p in initial.block_poses.items()}
    conf = conf_from_state(initial)

    init = [
        ('CanMove',),
        ('Mode', mode),
        ('AtMode', mode),
        ('Conf', mode, conf),
        ('AtConf', conf),
    ]

    goal = Exists(['?m', '?q'], And(('GoalState', '?m', '?q'),
        ('AtMode', '?m'), ('AtConf', '?q')))

    stream_map = {
        's-forward': from_gen_fn(sample_forward(tamp_problem)),
        's-intersection': from_gen_fn(sample_intersection(tamp_problem)),
        's-connection': from_gen_fn(sample_connection(tamp_problem)),
        't-goal': from_test(test_goal_state(tamp_problem)),
    }

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
コード例 #3
0
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
コード例 #4
0
def create_problem(goal, obstacles=(), regions={}, max_distance=.5):
    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))
    constant_map = {}

    q0 = array([0, 0])
    init = [
        ('Conf', q0),
        ('AtConf', q0),
    ] + [('Region', r) for r in regions]

    if isinstance(goal, str):
        goal = ('In', goal)
    else:
        init += [('Conf', goal)]
        goal = ('AtConf', goal)

    np.set_printoptions(precision=3)
    samples = []

    def region_gen(region):
        lower, upper = regions[region]
        area = np.product(upper - lower)
        # TODO: sample proportional to area
        while True:
            q = array(sample_box(regions[region]))
            samples.append(q)
            yield (q, )

    # http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf
    #d = 2
    #vol_free = (1 - 0) * (1 - 0)
    #vol_ball = math.pi * (1 ** 2)
    #gamma = 2 * ((1 + 1. / d) * (vol_free / vol_ball)) ** (1. / d)

    roadmap = []

    def connected_test(q1, q2):
        #n = len(samples)
        #threshold = gamma * (math.log(n) / n) ** (1. / d)
        threshold = max_distance
        are_connected = (get_distance(q1, q2) <= threshold) and \
                is_collision_free((q1, q2), obstacles)
        if are_connected:
            roadmap.append((q1, q2))
        return are_connected

    stream_map = {
        'sample-region': from_gen_fn(region_gen),
        'connect': from_test(connected_test),
        'distance': get_distance,
    }

    problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
                          init, goal)

    return problem, samples, roadmap
コード例 #5
0
def get_pddlstream(world,
                   debug=False,
                   collisions=True,
                   teleport=False,
                   parameter_fns={}):
    domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))

    # TODO: increase number of attempts when collecting data
    constant_map, initial_atoms, goal_formula = get_initial_and_goal(world)
    stream_map = {
        'sample-motion':
        from_fn(get_motion_fn(world, collisions=collisions,
                              teleport=teleport)),
        'sample-pick':
        from_gen_fn(get_pick_gen_fn(world, collisions=collisions)),
        'sample-place':
        from_fn(get_place_fn(world, collisions=collisions)),
        'sample-pose':
        from_gen_fn(get_stable_pose_gen_fn(world, collisions=collisions)),
        #'sample-grasp': from_gen_fn(get_grasp_gen_fn(world)),
        'sample-pour':
        from_gen_fn(
            get_pour_gen_fn(world,
                            collisions=collisions,
                            parameter_fns=parameter_fns)),
        'sample-push':
        from_gen_fn(
            get_push_gen_fn(world,
                            collisions=collisions,
                            parameter_fns=parameter_fns)),
        'sample-stir':
        from_gen_fn(
            get_stir_gen_fn(world,
                            collisions=collisions,
                            parameter_fns=parameter_fns)),
        'sample-scoop':
        from_gen_fn(
            get_scoop_gen_fn(world,
                             collisions=collisions,
                             parameter_fns=parameter_fns)),
        'sample-press':
        from_gen_fn(get_press_gen_fn(world, collisions=collisions)),
        'test-reachable':
        from_test(get_reachable_test(world)),
        'ControlPoseCollision':
        get_control_pose_collision_test(world, collisions=collisions),
        'ControlConfCollision':
        get_control_conf_collision_test(world, collisions=collisions),
        'PosePoseCollision':
        get_pose_pose_collision_test(world, collisions=collisions),
        'ConfConfCollision':
        get_conf_conf_collision_test(world, collisions=collisions),
    }
    if debug:
        # Uses an automatically constructed debug generator for each stream
        stream_map = DEBUG
    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
                       initial_atoms, goal_formula)
コード例 #6
0
def get_problem():
    convbelt = ConveyorBelt(problem_idx=0)
    problem_config = convbelt.problem_config
    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))

    pick_sampler = PickWithBaseUnif(convbelt)
    place_sampler = PlaceUnif(convbelt)
    constant_map = {}
    stream_map = {'gen-grasp': from_gen_fn(gen_grasp(pick_sampler)),
                  'gen-placement': from_gen_fn(gen_placement(convbelt, place_sampler)),
                  #'TrajPoseCollision': fn_from_constant(False)
                  'TrajPoseCollision': check_traj_collision(convbelt),
                  }


    obj_names = problem_config['obj_poses'].keys()
    obj_poses = problem_config['obj_poses'].values()
    obj_names = ['obj0', 'obj1', 'obj2', 'obj3', 'obj4']
    init = [('Pickable', obj_name) for obj_name in obj_names]
    init += [('Robot', 'pr2')]
    init += [('BaseConf', [0, 1.05, 0])]
    init += [('EmptyArm',)]
    init += [('ObjectZero', 'obj0')]
    init += [('ObjectOne', 'obj1')]
    init += [('ObjectTwo', 'obj2')]
    init += [('ObjectThree', 'obj3')]
    init += [('ObjectFour', 'obj4')]

    #goal = ('Picked', 'obj0')
    #goal = ('AtPose', 'obj0', obj_pose)
    goal = ['and', ('InLoadingRegion', 'obj0'),
                   ('InLoadingRegion', 'obj1'),
                   ('InLoadingRegion', 'obj2'),
                   ('InLoadingRegion', 'obj3'),
                   ('InLoadingRegion', 'obj4')]
    #goal = ['and', ('InLoadingRegion', 'obj0'),  ('InLoadingRegion', 'obj1')]

    #goal = ('InLoadingRegion', 'obj0')
    convbelt.env.SetViewer('qtcoin')
    return (domain_pddl, constant_map, stream_pddl, stream_map, init, goal), convbelt
コード例 #7
0
ファイル: run.py プロジェクト: Khodeir/pddlstream
def pddlstream_from_problem(problem):
    # TODO: push and attach to movable objects

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    # TODO: action to generically connect to the roadmap
    # TODO: could check individual vertices first
    # TODO: dynamically generate the roadmap in interesting parts of the space
    # TODO: visibility graphs for sparse roadmaps
    # TODO: approximate robot with isotropic geometry
    # TODO: make the effort finite if applied to the roadmap vertex

    samples = []
    init = []
    for robot, conf in problem.initial_confs.items():
        samples.append(conf)
        init += [('Robot', robot), ('Conf', robot, conf),
                 ('AtConf', robot, conf), ('Free', robot)]
    for body, pose in problem.initial_poses.items():
        init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)]

    goal_literals = []
    goal_literals += [('Holding', robot, body)
                      for robot, body in problem.goal_holding.items()]
    for robot, base_values in problem.goal_confs.items():
        q_goal = Conf(robot, get_base_joints(robot), base_values)
        samples.append(q_goal)
        init += [('Conf', robot, q_goal)]
        goal_literals += [('AtConf', robot, q_goal)]
    goal_formula = And(*goal_literals)

    # TODO: assuming holonomic for now
    [body] = problem.robots

    with LockRenderer():
        init += create_vertices(problem, body, samples)
        #init += create_edges(problem, body, samples)

    stream_map = {
        'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)),
        'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)),
        # TODO: sample pushes rather than picks/places
        'sample-grasp': from_gen_fn(get_grasp_generator(problem)),
        'compute-ik': from_fn(get_ik_fn(problem)),
        'compute-motion': from_fn(get_motion_fn(problem)),
        'test-reachable': from_test(lambda *args: False),
        'Cost': get_cost_fn(problem),
    }
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
                       init, goal_formula)
コード例 #8
0
ファイル: stripstream.py プロジェクト: sungbinlim/voot
def get_problem():
    namo = StripStreamNAMO()
    problem_config = namo.problem_config
    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))

    pick_sampler = PickWithBaseUnif(namo)
    place_sampler = PlaceUnif(namo)
    constant_map = {}
    stream_map = {'gen-grasp': from_gen_fn(gen_grasp(pick_sampler)),
                  'TrajPoseCollision': check_traj_collision(namo),
                  'TrajPoseCollisionWithObject': check_traj_collision_with_object(namo),
                  'gen-base-traj': from_gen_fn(gen_base_traj(namo)),
                  'gen-placement': from_gen_fn(gen_placement(namo, place_sampler)),
                  #'gen-base-traj-with-obj': from_fn(gen_base_traj_with_object(namo)),
                  }
    obj_names = problem_config['obj_poses'].keys()
    obj_poses = problem_config['obj_poses'].values()
    init = [('Pickable', obj_name) for obj_name in obj_names]
    init += [('Robot', 'pr2')]
    init += [('EmptyArm',)]
    init += [('Region', 'entire_region')]
    init += [('Region', 'loading_region')]
    #init += [('InRegion', 'obj0', 'entire_region')]
    #init += [('InRegion', 'obj1', 'entire_region')]
    init += [('AtPose', obj_name, obj_pose) for obj_name, obj_pose in zip(obj_names, obj_poses)]
    init += [('Pose', obj_name, obj_pose) for obj_name, obj_pose in zip(obj_names, obj_poses)]

    init_config = np.array([-1, 1, 0])
    goal_config = np.array([-2, 1, 0])
    init += [('BaseConf', init_config)]
    init += [('BaseConf', goal_config)]
    init += [('AtConf', init_config)]

    #goal = ['and', ('InRegion', 'obj0', 'loading_region')]
    #goal = ['and', ('InRegion', 'obj0', 'loading_region')]
    goal = ['and', ('Holding', 'obj1')]
    #goal = ['and', ('AtConf', goal_config), ('Placed', 'obj0')]
    #goal = ['and', ('not', ('EmptyArm',))]
    return (domain_pddl, constant_map, stream_pddl, stream_map, init, goal), namo
コード例 #9
0
ファイル: run.py プロジェクト: miquelramirez/pddlstream
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True):
    initial = tamp_problem.initial
    assert(initial.holding is None)

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    external_paths = []
    if use_stream:
        external_paths.append(get_file_path(__file__, 'stream.pddl'))
    if use_optimizer:
        external_paths.append(get_file_path(__file__, 'optimizer.pddl'))
    external_pddl = [read(path) for path in external_paths]

    constant_map = {}

    init = [
        ('CanMove',),
        ('Conf', initial.conf),
        ('AtConf', initial.conf),
        ('HandEmpty',),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()] + \
           [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
           [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \
           [('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]]

    goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]

    if tamp_problem.goal_conf is not None:
        goal_literals += [('AtConf', tamp_problem.goal_conf)]
    goal = And(*goal_literals)

    stream_map = {
        's-motion': from_fn(plan_motion),
        's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        't-region': from_test(get_region_test(tamp_problem.regions)),
        's-ik': from_fn(inverse_kin_fn),
        #'s-ik': from_gen_fn(unreliable_ik_fn),
        'distance': distance_fn,

        't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))),
        #'posecollision': collision_test, # Redundant
        'trajcollision': lambda *args: False,
    }
    if use_optimizer:
        stream_map.update({
            'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
            'rrt': from_fn(cfree_motion_fn),
        })
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
コード例 #10
0
ファイル: run.py プロジェクト: jingxixu/pddlstream
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert (initial.holding is None)

    known_poses = list(initial.block_poses.values()) + \
                  list(tamp_problem.goal_poses.values())

    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))

    q100 = np.array([100, 100])
    constant_map = {
        'q100': q100,
    }

    init = [
        #Type(q100, 'conf'),
        ('CanMove',),
        ('Conf', q100),
        ('Conf', initial.conf),
        ('AtConf', initial.conf),
        ('HandEmpty',),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', p) for p in known_poses] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()]
    # [('Pose', p) for p in known_poses + tamp_problem.poses] + \

    goal = And(*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()])

    def collision_test(p1, p2):
        return np.linalg.norm(p2 - p1) <= 1e-1

    def distance_fn(q1, q2):
        ord = 1  # 1 | 2
        return int(math.ceil(np.linalg.norm(q2 - q1, ord=ord)))

    # TODO: convert to lower case
    stream_map = {
        #'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))),
        'sample-pose':
        from_gen_fn(lambda: ((p, ) for p in tamp_problem.poses)),
        'inverse-kinematics': from_fn(lambda p: (p + GRASP, )),
        #'inverse-kinematics': IKGenerator,
        #'inverse-kinematics': IKFactGenerator,
        'collision-free': from_test(lambda *args: not collision_test(*args)),
        'collision': collision_test,
        #'constraint-solver': None,
        'distance': distance_fn,
    }

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #11
0
ファイル: run.py プロジェクト: syc7446/pddlstream
def pddlstream_from_tamp(tamp_problem,
                         use_stream=True,
                         use_optimizer=False,
                         collisions=True):

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    external_paths = []
    if use_stream:
        external_paths.append(get_file_path(__file__, 'stream.pddl'))
    if use_optimizer:
        external_paths.append(
            get_file_path(
                __file__,
                'optimizer/optimizer.pddl'))  # optimizer | optimizer_hard
    external_pddl = [read(path) for path in external_paths]

    constant_map = {}
    stream_map = {
        's-grasp':
        from_fn(lambda b: (GRASP, )),
        's-region':
        from_gen_fn(get_pose_gen(tamp_problem.regions)),
        's-ik':
        from_fn(inverse_kin_fn),
        #'s-ik': from_gen_fn(unreliable_ik_fn),
        's-motion':
        from_fn(plan_motion),
        't-region':
        from_test(get_region_test(tamp_problem.regions)),
        't-cfree':
        from_test(
            lambda *args: implies(collisions, not collision_test(*args))),
        'dist':
        distance_fn,
        'duration':
        duration_fn,
    }
    if use_optimizer:
        # To avoid loading gurobi
        stream_map.update({
            'gurobi':
            from_list_fn(
                get_optimize_fn(tamp_problem.regions, collisions=collisions)),
            'rrt':
            from_fn(cfree_motion_fn),
        })
    #stream_map = 'debug'

    init, goal = create_problem(tamp_problem)

    return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
                       init, goal)
コード例 #12
0
ファイル: run_stacking.py プロジェクト: shiyani21/stacking
def pddlstream_from_problem(robot, movable):
    domain_pddl = read('tamp/domain_stacking.pddl')
    stream_pddl = read('tamp/stream_stacking.pddl')
    constant_map = {}

    fixed = misc.get_fixed(robot, movable)

    stream_map = {
        'sample-pose-table':
        from_gen_fn(primitives.get_stable_gen_table(fixed)),
        'sample-pose-block':
        from_fn(primitives.get_stable_gen_block(fixed)),
        'sample-grasp':
        from_gen_fn(primitives.get_grasp_gen(robot)),
        'inverse-kinematics':
        from_fn(primitives.get_ik_fn(robot, fixed)),
        'plan-free-motion':
        from_fn(primitives.get_free_motion_gen(robot, fixed)),
        'plan-holding-motion':
        from_fn(primitives.get_holding_motion_gen(robot, fixed)),
    }

    return domain_pddl, constant_map, stream_pddl, stream_map
コード例 #13
0
def pddlstream_from_tamp(tamp_problem):
    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))

    constant_map = {}
    stream_map = {
        #'s-motion': from_fn(plan_motion),
        't-reachable': from_test(test_reachable),
        's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        't-region': from_test(get_region_test(tamp_problem.regions)),
        's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
        'dist': distance_fn,
    }
    init, goal = create_problem(tamp_problem)

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
コード例 #14
0
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert(initial.holding is None)

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    external_pddl = [
        read(get_file_path(__file__, 'stream.pddl')),
        #read(get_file_path(__file__, 'optimizer.pddl')),
    ]
    constant_map = {}

    init = [
        ('CanMove',),
        ('Conf', initial.conf),
        ('AtConf', initial.conf),
        ('HandEmpty',),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()] + \
           [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
           [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]

    goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]

    if tamp_problem.goal_conf is not None:
        goal_literals += [('AtConf', tamp_problem.goal_conf)]
    goal = And(*goal_literals)

    stream_map = {
        's-motion': from_fn(plan_motion),
        's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        't-region': from_test(get_region_test(tamp_problem.regions)),
        's-ik': from_fn(inverse_kin_fn),
        'distance': distance_fn,

        't-cfree': from_test(lambda *args: not collision_test(*args)),
        'posecollision': collision_test, # Redundant
        'trajcollision': lambda *args: False,
        'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
        'rrt': from_fn(cfree_motion_fn),
        #'reachable': from_test(reachable_test),
        #'Valid': valid_state_fn,
    }
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
コード例 #15
0
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert(initial.holding is None)

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    # TODO: can always make the state the set of fluents
    #state = tamp_problem.initial
    state = {
        R: tamp_problem.initial.conf,
        H: tamp_problem.initial.holding,
    }
    for b, p in tamp_problem.initial.block_poses.items():
        state[b] = p

    init = [
        ('State', state),
        ('AtState', state),
        ('Conf', initial.conf)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('Region', r) for r in tamp_problem.regions.keys()] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()] + \
           [('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
           [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
    goal = ('AtGoal',)

    stream_map = {
        'plan-motion': from_fn(plan_motion),
        'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        'test-region': from_test(get_region_test(tamp_problem.regions)),
        'inverse-kinematics':  from_fn(inverse_kin_fn),
        #'posecollision': collision_test,
        #'trajcollision': lambda *args: False,

        'forward-move': from_fn(move_fn),
        'forward-pick': from_fn(pick_fn),
        'forward-place': from_fn(place_fn),
        'test-goal': from_test(get_goal_test(tamp_problem)),
    }
    #stream_map = 'debug'

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #16
0
ファイル: run.py プロジェクト: yqj13777866390/pddlstream
def pddlstream_from_tamp(tamp_problem):
    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))

    # TODO: algorithm that prediscretized once
    constant_map = {}
    stream_map = {
        's-motion': from_fn(plan_motion),
        't-reachable': from_test(test_reachable),
        's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        't-region': from_test(get_region_test(tamp_problem.regions)),
        's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
        'dist': distance_fn,
    }
    init, goal = create_problem(tamp_problem)
    init.extend(('Grasp', b, GRASP) for b in tamp_problem.initial.block_poses)

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
コード例 #17
0
ファイル: run.py プロジェクト: Khodeir/pddlstream
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert(initial.holding is None)

    known_poses = list(initial.block_poses.values()) + \
                  list(tamp_problem.goal_poses.values())

    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))

    q100 = np.array([100, 100])
    constant_map = {
        'q100': q100, # As an example
    }

    init = [
        #Type(q100, 'conf'),
        ('CanMove',),
        ('Conf', q100),
        ('Conf', initial.conf),
        ('AtConf', initial.conf),
        ('HandEmpty',),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', p) for p in known_poses] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()]
    # [('Pose', p) for p in known_poses + tamp_problem.poses] + \

    goal = And(*[
        ('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()
    ])

    # TODO: convert to lower case
    stream_map = {
        #'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))),
        'sample-pose': from_gen_fn(lambda: ((p,) for p in tamp_problem.poses)),
        'inverse-kinematics':  from_fn(lambda p: (p + GRASP,)),
        'test-cfree': from_test(lambda *args: not collision_test(*args)),
        'collision': collision_test,
        'distance': distance_fn,
    }

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
コード例 #18
0
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert (initial.holding is None)

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    init = [
        ('CanMove',),
        ('Conf', initial.conf),
        ('AtConf', initial.conf),
        ('HandEmpty',),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('Region', r) for r in tamp_problem.regions.keys()] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()] + \
           [('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
           [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]

    goal_literals = [('HandEmpty',)] + \
                    [('In', b, r) for b, r in tamp_problem.goal_regions.items()]
    if tamp_problem.goal_conf is not None:
        goal_literals += [('AtConf', tamp_problem.goal_conf)]
    goal = And(*goal_literals)

    stream_map = {
        'plan-motion': from_fn(plan_motion),
        'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        'test-region': from_test(get_region_test(tamp_problem.regions)),
        'inverse-kinematics': from_fn(inverse_kin_fn),
        'collision-free': from_test(lambda *args: not collision_test(*args)),
        'cfree': lambda *args: not collision_test(*args),
        'posecollision': collision_test,
        'trajcollision': lambda *args: False,
        'distance': distance_fn,
        #'Valid': valid_state_fn,
    }
    #stream_map = 'debug'

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #19
0
def opt_from_graph(names, orders, infos={}):
    param_from_order = {order: PARAM_TEMPLATE.format(*order) for order in orders}
    fact_from_order = {order: (PREDICATE, param_from_order[order]) for order in orders}
    object_from_param = {param: parse_value(param) for param in param_from_order.values()}

    incoming_from_edges, outgoing_from_edges = neighbors_from_orders(orders)
    stream_plan = []
    for i, n in enumerate(names):
        #info = infos.get(n, StreamInfo(p_success=1, overhead=0, verbose=True))
        inputs = [param_from_order[n2, n] for n2 in incoming_from_edges[n]]
        outputs = [param_from_order[n, n2] for n2 in outgoing_from_edges[n]]
        #gen = get_gen(outputs=outputs, p_success=info.p_success)
        #gen = get_gen(infos[i], outputs=outputs)
        stream = Stream(
            name=n,
            #gen_fn=DEBUG,
            #gen_fn=from_gen(gen),
            gen_fn=from_gen_fn(get_gen_fn(outputs=outputs, **infos[i])),
            inputs=inputs,
            domain=[fact_from_order[n2, n] for n2 in incoming_from_edges[n]],
            fluents=[],
            outputs=outputs,
            certified=[fact_from_order[n, n2] for n2 in outgoing_from_edges[n]],
            #info=info,
            info=StreamInfo(),
        )
        # TODO: dump names

        print()
        print(stream)
        input_objects = safe_apply_mapping(stream.inputs, object_from_param)
        instance = stream.get_instance(input_objects)
        print(instance)
        output_objects = safe_apply_mapping(stream.outputs, object_from_param)
        result = instance.get_result(output_objects)
        print(result)
        stream_plan.append(result)

    opt_plan = OptPlan(action_plan=[], preimage_facts=[])
    opt_solution = OptSolution(stream_plan, opt_plan, cost=1)
    return opt_solution
コード例 #20
0
def pddlstream_from_tamp(tamp_problem):
    initial = tamp_problem.initial
    assert(not initial.holding) # is None

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    # TODO: can always make the state a set of fluents
    init = [
        ('State', initial),
        ('AtState', initial)] + \
           [('Robot', r) for r in initial.robot_confs.keys()] + \
           [('Conf', q) for q in initial.robot_confs.values()] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('Grasp', b, GRASP) for b in initial.block_poses.keys()] + \
           [('Region', r) for r in tamp_problem.regions.keys()] + \
           [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
           [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
    # TODO: could populate AtConf, AtPose, HandEmpty, Holding
    goal = ('AtGoal',)

    stream_map = {
        'plan-motion': from_fn(plan_motion),
        'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
        'test-region': from_test(get_region_test(tamp_problem.regions)),
        'inverse-kinematics':  from_fn(inverse_kin_fn),
        #'posecollision': collision_test,
        #'trajcollision': lambda *args: False,

        'forward-move': from_fn(move_fn),
        'forward-pick': from_fn(pick_fn),
        'forward-place': from_fn(place_fn),
        'test-goal': from_test(get_goal_test(tamp_problem)),
    }
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
コード例 #21
0
def pddlstream_from_tamp(tamp_problem,
                         use_stream=True,
                         use_optimizer=False,
                         collisions=True):
    initial = tamp_problem.initial
    assert (not initial.holding)

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    external_paths = []
    if use_stream:
        external_paths.append(get_file_path(__file__, 'stream.pddl'))
    if use_optimizer:
        external_paths.append(get_file_path(
            __file__, 'optimizer.pddl'))  # optimizer | optimizer_hard
    external_pddl = [read(path) for path in external_paths]

    constant_map = {}
    init = [
        ('Region', GROUND_NAME),
        Equal((TOTAL_COST,), 0)] + \
           [('Block', b) for b in initial.block_poses.keys()] + \
           [('Pose', b, p) for b, p in initial.block_poses.items()] + \
           [('Grasp', b, GRASP) for b in initial.block_poses] + \
           [('AtPose', b, p) for b, p in initial.block_poses.items()] + \
           [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()]
    goal_literals = []

    for r, q in initial.robot_confs.items():
        init += [
            ('Robot', r),
            ('CanMove', r),
            ('Conf', q),
            ('AtConf', r, q),
            ('HandEmpty', r),
        ]
        if tamp_problem.goal_conf is not None:
            #goal_literals += [('AtConf', tamp_problem.goal_conf)]
            goal_literals += [('AtConf', r, q)]

    for b, r in tamp_problem.goal_regions.items():
        if isinstance(r, str):
            init += [('Region', r), ('Placeable', b, r)]
            goal_literals += [('In', b, r)]
        else:
            init += [('Pose', b, r)]
            goal_literals += [('AtPose', b, r)]

    #goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
    goal = And(*goal_literals)

    stream_map = {
        's-motion':
        from_fn(plan_motion),
        's-region':
        from_gen_fn(get_pose_gen(tamp_problem.regions)),
        't-region':
        from_test(get_region_test(tamp_problem.regions)),
        's-ik':
        from_fn(inverse_kin_fn),
        #'s-ik': from_gen_fn(unreliable_ik_fn),
        'dist':
        distance_fn,
        'duration':
        duration_fn,
        't-cfree':
        from_test(
            lambda *args: implies(collisions, not collision_test(*args))),
    }
    if use_optimizer:
        # To avoid loading gurobi
        stream_map.update({
            'gurobi':
            from_list_fn(
                get_optimize_fn(tamp_problem.regions, collisions=collisions)),
            'rrt':
            from_fn(cfree_motion_fn),
        })
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
                       init, goal)
コード例 #22
0
def pddlstream_from_state(state, teleport=False):
    task = state.task
    robot = task.robot
    # TODO: infer open world from task

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {
        'base': 'base',
        'left': 'left',
        'right': 'right',
        'head': 'head',
    }

    #base_conf = state.poses[robot]
    base_conf = Conf(robot, get_group_joints(robot, 'base'),
                     get_group_conf(robot, 'base'))
    scan_cost = 1
    init = [
        ('BConf', base_conf),
        ('AtBConf', base_conf),
        Equal(('MoveCost', ), 1),
        Equal(('PickCost', ), 1),
        Equal(('PlaceCost', ), 1),
        Equal(('ScanCost', ), scan_cost),
        Equal(('RegisterCost', ), 1),
    ]
    holding_arms = set()
    holding_bodies = set()
    for attach in state.attachments.values():
        holding_arms.add(attach.arm)
        holding_bodies.add(attach.body)
        init += [('Grasp', attach.body, attach.grasp),
                 ('AtGrasp', attach.arm, attach.body, attach.grasp)]
    for arm in ARM_NAMES:
        joints = get_arm_joints(robot, arm)
        conf = Conf(robot, joints, get_joint_positions(robot, joints))
        init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
        if arm in task.arms:
            init += [('Controllable', arm)]
        if arm not in holding_arms:
            init += [('HandEmpty', arm)]
    for body in task.get_bodies():
        if body in holding_bodies:
            continue
        # TODO: no notion whether observable actually corresponds to the correct thing
        pose = state.poses[body]
        init += [
            ('Pose', body, pose),
            ('AtPose', body, pose),
            ('Observable', pose),
        ]

    init += [('Scannable', body) for body in task.rooms + task.surfaces]
    init += [('Registerable', body) for body in task.movable]
    init += [('Graspable', body) for body in task.movable]
    for body in task.get_bodies():
        supports = task.get_supports(body)
        if supports is None:
            continue
        for surface in supports:
            p_obs = state.b_on[body].prob(surface)
            cost = revisit_mdp_cost(0, scan_cost,
                                    p_obs)  # TODO: imperfect observation model
            init += [('Stackable', body, surface),
                     Equal(('LocalizeCost', surface, body), clip_cost(cost))]
            #if is_placement(body, surface):
            if is_center_stable(body, surface):
                if body in holding_bodies:
                    continue
                pose = state.poses[body]
                init += [('Supported', body, pose, surface)]

    for body in task.get_bodies():
        if state.is_localized(body):
            init.append(('Localized', body))
        else:
            init.append(('Uncertain', body))
        if body in state.registered:
            init.append(('Registered', body))

    goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
           [('On', b, s) for b, s in task.goal_on] + \
           [('Localized', b) for b in task.goal_localized] + \
           [('Registered', b) for b in task.goal_registered])

    stream_map = {
        'sample-pose':
        get_stable_gen(task),
        'sample-grasp':
        from_list_fn(get_grasp_gen(task)),
        'inverse-kinematics':
        from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
        'plan-base-motion':
        from_fn(get_motion_gen(task, teleport=teleport)),
        'test-vis-base':
        from_test(get_in_range_test(task, VIS_RANGE)),
        'test-reg-base':
        from_test(get_in_range_test(task, REG_RANGE)),
        'sample-vis-base':
        accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
                               max_attempts=25),
        'sample-reg-base':
        accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
                               max_attempts=25),
        'inverse-visibility':
        from_fn(get_inverse_visibility_fn(task)),
    }

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
                       init, goal)
コード例 #23
0
ファイル: run.py プロジェクト: caelan/pb-construction
def get_pddlstream(robot,
                   brick_from_index,
                   obstacle_from_name,
                   collisions=True,
                   teleport=False):
    domain_pddl = read(get_file_path(__file__, 'retired.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    conf = np.array(get_configuration(robot))
    init = [
        ('CanMove', ),
        ('Conf', conf),
        ('AtConf', conf),
        ('HandEmpty', ),
    ]

    goal_literals = [
        ('AtConf', conf),
        #('HandEmpty',),
    ]

    #indices = brick_from_index.keys()
    #indices = range(2, 5)
    indices = [4]
    for index in list(indices):
        indices.append(index + 6)

    for index in indices:
        brick = brick_from_index[index]
        init += [
            ('Graspable', index),
            ('Pose', index, brick.initial_pose),
            ('AtPose', index, brick.initial_pose),
            ('Pose', index, brick.goal_pose),
        ]
        goal_literals += [
            #('Holding', index),
            ('AtPose', index, brick.goal_pose),
        ]
        for index2 in brick.goal_supports:
            brick2 = brick_from_index[index2]
            init += [
                ('Supported', index, brick.goal_pose, index2,
                 brick2.goal_pose),
            ]
    goal = And(*goal_literals)

    if not collisions:
        obstacle_from_name = {}
    stream_map = {
        'sample-grasp':
        from_gen_fn(get_grasp_gen_fn(brick_from_index)),
        'inverse-kinematics':
        from_gen_fn(get_ik_gen_fn(robot, brick_from_index,
                                  obstacle_from_name)),
        'plan-motion':
        from_fn(
            get_motion_fn(robot,
                          brick_from_index,
                          obstacle_from_name,
                          teleport=teleport)),
        'TrajCollision':
        get_collision_test(robot, brick_from_index, collisions=collisions),
    }
    #stream_map = 'debug'

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #24
0
def pddlstream_from_problem(problem, collisions=True, teleport=False):
    #rovers = problem.rovers

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    camera = 'RGBD'
    mode = 'color'  # highres | lowres | color | depth

    init = []
    goal_literals = [
        #('Calibrated', camera, problem.rovers[0])
        #('Analyzed', problem.rovers[0], problem.rocks[0])
        #('ReceivedAnalysis', problem.rocks[0])
    ]

    init += [('Lander', l) for l in problem.landers]
    init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)]
    for rover in problem.rovers:
        base_joints = get_group_joints(rover, 'base')
        bq0 = Conf(rover, base_joints)
        head_joints = get_group_joints(rover, 'head')
        hq0 = Conf(rover, head_joints)
        init += [('Rover', rover), ('OnBoard', camera, rover),
                 ('BConf', bq0), ('AtBConf', rover, bq0),
                 ('HConf', hq0), ('AtHConf', rover, hq0)]
    for name in problem.stores:
        init += [('Store', name)]
        init += [('Free', rover, name) for rover in problem.rovers]
    for name in problem.objectives:
        init += [('Objective', name)]
        #initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name]
        goal_literals += [('ReceivedImage', name, mode)]
    for name in problem.rocks + problem.soils:
        # pose = Pose(name, get_pose(env.GetKinBody(name)))
        init += [('Rock', name)]  # , IsPose(name, pose), AtPose(name, pose)]
        #init += [('IsClass', name, cl) for cl in CLASSES if cl in name]

    #if problem.rocks:
    #    goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'),
    #                                               ('ReceivedAnalysis', '?rock'))))
    #if problem.soils:
    #    goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'),
    #                                               ('ReceivedAnalysis', '?soil')))) # TODO: soil class
    goal_formula = And(*goal_literals)

    custom_limits = {}
    if problem.limits is not None:
        for rover in problem.rovers:
            custom_limits.update(get_base_custom_limits(rover, problem.limits))

    stream_map = {
        'test-reachable': from_test(get_reachable_test(problem)),
        'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)),
        'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)),
        'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)),
        'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)),
        'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)),
    }
    #stream_map = 'debug'

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
コード例 #25
0
ファイル: run.py プロジェクト: miquelramirez/pddlstream
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 = {
        'world': 'world',
    }

    world = 'world'
    #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), # TODO: could make pose as well...
        ('AtBConf', initial_bq),
        ('AtAConf', world, None),
        ('AtPose', world, world, None),
    ] + [('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_JOINT_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), ('AtPose', arm, arm, None)]
        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', world, 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)),
        #'plan-base-motion': empty_gen(),
        'BTrajCollision':
        fn_from_constant(False),
    }
    # get_press_gen(problem, teleport=teleport)

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #26
0
def get_problem(mover, goal_objects, goal_region_name, config):
    directory = os.path.dirname(os.path.abspath(__file__))
    domain_pddl = read(os.path.join(directory, 'domain.pddl'))
    stream_pddl = read(os.path.join(directory, 'stream.pddl'))

    constant_map = {}
    stream_map = {
        'gen-pap': from_gen_fn(gen_pap(mover, config)),
    }

    obj_names = [obj.GetName() for obj in mover.objects]
    obj_poses = [
        get_body_xytheta(mover.env.GetKinBody(obj_name)).squeeze()
        for obj_name in obj_names
    ]

    initial_robot_conf = get_body_xytheta(mover.robot).squeeze()

    if mover.name == 'two_arm_mover':
        goal_region = 'home_region'
        nongoal_regions = ['loading_region']
    elif mover.name == 'one_arm_mover':
        goal_region = mover.target_box_region.name
        nongoal_regions = ['center_shelf_region']  #list(mover.shelf_regions)
    else:
        raise NotImplementedError

    print(goal_region, nongoal_regions, mover.regions.keys())

    init = [('Pickable', obj_name) for obj_name in obj_names]
    init += [('InRegion', obj_name,
              mover.get_region_containing(mover.env.GetKinBody(obj_name)).name)
             for obj_name in obj_names]
    init += [('Region', region) for region in nongoal_regions + [goal_region]]

    init += [('GoalObject', obj_name) for obj_name in goal_objects]
    init += [('NonGoalRegion', region) for region in nongoal_regions]

    init_state = CustomStateSaver(mover.env)
    init += [('State', init_state)]
    init += [('AtState', init_state)]

    # robot initialization
    init += [('EmptyArm', )]
    init += [('AtConf', initial_robot_conf)]
    init += [('BaseConf', initial_robot_conf)]

    # object initialization
    init += [('Pose', obj_pose)
             for obj_name, obj_pose in zip(obj_names, obj_poses)]
    init += [('PoseInRegion', obj_pose, 'loading_region')
             for obj_name, obj_pose in zip(obj_names, obj_poses)]
    init += [('AtPose', obj_name, obj_pose)
             for obj_name, obj_pose in zip(obj_names, obj_poses)]

    goal = ['and'] + [('InRegion', obj_name, goal_region_name)
                      for obj_name in goal_objects]

    print('Num init:', Counter(fact[0] for fact in init))
    print('Goal:', goal)
    print('Streams:', sorted(stream_map))

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #27
0
ファイル: problem.py プロジェクト: yuchen-x/SS-Replan
def get_streams(world, debug=False, teleport_base=False, **kwargs):
    stream_pddl = read(get_file_path(__file__, '../pddl/stream.pddl'))
    if debug:
        return stream_pddl, DEBUG
    stream_map = {
        'test-door':
        from_test(get_door_test(world)),
        'test-gripper':
        from_test(get_gripper_open_test(world)),
        'sample-pose':
        from_gen_fn(get_stable_gen(world, **kwargs)),
        'sample-grasp':
        from_gen_fn(get_grasp_gen(world)),
        'sample-nearby-pose':
        from_gen_fn(get_nearby_stable_gen(world, **kwargs)),
        'plan-pick':
        from_gen_fn(get_pick_gen_fn(world, **kwargs)),
        'plan-pull':
        from_gen_fn(get_pull_gen_fn(world, **kwargs)),
        'plan-press':
        from_gen_fn(get_press_gen_fn(world, **kwargs)),
        'plan-pour':
        from_gen_fn(get_pour_gen_fn(world, **kwargs)),
        'plan-base-motion':
        from_fn(
            get_base_motion_fn(world, teleport_base=teleport_base, **kwargs)),
        'plan-arm-motion':
        from_fn(get_arm_motion_gen(world, **kwargs)),
        'plan-gripper-motion':
        from_fn(get_gripper_motion_gen(world, **kwargs)),
        'plan-calibrate-motion':
        from_fn(get_calibrate_gen(world, **kwargs)),
        'test-near-pose':
        from_test(get_test_near_pose(world, **kwargs)),
        'test-near-joint':
        from_test(get_test_near_joint(world, **kwargs)),
        'fixed-plan-pick':
        from_gen_fn(get_fixed_pick_gen_fn(world, **kwargs)),
        'fixed-plan-pull':
        from_gen_fn(get_fixed_pull_gen_fn(world, **kwargs)),
        'fixed-plan-press':
        from_gen_fn(get_fixed_press_gen_fn(world, **kwargs)),
        'fixed-plan-pour':
        from_gen_fn(get_fixed_pour_gen_fn(world, **kwargs)),
        'compute-pose-kin':
        from_fn(get_compute_pose_kin(world)),
        # 'compute-angle-kin': from_fn(compute_angle_kin),
        'compute-detect':
        from_fn(get_compute_detect(world, **kwargs)),
        'sample-observation':
        from_gen_fn(get_sample_belief_gen(world, **kwargs)),
        'update-belief':
        from_fn(update_belief_fn(world, **kwargs)),
        'test-cfree-worldpose':
        from_test(get_cfree_worldpose_test(world, **kwargs)),
        'test-cfree-worldpose-worldpose':
        from_test(get_cfree_worldpose_worldpose_test(world, **kwargs)),
        'test-cfree-pose-pose':
        from_test(get_cfree_relpose_relpose_test(world, **kwargs)),
        'test-cfree-bconf-pose':
        from_test(get_cfree_bconf_pose_test(world, **kwargs)),
        'test-cfree-approach-pose':
        from_test(get_cfree_approach_pose_test(world, **kwargs)),
        'test-cfree-angle-angle':
        from_test(get_cfree_angle_angle_test(world, **kwargs)),
        'test-cfree-traj-pose':
        from_test(get_cfree_traj_pose_test(world, **kwargs)),
        'test-ofree-ray-pose':
        from_test(get_ofree_ray_pose_test(world, **kwargs)),
        'test-ofree-ray-grasp':
        from_test(get_ofree_ray_grasp_test(world, **kwargs)),
        'DetectCost':
        detect_cost_fn,
        #'MoveCost': move_cost_fn,
        # 'Distance': base_cost_fn,
    }
    return stream_pddl, stream_map
コード例 #28
0
def get_pddlstream_problem(task, context, collisions=True):
    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {}

    plant = task.mbp
    robot = task.robot
    robot_name = get_model_name(plant, robot)

    world = plant.world_frame()  # mbp.world_body()
    robot_joints = get_movable_joints(plant, robot)
    robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
    init = [
        ('Robot', robot_name),
        ('CanMove', robot_name),
        ('Conf', robot_name, robot_conf),
        ('AtConf', robot_name, robot_conf),
        ('HandEmpty', robot_name),
    ]
    goal_literals = []
    if task.reset_robot:
        goal_literals.append(('AtConf', robot_name, robot_conf), )

    for obj in task.movable:
        obj_name = get_model_name(plant, obj)
        #obj_frame = get_base_body(mbp, obj).body_frame()
        obj_pose = Pose(plant, world, obj,
                        get_world_pose(plant, context,
                                       obj))  # get_relative_transform
        init += [
            ('Graspable', obj_name),
            ('Pose', obj_name, obj_pose),
            #('InitPose', obj_name, obj_pose),
            ('AtPose', obj_name, obj_pose)
        ]
        for surface in task.surfaces:
            init += [('Stackable', obj_name, surface)]
            # TODO: detect already stacked

    for surface in task.surfaces:
        surface_name = get_model_name(plant, surface.model_index)
        if 'sink' in surface_name:
            init += [('Sink', surface)]
        if 'stove' in surface_name:
            init += [('Stove', surface)]

    for door in task.doors:
        door_body = plant.tree().get_body(door)
        door_name = door_body.name()
        door_joints = get_parent_joints(plant, door_body)
        door_conf = Conf(door_joints,
                         get_joint_positions(door_joints, context))
        init += [
            ('Door', door_name),
            ('Conf', door_name, door_conf),
            ('AtConf', door_name, door_conf),
        ]
        for positions in [get_door_positions(door_body, DOOR_OPEN)]:
            conf = Conf(door_joints, positions)
            init += [('Conf', door_name, conf)]
        if task.reset_doors:
            goal_literals += [('AtConf', door_name, door_conf)]

    for obj, transform in task.goal_poses.items():
        obj_name = get_model_name(plant, obj)
        obj_pose = Pose(plant, world, obj, transform)
        init += [('Pose', obj_name, obj_pose)]
        goal_literals.append(('AtPose', obj_name, obj_pose))
    for obj in task.goal_holding:
        goal_literals.append(
            ('Holding', robot_name, get_model_name(plant, obj)))
    for obj, surface in task.goal_on:
        goal_literals.append(('On', get_model_name(plant, obj), surface))
    for obj in task.goal_cooked:
        goal_literals.append(('Cooked', get_model_name(plant, obj)))

    goal = And(*goal_literals)
    print('Initial:', init)
    print('Goal:', goal)

    stream_map = {
        #'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)),
        'sample-reachable-pose':
        from_gen_fn(
            get_reachable_pose_gen_fn(task, context, collisions=collisions)),
        'sample-grasp':
        from_gen_fn(get_grasp_gen_fn(task)),
        'plan-ik':
        from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
        'plan-motion':
        from_fn(get_motion_fn(task, context, collisions=collisions)),
        'plan-pull':
        from_fn(get_pull_fn(task, context, collisions=collisions)),
        'TrajPoseCollision':
        get_collision_test(task, context, collisions=collisions),
        'TrajConfCollision':
        get_collision_test(task, context, collisions=collisions),
    }
    #stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging

    return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
コード例 #29
0
ファイル: run.py プロジェクト: hejia-zhang/pddlstream
LARGE = 5
SMALL = -1
#MAX_INT = 100000
MAX_INT = 5

pairs = set()


def addition(x1, x2):
    pairs.add((x1, x2))
    x2 = x1 + x2
    return (x2, )


STREAM_MAP = {
    'positive': from_gen_fn(lambda: ((x, ) for x in range(1, MAX_INT + 1))),
    'negative': from_gen_fn(lambda: ((-x, ) for x in range(1, MAX_INT + 1))),
    'addition': from_fn(addition),
    'test-large': from_test(lambda x: LARGE <= x),
    'test-small': from_test(lambda x: x <= SMALL),
    'cost': lambda x: 1. / (abs(x) + 1),
}


def problem1():
    #constant_map = {} # TODO: constant_map
    init = []
    terms = [('Integer', '?x1'), ('Large', '?x1'), ('Integer', '?x2'),
             ('minimize', ('Cost', '?x1')), ('minimize', ('Cost', '?x2'))]
    return SatisfactionProblem(STREAM_PDDL, STREAM_MAP, init, terms)
コード例 #30
0
ファイル: run.py プロジェクト: Khodeir/pddlstream
def pddlstream_from_problem(problem,
                            base_limits=None,
                            collisions=True,
                            teleport=False):
    robot = problem.robot

    domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
    stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
    constant_map = {
        '@sink': 'sink',
        '@stove': 'stove',
    }

    #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',), 1),
        Equal(('PlaceCost',), 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=True)  # TODO: supported here
        init += [('Graspable', body), ('Pose', body, pose),
                 ('AtPose', body, pose), ('Stackable', body, None)]
        for surface in problem.surfaces:
            if is_placement(body, surface):
                init += [('Supported', body, pose, surface)]
    for body, ty in problem.body_types:
        init += [('Type', body, ty)]

    bodies_from_type = get_bodies_from_type(problem)
    goal_literals = []
    if problem.goal_conf is not None:
        goal_conf = Conf(robot, get_group_joints(robot, 'base'),
                         problem.goal_conf)
        init += [('BConf', goal_conf)]
        goal_literals += [('AtBConf', goal_conf)]
    for ty, s in problem.goal_on:
        bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(
            ty) else [ty]
        init += [('Stackable', b, s) for b in bodies]
        goal_literals += [('On', ty, s)]
    goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \
                     [('Cleaned', b)  for b in problem.goal_cleaned] + \
                     [('Cooked', b)  for b in problem.goal_cooked]
    goal_formula = []
    for literal in goal_literals:
        parameters = [a for a in get_args(literal) if is_parameter(a)]
        if parameters:
            type_literals = [('Type', p, get_parameter_name(p))
                             for p in parameters]
            goal_formula.append(
                Exists(parameters, And(literal, *type_literals)))
        else:
            goal_formula.append(literal)
    goal_formula = And(*goal_formula)

    custom_limits = {}
    if base_limits is not None:
        custom_limits.update(get_custom_limits(robot, problem.base_limits))

    stream_map = {
        'sample-pose':
        from_gen_fn(get_stable_gen(problem, collisions=collisions)),
        'sample-grasp':
        from_list_fn(get_grasp_gen(problem, collisions=collisions)),
        #'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)),
        'inverse-kinematics':
        from_gen_fn(
            get_ik_ir_gen(problem,
                          custom_limits=custom_limits,
                          collisions=collisions,
                          teleport=teleport)),
        'plan-base-motion':
        from_fn(
            get_motion_gen(problem,
                           custom_limits=custom_limits,
                           collisions=collisions,
                           teleport=teleport)),
        'test-cfree-pose-pose':
        from_test(get_cfree_pose_pose_test(collisions=collisions)),
        'test-cfree-approach-pose':
        from_test(get_cfree_approach_pose_test(problem,
                                               collisions=collisions)),
        'test-cfree-traj-pose':
        from_test(get_cfree_traj_pose_test(robot, collisions=collisions)),
        #'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)),

        #'MoveCost': move_cost_fn,
        'Distance':
        distance_fn,
    }
    #stream_map = DEBUG

    return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
                       init, goal_formula)