def is_safe(self, elements, element_bodies):
# TODO: check the end-effector first
known_elements = set(self.safe_per_element) & set(elements)
if not all(self.safe_per_element[e] for e in known_elements):
return False
unknown_elements = randomize(set(elements) - known_elements)
if not unknown_elements:
return True
for trajectory in randomize(
self.trajectories
): # TODO: could cache each individual collision
intersecting = trajectory.get_intersecting()
for i in randomize(range(len(trajectory))):
set_joint_positions(trajectory.robot, trajectory.joints,
trajectory.path[i])
for element in unknown_elements:
body = element_bodies[element]
#if not pairwise_collision(trajectory.robot, body):
# self.set_unsafe(element)
# return False
for robot_link, bodies in intersecting[i].items():
#print(robot_link, bodies, len(bodies))
if (element_bodies[element]
in bodies) and pairwise_link_collision(
trajectory.robot,
robot_link,
body,
link2=BASE_LINK):
self.set_unsafe(element)
return False
self.update_safe(elements)
return True
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 test(name1, command1, name2, command2):
robot1, robot2 = index_from_name(robots, name1), index_from_name(
robots, name2)
if (robot1 == robot2) or not collisions:
return False
# TODO: check collisions between pairs of inflated adjacent element
for traj1, traj2 in randomize(
product(command1.trajectories, command2.trajectories)):
# TODO: use swept aabbs for element checks
aabbs1, aabbs2 = traj1.get_aabbs(), traj2.get_aabbs()
swept_aabbs1 = {
link: aabb_union(link_aabbs[link] for link_aabbs in aabbs1)
for link in aabbs1[0]
}
swept_aabbs2 = {
link: aabb_union(link_aabbs[link] for link_aabbs in aabbs2)
for link in aabbs2[0]
}
swept_overlap = [
(link1, link2)
for link1, link2 in product(swept_aabbs1, swept_aabbs2)
if aabb_overlap(swept_aabbs1[link1], swept_aabbs2[link2])
]
if not swept_overlap:
continue
# for l1 in set(map(itemgetter(0), swept_overlap)):
# draw_aabb(swept_aabbs1[l1], color=RED)
# for l2 in set(map(itemgetter(1), swept_overlap)):
# draw_aabb(swept_aabbs2[l2], color=BLUE)
for index1, index2 in product(randomize(range(len(traj1.path))),
randomize(range(len(traj2.path)))):
overlap = [(link1, link2)
for link1, link2 in swept_overlap if aabb_overlap(
aabbs1[index1][link1], aabbs2[index2][link2])]
#overlap = list(product(aabbs1[index1], aabbs2[index2]))
if not overlap:
continue
set_configuration(robot1, traj1.path[index1])
set_configuration(robot2, traj2.path[index2])
#wait_if_gui()
#if pairwise_collision(robot1, robot2):
# return True
for link1, link2 in overlap:
if pairwise_link_collision(robot1, link1, robot2, link2):
#wait_if_gui()
return True
return False
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()