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