def optimize_angle(robot, link, element_pose, translation, direction, reverse, initial_angles, collision_fn, max_error=1e-2): movable_joints = get_movable_joints(robot) initial_conf = get_joint_positions(robot, movable_joints) best_error, best_angle, best_conf = max_error, None, None for i, angle in enumerate(initial_angles): grasp_pose = get_grasp_pose(translation, direction, angle, reverse) target_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, target_pose) # if conf is None: # continue #if pairwise_collision(robot, robot): conf = get_joint_positions(robot, movable_joints) if not collision_fn(conf): link_pose = get_link_pose(robot, link) error = get_distance(point_from_pose(target_pose), point_from_pose(link_pose)) if error < best_error: # TODO: error a function of direction as well best_error, best_angle, best_conf = error, angle, conf # wait_for_interrupt() if i != len(initial_angles) - 1: set_joint_positions(robot, movable_joints, initial_conf) #print(best_error, translation, direction, best_angle) if best_conf is not None: set_joint_positions(robot, movable_joints, best_conf) #wait_for_interrupt() return best_angle, best_conf
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_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)) 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_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 main(precompute=False): parser = argparse.ArgumentParser() # djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi parser.add_argument('-p', '--problem', default='simple_frame', help='The name of the problem to solve') parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions with obstacles') parser.add_argument('-m', '--motions', action='store_true', help='Plans motions between each extrusion') parser.add_argument('-v', '--viewer', action='store_true', help='Enables the viewer during planning (slow!)') args = parser.parse_args() print('Arguments:', args) # TODO: setCollisionFilterGroupMask # TODO: fail if wild stream produces unexpected facts # TODO: try search at different cost levels (i.e. w/ and w/o abstract) # TODO: submodule in https://github.mit.edu/yijiangh/assembly-instances elements, node_points, ground_nodes = load_extrusion(args.problem) node_order = list(range(len(node_points))) #np.random.shuffle(node_order) node_order = sorted(node_order, key=lambda n: node_points[n][2]) elements = sorted(elements, key=lambda e: min(node_points[n][2] for n in e)) #node_order = node_order[:100] ground_nodes = [n for n in ground_nodes if n in node_order] elements = [ element for element in elements if all(n in node_order for n in element) ] #plan = plan_sequence_test(node_points, elements, ground_nodes) connect(use_gui=args.viewer) floor, robot = load_world() obstacles = [floor] initial_conf = get_joint_positions(robot, get_movable_joints(robot)) #dump_body(robot) #if has_gui(): # draw_model(elements, node_points, ground_nodes) # wait_for_interrupt('Continue?') #joint_weights = compute_joint_weights(robot, num=1000) #elements = elements[:50] # 10 | 50 | 100 | 150 #debug_elements(robot, node_points, node_order, elements) element_bodies = dict(zip(elements, create_elements(node_points, elements))) if precompute: pr = cProfile.Profile() pr.enable() trajectories = sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) user_input('Continue?') else: trajectories = [] plan = plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories, collisions=not args.cfree) if args.motions: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan) disconnect() display_trajectories(ground_nodes, plan)
def main(viewer=True): # TODO: setCollisionFilterGroupMask # TODO: only produce collisions rather than collision-free # TODO: return collisions using wild-stream functionality # TODO: handle movements between selected edges # TODO: fail if wild stream produces unexpected facts # TODO: try search at different cost levels (i.e. w/ and w/o abstract) # https://github.mit.edu/yijiangh/assembly-instances #read_minizinc_data(os.path.join(root_directory, 'print_data.txt')) #return # djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi elements, node_points, ground_nodes = load_extrusion('voronoi') node_order = list(range(len(node_points))) #np.random.shuffle(node_order) node_order = sorted(node_order, key=lambda n: node_points[n][2]) elements = sorted(elements, key=lambda e: min(node_points[n][2] for n in e)) #node_order = node_order[:100] ground_nodes = [n for n in ground_nodes if n in node_order] elements = [ element for element in elements if all(n in node_order for n in element) ] print('Nodes: {} | Ground: {} | Elements: {}'.format( len(node_points), len(ground_nodes), len(elements))) #plan = plan_sequence_test(node_points, elements, ground_nodes) connect(use_gui=viewer) floor, robot = load_world() obstacles = [floor] initial_conf = get_joint_positions(robot, get_movable_joints(robot)) #dump_body(robot) #if has_gui(): # draw_model(elements, node_points, ground_nodes) # wait_for_interrupt('Continue?') #joint_weights = compute_joint_weights(robot, num=1000) #elements = elements[:5] #elements = elements[:10] #elements = elements[:25] #elements = elements[:35] #elements = elements[:50] #elements = elements[:75] #elements = elements[:100] #elements = elements[:150] #elements = elements[150:] # TODO: prune if it collides with any of its supports # TODO: prioritize choices that don't collide with too many edges # TODO: accumulate edges along trajectory #test_grasps(robot, node_points, elements) #test_print(robot, node_points, elements) #return #for element in elements: # color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0, 0) # draw_element(node_points, element, color=color) #wait_for_interrupt('Continue?') # TODO: topological sort #node = node_order[40] #node_neighbors = get_node_neighbors(elements) #for element in node_neighbors[node]: # color = (0, 1, 0) if element_supports(element, node, node_points) else (1, 0, 0) # draw_element(node_points, element, color) #element = elements[-1] #draw_element(node_points, element, (0, 1, 0)) #incoming_edges, _ = neighbors_from_orders(get_supported_orders(elements, node_points)) #supporters = [] #retrace_supporters(element, incoming_edges, supporters) #for e in supporters: # draw_element(node_points, e, (1, 0, 0)) #wait_for_interrupt('Continue?') #for name, args in plan: # n1, e, n2 = args # draw_element(node_points, e) # user_input('Continue?') #test_ik(robot, node_order, node_points) element_bodies = dict(zip(elements, create_elements(node_points, elements))) precompute = False if precompute: pr = cProfile.Profile() pr.enable() trajectories = sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) user_input('Continue?') else: trajectories = [] motions = False plan = plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories) if motions: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan) disconnect() display_trajectories(ground_nodes, plan)
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 get_base_conf(robot): return get_joint_positions(robot, get_base_joints(robot))