class Overlaid(KinematicsMixin, RelativeObjectState, BooleanState):
@staticmethod
def get_dependencies():
return KinematicsMixin.get_dependencies() + RelativeObjectState.get_dependencies() + [Touching]
def _set_value(self, other, new_value):
if not new_value:
raise NotImplementedError("Overlaid does not support set_value(False)")
if not (self.obj.prim_type == PrimType.CLOTH and other.prim_type == PrimType.RIGID):
raise ValueError("Overlaid state requires obj1 is cloth and obj2 is rigid.")
state = og.sim.dump_state(serialized=False)
# Get the top center of the object below
aabb_low, aabb_hi = other.states[AABB].get_value()
top_center = np.array([(aabb_low[0] + aabb_hi[0]) / 2.0, (aabb_low[1] + aabb_hi[1]) / 2.0, aabb_hi[2]])
# Reset the cloth
self.obj.root_link.reset()
# Add the half-extent of the object on top in the z-axis with additional offset
aabb_low, aabb_hi = self.obj.states[AABB].get_value()
pos = top_center + np.array([0., 0., (aabb_hi - aabb_low)[2] / 2.0 + m.SAMPLING_Z_OFFSET])
for _ in range(10):
# Use a random orientation in the z-axis
orn = T.euler2quat(np.array([0., 0., np.random.uniform(0, np.pi * 2)]))
self.obj.set_position_orientation(pos, orn)
self.obj.root_link.reset()
self.obj.keep_still()
# Let it fall for 0.2 second
for _ in range(int(0.2 / og.sim.get_physics_dt())):
og.sim.step_physics()
if len(self.obj.states[ContactBodies].get_value()) > 0:
break
self.obj.keep_still()
if self.get_value(other) == new_value:
return True
else:
og.sim.load_state(state, serialized=False)
return False
def _get_value(self, other):
"""
Check whether the (cloth) object is overlaid on the other (rigid) object.
First, the two objects need to be Touching.
Then, the convex hull of the particles of the cloth object needs to cover a decent percentage of the
base aligned bounding box of the other rigid object.
"""
if not (self.obj.prim_type == PrimType.CLOTH and other.prim_type == PrimType.RIGID):
raise ValueError("Overlaid state requires obj1 is cloth and obj2 is rigid.")
# Make sure the two objects are touching.
touching = self.obj.states[Touching].get_value(other)
if not touching:
return False
# Compute the convex hull of the particles of the cloth object.
points = self.obj.root_link.keypoint_particle_positions[:, :2]
cloth_hull = ConvexHull(points)
# Compute the base aligned bounding box of the rigid object.
bbox_center, bbox_orn, bbox_extent, _ = other.get_base_aligned_bbox(xy_aligned=True)
vertices_local = np.array(list(itertools.product((1, -1), repeat=3))) * (bbox_extent / 2)
vertices = trimesh.transformations.transform_points(vertices_local, T.pose2mat((bbox_center, bbox_orn)))
rigid_hull = ConvexHull(vertices[:, :2])
# The goal is to find the intersection of the convex hull and the bounding box.
# We can do so with HalfspaceIntersection, which takes as input a list of equations that define the half spaces,
# and an interior point. We assume the center of the bounding box is an interior point.
interior_pt = vertices.mean(axis=0)[:2]
half_spaces = np.vstack((cloth_hull.equations, rigid_hull.equations))
try:
half_space_intersection = HalfspaceIntersection(half_spaces, interior_pt)
except QhullError:
# The bbox center of the rigid body does not lie in the intersection, return False.
return False
# Compute the ratio between the intersection area and the bounding box area in the x-y plane.
# When input points are 2-dimensional, this is the area of the convex hull.
# Ref: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.ConvexHull.html
intersection_area = ConvexHull(half_space_intersection.intersections).volume
rigid_xy_area = bbox_extent[0] * bbox_extent[1]
return (intersection_area / rigid_xy_area) > m.OVERLAP_AREA_PERCENTAGE