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()
