def test_block(visualize): #arms = [LEFT_ARM] arms = ARMS block_name = create_name('greenblock', 1) tray_name = create_name('tray', 1) world, table_body = create_world([tray_name, block_name], visualize=visualize) with ClientSaver(world.perception.client): block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON #attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame') #dump_body(world.perception.pr2) #block_y = 0.0 block_y = -0.4 world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat()) world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat()) update_world(world, table_body) init = [ ('Stackable', block_name, tray_name, TOP), ] goal = [ ('On', block_name, tray_name, TOP), ] #goal = [ # ('Grasped', arms[0], block_name), #] task = Task(init=init, goal=goal, arms=arms, empty_arms=True) return world, task
def test_pour(visualize): arms = [LEFT_ARM] #cup_name, bowl_name = 'bluecup', 'bluebowl' #cup_name, bowl_name = 'cup_7', 'cup_8' #cup_name, bowl_name = 'cup_7-1', 'cup_7-2' #cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2) cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup bowl_name = create_name('bowl', 1) # bowl | steel_bowl world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize) with ClientSaver(world.perception.client): cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat()) world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat()) update_world(world, table_body) init = [ ('Contains', cup_name, COFFEE), #('Graspable', bowl_name), ] goal = [ ('Contains', bowl_name, COFFEE), ] task = Task(init=init, goal=goal, arms=arms, empty_arms=True, reset_arms=True, reset_items=True) return world, task
def test_stack_pour(visualize): arms = [LEFT_ARM] bowl_name = create_name('whitebowl', 1) cup_name = create_name('bluecup', 1) purple_name = create_name('purpleblock', 1) items = [bowl_name, cup_name, purple_name] world, table_body = create_world(items, visualize=visualize) cup_x = 0.5 initial_poses = { bowl_name: ([cup_x, -0.2, 0.0], unit_quat()), cup_name: ([cup_x, 0.1, 0.0], unit_quat()), purple_name: ([cup_x, 0.4, 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) update_world(world, table_body) init = [ ('Contains', cup_name, COFFEE), ('Stackable', bowl_name, purple_name, TOP), ] goal = [ ('Contains', bowl_name, COFFEE), ('On', bowl_name, purple_name, TOP), ] task = Task(init=init, goal=goal, arms=arms, graspable=['whitebowl']) return world, task
def test_push_pour(visualize): arms = ARMS cup_name = create_name('bluecup', 1) bowl_name = create_name('bowl', 1) items = [bowl_name, cup_name, bowl_name] world, table_body = create_world(items, visualize=visualize) set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0])) with ClientSaver(world.perception.client): cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat()) world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat()) update_world(world, table_body) # TODO: can prevent the right arm from being able to pick init = [ ('Contains', cup_name, COFFEE), #('Graspable', bowl_name), ('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions ] goal = [ ('Contains', bowl_name, COFFEE), ('InRegion', bowl_name, LEFT_ARM), ] task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name]) # Most of the time places the cup to exchange arms return world, task
def get_water_path(bowl_body, bowl_pose, cup_body, pose_waypoints): cup_pose = pose_waypoints[0] bowl_origin_from_base = get_urdf_from_base( bowl_body) # TODO: reference pose cup_origin_from_base = get_urdf_from_base(cup_body) ray_start = point_from_pose(multiply(cup_pose, cup_origin_from_base)) ray_end = point_from_pose(multiply(bowl_pose, bowl_origin_from_base)) water_path = interpolate_poses((ray_start, unit_quat()), (ray_end, unit_quat()), pos_step_size=0.01) return water_path
def get_urdf_from_z_axis(body, z_fraction, reference_quat=unit_quat()): # AKA the pose of the body's center wrt to the body's origin # z_fraction=0. => bottom, z_fraction=0.5 => center, z_fraction=1. => top ref_from_urdf = (unit_point(), reference_quat) center_in_ref, (_, height) = approximate_as_cylinder(body, body_pose=ref_from_urdf) center_in_ref[2] += (z_fraction - 0.5) * height ref_from_center = (center_in_ref, unit_quat() ) # Maps from center frame to origin frame urdf_from_center = multiply(invert(ref_from_urdf), ref_from_center) return urdf_from_center
def test_holding(visualize, num_blocks=2): arms = [LEFT_ARM] #item_type = 'greenblock' #item_type = 'bowl' item_type = 'purple_cup' item_poses = [ ([0.6, +0.4, 0.0], z_rotation(np.pi / 2)), ([0.6, -0.4, 0.0], unit_quat()), ] items = [create_name(item_type, i) for i in range(min(len(item_poses), num_blocks))] world, table_body = create_world(items, visualize=visualize) with ClientSaver(world.perception.client): for name, pose in zip(items, item_poses): point, quat = pose point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON world.perception.set_pose(name, point, quat) update_world(world, table_body) init = [ ('Graspable', item) for item in items ] goal = [ #('Holding', items[0]), ('HoldingType', item_type), ] 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 add_table_surfaces(world): table_pose = None for body_info in world.perception.surfaces: if body_info.type == TABLE: table_pose = body_info.pose assert table_pose is not None initial_poses = { STOVE_NAME: (STOVE_POSITION, unit_quat()), PLACEMAT_NAME: (PLACEMAT_POSITION, unit_quat()), BUTTON_NAME: (BUTTON_POSITION, unit_quat()), } with ClientSaver(world.perception.client): for name, local_pose in initial_poses.items(): world_pose = multiply(table_pose, local_pose) from perception_tools.retired.ros_perception import BodyInfo world.perception.surfaces.append( BodyInfo(name, None, world_pose, name))
def test_cup(visualize): arms = [LEFT_ARM] cup_name = create_name('greenblock', 1) block_name = create_name('purpleblock', 1) # greenblock tray_name = create_name('tray', 1) world, table_body = create_world([tray_name, cup_name, block_name], visualize=visualize) #cup_x = 0.6 cup_x = 0.8 block_x = cup_x - 0.15 initial_poses = { cup_name: ([cup_x, 0.0, 0.0], unit_quat()), block_name: ([block_x, 0.0, 0.0], unit_quat()), # z_rotation(np.pi/2) tray_name: ([0.6, 0.4, 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) update_world(world, table_body) #with ClientSaver(world.perception.client): # draw_pose(get_pose(world.perception.sim_bodies['bluecup'])) # draw_aabb(get_aabb(world.perception.sim_bodies['bluecup'])) # wait_for_interrupt() init = [ #('Contains', cup_name, WATER), ('Stackable', cup_name, tray_name, TOP), ] goal = [ #('Grasped', goal_arm, block_name), #('Grasped', goal_arm, cup_name), ('On', cup_name, tray_name, TOP), #('On', cup_name, TABLE_NAME, TOP), #('Empty', goal_arm), ] task = Task(init=init, goal=goal, arms=arms) return world, task
def test_stacking(visualize): # TODO: move & stack arms = [LEFT_ARM] #arms = ARMS cup_name = create_name('bluecup', 1) # bluecup | spoon green_name = create_name('greenblock', 1) purple_name = create_name('purpleblock', 1) items = [cup_name, green_name, purple_name] world, table_body = create_world(items, visualize=visualize) cup_x = 0.5 initial_poses = { cup_name: ([cup_x, -0.2, 0.0], unit_quat()), #cup_name: ([cup_x, -0.2, 0.3], unit_quat()), green_name: ([cup_x, 0.1, 0.0], unit_quat()), #green_name: ([cup_x, 0.1, 0.0], z_rotation(np.pi / 4)), # TODO: be careful about the orientation when stacking purple_name: ([cup_x, 0.4, 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) update_world(world, table_body) init = [ #('Stackable', cup_name, purple_name, TOP), ('Stackable', cup_name, green_name, TOP), ('Stackable', green_name, purple_name, TOP), ] goal = [ #('On', cup_name, purple_name, TOP), ('On', cup_name, green_name, TOP), ('On', green_name, purple_name, TOP), ] task = Task(init=init, goal=goal, arms=arms) return world, task
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 gen_fn(index, pose, grasp): body = brick_from_index[index].body set_pose(body, pose.value) obstacles = list(obstacle_from_name.values()) # + [body] collision_fn = get_collision_fn( robot, movable_joints, obstacles=obstacles, attachments=[], self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, custom_limits=get_custom_limits(robot)) attach_pose = multiply(pose.value, invert(grasp.attach)) approach_pose = multiply(attach_pose, (approach_vector, unit_quat())) # approach_pose = multiply(pose.value, invert(grasp.approach)) for _ in range(max_attempts): if USE_IKFAST: attach_conf = sample_tool_ik(robot, attach_pose) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed attach_conf = inverse_kinematics(robot, tool_link, attach_pose) if (attach_conf is None) or collision_fn(attach_conf): continue set_joint_positions(robot, movable_joints, attach_conf) #if USE_IKFAST: # approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf) #else: approach_conf = inverse_kinematics(robot, tool_link, approach_pose) if (approach_conf is None) or collision_fn(approach_conf): continue set_joint_positions(robot, movable_joints, approach_conf) path = plan_direct_joint_motion( robot, movable_joints, attach_conf, obstacles=obstacles, self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions) if path is None: # TODO: retreat continue #path = [approach_conf, attach_conf] attachment = Attachment(robot, tool_link, grasp.attach, body) traj = MotionTrajectory(robot, movable_joints, path, attachments=[attachment]) yield approach_conf, traj break
def test_clutter(visualize, num_blocks=5, num_beads=0): arms = [LEFT_ARM] cup_name = create_name('bluecup', 1) bowl_name = create_name('bowl', 1) # bowl | cup_7 clutter = [create_name('greenblock', i) for i in range(num_blocks)] world, table_body = create_world([cup_name, bowl_name] + clutter, visualize=visualize) with ClientSaver(world.perception.client): cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON block_z = None if 0 < num_blocks: block_z = stable_z(world.perception.sim_bodies[clutter[0]], table_body) # TODO(lagrassa): first pose collides with the bowl xy_poses = [(0.6, -0.1), (0.5, -0.2), (0.6, 0.11), (0.45, 0.12), (0.5, 0.3), (0.7, 0.3)] world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat()) world.perception.set_pose(bowl_name, Point(0.6, -0.1, bowl_z), unit_quat()) #if 0 < num_beads: # world.perception.add_beads(cup_name, num_beads, bead_radius=0.007, bead_mass=0.005) if block_z is not None: clutter_poses = [np.append(xy, block_z) for xy in reversed(xy_poses)] for obj, pose in zip(clutter, clutter_poses): world.perception.set_pose(obj, pose, unit_quat()) update_world(world, table_body) #init = [('Graspable', name) for name in [cup_name, bowl_name] + clutter] init = [ ('Contains', cup_name, COFFEE), ] goal = [ ('Contains', bowl_name, COFFEE), ] task = Task(init=init, goal=goal, arms=arms) return world, task
def test_cook(visualize): # TODO: can also disable collision bodies for stove & placemat arms = [LEFT_ARM] broccoli_name = 'greenblock' stove_name = 'stove' placemat_name = 'placemat' button_name = 'button' items = [stove_name, button_name, placemat_name, broccoli_name] world, table_body = create_world(items, visualize=visualize) update_world(world, table_body) initial_poses = { broccoli_name: ([-0.1, 0.0, 0.0], z_rotation(np.pi/2)), stove_name: (STOVE_POSITION, unit_quat()), placemat_name: (PLACEMAT_POSITION, unit_quat()), button_name: (BUTTON_POSITION, unit_quat()), } with ClientSaver(world.perception.client): for name, local_pose in initial_poses.items(): world_pose = multiply(TABLE_POSE, local_pose) #z = stable_z(world.perception.sim_bodies[name], table_body) # Slightly above world_pose world.perception.set_pose(name, *world_pose) init = [ ('IsButton', button_name, stove_name), ('Stackable', broccoli_name, stove_name, TOP), ('Stackable', broccoli_name, placemat_name, TOP), ] goal = [ ('Cooked', broccoli_name), ('On', broccoli_name, placemat_name, TOP), ] task = Task(init=init, goal=goal, arms=arms) return world, task
from plan_tools.planner import Task from plan_tools.ros_world import ROSWorld from plan_tools.samplers.grasp import hold_item from pybullet_tools.pr2_utils import inverse_visibility, set_group_conf, arm_from_arm, \ WIDE_LEFT_ARM, rightarm_from_leftarm, open_arm from pybullet_tools.pr2_problems import create_floor from pybullet_tools.utils import ClientSaver, get_point, unit_quat, unit_pose, link_from_name, draw_point, \ HideOutput, stable_z, quat_from_euler, Euler, set_camera_pose, z_rotation, multiply, \ get_quat, create_mesh, set_point, set_quat, set_pose, wait_for_user, \ get_visual_data, read_obj, BASE_LINK, \ approximate_as_prism, Pose, draw_mesh, rectangular_mesh, apply_alpha, Point TABLE_NAME = create_name(TABLE, 1) # TODO: randomly sample from interval TABLE_POSE = ([0.55, 0.0, 0.735], unit_quat()) Z_EPSILON = 1e-3 STOVE_POSITION = np.array([0.0, 0.4, 0.0]) PLACEMAT_POSITION = np.array([0.1, 0.0, 0.0]) BUTTON_POSITION = STOVE_POSITION - np.array([0.15, 0.0, 0.0]) TORSO_POSITION = 0.325 # TODO: maintain average distance betweeen pr2 and table CAMERA_OPTICAL_FRAME = 'head_mount_kinect_rgb_optical_frame' CAMERA_FRAME = 'head_mount_kinect_rgb_link' #CAMERA_FRAME = 'high_def_frame' #CAMERA_OPTICAL_FRAME = 'high_def_optical_frame' #VIEWER_POINT = np.array([1.25, -0.5, 1.25]) # Angled VIEWER_POINT = np.array([1.25, 0., 1.25]) # Wide
def lookup_orientation(name, quat_from_model): for model, quat in quat_from_model.items(): if is_obj_type(name, model): return quat return unit_quat()
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 = '../conrob_pybullet/models/kuka_kr6_r900/urdf/kuka_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 main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def eval_task_with_seed(domain, seed, learner, sample_strategy=DIVERSELK, task_lengthscale=None, obstacle=True): if task_lengthscale is not None: learner.task_lengthscale = task_lengthscale print('sample a new task') current_wd, trial_wd = start_task() ### fill in additional object item_ranges = {} ### sim_world, collector, task, feature, evalfunc, saver = sample_task_with_seed( domain.skill, seed=seed, visualize=False, item_ranges=item_ranges) context = domain.context_from_feature(feature) print('context=', *context) # create coffee machine if obstacle: bowl_name = 'bowl' cup_name = 'cup' if domain.skill == 'scoop': cup_name = 'spoon' for key in sim_world.perception.sim_bodies: if bowl_name in key: bowl_name = key if cup_name in key: cup_name = key cylinder_size = (.01, .03) dim_bowl = get_aabb_extent( p.getAABB(sim_world.perception.sim_bodies[bowl_name])) dim_cup = get_aabb_extent( p.getAABB(sim_world.perception.sim_bodies[cup_name])) bowl_point = get_point(sim_world.perception.sim_bodies[bowl_name]) bowl_point = np.array(bowl_point) cylinder_point = bowl_point.copy() cylinder_offset = 1.2 if domain.skill == 'scoop': cylinder_offset = 1.6 cylinder_point[2] += (dim_bowl[2] + dim_cup[2]) * (np.random.random_sample() * .4 + cylinder_offset) \ + cylinder_size[1] / 2. # TODO figure out the task space cylinder_name = create_name('cylinder', 1) obstacle = create_cylinder(*cylinder_size, color=(0, 0, 0, 1)) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(cylinder_name, cylinder_size, 1, 1) sim_world.perception.sim_bodies[cylinder_name] = obstacle sim_world.perception.sim_items[cylinder_name] = None set_pose(obstacle, (cylinder_point, unit_quat())) box_name = create_name('box', 1) box1_size = (max(.17, dim_bowl[0] / 2 + cylinder_size[0] * 2), 0.01, 0.01) if domain.skill == 'scoop': box1_size = (max(.23, dim_bowl[0] / 2 + cylinder_size[0] * 2 + 0.05), 0.01, 0.01) obstacle = create_box(*box1_size) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box1_size, 1, 1) sim_world.perception.sim_bodies[box_name] = obstacle sim_world.perception.sim_items[box_name] = None box1_point = cylinder_point.copy() box1_point[2] += cylinder_size[1] / 2 + box1_size[2] / 2 box1_point[0] += box1_size[0] / 2 - cylinder_size[0] * 2 set_pose(obstacle, (box1_point, unit_quat())) box_name = create_name('box', 2) box2_size = (0.01, .01, box1_point[2] - bowl_point[2] + box1_size[2] / 2) obstacle = create_box(*box2_size) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box2_size, 1, 1) sim_world.perception.sim_bodies[box_name] = obstacle sim_world.perception.sim_items[box_name] = None box2_point = bowl_point.copy() box2_point[2] += box2_size[2] / 2 box2_point[0] = box1_point[0] + box1_size[0] / 2 set_pose(obstacle, (box2_point, unit_quat())) result = get_sample_strategy_result(sample_strategy, learner, context, sim_world, collector, task, feature, evalfunc, saver) print('context=', *context) complete_task(sim_world, current_wd, trial_wd) return result