def grid_search_region_placement(oracle,
object_name,
region_name,
attempts=INF,
dtheta=PI / 16):
obj = oracle.bodies[object_name]
region = oracle.regions[region_name]
with oracle.body_saver(object_name):
orientations = [] #deque()
for theta in irange(
2 * pi, step=dtheta
): # TODO - binary search over successively smaller dthetas?
set_quat(obj, quat_from_axis_angle(0, 0, theta))
aabb = aabb_from_body(obj)
ranges = []
for k in range(2):
low, high = region.box[k][0] + aabb.extents(
)[k], region.box[k][1] - aabb.extents()[k]
if low > high: break
ranges.append((low, high))
if len(ranges) == 2:
orientations.append((theta, tuple(ranges)))
while len(orientations) != 0:
theta, ranges = choice(orientations)
set_quat(obj, quat_from_axis_angle(0, 0, uniform(0, 2 * PI)))
point = [uniform(low, high) for low, high in ranges] + [
region.z + aabb.extents()[2] + REGION_PLACEMENT_Z_OFFSET
]
set_point(obj, point + get_point(obj) - aabb.pos())
if not (obstacle_collision(oracle, object_name)
or object_collision(oracle, object_name)):
return oracle.get_pose(object_name)
return None
def cached_region_placements(oracle,
body_name,
region_names,
order=None,
random=False):
if random:
poses = randomize(
reduce(operator.add,
(oracle.random_placement_cache(body_name, region_name)
for region_name in region_names)))
else:
poses = randomize(
reduce(operator.add,
(oracle.grid_placement_cache(body_name, region_name)
for region_name in region_names)))
# TODO - merge them
if order is not None: poses = order(poses)
body_saver = oracle.body_saver(body_name)
for pose in poses:
oracle.set_pose(body_name, pose)
if not hasattr(pose, 'obstacle_collision'):
pose.obstacle_collision = obstacle_collision(oracle, body_name)
if not (pose.obstacle_collision or object_collision(
oracle, body_name)): # TODO - cache object_collision as well
body_saver.Restore()
yield pose
def random_region_placements(oracle,
body_name,
region_names,
region_weights=False,
max_failures=INF):
if region_weights is True:
region_weights = weight_regions(oracle, body_name, region_names)
sample_region = lambda: (choice(region_names) if region_weights is False
else sample_categorical(region_weights))
body_saver = oracle.body_saver(body_name)
failures = 0
while failures <= max_failures:
failures += 1
quat = quat_from_z_rot(uniform(0, 2 * PI))
aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
region = oracle.get_region(sample_region())
cspace = region.cspace(aabb)
if cspace is None: continue
point = np.array(
[uniform(*range) for range in cspace] +
[region.z + aabb.extents()[2] +
REGION_PLACEMENT_Z_OFFSET]) - aabb.pos(
) # NOTE - okay because the aabb is relative to unit_trans()
pose = pose_from_quat_point(quat, point)
set_pose(oracle.get_body(body_name), pose)
if not (obstacle_collision(oracle, body_name)
or object_collision(oracle, body_name)):
body_saver.Restore()
yield Pose(pose)
failures = 0
def random_edge_placements(oracle,
body_name,
region_name,
z=None,
use_quat=None,
bias_point=None,
max_failures=INF):
body_saver = oracle.body_saver(body_name)
edge_dist = ClosestEdgeDistribution(oracle.get_region(region_name), bias_point) if bias_point is not None \
else UniformEdgeDistribution(oracle.get_region(region_name))
failures = 0
while failures <= max_failures:
failures += 1
quat = quat_from_z_rot(uniform(0, 2 *
PI)) if use_quat is None else use_quat
aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
point = edge_dist.sample() + (aabb.extents(
)[2] + REGION_PLACEMENT_Z_OFFSET) * unit_z() - aabb.pos()
if z is not None: point[2] = z
pose = pose_from_quat_point(quat, point)
set_pose(oracle.get_body(body_name), pose)
if not (obstacle_collision(oracle, body_name)
or object_collision(oracle, body_name)):
body_saver.Restore()
yield Pose(pose)
failures = 0
def center_stackings(
oracle,
body_name,
stackings,
max_failures=INF): # NOTE - aligns the center of the bounding boxes
body_saver = oracle.body_saver(body_name)
failures = 0
while failures <= max_failures:
failures += 1
quat = quat_from_z_rot(uniform(0, 2 * PI))
aabb = oracle.get_aabb(body_name, trans_from_quat(quat))
base_name, base_pose = choice(stackings)
base_aabb = oracle.get_aabb(base_name,
trans_from_pose(base_pose.value))
point = base_aabb.pos() + (base_aabb.extents()[2] + aabb.extents(
)[2] + REGION_PLACEMENT_Z_OFFSET) * unit_z() - aabb.pos()
pose = Pose(pose_from_quat_point(quat, point))
oracle.set_pose(body_name, pose)
if not (obstacle_collision(oracle, body_name)
or object_collision(oracle, body_name)):
body_saver.Restore()
yield pose
failures = 0