def fix_pose(self, name, pose=None, fraction=0.5): body = self.get_body(name) if pose is None: pose = get_pose(body) else: set_pose(body, pose) # TODO: can also drop in simulation x, y, z = point_from_pose(pose) roll, pitch, yaw = euler_from_quat(quat_from_pose(pose)) quat = quat_from_euler(Euler(yaw=yaw)) set_quat(body, quat) surface_name = self.get_supporting(name) if surface_name is None: return None, None if fraction == 0: new_pose = (Point(x, y, z), quat) return new_pose, surface_name surface_aabb = compute_surface_aabb(self, surface_name) new_z = (1 - fraction) * z + fraction * stable_z_on_aabb( body, surface_aabb) point = Point(x, y, new_z) set_point(body, point) print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format( name, roll, pitch, new_z - z)) new_pose = (point, quat) return new_pose, surface_name
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE): pre_direction = pre_distance * get_unit_vector([0, 0, 1]) #half_extent = 1.0*FINGER_EXTENT[2] # Collides half_extent = 1.05 * FINGER_EXTENT[2] grasps = [] for link in get_link_subtree(world.kitchen, joint): if 'handle' in get_link_name(world.kitchen, link): # TODO: can adjust the position and orientation on the handle for yaw in [0, np.pi]: # yaw=0 DOESN'T WORK WITH LULA handle_grasp = (Point(z=-half_extent), quat_from_euler( Euler(roll=np.pi, pitch=np.pi / 2, yaw=yaw))) #if not pull: # handle_pose = get_link_pose(world.kitchen, link) # for distance in np.arange(0., 0.05, step=0.001): # pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp) # tool_pose = multiply(handle_pose, invert(pregrasp)) # set_tool_pose(world, tool_pose) # # TODO: check collisions # wait_for_user() handle_pregrasp = multiply((pre_direction, unit_quat()), handle_grasp) grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp)) return grasps
def test_push(visualize): arms = [LEFT_ARM] block_name = create_name('purpleblock', 1) # greenblock | purpleblock world, table_body = create_world([block_name], visualize=visualize) with ClientSaver(world.perception.client): block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON #pos = (0.6, 0, block_z) pos = (0.5, 0.2, block_z) world.perception.set_pose(block_name, pos, quat_from_euler(Euler(yaw=-np.pi/6))) update_world(world, table_body) # TODO: push into reachable region goal_pos2d = np.array(pos[:2]) + np.array([0.025, 0.1]) #goal_pos2d = np.array(pos[:2]) + np.array([0.025, -0.1]) #goal_pos2d = np.array(pos[:2]) + np.array([-0.1, -0.05]) #goal_pos2d = np.array(pos[:2]) + np.array([0.15, -0.05]) #goal_pos2d = np.array(pos[:2]) + np.array([0, 0.7]) # Intentionally not feasible draw_point(np.append(goal_pos2d, [block_z]), size=0.05, color=(1, 0, 0)) init = [ ('CanPush', block_name, goal_pos2d), ] goal = [ ('InRegion', block_name, goal_pos2d), ] task = Task(init=init, goal=goal, arms=arms) return world, task
def main(use_pr2_drake=True): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") table_path = "models/table_collision/table.urdf" table = load_pybullet(table_path, fixed_base=True) set_quat(table, quat_from_euler(Euler(yaw=PI / 2))) # table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf obstacles = [plane, table] pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) z = base_aligned_z(pr2) print(z) #z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3))) print(z) set_point(pr2, Point(z=z)) print(get_aabb(pr2)) wait_if_gui() base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) add_line(base_start, base_goal, color=RED) print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles) else: test_base_motion(pr2, base_start, base_goal, obstacles=obstacles) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) wait_if_gui('Finish?') disconnect()
def test_shelves(visualize): arms = [LEFT_ARM] # TODO: smaller (2) shelves # TODO: sample with respect to the link it will supported by # shelves.off | shelves_points.off | tableShelves.off # import os # data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/' # mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off')) # draw_mesh(mesh) # wait_for_interrupt() start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4 end_link = 'shelf1' shelf_name = 'two_shelves' #shelf_name = 'three_shelves' cup_name = create_name('bluecup', 1) world, table_body = create_world([shelf_name, cup_name], visualize=visualize) cup_x = 0.65 #shelf_x = 0.8 shelf_x = 0.75 initial_poses = { shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))), #cup_name: ([cup_x, 0.0, 0.0], unit_quat()), } with ClientSaver(world.perception.client): for name, pose in initial_poses.items(): point, quat = pose point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON world.perception.set_pose(name, point, quat) shelf_body = world.perception.sim_bodies[shelf_name] #print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)]) #print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])]) #shelf_link = None shelf_link = link_from_name(shelf_body, start_link) cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat()) update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5])) init = [ ('Stackable', cup_name, shelf_name, end_link), ] goal = [ ('On', cup_name, shelf_name, end_link), #('On', cup_name, TABLE_NAME, TOP), #('Holding', cup_name), ] task = Task(init=init, goal=goal, arms=arms) return world, task
def create_elements_bodies(node_points, elements, color=apply_alpha(RED, alpha=1), diameter=ELEMENT_DIAMETER, shrink=ELEMENT_SHRINK): # TODO: could scale the whole environment # TODO: create a version without shrinking for transit planning # URDF_USE_IMPLICIT_CYLINDER element_bodies = [] for (n1, n2) in elements: p1, p2 = node_points[n1], node_points[n2] height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0) #if height == 0: # Cannot keep this here # continue center = (p1 + p2) / 2 # extents = (p2 - p1) / 2 delta = p2 - p1 x, y, z = delta phi = np.math.atan2(y, x) theta = np.math.acos(z / np.linalg.norm(delta)) quat = quat_from_euler(Euler(pitch=theta, yaw=phi)) # p1 is z=-height/2, p2 is z=+height/2 # Much smaller than cylinder # Also faster, C_shape 177.398 vs 400 body = create_box(diameter, diameter, height, color=color, mass=STATIC_MASS) set_color(body, color) set_point(body, center) set_quat(body, quat) #dump_body(body, fixed=True) # Visually, smallest diameter is 2e-3 # The geometries and bounding boxes seem correct though # TODO: create_cylinder takes in a radius not diameter #body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS) #print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height)) #print(get_aabb_extent(get_aabb(body)).round(6).tolist()) #print(get_visual_data(body)) #print(get_collision_data(body)) #set_point(body, center) #set_quat(body, quat) #draw_aabb(get_aabb(body)) element_bodies.append(body) #wait_for_user() return element_bodies
def test_stir(visualize): # TODO: change the stirrer coordinate frame to be on its side arms = [LEFT_ARM] #spoon_name = 'spoon' # spoon.urdf is very messy and has a bad bounding box #spoon_quat = multiply_quats(quat_from_euler(Euler(pitch=-np.pi/2 - np.pi/16)), quat_from_euler(Euler(yaw=-np.pi/8))) #spoon_name, spoon_quat = 'stirrer', quat_from_euler(Euler(roll=-np.pi/2, yaw=np.pi/2)) spoon_name, spoon_quat = 'grey_spoon', quat_from_euler(Euler(yaw=-np.pi/2)) # grey_spoon | orange_spoon | green_spoon # *_spoon points in +y #bowl_name = 'bowl' bowl_name = 'whitebowl' items = [spoon_name, bowl_name] world, table_body = create_world(items, visualize=visualize) set_quat(world.get_body(spoon_name), spoon_quat) # get_reference_pose(spoon_name) initial_positions = { #spoon_name: [0.5, -0.2, 0], spoon_name: [0.3, 0.5, 0], bowl_name: [0.6, 0.1, 0], } with ClientSaver(world.perception.client): for name, point in initial_positions.items(): body = world.perception.sim_bodies[name] point[2] += stable_z(body, table_body) + Z_EPSILON world.perception.set_pose(name, point, get_quat(body)) #draw_aabb(get_aabb(body)) #wait_for_interrupt() #enable_gravity() #for i in range(10): # simulate_for_sim_duration(sim_duration=0.05, frequency=0.1) # print(get_quat(world.perception.sim_bodies[spoon_name])) # raw_input('Continue?') update_world(world, table_body) init_holding = hold_item(world, arms[0], spoon_name) assert init_holding is not None # TODO: add the concept of a recipe init = [ ('Contains', bowl_name, COFFEE), ('Contains', bowl_name, SUGAR), ] goal = [ #('Holding', spoon_name), ('Mixed', bowl_name), ] task = Task(init=init, init_holding=init_holding, goal=goal, arms=arms, reset_arms=False) return world, task
def test_scoop(visualize): # TODO: start with the spoon in a hand arms = [LEFT_ARM] spoon_name = 'grey_spoon' # green_spoon | grey_spoon | orange_spoon spoon_quat = quat_from_euler(Euler(yaw=-np.pi/2)) # *_spoon points in +y bowl1_name = 'whitebowl' bowl2_name = 'bowl' items = [spoon_name, bowl1_name, bowl2_name] world, table_body = create_world(items, visualize=visualize) set_quat(world.get_body(spoon_name), spoon_quat) initial_positions = { spoon_name: [0.3, 0.5, 0], bowl1_name: [0.5, 0.1, 0], bowl2_name: [0.6, 0.5, 0], } with ClientSaver(world.perception.client): for name, point in initial_positions.items(): body = world.perception.sim_bodies[name] point[2] += stable_z(body, table_body) + Z_EPSILON world.perception.set_pose(name, point, get_quat(body)) update_world(world, table_body) init = [ ('Contains', bowl1_name, COFFEE), #('Contains', spoon_name, WATER), ] goal = [ #('Contains', spoon_name, WATER), ('Contains', bowl2_name, COFFEE), ] task = Task(init=init, goal=goal, arms=arms, reset_arms=True) # TODO: plan while not spilling the spoon return world, task
} GRIPPER_LINKS = { 'left': 'l_gripper_palm_link', 'right': 'r_gripper_palm_link', } STABLE_QUATS = { # TODO(caelan): rename to placement orientation #'bluecup': quat_from_euler(Euler(roll=np.pi)), #'bowl': quat_from_euler(Euler(roll=np.pi)), #'spoon': (0.11923844791406242, -0.7234117212002504, -0.13249733589431234, 0.6670098426185677), } LIQUID_QUATS = { 'spoon': quat_from_euler(Euler(pitch=np.pi)), } STIR_QUATS = { 'spoon': quat_from_euler(Euler(roll=-np.pi / 2)), } # TODO: avoid using this COLORS = { 'red': (1, 0, 0), 'green': (0, 1, 0), 'blue': (0, 0, 1), 'black': (0, 0, 0), 'white': (1, 1, 1), 'purple': (1, 0, 1), 'orange': (1, 0.6, 0),
def gen(obj_name, surface_name): obj_body = world.get_body(obj_name) surface_body = world.kitchen if surface_name in ENV_SURFACES: surface_body = world.environment_bodies[surface_name] surface_aabb = compute_surface_aabb(world, surface_name) learned_poses = load_placements(world, surface_name) if learned else [ ] # TODO: GROW_PLACEMENT yaw_range = (-np.pi, np.pi) #if world.is_real(): # center = -np.pi/4 # half_extent = np.pi / 16 # yaw_range = (center-half_extent, center+half_extent) while True: for _ in range(max_attempts): if surface_name in STOVES: surface_link = link_from_name(world.kitchen, surface_name) world_from_surface = get_link_pose(world.kitchen, surface_link) z = stable_z_on_aabb( obj_body, surface_aabb) - point_from_pose(world_from_surface)[2] yaw = random.uniform(*yaw_range) body_pose_surface = Pose(Point(z=z + z_offset), Euler(yaw=yaw)) body_pose_world = multiply(world_from_surface, body_pose_surface) elif learned: if not learned_poses: return surface_pose_world = get_surface_reference_pose( surface_body, surface_name) sampled_pose_surface = multiply( surface_pose_world, random.choice(learned_poses)) [x, y, _] = point_from_pose(sampled_pose_surface) _, _, yaw = euler_from_quat( quat_from_pose(sampled_pose_surface)) dx, dy = np.random.normal( scale=pos_scale, size=2) if pos_scale else np.zeros(2) # TODO: avoid reloading z = stable_z_on_aabb(obj_body, surface_aabb) yaw = random.uniform(*yaw_range) #yaw = wrap_angle(yaw + np.random.normal(scale=rot_scale)) quat = quat_from_euler(Euler(yaw=yaw)) body_pose_world = ([x + dx, y + dy, z + z_offset], quat) # TODO: project onto the surface else: # TODO: halton sequence # unit_generator(d, use_halton=True) body_pose_world = sample_placement_on_aabb( obj_body, surface_aabb, epsilon=z_offset, percent=2.0) if body_pose_world is None: continue # return? if visibility and not is_visible_by_camera( world, point_from_pose(body_pose_world)): continue # TODO: make sure the surface is open when doing this robust = True if robust_radius != 0.: for theta in np.linspace(0, 5 * np.pi, num=8): x, y = robust_radius * unit_from_theta(theta) delta_body = Pose(Point(x, y)) delta_world = multiply(body_pose_world, delta_body) set_pose(obj_body, delta_world) if not test_supported(world, obj_body, surface_name, collisions=collisions): robust = False break set_pose(obj_body, body_pose_world) if robust and test_supported( world, obj_body, surface_name, collisions=collisions): rp = create_relative_pose(world, obj_name, surface_name) yield (rp, ) break else: yield None