def compute_surface_aabb(world, surface_name): if surface_name in ENV_SURFACES: # TODO: clean this up # TODO: the aabb for golf is off the table surface_body = world.environment_bodies[surface_name] return get_aabb(surface_body) surface_body = world.kitchen surface_name, shape_name, _ = surface_from_name(surface_name) surface_link = link_from_name(surface_body, surface_name) surface_pose = get_link_pose(surface_body, surface_link) if shape_name == SURFACE_TOP: surface_aabb = get_aabb(surface_body, surface_link) elif shape_name == SURFACE_BOTTOM: data = sorted(get_collision_data(surface_body, surface_link), key=lambda d: point_from_pose(get_data_pose(d))[2])[0] extent = np.array(get_data_extents(data)) aabb = AABB(-extent/2., +extent/2.) vertices = apply_affine(multiply(surface_pose, get_data_pose(data)), get_aabb_vertices(aabb)) surface_aabb = aabb_from_points(vertices) else: [data] = filter(lambda d: d.filename != '', get_collision_data(surface_body, surface_link)) meshes = read_obj(data.filename) #colors = spaced_colors(len(meshes)) #set_color(surface_body, link=surface_link, color=np.zeros(4)) mesh = meshes[shape_name] #for i, (name, mesh) in enumerate(meshes.items()): mesh = tform_mesh(multiply(surface_pose, get_data_pose(data)), mesh=mesh) surface_aabb = aabb_from_points(mesh.vertices) #add_text(surface_name, position=surface_aabb[1]) #draw_mesh(mesh, color=colors[i]) #wait_for_user() #draw_aabb(surface_aabb) #wait_for_user() return surface_aabb
def test_aabb(robot): base_link = link_from_name(robot, BASE_LINK_NAME) region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) # bodies = get_bodies_in_region(region_aabb) # print(len(bodies), bodies) # for body in get_bodies(): # set_pose(body, Pose()) #step_simulation() # Need to call before get_bodies_in_region #update_scene() for i in range(3): with timer(message='{:f}'): bodies = get_bodies_in_region( region_aabb) # This does cache some info print(i, 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 aabb = get_aabb(robot) # aabb = get_subtree_aabb(robot, base_link) print(aabb) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
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 visualize_cartesian_path(body, pose_path): for i, pose in enumerate(pose_path): set_pose(body, pose) print('{}/{}) continue?'.format(i, len(pose_path))) wait_for_user() handles = draw_pose(get_pose(body)) handles.extend(draw_aabb(get_aabb(body))) print('Finish?') wait_for_user() for h in handles: remove_debug(h)
def get_aabbs(self): #traj.aabb = aabb_union(map(get_turtle_traj_aabb, traj.iterate())) # TODO: union if self.aabbs is not None: return self.aabbs self.aabbs = [] links = get_all_links(self.robot) with BodySaver(self.robot): for conf in self.path: set_joint_positions(self.robot, self.joints, conf) self.aabbs.append( {link: get_aabb(self.robot, link) for link in links}) return self.aabbs
def get_contained_beads(body, beads, height_fraction=1.0, top_threshold=0.0): #aabb = get_aabb(body) center, extent = approximate_as_prism(body, body_pose=get_pose(body)) lower = center - extent / 2. upper = center + extent / 2. _, _, z1 = lower x2, y2, z2 = upper z_upper = z1 + height_fraction * (z2 - z1) + top_threshold new_aabb = AABB(lower, np.array([x2, y2, z_upper])) #handles = draw_aabb(new_aabb) return { bead for bead in beads if aabb_contains_aabb(get_aabb(bead), new_aabb) }
def read_mass(scale_body, max_height=0.5, tolerance=1e-2): # Approximation: ignores connectivity outside box and bodies not resting on the body # TODO: could also add a force sensor and estimate the force scale_aabb = get_aabb(scale_body) extent = scale_aabb.upper - scale_aabb.lower lower = scale_aabb.lower + np.array([0, 0, extent[2] ]) - tolerance * np.ones(3) upper = scale_aabb.upper + np.array([0, 0, max_height ]) + tolerance * np.ones(3) above_aabb = AABB(lower, upper) total_mass = 0. #handles = draw_aabb(above_aabb) for body, link in get_bodies_in_region(above_aabb): if scale_body == body: continue link_aabb = get_aabb(body, link) if aabb_contains_aabb(link_aabb, above_aabb): total_mass += get_mass(body, link) else: #print(get_name(body), get_link_name(body, link)) #handles.extend(draw_aabb(link_aabb)) pass return total_mass
def draw_forward_reachability(world, arms, grasp_type='top', color=(1, 0, 0)): for name, body in world.perception.sim_bodies.items(): if name.startswith(TABLE): table = body break else: return False if not DEBUG: return True with ClientSaver(world.perception.client): lower, upper = get_aabb(table) for arm in arms: vertices = [ np.append(vertex, upper[2] + 1e-3) for vertex in compute_forward_reachability( world.perception.pr2, arm=arm, grasp_type=grasp_type) ] add_segments(vertices, closed=True, color=color) return True
def get_intersecting(self): if self.intersecting: return self.intersecting robot_links = get_all_links(self.robot) # TODO: might need call step_simulation with BodySaver(self.robot): for conf in self.path: set_joint_positions(self.robot, self.joints, conf) intersecting = {} for robot_link in robot_links: for body, _ in get_bodies_in_region( get_aabb(self.robot, link=robot_link)): if body != self.robot: intersecting.setdefault(robot_link, set()).add(body) #print(get_bodies_in_region(get_aabb(self.robot, robot_link))) #draw_aabb(get_aabb(self.robot, robot_link)) #wait_for_user() self.intersecting.append(intersecting) return self.intersecting
def load_world(use_floor=True): obstacles = [] #side, height = 10, 0.01 robot = load_robot() with HideOutput(): lower, _ = get_aabb(robot) if use_floor: #floor = load_model('models/short_floor.urdf', fixed_base=True) #add_data_path() #floor = load_pybullet('plane.urdf', fixed_base=True) #set_color(floor, TAN) #floor = create_box(w=side, l=side, h=height, color=apply_alpha(GROUND_COLOR)) floor = create_plane(color=apply_alpha(GROUND_COLOR)) obstacles.append(floor) #set_point(floor, Point(z=lower[2])) set_point(floor, Point(x=1.2, z=0.023 - 0.025)) # -0.02 else: floor = None # TODO: make this an empty list of obstacles #set_all_static() return obstacles, robot
def is_safe(self, obstacles): checked_bodies = set(self.safe_from_body) & set(obstacles) if any(not self.safe_from_body[e] for e in checked_bodies): return False unchecked_bodies = randomize(set(obstacles) - checked_bodies) if not unchecked_bodies: return True for tool_pose in randomize(self.tool_path): self.extrusion.end_effector.set_pose(tool_pose) tool_aabb = get_aabb( self.extrusion.end_effector.body) # TODO: cache nearby_bodies nearby_bodies = { b for b, _ in get_bodies_in_region(tool_aabb) if b in unchecked_bodies } for body in nearby_bodies: if pairwise_collision(self.extrusion.end_effector.body, body): self.safe_from_body[body] = False return False return True
def tool_path_collision(end_effector, element_pose, translation_path, direction, angle, reverse, obstacles): # TODO: allow sampling in the full sphere by checking collision with an element while sliding for tool_pose in randomize( compute_tool_path(element_pose, translation_path, direction, angle, reverse)): end_effector.set_pose(tool_pose) #bodies = obstacles tool_aabb = get_aabb(end_effector.body) # TODO: could just translate #handles = draw_aabb(tool_aabb) bodies = { b for b, _ in get_bodies_in_region(tool_aabb) if b in obstacles } #print(bodies) #for body, link in bodies: # handles.extend(draw_aabb(get_aabb(body, link))) #wait_for_user() #remove_handles(handles) if any(pairwise_collision(end_effector.body, obst) for obst in bodies): # TODO: sort by angle with smallest violation return True return False
def draw_names(world, **kwargs): # TODO: adjust the colors? handles = [] if not DEBUG: return handles with ClientSaver(world.perception.client): for name, body in world.perception.sim_bodies.items(): #add_body_name(body, **kwargs) with PoseSaver(body): set_pose(body, unit_pose()) lower, upper = get_aabb( body ) # TODO: multi-link bodies doesn't seem to update when moved handles.append( add_text(name, position=upper, parent=body, **kwargs)) # parent_link=0, #handles.append(draw_pose(get_pose(body))) #handles.extend(draw_base_limits(get_base_limits(world.pr2), color=(1, 0, 0))) #handles.extend(draw_circle(get_point(world.pr2), MAX_VIS_DISTANCE, color=(0, 0, 1))) #handles.extend(draw_circle(get_point(world.pr2), MAX_REG_DISTANCE, color=(0, 0, 1))) #from plan_tools.debug import test_voxels #test_voxels(world) return handles
def element_collision_fn(q): if collision_fn(q): return True #step_simulation() # Might only need to call this once for robot_link in robot_links: #bodies = obstacles aabb = get_aabb(robot, link=robot_link) bodies = { b for b, _ in get_bodies_in_region(aabb) if b in obstacles } #set_joint_positions(robot, joints, q) # Need to reset #draw_aabb(aabb) #print(robot_link, get_link_name(robot, robot_link), len(bodies), aabb) #print(sum(pairwise_link_collision(robot, robot_link, body, link2=0) for body, _ in region_bodies)) #print(sum(pairwise_collision(robot, body) for body, _ in region_bodies)) #wait_for_user() if any( pairwise_link_collision( robot, robot_link, body, link2=BASE_LINK) for body in bodies): #wait_for_user() return True return False
def get_world_aabb(self): return aabb_union(get_aabb(body) for body in self.fixed) # self.all_bodies
def get_base_aabb(self): franka_links = get_link_subtree(self.robot, self.franka_link) base_links = get_link_subtree(self.robot, self.base_link) return aabb_union( get_aabb(self.robot, link) for link in set(base_links) - set(franka_links))
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()