def cook_block(world, fixed=False, **kwargs): add_kinect(world) # previously needed to be after set_all_static? if fixed: set_fixed_base(world) entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D) set_all_static() initial_surface = 'indigo_tmp' sample_placement(world, entity_name, initial_surface, learned=True) prior = { entity_name: UniformDist([initial_surface]), } return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], #goal_detected=[entity_name], #goal_holding=entity_name, goal_cooked=[entity_name], #goal_on={entity_name: goal_surface}, return_init_bq=True, return_init_aq=True, #goal_open=[joint_name], #goal_closed=ALL_JOINTS, )
def sugar_drawer(world, fixed=False, **kwargs): add_kinect(world) if fixed: set_fixed_base(world) open_drawer = random.choice(ZED_DRAWERS) world.open_door( joint_from_name(world.kitchen, JOINT_TEMPLATE.format(open_drawer))) # open_all_doors(world) prior = {} block_name = add_block(world, idx=0, pose2d=BOX_POSE2D) prior[block_name] = DeltaDist('indigo_tmp') sugar_name = add_sugar_box(world, idx=0) prior[sugar_name] = DeltaDist(open_drawer) sample_placement(world, sugar_name, open_drawer, learned=True) set_all_static() return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], goal_on={block_name: open_drawer}, return_init_bq=True, return_init_aq=True, #goal_open=[JOINT_TEMPLATE.format(open_drawer)], goal_closed=ALL_JOINTS, )
def regrasp_block(world, fixed=False, **kwargs): add_kinect(world) if fixed: set_fixed_base(world) entity_name = add_block(world, idx=0) set_all_static() #world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(LEFT_DOOR))) #drawer = random.choice(ZED_DRAWERS) drawer = 'indigo_tmp' sample_placement(world, entity_name, drawer, learned=True) prior = { entity_name: UniformDist(drawer), } return Task( world, prior=prior, movable_base=not fixed, #grasp_types=[SIDE_GRASP], #, TOP_GRASP], #goal_holding=entity_name, goal_on={entity_name: LEFT_DOOR}, #return_init_bq=True, return_init_aq=True, #goal_open=[JOINT_TEMPLATE.format(LEFT_DOOR)] #goal_closed=ALL_JOINTS, )
def _update_initial(self): # TODO: store initial poses as well? self.initial_saver = WorldSaver() self.goal_bq = FConf(self.robot, self.base_joints) self.goal_aq = FConf(self.robot, self.arm_joints) if are_confs_close(self.goal_aq, self.carry_conf): self.goal_aq = self.carry_conf self.goal_gq = FConf(self.robot, self.gripper_joints) self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq] set_all_static()
def stow_block(world, num=1, fixed=False, **kwargs): add_kinect(world) # previously needed to be after set_all_static? if fixed: set_fixed_base(world) # initial_surface = random.choice(DRAWERS) # COUNTERS | DRAWERS | SURFACES | CABINETS initial_surface = 'indigo_tmp' # hitman_tmp | indigo_tmp | range | front_right_stove # initial_surface = 'indigo_drawer_top' goal_surface = 'indigo_drawer_top' # baker | hitman_drawer_top | indigo_drawer_top | hitman_tmp | indigo_tmp print('Initial surface: | Goal surface: ', initial_surface, initial_surface) prior = {} goal_on = {} for idx in range(num): entity_name = add_block(world, idx=idx, pose2d=SPAM_POSE2D) prior[entity_name] = DeltaDist(initial_surface) goal_on[entity_name] = goal_surface if not fixed: sample_placement(world, entity_name, initial_surface, learned=True) #obstruction_name = add_box(world, idx=0) #sample_placement(world, obstruction_name, 'hitman_tmp') set_all_static() # dump_world() # open_names = [ # 'chewie_door_right_joint', # 'dagger_door_right_joint', # 'hitman_drawer_bottom_joint', # # 'indigo_drawer_top_joint', # ] # for joint_name in open_names: # world.open_door(joint_from_name(world.kitchen, joint_name)) return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], #goal_holding=list(prior)[0], goal_hand_empty=True, goal_on=goal_on, #goal_cooked=list(prior), return_init_bq=True, return_init_aq=True, #goal_open=[joint_name], goal_closed=ALL_JOINTS)
def detect_block(world, fixed=False, **kwargs): add_kinect(world) #for side in CAMERAS[:1]: # add_kinect(world, side) #add_kinects(world) if fixed: set_fixed_base(world) #plane = create_plane([1, 0, 0]) #set_point(plane, [MIN_PLACEMENT_X, 0, 0]) #wait_for_user() block_poses = [(0.1, 1.05, 0.), (0.1, 1.3, 0.)] entity_name = add_block(world, idx=0, pose2d=random.choice(block_poses)) sugar_name = add_sugar_box(world, idx=0, pose2d=(0.2, 1.35, np.pi / 4)) cracker_name = add_cracker_box(world, idx=1, pose2d=(0.2, 1.1, np.pi / 4)) #other_name = add_box(world, idx=1) set_all_static() #goal_surface = 'indigo_drawer_top' #initial_distribution = UniformDist([goal_surface]) # indigo_tmp #initial_surface = initial_distribution.sample() #if random.random() < 0.0: # # TODO: sometimes base/arm failure causes the planner to freeze # # Freezing is because the planner is struggling to find new samples # sample_placement(world, entity_name, initial_surface, learned=True) #sample_placement(world, other_name, 'hitman_tmp', learned=True) prior = { entity_name: UniformDist(['indigo_tmp']), # 'indigo_tmp', 'indigo_drawer_top' sugar_name: DeltaDist('indigo_tmp'), cracker_name: DeltaDist('indigo_tmp'), } return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], return_init_bq=True, return_init_aq=True, #goal_detected=[entity_name], goal_holding=cracker_name, #goal_on={entity_name: random.choice(ZED_DRAWERS)}, goal_cooked=[entity_name], goal_closed=ALL_JOINTS, )
def cook_meal(world, fixed=False, **kwargs): add_kinect(world) # previously needed to be after set_all_static? if fixed: set_fixed_base(world) prior = {} soup_name = add_ycb(world, 'tomato_soup_can', pose2d=[0.1, 0.9, +np.pi / 8]) prior[soup_name] = DeltaDist('indigo_tmp') if not fixed: sample_placement(world, soup_name, 'indigo_tmp', learned=True) mustard_name = add_ycb(world, 'mustard_bottle', pose2d=[0.25, 1.2, -np.pi / 8]) prior[mustard_name] = DeltaDist('indigo_tmp') if not fixed: sample_placement(world, mustard_name, 'indigo_tmp', learned=True) stove = STOVES[-1] bowl_name = add_ycb(world, 'bowl') prior[bowl_name] = DeltaDist(stove) sample_placement(world, bowl_name, stove, learned=True) #print(get_pose(world.get_body(bowl_name))) set_all_static() return Task( world, prior=prior, movable_base=not fixed, #grasp_types=[TOP_GRASP], init_liquid=[(soup_name, 'tomato'), (mustard_name, 'mustard')], #goal_liquid=[(bowl_name, 'tomato'), (bowl_name, 'mustard')], #goal_holding=soup_name, goal_hand_empty=True, #goal_cooked=[bowl_name], goal_cooked=['tomato', 'mustard'], return_init_bq=True, return_init_aq=True, #goal_open=[joint_name], goal_closed=ALL_JOINTS)
def swap_drawers(world, fixed=False, **kwargs): # Starts in the incorrect drawer add_kinect(world) # previously needed to be after set_all_static? if fixed: set_fixed_base(world) entity_name = add_block(world, idx=0, pose2d=BOX_POSE2D) set_all_static() #open_all_doors(world) #initial_surface, goal_surface = 'indigo_tmp', 'indigo_drawer_top' #initial_surface, goal_surface = 'indigo_drawer_top', 'indigo_drawer_top' #initial_surface, goal_surface = 'indigo_drawer_bottom', 'indigo_drawer_bottom' #initial_surface, goal_surface = ZED_DRAWERS #initial_surface, goal_surface = reversed(ZED_DRAWERS) initial_surface, goal_surface = randomize(ZED_DRAWERS) if initial_surface != 'indigo_tmp': sample_placement(world, entity_name, initial_surface, learned=True) #joint_name = JOINT_TEMPLATE.format(goal_surface) #world.open_door(joint_from_name(world.kitchen, JOINT_TEMPLATE.format(goal_surface))) # TODO: declare success if already believe it's in the drawer or require detection? prior = { #entity_name: UniformDist([initial_surface]), entity_name: UniformDist(ZED_DRAWERS), #entity_name: UniformDist(['indigo_tmp', 'indigo_drawer_top', 'indigo_drawer_bottom']), } return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], #goal_detected=[entity_name], #goal_holding=entity_name, #goal_cooked=[entity_name], goal_on={entity_name: goal_surface}, return_init_bq=True, return_init_aq=True, #goal_open=[joint_name], #goal_closed=[JOINT_TEMPLATE.format(initial_surface)], #goal_closed=[JOINT_TEMPLATE.format(goal_surface)], # TODO: this caused non-fixed base planning to fail due to cost goal_closed=ALL_JOINTS, )
def inspect_drawer(world, fixed=False, **kwargs): # Starts in the correct drawer add_kinect(world) if fixed: set_fixed_base(world) entity_name = add_block(world, idx=0) set_all_static() drawer = random.choice(ZED_DRAWERS) sample_placement(world, entity_name, drawer, learned=True) prior = { entity_name: UniformDist(ZED_DRAWERS), } return Task( world, prior=prior, movable_base=not fixed, grasp_types=[TOP_GRASP], goal_on={entity_name: drawer}, return_init_bq=True, return_init_aq=True, goal_closed=ALL_JOINTS, )
def hold_block(world, num=5, fixed=False, **kwargs): add_kinect(world) if fixed: set_fixed_base(world) # TODO: compare with the NN grasp prediction in clutter # TODO: consider a task where most directions are blocked except for one initial_surface = 'indigo_tmp' # initial_surface = 'dagger_door_left' # joint_name = JOINT_TEMPLATE.format(initial_surface) #world.open_door(joint_from_name(world.kitchen, joint_name)) #open_all_doors(world) prior = {} # green_name = add_block(world, idx=0, pose2d=BOX_POSE2D) green_name = add_box(world, 'green', idx=0) prior[green_name] = DeltaDist(initial_surface) sample_placement(world, green_name, initial_surface, learned=True) for idx in range(num): red_name = add_box(world, 'red', idx=idx) prior[red_name] = DeltaDist(initial_surface) sample_placement(world, red_name, initial_surface, learned=True) set_all_static() return Task( world, prior=prior, movable_base=not fixed, # grasp_types=GRASP_TYPES, grasp_types=[SIDE_GRASP], return_init_bq=True, return_init_aq=True, goal_holding=green_name, #goal_closed=ALL_JOINTS, )
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 main(): parser = argparse.ArgumentParser() parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument('--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-s', '--seed', default=None, type=int, help='The random seed to use.') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) seed = args.seed #seed = 0 #seed = time.time() set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) print('Random seed:', get_random_seed(), random.random()) print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1() draw_base_limits(base_limits) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) base_link = link_from_name(robot, BASE_LINK_NAME) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) set_all_static() # Doesn't seem to affect region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) step_simulation() # Need to call before get_bodies_in_region #update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331 bodies = get_bodies_in_region(region_aabb) print(len(bodies), bodies) # https://github.com/bulletphysics/bullet3/search?q=broadphase # https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93 # https://andysomogyi.github.io/mechanica/bullet.html # http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual #draw_pose(get_link_pose(robot, base_link), length=0.5) for conf in [get_joint_positions(robot, base_joints), goal_conf]: draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH) aabb = get_aabb(robot) #aabb = get_subtree_aabb(robot, base_link) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link)) ######################### saver = WorldSaver() start_time = time.time() profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None profiler.save() with LockRenderer(lock=args.lock): path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles, custom_limits=custom_limits, resolutions=resolutions, use_aabb=True, cache=True, max_distance=0., restarts=2, iterations=20, smooth=20) # 20 | None saver.restore() #wait_for_duration(duration=1e-3) profiler.restore() ######################### solved = path is not None length = INF if path is None else len(path) cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path)) print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: disconnect() return iterate_path(robot, base_joints, path) disconnect()