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 main():
# TODO: move to pybullet-planning for now
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(args)
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1)
floor = create_floor()
with HideOutput():
robot = load_pybullet(get_model_path(ROOMBA_URDF),
fixed_base=True,
scale=2.0)
for link in get_all_links(robot):
set_color(robot, link=link, color=ORANGE)
robot_z = stable_z(robot, floor)
set_point(robot, Point(z=robot_z))
#set_base_conf(robot, rover_confs[i])
data_path = add_data_path()
shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
fixed_base=True) # TODO: returns a tuple
dump_body(shelf)
#draw_aabb(get_aabb(shelf))
wait_for_user()
disconnect()
def create_gripper(robot, visual=False):
gripper_link = link_from_name(robot, get_gripper_link(robot))
links = get_link_descendants(robot, gripper_link) # get_link_descendants | get_link_subtree
with LockRenderer():
# Actually uses the parent of the first link
gripper = clone_body(robot, links=links, visual=False, collision=True) # TODO: joint limits
if not visual:
for link in get_all_links(gripper):
set_color(gripper, np.zeros(4), link)
#dump_body(robot)
#dump_body(gripper)
#user_input()
return gripper
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_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 test_ycb(world):
name = 'mustard_bottle' # potted_meat_can | cracker_box | sugar_box | mustard_bottle | bowl
path_from_name = get_ycb_objects()
print(path_from_name)
ycb_directory = os.path.join(YCB_DIRECTORY, path_from_name[name], SENSOR)
obj_path = os.path.join(ycb_directory, OBJ_PATH)
image_path = os.path.join(ycb_directory, PNG_PATH)
print(obj_path)
#body = load_pybullet(obj_path) #, fixed_base=True)
body = create_obj(obj_path, color=WHITE)
set_texture(body, image_path)
for link in get_all_links(body):
set_color(body, link=link, color=WHITE)
wait_if_gui()
def __init__(self, robot, ee_link, tool_link, color=TRANSPARENT, **kwargs):
self.robot = robot
self.ee_link = ee_link
self.tool_link = tool_link
self.tool_from_ee = get_relative_pose(self.robot, self.ee_link,
self.tool_link)
tool_links = get_link_subtree(robot, self.ee_link)
self.body = clone_body(robot,
links=tool_links,
visual=False,
collision=True,
**kwargs)
set_static(self.body)
if color is not None:
for link in get_all_links(self.body):
set_color(self.body, color, link) # None = white
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
