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_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(robot, movable=[], teleport=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} print('Robot:', robot) conf = BodyConf(robot, get_configuration(robot)) init = [('CanMove',), ('Conf', conf), ('AtConf', conf), ('HandEmpty',)] fixed = get_fixed(robot, movable) print('Movable:', movable) print('Fixed:', fixed) for body in movable: pose = BodyPose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose)] for surface in fixed: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: init += [('Sink', body)] if 'stove' in name: init += [('Stove', body)] body = movable[0] goal = ('and', ('AtConf', conf), #('Holding', body), #('On', body, fixed[1]), #('On', body, fixed[2]), #('Cleaned', body), ('Cooked', body), ) stream_map = { 'sample-pose': from_gen_fn(get_stable_gen(fixed)), 'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)), 'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)), 'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)), 'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)), 'TrajCollision': get_movable_collision_test(), } if USE_SYNTHESIZERS: stream_map.update({ 'plan-free-motion': empty_gen(), 'plan-holding-motion': empty_gen(), }) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def test_print(robot, node_points, elements): #elements = [elements[0]] elements = [elements[-1]] [element_body] = create_elements(node_points, elements) wait_for_interrupt() phi = 0 #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] grasp_rotations = [sample_direction() for _ in range(25)] link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for grasp_rotation in grasp_rotations: n1, n2 = elements[0] length = np.linalg.norm(node_points[n2] - node_points[n1]) set_joint_positions(robot, movable_joints, sample_fn()) for t in np.linspace(-length / 2, length / 2, 10): #element_translation = Pose(point=Point(z=-t)) #grasp_pose = multiply(grasp_rotation, element_translation) #reverse = Pose(euler=Euler()) reverse = Pose(euler=Euler(roll=np.pi)) grasp_pose = get_grasp_pose(t, grasp_rotation, 0) grasp_pose = multiply(grasp_pose, reverse) element_pose = get_pose(element_body) link_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, link_pose) wait_for_interrupt()
def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) # _, upper = robot_aabb # radius = upper[0] extent = get_aabb_extent(robot_aabb) radius = max(extent[:2]) / 2. #draw_aabb(robot_aabb) center, (diameter, height) = approximate_as_cylinder(body) distance = radius + diameter / 2. + epsilon _, _, z = get_point(body) # Assuming already placed stably for [scale] in unit_generator(d=1, use_halton=use_halton): #theta = PI # 0 | PI theta = random.uniform(*theta_interval) position = np.append(distance * unit_from_theta(theta=theta), [z]) # TODO: halton yaw = scale * 2 * PI - PI quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose if draw: world_pose = multiply(get_link_pose(robot, link), grasp.value) set_pose(body, world_pose) handles = draw_pose(get_pose(body), length=1) wait_for_user() remove_handles(handles) #continue yield (grasp, )
def plan_commands(state, viewer=False, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world sim_world = connect(use_gui=viewer) #clone_world(client=sim_world) task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) mapping = clone_world(client=sim_world, exclude=[task.robot]) assert all(i1 == i2 for i1, i2 in mapping.items()) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, hierarchy=hierarchy, debug=False, success_cost=MAX_COST, verbose=verbose) plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) commands = post_process(state, plan) pr.disable() if profile: pstats.Stats(pr).sort_stats('cumtime').print_stats(10) saved_world.restore() disconnect() return commands
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF #robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z)) wait_for_user() block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
def extract_point2d(v): if isinstance(v, Conf): return v.values[:2] if isinstance(v, Pose): return point_from_pose(v.value)[:2] if isinstance(v, SharedOptValue): if v.stream == 'sample-pose': r, = v.values return point_from_pose(get_pose(r))[:2] if v.stream == 'inverse-kinematics': p, = v.values return extract_point2d(p) if isinstance(v, CustomValue): if v.stream == 'p-sp': r, = v.values return point_from_pose(get_pose(r))[:2] if v.stream == 'q-ik': p, = v.values return extract_point2d(p) raise ValueError(v.stream)
def extract_point2d(v): if isinstance(v, Conf): return v.values[:2] if isinstance(v, Pose): return point_from_pose(v.value)[:2] if v.stream == 'p-sp': r, = v.inputs return point_from_pose(get_pose(r))[:2] if v.stream == 'q-ik': p, = v.inputs return extract_point2d(p) return ValueError(v.stream)
def plan_commands(state, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) sim_world = connect(use_gui=False) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) #robot = load_model(DRAKE_PR2_URDF, fixed_base=True) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) clone_world(client=sim_world, exclude=[task.robot]) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_cost=MAX_COST, verbose=verbose) pr.disable() plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) print('Real cost:', float(cost) / SCALE_COST) if profile: pstats.Stats(pr).sort_stats('tottime').print_stats(10) saved_world.restore() commands = post_process(state, plan) disconnect() return commands
def compute_direction_path(robot, length, reverse, element_body, direction, collision_fn): step_size = 0.0025 # 0.005 #angle_step_size = np.pi / 128 angle_step_size = np.math.radians(0.25) angle_deltas = [-angle_step_size, 0, angle_step_size] #num_initial = 12 num_initial = 1 steps = np.append(np.arange(-length / 2, length / 2, step_size), [length / 2]) #print('Length: {} | Steps: {}'.format(length, len(steps))) #initial_angles = [wrap_angle(angle) for angle in np.linspace(0, 2*np.pi, num_initial, endpoint=False)] initial_angles = [ wrap_angle(angle) for angle in np.random.uniform(0, 2 * np.pi, num_initial) ] movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) set_joint_positions(robot, movable_joints, sample_fn()) link = link_from_name(robot, TOOL_NAME) element_pose = get_pose(element_body) current_angle, current_conf = optimize_angle(robot, link, element_pose, steps[0], direction, reverse, initial_angles, collision_fn) if current_conf is None: return None # TODO: constrain maximum conf displacement # TODO: alternating minimization for just position and also orientation trajectory = [current_conf] for translation in steps[1:]: #set_joint_positions(robot, movable_joints, current_conf) initial_angles = [ wrap_angle(current_angle + delta) for delta in angle_deltas ] current_angle, current_conf = optimize_angle(robot, link, element_pose, translation, direction, reverse, initial_angles, collision_fn) if current_conf is None: return None trajectory.append(current_conf) return trajectory
def load_pick_and_place(extrusion_name, scale=MILLIMETER, max_bricks=6): assert extrusion_name == 'choreo_brick_demo' root_directory = os.path.dirname(os.path.abspath(__file__)) bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY, 'bricks') print('Name: {}'.format(extrusion_name)) with open( os.path.join(bricks_directory, PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f: json_data = json.loads(f.read()) kuka_urdf = '../models/framefab_kr6_r900_support/urdf/kr6_r900_mit_suction_gripper.urdf' obj_directory = os.path.join(bricks_directory, 'meshes', 'collision') with HideOutput(): #world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf')) robot = load_pybullet(os.path.join(root_directory, kuka_urdf), fixed_base=True) #set_point(robot, (0.14, 0, 0)) #dump_body(robot) pick_base_point = parse_point(json_data['pick_base_center_point']) draw_pose((pick_base_point, unit_quat())) place_base_point = parse_point(json_data['place_base_center_point']) draw_pose((place_base_point, unit_quat())) # workspace_geo_place_support_object_11 = pick_left_over_bricks_11 obstacle_from_name = {} for filename in json_data['pick_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1)) for filename in json_data['place_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1)) brick_from_index = {} for json_element in json_data['sequenced_elements']: index = json_element['order_id'] pick_body = create_obj(os.path.join( obj_directory, json_element['pick_element_geometry_file_name']), scale=scale, color=(0, 0, 1, 1)) add_body_name(pick_body, index) #place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']), # scale=scale, color=(0, 1, 0, 1)) #add_body_name(place_body, name) world_from_obj_pick = get_pose(pick_body) # [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane', # u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane'] ee_grasp_poses = [{ name: pose_from_tform(parse_transform(approach)) for name, approach in json_grasp.items() } for json_grasp in json_element['grasps']] # pick_grasp_plane is at the top of the object with z facing downwards grasp_index = 0 world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane'] world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane'] #draw_pose(world_from_ee_pick, length=0.04) #draw_pose(world_from_ee_place, length=0.04) ee_from_obj = multiply(invert(world_from_ee_pick), world_from_obj_pick) # Using pick frame world_from_obj_place = multiply(world_from_ee_place, ee_from_obj) #set_pose(pick_body, world_from_obj_place) grasps = [ Grasp( index, num, *[ multiply(invert(world_from_ee_pick[name]), world_from_obj_pick) for name in GRASP_NAMES ]) for num, world_from_ee_pick in enumerate(ee_grasp_poses) ] brick_from_index[index] = Brick( index=index, body=pick_body, initial_pose=WorldPose(index, world_from_obj_pick), goal_pose=WorldPose(index, world_from_obj_place), grasps=grasps, goal_supports=json_element.get('place_contact_ngh_ids', [])) # pick_contact_ngh_ids are movable element contact partial orders # pick_support_surface_file_names are fixed element contact partial orders return robot, brick_from_index, obstacle_from_name
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 __init__(self, body, start, end): self.body = body self.start = start self.end = end self.pose = get_pose(self.body) self.visual_data = get_visual_data(self.body)
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path( WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z + 1), target_point=Point(z=block_z)) block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi / 2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()