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
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_push.pddl')) stream_pddl = read(os.path.join(directory, 'stream_push.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', p) for p in known_poses] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('GoalPose', p) for p in tamp_problem.goal_poses.values()] goal = And(('AtConf', initial.conf), *[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()]) # TODO: convert to lower case stream_map = { 'push-target': from_list_fn(push_target_fn), 'push-direction': push_direction_gen_fn, 'test-cfree': from_test(lambda *args: not collision_test(*args)), 'distance': distance_fn, } return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream_info(robot, fixed, movable, add_slanted_grasps, approach_frame, use_vision, home_poses=None): domain_pddl = read('tamp/domain_stacking.pddl') stream_pddl = read('tamp/stream_stacking.pddl') constant_map = {} fixed = [f for f in fixed if f is not None] stream_map = { 'sample-pose-table': from_list_fn(primitives.get_stable_gen_table(fixed)), 'sample-pose-home': from_list_fn(primitives.get_stable_gen_home(home_poses, fixed)), 'sample-pose-block': from_fn(primitives.get_stable_gen_block(fixed)), 'sample-grasp': from_list_fn( primitives.get_grasp_gen(robot, add_slanted_grasps=True, add_orthogonal_grasps=False)), # 'sample-grasp': from_gen_fn(primitives.get_grasp_gen(robot, add_slanted_grasps=True, add_orthogonal_grasps=False)), 'pick-inverse-kinematics': from_fn( primitives.get_ik_fn(robot, fixed, approach_frame='gripper', backoff_frame='global', use_wrist_camera=use_vision)), 'place-inverse-kinematics': from_fn( primitives.get_ik_fn(robot, fixed, approach_frame='global', backoff_frame='gripper', use_wrist_camera=False)), '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
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)
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 = { 'connect': from_list_fn(get_connect(tamp_problem.initial)), 't-cfree': from_test(test_cfree), '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)
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)
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
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)
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)
def create_optimizer(min_take=0, max_take=INF, max_wallet=INF, max_time=5, integer=True, diagnose=True, verbose=True): # https://www.gurobi.com/documentation/8.1/examples/diagnose_and_cope_with_inf.html # https://www.gurobi.com/documentation/8.1/examples/tsp_py.html#subsubsection:tsp.py # https://examples.xpress.fico.com/example.pl?id=Infeasible_python # https://www.fico.com/fico-xpress-optimization/docs/latest/examples/python/GUID-E77AC35C-9488-3F0A-98B4-7F5FD81AFF1D.html if not has_gurobi(): raise ImportError( 'This generator requires Gurobi: http://www.gurobi.com/') from gurobipy import Model, GRB, quicksum, GurobiError def fn(outputs, facts, hint={}): print(outputs, facts) model = Model(name='TAMP') model.setParam(GRB.Param.OutputFlag, verbose) if max_time < INF: model.setParam(GRB.Param.TimeLimit, max_time) var_from_param = {} for fact in facts: prefix, args = fact[0], fact[1:] if prefix in ['wcash', 'pcash', 'mcash']: cash, = args if is_parameter(cash): # TODO: scale by 100 for cents var_from_param[cash] = model.addVar( lb=0, ub=GRB.INFINITY, vtype=GRB.INTEGER if integer else GRB.CONTINUOUS) if prefix == 'wcash': model.addConstr(var_from_param[cash] >= min_take) if max_take < INF: model.addConstr(var_from_param[cash] <= max_take) if (prefix == 'pcash') and (max_wallet < INF): model.addConstr(var_from_param[cash] <= max_wallet) if prefix == 'mcash': # min_balance >= 0 pass get_var = lambda p: var_from_param[p] if is_parameter( p) else p # var_from_param.get(p, p) objective_terms = [] for index, fact in enumerate(facts): name = str(index) if fact[0] == MINIMIZE: fact = fact[1] func, args = fact[0], map(get_var, fact[1:]) if func == 'withdrawcost': cash, = args objective_terms.append(cash) elif fact[0] == NOT: fact = fact[1] predicate, args = fact[0], map(get_var, fact[1:]) else: prefix, args = fact[0], map(get_var, fact[1:]) if prefix == 'ge': cash1, cash2 = args model.addConstr(cash1 >= cash2, name=name) elif prefix == 'withdraw': wcash, pcash1, pcash2, mcash1, mcash2 = args model.addConstr(pcash1 + wcash == pcash2, name=name) model.addConstr(mcash1 - wcash == mcash2, name=name) model.setObjective(quicksum(objective_terms), sense=GRB.MINIMIZE) try: model.optimize() except GurobiError as e: raise e objective = 0 if objective_terms: objective = INF if model.status == GRB.INFEASIBLE else model.objVal print('Objective: {:.3f} | Solutions: {} | Status: {}'.format( objective, model.solCount, model.status)) # https://www.gurobi.com/documentation/9.0/refman/optimization_status_codes.html if not model.solCount: # GRB.INFEASIBLE | GRB.INF_OR_UNBD | OPTIMAL | SUBOPTIMAL | UNBOUNDED return OptimizerOutput() assignment = tuple(get_var(out).x for out in outputs) return OptimizerOutput(assignments=[assignment]) return from_list_fn(fn)