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