def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs): use_width = world.robot_name == FRANKA_CARTER body = world.get_body(name) #fraction = 0.25 obj_type = type_from_name(name) body_pose = REFERENCE_POSE.get(obj_type, unit_pose()) center, extent = approximate_as_prism(body, body_pose) for grasp_type in grasp_types: if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)): continue #assert is_valid_grasp_type(name, grasp_type) if grasp_type == TOP_GRASP: grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5 pre_direction = pre_distance * get_unit_vector([0, 0, 1]) post_direction = unit_point() generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose, grasp_length=grasp_length, max_width=np.inf, **kwargs) elif grasp_type == SIDE_GRASP: # Take max of height and something grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little x, z = pre_distance * get_unit_vector([3, -1]) pre_direction = [0, 0, x] post_direction = [0, 0, z] top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0] # Under grasps are actually easier for this robot # TODO: bug in under in that it grasps at the bottom generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose, grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs) # if world.robot_name == FRANKA_CARTER else unit_pose() generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp) for grasp in generator for yaw in [0, np.pi]) else: raise ValueError(grasp_type) grasp_poses = randomize(list(generator)) if obj_type in CYLINDERS: # TODO: filter first grasp_poses = (multiply(grasp_pose, Pose(euler=Euler( yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses)) for i, grasp_pose in enumerate(grasp_poses): pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose, Pose(point=post_direction)) grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose) with BodySaver(body): grasp.get_attachment().assign() with BodySaver(world.robot): grasp.grasp_width = close_until_collision( world.robot, world.gripper_joints, bodies=[body]) #print(get_joint_positions(world.robot, world.arm_joints)[-1]) #draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link) #grasp.get_attachment().assign() #wait_for_user() ##for value in get_joint_limits(world.robot, world.arm_joints[-1]): #for value in [-1.8973, 0, +1.8973]: # set_joint_position(world.robot, world.arm_joints[-1], value) # grasp.get_attachment().assign() # wait_for_user() if use_width and (grasp.grasp_width is None): continue yield grasp
def test_grasps(robot, node_points, elements): element = elements[-1] [element_body] = create_elements(node_points, [element]) phi = 0 link = link_from_name(robot, TOOL_NAME) link_pose = get_link_pose(robot, link) draw_pose(link_pose) #, parent=robot, parent_link=link) wait_for_user() n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] length = np.linalg.norm(p2 - p1) # Bottom of cylinder is p1, top is p2 print(element, p1, p2) for phi in np.linspace(0, np.pi, 10, endpoint=True): theta = np.pi / 4 for t in np.linspace(-length / 2, length / 2, 10): translation = Pose( point=Point(z=-t) ) # Want to be moving backwards because +z is out end effector direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi)) angle = Pose(euler=Euler(yaw=1.2)) grasp_pose = multiply(angle, direction, translation) element_pose = multiply(link_pose, grasp_pose) set_pose(element_body, element_pose) wait_for_user()
def sample_push_contact(world, feature, parameter, under=False): robot = world.robot arm = feature['arm_name'] body = world.get_body(feature['block_name']) push_yaw = feature['push_yaw'] center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw))) max_backoff = width + 0.1 # TODO: add gripper bounding box tool_link = link_from_name(robot, TOOL_FRAMES[arm]) tool_pose = get_link_pose(robot, tool_link) gripper_link = link_from_name(robot, GRIPPER_LINKS[arm]) collision_links = get_link_subtree(robot, gripper_link) urdf_from_center = Pose(point=center) reverse_z = Pose(euler=Euler(pitch=math.pi)) rotate_theta = Pose(euler=Euler(yaw=push_yaw)) #translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt'])) grasps = [] for i in range(1 + under): flip_gripper = Pose(euler=Euler(yaw=i * math.pi)) for x in np.arange(0, max_backoff, step=0.01): translate_x = Pose(point=Point(x=-x)) grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta, reverse_z, invert(urdf_from_center)) set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose)) if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.): grasps.append(grasp_pose) break return grasps
def get_spoon_grasps(name, body, under=False, grasp_length=0.02): # TODO: scale thickness based on size thickness = SPOON_THICKNESSES[get_type(name)] # Origin is in the center of the spoon's hemisphere head # grasp_length depends on the grasp position center, extent = approximate_as_prism(body) for k in range(1 + under): rotate_y = Pose(euler=Euler(pitch=-np.pi / 2 + np.pi * k)) rotate_z = Pose(euler=Euler(yaw=-np.pi / 2)) length = (-center + extent / 2)[1] - grasp_length translate_x = Pose(point=Point(x=length, y=-thickness / 2.)) gripper_from_spoon = multiply(translate_x, rotate_z, rotate_y) yield gripper_from_spoon
def observe_pybullet(world): # TODO: randomize robot's pose # TODO: probabilities based on whether in viewcone or not # TODO: sample from poses on table # world_saver = WorldSaver() visible_entities = are_visible(world) detections = {} assert OBS_P_FP == 0 for name in visible_entities: # TODO: false positives if random.random() < OBS_P_FN: continue body = world.get_body(name) pose = get_pose(body) dx, dy = np.random.multivariate_normal(mean=np.zeros(2), cov=math.pow(OBS_POS_STD, 2) * np.eye(2)) dyaw, = np.random.multivariate_normal(mean=np.zeros(1), cov=math.pow(OBS_ORI_STD, 2) * np.eye(1)) print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format( name, dx, dy, dyaw)) noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw)) observed_pose = multiply(pose, noise_pose) #world.get_body_type(name) detections.setdefault(name, []).append( observed_pose) # TODO: use type instead #world_saver.restore() return detections
def relative_detections(belief, detections): world = belief.world rel_detections = {} world_aabb = world.get_world_aabb() for name in detections: if name == belief.holding: continue body = world.get_body(name) for observed_pose in detections[name]: world_z_axis = np.array([0, 0, 1]) local_z_axis = tform_point(observed_pose, world_z_axis) if np.pi / 2 < angle_between(world_z_axis, local_z_axis): observed_pose = multiply(observed_pose, Pose(euler=Euler(roll=np.pi))) if not aabb_contains_point(point_from_pose(observed_pose), world_aabb): continue set_pose(body, observed_pose) support = world.get_supporting(name) #assert support is not None # Could also fix as relative to the world if support is None: # TODO: prune if nowhere near a surface (e.g. on the robot) relative_pose = create_world_pose(world, name, init=True) else: relative_pose = create_relative_pose(world, name, support, init=True) rel_detections.setdefault(name, []).append(relative_pose) # relative_pose.assign() return rel_detections
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 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 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 sample_stir_trajectory(world, feature, parameter): bowl_urdf_from_center = get_urdf_from_base( world.bodies[feature['bowl_name']]) stirrer_quat = lookup_orientation(feature['spoon_name'], STIR_QUATS) stirrer_urdf_from_center = get_urdf_from_base( world.bodies[feature['spoon_name']], reference_quat=stirrer_quat) stir_pos = np.array([0., 0., parameter['stir_z']]) entry_pos = np.array([0, 0, parameter['entry_z']]) # TODO: could rotate the wrist in place # TODO: could reverse the trajectory afterwards # TODO: could connect up the start and end step_size = np.pi / 16 stir_positions = [ stir_pos + parameter['stir_radius'] * np.array([math.cos(theta), math.sin(theta), 0]) for theta in np.arange(0, parameter['revolutions'] * 2 * np.pi, step_size) ] # Counter-clockwise entry_pos = stir_positions[0] + (entry_pos - stir_pos) exit_pos = stir_positions[-1] + (entry_pos - stir_pos) initial_yaw = random.uniform(-np.pi, np.pi) rotate_stirrer = Pose(euler=Euler(yaw=initial_yaw)) # TODO(caelan): check the ordering/inversion when references are not the identity return [ multiply(bowl_urdf_from_center, Pose(point=point), rotate_stirrer, invert(stirrer_urdf_from_center)) for point in ([entry_pos] + stir_positions + [exit_pos]) ]
def test_print(robot, node_points, elements): #elements = [elements[0]] elements = [elements[-1]] [element_body] = create_elements(node_points, elements) wait_for_user() 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_user()
def mirror_robot(robot1, node_points): # TODO: place robots side by side or diagonal across set_extrusion_camera(node_points, theta=-np.pi / 3) #draw_pose(Pose()) centroid = np.average(node_points, axis=0) centroid_pose = Pose(point=centroid) #draw_pose(Pose(point=centroid)) # print(centroid) scale = 0. # 0.15 vector = get_point(robot1) - centroid set_pose(robot1, Pose(point=Point(*+scale * vector[:2]))) # Inner product of end-effector z with base->centroid or perpendicular to this line # Partition by sides robot2 = load_robot() set_pose( robot2, Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi))) # robots = [robot1] robots = [robot1, robot2] for robot in robots: set_configuration(robot, DUAL_CONF) # joint1 = get_movable_joints(robot)[0] # set_joint_position(robot, joint1, np.pi / 8) draw_pose(get_pose(robot), length=0.25) return robots
def __init__(self, end_effector, element_bodies, node_points, ground_nodes, element, reverse, max_directions=INF): # TODO: more directions for ground collisions self.end_effector = end_effector self.element = element self.reverse = reverse self.max_directions = max_directions self.element_pose = get_pose(element_bodies[element]) n1, n2 = element length = get_distance(node_points[n1], node_points[n2]) self.translation_path = np.append( np.arange(-length / 2, length / 2, STEP_SIZE), [length / 2]) if ORTHOGONAL_GROUND and is_ground(element, ground_nodes): # TODO: orthogonal to the ground self.direction_generator = cycle( [Pose(euler=Euler(roll=0, pitch=0))]) else: self.direction_generator = get_direction_generator( self.end_effector, use_halton=False) self.trajectories = []
def pose2d_on_surface(world, entity_name, surface_name, pose2d=UNIT_POSE2D): x, y, yaw = pose2d body = world.get_body(entity_name) surface_aabb = compute_surface_aabb(world, surface_name) z = stable_z_on_aabb(body, surface_aabb) pose = Pose(Point(x, y, z), Euler(yaw=yaw)) set_pose(body, pose) return pose
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()
def gen_fn(index): brick = brick_from_index[index] while True: original_grasp = random.choice(brick.grasps) theta = random.uniform(-np.pi, +np.pi) rotation = Pose(euler=Euler(yaw=theta)) new_attach = multiply(rotation, original_grasp.attach) grasp = Grasp(None, None, None, new_attach, None) yield grasp,
def rotate_assembly(elements, node_points, ground_nodes, yaw=0.): # TODO: more general affine transformations world_from_base = Pose(point=get_base_centroid(node_points, ground_nodes)) points_base = apply_affine(invert(world_from_base), node_points) rotation = Pose(euler=Euler(yaw=yaw)) points_world = list(map(np.array, apply_affine(multiply(world_from_base, rotation), points_base))) #draw_model(elements, scaled_node_points, ground_nodes, color=RED) #wait_if_gui() return points_world
def rotate_problem(problem_path, roll=np.pi): tform = Pose(euler=Euler(roll=roll)) json_data = affine_extrusion(problem_path, tform) path = 'rotated.json' # TODO: create folder with open(path, 'w') as f: json.dump(json_data, f, indent=2, sort_keys=True) problem_path = path # TODO: rotate the whole robot as well # TODO: could also use the z heuristic when upside down return problem_path
def pour_path_from_parameter(world, bowl_name, cup_name): bowl_body = world.get_body(bowl_name) bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body) cup_body = world.get_body(cup_name) cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body) ##### obj_type = type_from_name(cup_name) if obj_type in [MUSTARD]: initial_pitch = final_pitch = -np.pi radius = 0 else: initial_pitch = 0 # different if mustard final_pitch = -3 * np.pi / 4 radius = bowl_d / 2 #axis_in_cup_center_x = -0.05 axis_in_cup_center_x = 0 # meters #axis_in_cup_center_z = -cup_h/2. axis_in_cup_center_z = 0. # meters #axis_in_cup_center_z = +cup_h/2. # tl := top left | tr := top right cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2]) cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z) cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0]) cup_tl_pour_pitch = final_pitch - cup_tl_angle cup_radius2d = np.linalg.norm([cup_tl_in_axis]) pivot_in_bowl_tr = Point( x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01), z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET)) pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr base_from_pivot = Pose( Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z)) ##### assert -np.pi <= final_pitch <= initial_pitch pitches = [initial_pitch] if final_pitch != initial_pitch: pitches = list(np.arange(final_pitch, initial_pitch, np.pi / 16)) + pitches cup_path_in_bowl = [] for pitch in pitches: rotate_pivot = Pose( euler=Euler(pitch=pitch) ) # Can also interpolate directly between start and end quat cup_path_in_bowl.append( multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center), rotate_pivot, invert(base_from_pivot), invert(Pose(point=cup_center)))) return cup_path_in_bowl
def gen_fn(arm, bowl_name, bowl_pose, stirrer_name, grasp): if bowl_name == stirrer_name: return bowl_body = world.bodies[bowl_name] attachment = get_grasp_attachment(world, arm, grasp) feature = get_stir_feature(world, bowl_name, stirrer_name) parameter_generator = parameter_fn(world, feature) if revisit: parameter_generator = cycle(parameter_generator) for parameter in parameter_generator: for _ in range(max_attempts): set_gripper_position(world.robot, arm, grasp.grasp_width) set_pose(bowl_body, bowl_pose) stirrer_path_bowl = sample_stir_trajectory( world, feature, parameter) rotate_bowl = Pose(euler=Euler( yaw=random.uniform(-np.pi, np.pi))) stirrer_path = [ multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl) for cup_pose_bowl in stirrer_path_bowl ] #visualize_cartesian_path(world.get_body(stirrer_name), stirrer_path) arm_path = plan_attachment_motion( world.robot, arm, attachment, stirrer_path, obstacles=set(obstacles) - {bowl_body}, self_collisions=collisions, disabled_collisions=disabled_collisions, collision_buffer=collision_buffer, attachment_collisions=False) if arm_path is None: continue pre_conf = Conf(arm_path[0]) post_conf = Conf(arm_path[-1]) control = Control({ 'action': 'stir', 'objects': [bowl_name, stirrer_name], 'feature': feature, 'parameter': parameter, 'context': Context( savers=[BodySaver(world.robot) ], # TODO: robot might be at the wrong conf attachments={stirrer_name: attachment}), 'commands': [ ArmTrajectory(arm, arm_path), ], }) yield (pre_conf, post_conf, control)
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 pour_path_from_parameter(world, feature, parameter, cup_yaw=None): cup_body = world.get_body(feature['cup_name']) #cup_urdf_from_center = get_urdf_from_center(cup_body, reference_quat=get_liquid_quat(feature['cup_name'])) ref_from_urdf = (unit_point(), get_liquid_quat(feature['cup_name'])) cup_center_in_ref, _ = approximate_as_prism(cup_body, body_pose=ref_from_urdf) cup_center_in_ref[: 2] = 0 # Assumes the xy pour center is specified by the URDF (e.g. for spoons) cup_urdf_from_center = multiply(invert(ref_from_urdf), Pose(point=cup_center_in_ref)) # TODO: allow some deviation around cup_yaw for spoons if cup_yaw is None: cup_yaw = random.choice([0, np.pi]) if 'spoon' in feature['cup_name'] \ else random.uniform(-np.pi, np.pi) z_rotate_cup = Pose(euler=Euler(yaw=cup_yaw)) bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter) if RELATIVE_POUR: parameter = scale_parameter(feature, parameter, RELATIVE_POUR_SCALING, descale=True) base_from_pivot = Pose( Point(x=parameter['axis_in_cup_x'], z=parameter['axis_in_cup_z'])) initial_pitch = 0 final_pitch = parameter['pitch'] assert -np.pi <= final_pitch <= initial_pitch cup_path_in_bowl = [] for pitch in list(np.arange(final_pitch, initial_pitch, np.pi / 16)) + [initial_pitch]: rotate_pivot = Pose( euler=Euler(pitch=pitch) ) # Can also interpolate directly between start and end quat cup_path_in_bowl.append( multiply(bowl_from_pivot, rotate_pivot, invert(base_from_pivot), z_rotate_cup, invert(cup_urdf_from_center))) cup_times = constant_velocity_times(cup_path_in_bowl) # TODO: check for collisions here? return cup_path_in_bowl, cup_times
def get_direction_generator(end_effector, **kwargs): world_from_base = get_pose(end_effector.robot) lower = [-np.pi / 2, -np.pi / 2] upper = [+np.pi / 2, +np.pi / 2] for [roll, pitch] in interval_generator(lower, upper, **kwargs): ##roll = random.uniform(0, np.pi) #roll = np.pi/4 #pitch = random.uniform(0, 2*np.pi) #return Pose(euler=Euler(roll=np.pi / 2 + roll, pitch=pitch)) #roll = random.uniform(-np.pi/2, np.pi/2) #pitch = random.uniform(-np.pi/2, np.pi/2) pose = Pose(euler=Euler(roll=roll, pitch=pitch)) yield pose
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 sample_scoop_trajectory(world, feature, parameter, collisions=True): bowl_body = world.get_body(feature['bowl_name']) bowl_urdf_from_center = get_urdf_from_base(bowl_body) # get_urdf_from_base spoon_body = world.get_body(feature['spoon_name']) spoon_quat = lookup_orientation(feature['spoon_name'], STIR_QUATS) spoon_urdf_from_center = get_urdf_from_base( spoon_body, reference_quat=spoon_quat) # get_urdf_from_base if RELATIVE_SCOOP: parameter = scale_parameter(feature, parameter, RELATIVE_SCOOP_SCALING, descale=True) #entry_z = parameter['entry_z'] # Ends up colliding quite frequently entry_z = feature['bowl_height'] + TOP_OFFSET_Z exit_z = parameter['exit_z'] + feature[ 'bowl_height'] + feature['spoon_length'] / 2 entry_pos = np.array([0., parameter['start_scoop_y'], entry_z]) start_pos = np.array( [0., parameter['start_scoop_y'], parameter['scoop_z']]) end_pos = np.array([0., parameter['end_scoop_y'], parameter['scoop_z']]) exit_pos = np.array([0., parameter['exit_y'], exit_z]) spoon_path_in_bowl = interpolate_pose_waypoints( [Pose(point=point) for point in [entry_pos, start_pos, end_pos]] + [(Pose(exit_pos, Euler(roll=-np.pi / 2)))]) # TODO(caelan): check the ordering/inversion when references are not the identity spoon_path = [ multiply(bowl_urdf_from_center, spoon_pose_in_bowl, invert(spoon_urdf_from_center)) for spoon_pose_in_bowl in spoon_path_in_bowl ] #draw_pose(get_pose(bowl_body)) #set_pose(spoon_body, multiply(get_pose(bowl_body), bowl_urdf_from_center, # Pose(point=start_pos), invert(spoon_urdf_from_center))) #set_pose(bowl_body, Pose()) #wait_for_user() collision_path = [spoon_path[0], spoon_path[-1]] #collision_path = [spoon_path[-1]] if collisions and bowl_path_collision(bowl_body, spoon_body, collision_path): return None return spoon_path
def pose_from_pose2d(self, pose2d, surface): #assert surface in self.poses_from_surface #reference_pose = self.poses_from_surface[surface][0] body = self.world.get_body(self.name) surface_aabb = compute_surface_aabb(self.world, surface) world_from_surface = get_surface_reference_pose( self.world.kitchen, surface) if DIM == 2: x, y = pose2d[:DIM] yaw = np.random.uniform(*CIRCULAR_LIMITS) else: x, y, yaw = pose2d z = stable_z_on_aabb( body, surface_aabb) + Z_EPSILON - point_from_pose(world_from_surface)[2] point = Point(x, y, z) surface_from_body = Pose(point, Euler(yaw=yaw)) set_pose(body, multiply(world_from_surface, surface_from_body)) return create_relative_pose(self.world, self.name, surface)
def transform_json(problem): original_path = get_extrusion_path(problem) #new_path = '{}_transformed.json'.format(problem) new_path = 'transformed.json'.format(problem) # TODO: create folder #pose = Pose(euler=Euler(roll=np.pi / 2.)) yaw = np.pi / 4. pose = Pose(euler=Euler(yaw=yaw)) tform = tform_from_pose(pose) tform = rotation_matrix(angle=yaw, direction=[0, 0, 1]) scale = SCALE_ASSEMBLY.get(problem, DEFAULT_SCALE) #tform = scale*np.identity(4) tform = scale_matrix(scale, origin=None, direction=None) json_data = affine_extrusion(original_path, tform) write_json(new_path, json_data) # TODO: rotate the whole robot as well # TODO: could also use the z heuristic when upside down return new_path
def _initialize_environment(self): # wall to fridge: 4cm # fridge to goal: 1.5cm # hitman to range: 3.5cm # range to indigo: 3.5cm self.environment_poses = read_json(POSES_PATH) root_from_world = get_link_pose(self.kitchen, self.world_link) for name, world_from_part in self.environment_poses.items(): if name in ['range']: continue visual_path = os.path.join(KITCHEN_LEFT_PATH, '{}.obj'.format(name)) collision_path = os.path.join(KITCHEN_LEFT_PATH, '{}_collision.obj'.format(name)) mesh_path = None for path in [collision_path, visual_path]: if os.path.exists(path): mesh_path = path break if mesh_path is None: continue body = load_pybullet(mesh_path, fixed_base=True) root_from_part = multiply(root_from_world, world_from_part) if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']: (pos, quat) = root_from_part # TODO: still not totally aligned pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005]) root_from_part = (pos, quat) self.environment_bodies[name] = body set_pose(body, root_from_part) # TODO: release bounding box or convex hull # TODO: static object nonconvex collisions if TABLE_NAME in self.environment_bodies: body = self.environment_bodies[TABLE_NAME] _, (w, l, _) = approximate_as_prism(body) _, _, z = get_point(body) new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z), Euler(yaw=np.pi / 2)) set_pose(body, new_pose)