class Inside(TensorizedRelativeState, KinematicsMixin, BooleanStateMixin):
# Used by _inside_aabb_prefilter_kernel and the halfspace_test/mesh_reduce kernels to read
# inner AABB centers and container AABBs from AABB.VALUES_WP.
_aabb_idx = None # wp.array (N,) int32 — Inside-N → AABB-N
# All container-meta-link visual meshes across every scene are flattened into
# one global table of length M. M = total number of containers scross scenes
# Which *container* object that owns this mesh, or -1 if
# the parent object isn't Inside-tracked.
_mesh_container_idx = None # wp.array (M,) int32
# Scene index of the container that owns this mesh.
_mesh_scene_idx = None # wp.array (M,) int32
# RigidBodyViewAPI flat index of the parent link.
_mesh_parent_link = None # wp.array (M,) int32
# Static matrix transforming a point in parent-link-frame into mesh-local-unscaled frame.
# inv_world_kernel composes this with rigid_inverse_mat44(parent_link_world) each step.
_mesh_inv_local_w_scale = None # wp.array (M,) mat44
# Reverse lookup: which mesh does each face belong to.
_face_to_mesh = None # wp.array (F_total,) int32
# Flat per-face data for all container meshes, concatenated end-to-end.
_face_centroid = None # wp.array (F_total,) vec3
_face_normal = None # wp.array (F_total,) vec3
# Scratches used by kernels.
# Per-mesh inverse "world → mesh-local-unscaled" transform.
# Written by inv_world_kernel each step; consumed by halfspace_test_kernel.
_inv_world = None # wp.array (M,) mat44
# Scratch written by aabb_prefilter_kernel (1 if inner_i's AABB center
# lies inside container_j's AABB, else 0).
_prefilter = None # wp.array3d (S, N, N) int32
# Per-mesh "saw at least one failing halfspace" flag, atomic_max target.
_outside_flag = None # wp.array3d (S, N, M) int32
# atomic_max target for "any mesh of container_j contains inner_i's center".
_pair_scratch = None # wp.array3d (S, N, N) int32
@classproperty
def value_shape(cls):
return ()
@classproperty
def value_type(cls):
return th.bool
@classproperty
def value_name(cls):
return "inside"
@classmethod
def get_dependencies(cls):
deps = super().get_dependencies()
deps.add(AABB)
return deps
@classmethod
def global_initialize(cls):
super().global_initialize()
cls._aabb_idx = None
cls._mesh_container_idx = None
cls._mesh_scene_idx = None
cls._mesh_parent_link = None
cls._mesh_inv_local_w_scale = None
cls._face_to_mesh = None
cls._face_centroid = None
cls._face_normal = None
cls._inv_world = None
cls._prefilter = None
cls._outside_flag = None
cls._pair_scratch = None
@classmethod
def initialize_view(cls):
super().initialize_view()
S = len(cls.IDX_OBJS)
N = len(cls.OBJ_IDXS)
if S == 0 or N == 0:
cls._aabb_idx = None
cls._inv_world = None
cls._prefilter = None
cls._pair_scratch = None
return
# Build Inside-N → AABB-N
aabb_idx_cpu = th.full((N,), -1, dtype=th.int32)
aabb_map = AABB.OBJ_IDXS or {}
for rel_path, idx in cls.OBJ_IDXS.items():
aabb_idx_cpu[idx] = aabb_map.get(rel_path, -1)
cls._aabb_idx = lazy.isaacsim.core.utils.warp.tensor.create_tensor_from_list(
aabb_idx_cpu, "int32", device="cuda"
)
# Walk every Inside-tracked object's container meta-links and collect each visual mesh.
# Only USD Mesh-typed visual meshes are supported; primitive types are skipped.
mesh_records = [] # list of dicts; rolled into the flat tables below.
face_centroids_list = [] # CPU torch tensors, concatenated at the end
face_normals_list = []
face_to_mesh_list = [] # CPU ints; for each face f appends the index of its owning mesh in mesh_records.
for scene_idx, scene_row in enumerate(cls.IDX_OBJS):
for container_idx, container_obj in enumerate(scene_row):
if container_obj is None:
continue
if container_obj.prim_type == PrimType.CLOTH:
continue
for link in container_obj.links.values():
if not link.is_meta_link:
continue
if link.meta_link_type not in contains_m.CONTAINER_META_LINK_TYPES:
continue
parent_flat = RigidBodyViewAPI.get_flat_idx(link.prim_path)
if parent_flat is None:
continue
# Link's rigid world transform at init time, used to derive the static
# mesh→link-local-w-scale piece.
link_world_init = T.pose2mat(link.get_position_orientation())
link_world_inv_init = th.linalg.inv(link_world_init)
for mesh in link.visual_meshes.values():
if mesh._mesh_type != "Mesh":
continue
# Build one outward-facing halfspace per convex hull facet, matching
# the hull that GeomPrim.check_local_points_in_volume() tests against.
hull_faces = th.as_tensor(mesh.delaunay_triangulation.convex_hull, dtype=th.long)
hull_vertices = mesh.points[hull_faces]
centroids = hull_vertices.mean(dim=1) # (F_hull, 3) local-unscaled
edge1 = hull_vertices[:, 1] - hull_vertices[:, 0]
edge2 = hull_vertices[:, 2] - hull_vertices[:, 0]
normals = th.cross(edge1, edge2, dim=1)
normal_lengths = th.linalg.vector_norm(normals, dim=1, keepdim=True)
normals = normals / th.clamp(normal_lengths, min=1e-8)
normals = _orient_face_normals_outward(
points=mesh.points,
face_centroids=centroids,
face_normals=normals,
) # (F_hull, 3) local-unscaled
face_count = centroids.shape[0]
if face_count == 0:
continue
mesh_scaled_world_init = mesh.scaled_transform
local_w_scale = link_world_inv_init @ mesh_scaled_world_init
inv_local_w_scale = th.linalg.inv(local_w_scale)
mesh_idx = len(mesh_records)
face_centroids_list.append(centroids.to(th.float32))
face_normals_list.append(normals.to(th.float32))
face_to_mesh_list.extend([mesh_idx] * face_count)
mesh_records.append(
{
"container": container_idx,
"scene": scene_idx,
"parent_link": parent_flat,
"inv_local_w_scale": inv_local_w_scale.to(th.float32),
}
)
M = len(mesh_records)
if M == 0:
cls._mesh_container_idx = None
cls._mesh_scene_idx = None
cls._mesh_parent_link = None
cls._mesh_inv_local_w_scale = None
cls._face_to_mesh = None
cls._face_centroid = None
cls._face_normal = None
cls._inv_world = None
cls._outside_flag = None
else:
cls._mesh_container_idx = lazy.isaacsim.core.utils.warp.tensor.create_tensor_from_list(
[r["container"] for r in mesh_records], "int32", device="cuda"
)
cls._mesh_scene_idx = lazy.isaacsim.core.utils.warp.tensor.create_tensor_from_list(
[r["scene"] for r in mesh_records], "int32", device="cuda"
)
cls._mesh_parent_link = lazy.isaacsim.core.utils.warp.tensor.create_tensor_from_list(
[r["parent_link"] for r in mesh_records], "int32", device="cuda"
)
# mat44 / vec3 have no scalar-only helper — wp.array reinterprets the CPU torch
# buffer's (M, 4, 4) float32 as (M,) mat44 and (F, 3) float32 as (F,) vec3.
inv_local_stack_cpu = th.stack([r["inv_local_w_scale"] for r in mesh_records]) # (M, 4, 4) CPU
cls._mesh_inv_local_w_scale = wp.array(inv_local_stack_cpu, dtype=wp.mat44, device="cuda")
# Flat face arrays. M > 0 here and every retained mesh contributed at least one
# face (zero-face meshes are filtered above), so F_total > 0.
face_centroids_flat_cpu = th.cat(face_centroids_list, dim=0).contiguous() # (F, 3) CPU
face_normals_flat_cpu = th.cat(face_normals_list, dim=0).contiguous() # (F, 3) CPU
cls._face_centroid = wp.array(face_centroids_flat_cpu, dtype=wp.vec3, device="cuda")
cls._face_normal = wp.array(face_normals_flat_cpu, dtype=wp.vec3, device="cuda")
cls._face_to_mesh = lazy.isaacsim.core.utils.warp.tensor.create_tensor_from_list(
face_to_mesh_list, "int32", device="cuda"
)
# Per-step scratch — allocate directly as wp.array.
cls._inv_world = wp.zeros(M, dtype=wp.mat44, device="cuda")
cls._outside_flag = wp.zeros((S, N, M), dtype=wp.int32, device="cuda")
cls._prefilter = wp.zeros((S, N, N), dtype=wp.int32, device="cuda")
cls._pair_scratch = wp.zeros((S, N, N), dtype=wp.int32, device="cuda")
@classmethod
def _update_values(cls, values):
if (
cls.VALUES_WP is None
or cls._aabb_idx is None
or cls._prefilter is None
or cls._pair_scratch is None
or AABB.VALUES_WP is None
or RigidBodyViewAPI.POSE_MATRICES is None
):
return
S, N, _ = values.shape
if S == 0 or N == 0:
return
cls._pair_scratch.zero_()
wp.launch(
kernel=_inside_aabb_prefilter_kernel,
dim=(S, N, N),
inputs=[AABB.VALUES_WP, cls._aabb_idx, cls._prefilter],
device="cuda",
)
if cls._mesh_container_idx is not None:
M = cls._mesh_parent_link.shape[0]
F_total = cls._face_centroid.shape[0]
# refresh per-mesh world→local inverse from current link poses.
wp.launch(
kernel=_inside_inv_world_kernel,
dim=M,
inputs=[
RigidBodyViewAPI.POSE_MATRICES,
cls._mesh_parent_link,
cls._mesh_inv_local_w_scale,
cls._inv_world,
],
device="cuda",
)
# Each face independently votes "outside" via atomic_max into outside_flag.
# Mesh "contains" iff no face voted outside → reduce kernel writes pair_scratch.
cls._outside_flag.zero_()
wp.launch(
kernel=_inside_halfspace_test_kernel,
dim=(S, N, F_total),
inputs=[
AABB.VALUES_WP,
cls._aabb_idx,
cls._prefilter,
cls._inv_world,
cls._face_to_mesh,
cls._mesh_container_idx,
cls._mesh_scene_idx,
cls._face_centroid,
cls._face_normal,
cls._outside_flag,
],
device="cuda",
)
wp.launch(
kernel=_inside_mesh_reduce_kernel,
dim=(S, N, M),
inputs=[
cls._aabb_idx,
cls._prefilter,
cls._mesh_container_idx,
cls._mesh_scene_idx,
cls._outside_flag,
cls._pair_scratch,
],
device="cuda",
)
# int32 pair to uint8 VALUES, zero diagonal
wp.launch(
kernel=_inside_finalize_kernel,
dim=(S, N, N),
inputs=[cls._pair_scratch, cls.VALUES_WP],
device="cuda",
)
def _get_value(self, other):
if other.prim_type == PrimType.CLOTH:
raise ValueError("Cannot detect if an object is inside a cloth object.")
return super()._get_value(other)
def _set_value(self, other, new_value, reset_before_sampling=False, use_trav_map=False):
"""
Set the Inside state for this object with respect to another object (container).
This samples a random position inside the container's fillable volume, places the object,
lets it settle via physics, and verifies it's still inside.
The sampling strategy uses a two-phase approach:
1. First half of attempts: Sample from inset AABB (container bounds minus object extent).
This ensures the sampled position is at the object's centroid, not near the edges,
which makes it more likely to fit inside.
2. Second half of attempts: Sample from full container AABB. This is a fallback for
cases where the object is too large to fit entirely within the inset bounds.
Each candidate pose passes through several rejection-sampling stages:
1. The sampled point must lie inside the container's fillable volume.
2. After placement and a single physics step, the object must not already be
intersecting anything (catches interpenetration with container walls).
3. The object must come into contact with something after placement and half a second of
physics steps.
4. After settling, the container's root pose must not have moved
beyond CONTAINER_POSITION_CHANGE_THRESHOLD / CONTAINER_ORIENTATION_CHANGE_THRESHOLD.
5. The container's articulated joints must not have moved beyond the per-DOF-type
CONTAINER_JOINT_POSITION_DELTA_THRESHOLD_{TRANSLATION,ROTATION} thresholds
(catches cases where the placed object swings a door/lid or pushes a drawer).
6. The object must still register as Inside the container after settling, and
reachable via the traversability map if use_trav_map is enabled.
Args:
other: The container object to place this object inside.
new_value: True to set Inside state (only True is supported).
reset_before_sampling: If True, reset this object before sampling.
use_trav_map: Whether to use traversability-based reachability checks.
Returns:
True if successfully placed inside, False otherwise.
"""
if not new_value:
raise NotImplementedError("Inside does not support set_value(False)")
if other.prim_type == PrimType.CLOTH:
raise ValueError("Cannot set an object inside a cloth object.")
# Save the initial position and orientation of the container
container_pos_initial, container_orn_initial = other.get_position_orientation()
container_joint_positions_initial = other.get_joint_positions() if other.n_joints > 0 else None
# Find the container's fillable meta link (fillable or openfillable)
container_link = None
for link in other.links.values():
if link.is_meta_link and link.meta_link_type in macros.object_states.contains.CONTAINER_META_LINK_TYPES:
container_link = link
break
assert container_link is not None, f"Container object {other.name} must have a fillable meta link"
# Save simulator state for restoration on failed attempts
state = og.sim.dump_state(serialized=False)
if reset_before_sampling:
self.obj.reset()
# Get container's fillable volume bounds in world frame
aabb_low, aabb_high = container_link.visual_aabb
# Get the object extent to compute inset bounds
obj_extent = self.obj.aabb_extent
# Inset the container AABB by half the object extent in each dimension.
# This ensures the sampled position (used as object centroid) won't place
# any part of the object outside the container bounds.
inset_aabb_low = aabb_low + obj_extent / 2.0
inset_aabb_high = aabb_high - obj_extent / 2.0
# Calculate the total attempt count. Here we don't have a sense of high/low-level attempts,
# so to make the same numbr of attempts as the original implementation, we just multiply
# the two sampling parameters.
total_attempts = os_m.DEFAULT_HIGH_LEVEL_SAMPLING_ATTEMPTS * os_m.DEFAULT_LOW_LEVEL_SAMPLING_ATTEMPTS
if use_trav_map:
reachability_context = get_reachability_sampling_context(
objB=other,
predicate="inside",
use_trav_map=use_trav_map,
warn_on_scene_mismatch=False,
)
use_trav_map = reachability_context is not None
for attempt_idx in range(total_attempts):
# Sample orientation if the object supports random orientations, otherwise use default
orientation = (
self.obj.sample_orientation()
if (hasattr(self.obj, "orientations") and self.obj.orientations is not None)
else th.tensor([0, 0, 0, 1.0])
)
# Also add a random world Z-axis offset to the orientation
random_z_orientation = T.axisangle2quat(th.as_tensor([0, 0, th.rand(1) * 2 * th.pi]))
orientation = T.quat_multiply(orientation, random_z_orientation)
# First half: use inset bounds (smarter sampling)
# Second half: use full bounds (fallback for large objects)
# Also fallback if inset bounds are invalid (object too large for container)
if attempt_idx < total_attempts // 2 and th.all(inset_aabb_low < inset_aabb_high):
pos = inset_aabb_low + th.rand(3) * (inset_aabb_high - inset_aabb_low)
else:
pos = aabb_low + th.rand(3) * (aabb_high - aabb_low)
# Rejection sampling #1: Verify the sampled point is actually inside the container volume
if not container_link.check_points_in_volume(pos.unsqueeze(0)).item():
og.sim.load_state(state, serialized=False)
continue
# Add small z-offset to avoid spawning inside the container floor
pos[2] += 0.01
self.obj.set_position_orientation(position=pos, orientation=orientation)
self.obj.keep_still()
# Step physics once so the contact buffer gets populated for the newly-placed object
og.sim.step_physics()
# Rejection sampling #2: Reject if the object is already intersecting anything
# immediately after placement (e.g. interpenetration with a container wall).
if RigidContactAPI.is_in_contact(
scene_idx=self.obj.scene.idx,
query_set=[self.obj],
with_set=None,
ignore_set=None,
current_only=True,
):
og.sim.load_state(state, serialized=False)
continue
# Rejection sampling #3: step until contact is made or max steps reached
# (0.5 seconds of sim time) to let the object settle onto a resting surface.
# If it can't get into contact by then, we reject the placement.
n_steps_max = int(0.5 / og.sim.get_physics_dt())
for _ in range(n_steps_max):
og.sim.step_physics()
if RigidContactAPI.is_in_contact(
scene_idx=self.obj.scene.idx,
query_set=[self.obj],
with_set=None,
ignore_set=None,
current_only=True,
):
break
else:
og.sim.load_state(state, serialized=False)
continue
self.obj.keep_still()
other.keep_still()
# Step a few more times to let velocity stabilize
for _ in range(5):
og.sim.step_physics()
settle_step_idx = 0
while th.norm(self.obj.get_linear_velocity()) > 1e-3 and settle_step_idx < n_steps_max:
og.sim.step_physics()
settle_step_idx += 1
# Rejection sampling #4: Reject if the container's root pose drifted past the
# position/orientation thresholds (i.e. the placed object pushed the container).
container_pos, container_orn = other.get_position_orientation()
position_difference = th.norm(container_pos - container_pos_initial)
orientation_difference = T.get_orientation_diff_in_radian(container_orn, container_orn_initial)
if (
position_difference > m.CONTAINER_POSITION_CHANGE_THRESHOLD
or orientation_difference > m.CONTAINER_ORIENTATION_CHANGE_THRESHOLD
):
og.sim.load_state(state, serialized=False)
continue
# Rejection sampling #5: Reject if any of the container's articulated joints moved
# past the per-DOF-type delta thresholds (e.g. placed object swung a lid or pushed
# a drawer). Thresholds are applied separately for rotational and translational DOFs.
if container_joint_positions_initial is not None:
container_joint_positions_final = other.get_joint_positions()
joint_thresholds = th.where(
other.get_joint_dof_types(),
m.CONTAINER_JOINT_POSITION_DELTA_THRESHOLD_ROTATION,
m.CONTAINER_JOINT_POSITION_DELTA_THRESHOLD_TRANSLATION,
)
container_joint_positions_delta = th.abs(
container_joint_positions_final - container_joint_positions_initial
)
if th.any(container_joint_positions_delta > joint_thresholds):
og.sim.load_state(state, serialized=False)
continue
# Rejection sampling #6: Verify object is still inside after settling and within reach if using trav map.
if self.get_value(other):
if use_trav_map:
settled_pos, _ = self.obj.get_position_orientation()
if not is_pose_reachable_for_predicate(
pos=settled_pos,
objB=other,
predicate="inside",
reachability_context=reachability_context,
):
og.sim.load_state(state, serialized=False)
continue
return True
# Reset the simulator state to the initial state
og.sim.load_state(state, serialized=False)
return False