def sample(self):
pos_arr = []
quat_arr = []
placed_objects = []
if (len(self.mujoco_objects) > len(self.pos_list)):
print("More objects than position given! breaking")
raise RandomizationError("Cannot place all objects on the desk")
index = -1
for obj_mjcf in self.mujoco_objects:
index += 1
horizontal_radius = obj_mjcf.get_horizontal_radius()
bottom_offset = obj_mjcf.get_bottom_offset()
object_x = self.pos_list[index][0]
object_y = self.pos_list[index][1]
# objects cannot overlap
location_valid = True
obj_count = -1
# There is no check for whether location is valid
pos = (self.table_top_offset - bottom_offset +
np.array([object_x, object_y, 0]))
placed_objects.append((object_x, object_y, horizontal_radius))
# random z-rotation
quat = [
np.cos(self.pos_list[index][2] / 2), 0, 0,
np.sin(self.pos_list[index][2] / 2)
]
quat_arr.append(quat)
pos_arr.append(pos)
return pos_arr, quat_arr
def sample(self):
pos_arr = []
quat_arr = []
placed_objects = []
for obj_name, obj_mjcf in self.mujoco_objects.items():
horizontal_radius = obj_mjcf.get_horizontal_radius()
bottom_offset = obj_mjcf.get_bottom_offset()
success = False
for i in range(5000): # 1000 retries
if obj_name.startswith("SquareNut"):
x_range = [
-self.table_size[0] / 2 + horizontal_radius,
-horizontal_radius,
]
y_range = [horizontal_radius, self.table_size[0] / 2]
else:
x_range = [
-self.table_size[0] / 2 + horizontal_radius,
-horizontal_radius,
]
y_range = [-self.table_size[0] / 2, -horizontal_radius]
object_x = self.sample_x(horizontal_radius, x_range=x_range)
object_y = self.sample_y(horizontal_radius, y_range=y_range)
object_z = self.sample_z(0.01)
# objects cannot overlap
location_valid = True
pos = (self.table_top_offset - bottom_offset +
np.array([object_x, object_y, object_z]))
for pos2, r in placed_objects:
if (np.linalg.norm(pos - pos2, 2) <= r + horizontal_radius
and abs(pos[2] - pos2[2]) < 0.021):
location_valid = False
break
if location_valid:
# location is valid, put the object down
placed_objects.append((pos, horizontal_radius))
# random z-rotation
quat = self.sample_quat()
quat_arr.append(quat)
pos_arr.append(pos)
success = True
break
# bad luck, reroll
if not success:
raise RandomizationError(
"Cannot place all objects on the desk")
return pos_arr, quat_arr
def place_objects_plus_fixed(self):
"""Places objects randomly until no collisions or max iterations hit.
and the object in use to a defined location
"""
placed_objects = []
index = 0
# place objects by rejection sampling
for _, obj_mjcf in self.mujoco_objects.items():
horizontal_radius = obj_mjcf.get_horizontal_radius()
bottom_offset = obj_mjcf.get_bottom_offset()
success = False
# Fixing position of defined object
if index == self.obj_id:
pos = self.obj_fixed_loc[0]
placed_objects.append((pos, horizontal_radius))
self.objects[index].set("pos", array_to_string(pos))
quat = self.obj_fixed_loc[1]
self.objects[index].set("quat", array_to_string(quat))
success = True
continue
for _ in range(5000): # 5000 retries
bin_x_half = self.bin_size[0] / 2 - horizontal_radius - 0.05
bin_y_half = self.bin_size[1] / 2 - horizontal_radius - 0.05
object_x = np.random.uniform(high=bin_x_half, low=-bin_x_half)
object_y = np.random.uniform(high=bin_y_half, low=-bin_y_half)
# make sure objects do not overlap
object_xy = np.array([object_x, object_y, 0])
pos = self.bin_offset - bottom_offset + object_xy
location_valid = True
for pos2, r in placed_objects:
dist = np.linalg.norm(pos[:2] - pos2[:2], np.inf)
if dist <= r + horizontal_radius:
location_valid = False
break
# place the object
if location_valid:
# add object to the position
placed_objects.append((pos, horizontal_radius))
self.objects[index].set("pos", array_to_string(pos))
# random z-rotation
quat = self.sample_quat()
self.objects[index].set("quat", array_to_string(quat))
success = True
break
# raise error if all objects cannot be placed after maximum retries
if not success:
raise RandomizationError("Cannot place all objects in the bins")
index += 1
def sample(self):
pos_arr = []
quat_arr = []
placed_objects = []
index = 0
for i, obj_mjcf in enumerate(self.mujoco_objects):
horizontal_radius = obj_mjcf.get_horizontal_radius()
bottom_offset = obj_mjcf.get_bottom_offset()
success = False
for i in range(5000): # 1000 retries
object_x = self.sample_x(horizontal_radius, i)
object_y = self.sample_y(horizontal_radius, i)
# objects cannot overlap
location_valid = True
for x, y, r in placed_objects:
if (np.linalg.norm([object_x - x, object_y - y], 2) <=
r + horizontal_radius):
location_valid = False
break
if location_valid:
# location is valid, put the object down
pos = (self.table_top_offset - bottom_offset +
np.array([object_x, object_y, 0]))
placed_objects.append(
(object_x, object_y, horizontal_radius))
# random z-rotation
quat = self.sample_quat()
quat_arr.append(quat)
pos_arr.append(pos)
success = True
break
# bad luck, reroll
if not success:
raise RandomizationError(
"Cannot place all objects on the desk")
index += 1
return pos_arr, quat_arr
def sample(self, fixtures=None, reference=None, on_top=True):
"""
Uniformly sample relative to this sampler's reference_pos or @reference (if specified).
Args:
fixtures (dict): dictionary of current object placements in the scene as well as any other relevant
obstacles that should not be in contact with newly sampled objects. Used to make sure newly
generated placements are valid. Should be object names mapped to (pos, quat, MujocoObject)
reference (str or 3-tuple or None): if provided, sample relative placement. Can either be a string, which
corresponds to an existing object found in @fixtures, or a direct (x,y,z) value. If None, will sample
relative to this sampler's `'reference_pos'` value.
on_top (bool): if True, sample placement on top of the reference object. This corresponds to a sampled
z-offset of the current sampled object's bottom_offset + the reference object's top_offset
(if specified)
Return:
dict: dictionary of all object placements, mapping object_names to (pos, quat, obj), including the
placements specified in @fixtures. Note quat is in (w,x,y,z) form
Raises:
RandomizationError: [Cannot place all objects]
AssertionError: [Reference object name does not exist, invalid inputs]
"""
# Standardize inputs
placed_objects = {} if fixtures is None else copy(fixtures)
if reference is None:
base_offset = self.reference_pos
elif type(reference) is str:
assert reference in placed_objects, "Invalid reference received. Current options are: {}, requested: {}"\
.format(placed_objects.keys(), reference)
ref_pos, _, ref_obj = placed_objects[reference]
base_offset = np.array(ref_pos)
if on_top:
base_offset += np.array((0, 0, ref_obj.top_offset[-1]))
else:
base_offset = np.array(reference)
assert base_offset.shape[0] == 3, "Invalid reference received. Should be (x,y,z) 3-tuple, but got: {}"\
.format(base_offset)
# Sample pos and quat for all objects assigned to this sampler
for obj in self.mujoco_objects:
# First make sure the currently sampled object hasn't already been sampled
assert obj.name not in placed_objects, "Object '{}' has already been sampled!".format(
obj.name)
horizontal_radius = obj.horizontal_radius
bottom_offset = obj.bottom_offset
success = False
for i in range(5000): # 5000 retries
object_x = self._sample_x(horizontal_radius) + base_offset[0]
object_y = self._sample_y(horizontal_radius) + base_offset[1]
object_z = self.z_offset + base_offset[2]
if on_top:
object_z -= bottom_offset[-1]
# objects cannot overlap
location_valid = True
if self.ensure_valid_placement:
for (x, y, z), _, other_obj in placed_objects.values():
if (np.linalg.norm((object_x - x, object_y - y)) <=
other_obj.horizontal_radius + horizontal_radius
) and (object_z - z <= other_obj.top_offset[-1] -
bottom_offset[-1]):
location_valid = False
break
if location_valid:
# random rotation
quat = self._sample_quat()
# multiply this quat by the object's initial rotation if it has the attribute specified
if hasattr(obj, "init_quat"):
quat = quat_multiply(quat, obj.init_quat)
# location is valid, put the object down
pos = (object_x, object_y, object_z)
placed_objects[obj.name] = (pos, quat, obj)
success = True
break
if not success:
raise RandomizationError("Cannot place all objects ):")
return placed_objects
def sample(self,
fixtures=None,
return_placements=False,
reference_object_name=None,
sample_on_top=False):
"""
Uniformly sample on a surface (not necessarily table surface).
Args:
fixtures (dict): current dictionary of object placements in the scene. Used to make sure
generated placements are valid.
return_placements (bool): if True, return the updated dictionary
of object placements.
reference_object_name (str): if provided, sample placement relative to this object's
placement (which must be provided in @fixtures).
sample_on_top (bool): if True, sample placement on top of the reference object.
Return:
2-tuple or 3-tuple:
- (list) list of placed object positions
- (list) list of placed object quaternions
- (dict) if @return_placements is True, returns a dictionary of all
object placements, including the ones placed by this sampler.
Raises:
RandomizationError: [Cannot place all objects]
AssertionError: [Reference object name does not exist]
"""
pos_arr = []
quat_arr = []
if fixtures is None:
placed_objects = {}
else:
placed_objects = deepcopy(fixtures)
# compute reference position
base_offset = self.table_top_offset
if reference_object_name is not None:
assert reference_object_name in placed_objects
reference_pos, reference_mjcf = placed_objects[
reference_object_name]
base_offset[:2] = reference_pos[:2]
if sample_on_top:
base_offset[-1] = reference_pos[
-1] + reference_mjcf.get_top_offset()[-1] # set surface z
index = 0
for obj_name, obj_mjcf in self.mujoco_objects.items():
horizontal_radius = obj_mjcf.get_horizontal_radius()
bottom_offset = obj_mjcf.get_bottom_offset()
success = False
for i in range(5000): # 5000 retries
object_x = self.sample_x(horizontal_radius) + base_offset[0]
object_y = self.sample_y(horizontal_radius) + base_offset[1]
object_z = base_offset[2] + self.z_offset - bottom_offset[-1]
# objects cannot overlap
location_valid = True
for (x, y, z), other_obj_mjcf in placed_objects.values():
if (np.linalg.norm([object_x - x, object_y - y], 2) <=
other_obj_mjcf.get_horizontal_radius() +
horizontal_radius) and (
object_z - z <=
other_obj_mjcf.get_top_offset()[-1] -
bottom_offset[-1]):
location_valid = False
break
if location_valid:
# location is valid, put the object down
pos = (object_x, object_y, object_z)
placed_objects[obj_name] = (pos, obj_mjcf)
# random z-rotation
quat = self.sample_quat()
# multiply this quat by the object's initial rotation if it has the attribute specified
if hasattr(obj_mjcf, "init_quat"):
quat = quat_multiply(quat, obj_mjcf.init_quat)
quat_arr.append(quat)
pos_arr.append(pos)
success = True
break
if not success:
raise RandomizationError(
"Cannot place all objects on the desk")
index += 1
if return_placements:
return pos_arr, quat_arr, placed_objects
return pos_arr, quat_arr