def test_caching(robot, obstacles): with timer(message='{:f}'): #update_scene() # 5.19752502441e-05 step_simulation() # 0.000210046768188 with timer(message='{:f}'): #print(get_aabb(robot, link=None, only_collision=True)) print(contact_collision()) # 2.50339508057e-05 for _ in range(3): with timer(message='{:f}'): #print(get_aabb(robot, link=None, only_collision=True)) # Recomputes each time print(contact_collision()) # 1.69277191162e-05 print() obstacle = obstacles[-1] #for link in get_all_links(robot): # set_collision_pair_mask(robot, obstacle, link1=link, enable=False) # Doesn't seem to affect pairwise_collision with timer('{:f}'): print(pairwise_collision(robot, obstacle)) # 0.000031 links = get_all_links(robot) links = [link for link in get_all_links(robot) if can_collide(robot, link)] #links = randomize(links) with timer('{:f}'): print( any( pairwise_collision(robot, obstacle, link1=link) for link in links # 0.000179 )) #if aabb_overlap(get_aabb(robot, link), get_aabb(obstacles[-1])))) #if can_collide(robot, link))) with timer('{:f}'): print(pairwise_collision(robot, obstacle))
def simulate_curve(robot, joints, curve): #set_joint_positions(robot, joints, curve(random.uniform(curve.x[0], curve.x[-1]))) wait_if_gui(message='Begin?') #controller = follow_curve_old(robot, joints, curve) controller = follow_curve(robot, joints, curve) for _ in controller: step_simulation() #wait_if_gui() #wait_for_duration(duration=time_step) #time.sleep(time_step) wait_if_gui(message='Finish?')
def get_element_collision_fn(robot, obstacles): joints = get_movable_joints(robot) disabled_collisions = get_disabled_collisions(robot) custom_limits = { } # get_custom_limits(robot) # specified within the kuka URDF robot_links = get_all_links(robot) # Base link isn't real #robot_links = get_links(robot) collision_fn = get_collision_fn(robot, joints, obstacles=[], attachments=[], self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, custom_limits=custom_limits) # TODO: precompute bounding boxes and manually check #for body in obstacles: # Calling before get_bodies_in_region does not act as step_simulation # get_aabb(body, link=None) step_simulation() # Okay to call only once and then just ignore the robot # TODO: call this once globally 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 return element_collision_fn
def add_camera(self, name, pose, camera_matrix, max_depth=KINECT_DEPTH, display=False): cone = get_viewcone(depth=max_depth, camera_matrix=camera_matrix, color=apply_alpha(RED, 0.1), mass=0, collision=False) set_pose(cone, pose) if display: kinect = load_pybullet(KINECT_URDF, fixed_base=True) set_pose(kinect, pose) set_color(kinect, BLACK) self.add(name, kinect) self.cameras[name] = Camera(cone, camera_matrix, max_depth) draw_pose(pose) step_simulation() return name
def rest_for_duration(self, duration): if not self.execute_motor_control: return sim_duration = 0.0 body = self.robot position_gain = 0.02 with ClientSaver(self.client): # TODO: apply to all joints dt = get_time_step() movable_joints = get_movable_joints(body) target_conf = get_joint_positions(body, movable_joints) while sim_duration < duration: p.setJointMotorControlArray( body, movable_joints, p.POSITION_CONTROL, targetPositions=target_conf, targetVelocities=[0.0] * len(movable_joints), positionGains=[position_gain] * len(movable_joints), # velocityGains=[velocity_gain] * len(movable_joints), physicsClientId=self.client) step_simulation() sim_duration += dt
def simulate(controller=None, max_duration=INF, max_steps=INF, print_rate=1., sleep=None): enable_gravity() dt = get_time_step() print('Time step: {:.6f} sec'.format(dt)) start_time = last_print = time.time() for step in irange(max_steps): duration = step * dt if duration >= max_duration: break if (controller is not None) and is_empty(controller): break step_simulation() synchronize_viewer() if elapsed_time(last_print) >= print_rate: print( 'Sim step: {} | Sim time: {:.3f} sec | Elapsed time: {:.3f} sec' .format(step, duration, elapsed_time(start_time))) last_print = time.time() if sleep is not None: time.sleep(sleep)
def follow_trajectory(self, joints, path, times_from_start=None, real_time_step=0.0, waypoint_tolerance=1e-2 * np.pi, goal_tolerance=5e-3 * np.pi, max_sim_duration=1.0, print_sim_frequency=1.0, **kwargs): # real_time_step = 1e-1 # Can optionally sleep to slow down visualization start_time = time.time() if times_from_start is not None: assert len(path) == len(times_from_start) current_positions = get_joint_positions(self.robot, joints) differences = [(np.array(q2) - np.array(q1)) / (t2 - t1) for q1, t1, q2, t2 in zip([current_positions] + path, [0.] + times_from_start, path, times_from_start)] velocity_path = differences[1:] + [np.zeros(len(joints)) ] # Using velocity at endpoints else: velocity_path = [None] * len(path) sim_duration = 0.0 sim_steps = 0 last_print_sim_duration = sim_duration with ClientSaver(self.client): dt = get_time_step() # TODO: fit using splines to get velocity info for num, positions in enumerate(path): if self.execute_motor_control: sim_start = sim_duration tolerance = goal_tolerance if (num == len(path) - 1) else waypoint_tolerance velocities = velocity_path[num] for _ in joint_controller_hold2(self.robot, joints, positions, velocities, tolerance=tolerance, **kwargs): step_simulation() # print(get_joint_velocities(self.robot, joints)) # print([get_joint_torque(self.robot, joint) for joint in joints]) sim_duration += dt sim_steps += 1 time.sleep(real_time_step) if print_sim_frequency <= (sim_duration - last_print_sim_duration): print( 'Waypoint: {} | Simulation steps: {} | Simulation seconds: {:.3f} | Steps/sec {:.3f}' .format(num, sim_steps, sim_duration, sim_steps / elapsed_time(start_time))) last_print_sim_duration = sim_duration if max_sim_duration <= (sim_duration - sim_start): print( 'Timeout of {:.3f} simulation seconds exceeded!' .format(max_sim_duration)) break # control_joints(self.robot, arm_joints, positions) else: set_joint_positions(self.robot, joints, positions) print( 'Followed {} waypoints in {:.3f} simulation seconds and {} simulation steps' .format(len(path), sim_duration, sim_steps))
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()
def close_gripper_test(problem): joints = joints_from_names(problem.robot, PR2_GROUPS['left_gripper']) values = [get_min_limit(problem.robot, joint) for joint in joints] for _ in joint_controller_hold(problem.robot, joints, values): enable_gravity() step_simulation()