Skip to content

sim_utils

check_collision(prims=None, prims_check=None, prims_exclude=None, step_physics=False)

Checks if any valid collisions occurred during the most recent physics timestep associated with prims @prims

Parameters:

Name Type Description Default
prims None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

Prim(s) to check for collision. If None, will check against all objects currently in the scene.

None
prims_check None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

If specified, will only check for collisions with these specific prim(s)

None
prims_exclude None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

If specified, will explicitly ignore any collisions with these specific prim(s)

None
step_physics bool

Whether to step the physics first before checking collisions. Default is False

False

Returns:

Name Type Description
bool

True if a valid collision has occurred, else False

Source code in utils/sim_utils.py
def check_collision(prims=None, prims_check=None, prims_exclude=None, step_physics=False):
    """
    Checks if any valid collisions occurred during the most recent physics timestep associated with prims @prims

    Args:
        prims (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): Prim(s) to check for collision.
            If None, will check against all objects currently in the scene.
        prims_check (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): If specified, will
            only check for collisions with these specific prim(s)
        prims_exclude (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): If specified, will
            explicitly ignore any collisions with these specific prim(s)
        step_physics (bool): Whether to step the physics first before checking collisions. Default is False

    Returns:
        bool: True if a valid collision has occurred, else False
    """
    return len(get_collisions(
        prims=prims,
        prims_check=prims_check,
        prims_exclude=prims_exclude,
        step_physics=step_physics)) > 0

filter_collisions(collisions, filter_prims)

Filters collision pairs @collisions based on a set of prims @filter_prims.

Parameters:

Name Type Description Default
collisions set of 2-tuple

Collision pairs that should be filtered

required
filter_prims EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

Prim(s) specifying which collisions to filter for. Any collisions that include prims from this filter set will be removed

required

Returns:

Type Description

set of 2-tuple: Filtered collision pairs

Source code in utils/sim_utils.py
def filter_collisions(collisions, filter_prims):
    """
    Filters collision pairs @collisions based on a set of prims @filter_prims.

    Args:
        collisions (set of 2-tuple): Collision pairs that should be filtered
        filter_prims (EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): Prim(s) specifying which
            collisions to filter for. Any collisions that include prims from this filter
            set will be removed

    Returns:
        set of 2-tuple: Filtered collision pairs
    """
    paths = prims_to_rigid_prim_set(filter_prims)

    filtered_collisions = set()
    for pair in collisions:
        if set(pair).isdisjoint(paths):
            filtered_collisions.add(pair)

    return filtered_collisions

get_collisions(prims=None, prims_check=None, prims_exclude=None, step_physics=False)

Grab collisions that occurred during the most recent physics timestep associated with prims @prims

Parameters:

Name Type Description Default
prims None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

Prim(s) to check for collision. If None, will check against all objects currently in the scene.

None
prims_check None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

If specified, will only check for collisions with these specific prim(s)

None
prims_exclude None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim

If specified, will explicitly ignore any collisions with these specific prim(s)

None
step_physics bool

Whether to step the physics first before checking collisions. Default is False

False

Returns:

Type Description

set of 2-tuple: Unique collision pairs occurring in the simulation at the current timestep between the specified prim(s), represented by their prim_paths

Source code in utils/sim_utils.py
def get_collisions(prims=None, prims_check=None, prims_exclude=None, step_physics=False):
    """
    Grab collisions that occurred during the most recent physics timestep associated with prims @prims

    Args:
        prims (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): Prim(s) to check for collision.
            If None, will check against all objects currently in the scene.
        prims_check (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): If specified, will
            only check for collisions with these specific prim(s)
        prims_exclude (None or EntityPrim or RigidPrim or tuple of EntityPrim or RigidPrim): If specified, will
            explicitly ignore any collisions with these specific prim(s)
        step_physics (bool): Whether to step the physics first before checking collisions. Default is False

    Returns:
        set of 2-tuple: Unique collision pairs occurring in the simulation at the current timestep between the
            specified prim(s), represented by their prim_paths
    """
    # Make sure sim is playing
    assert og.sim.is_playing(), "Cannot get collisions while sim is not playing!"

    # Optionally step physics and then update contacts
    if step_physics:
        og.sim.step_physics()

    # Standardize inputs
    prims = og.sim.scene.objects if prims is None else prims if isinstance(prims, Iterable) else [prims]
    prims_check = [] if prims_check is None else prims_check if isinstance(prims_check, Iterable) else [prims_check]
    prims_exclude = [] if prims_exclude is None else prims_exclude if isinstance(prims_exclude, Iterable) else [prims_exclude]

    # Convert into prim paths to check for collision
    def get_paths_from_rigid_prims(inp_prims):
        return {prim.prim_path for prim in inp_prims}

    def get_contacts(inp_prims):
        return {(c.body0, c.body1) for prim in inp_prims for c in prim.contact_list()}

    rprims = prims_to_rigid_prim_set(prims)
    rprims_check = prims_to_rigid_prim_set(prims_check)
    rprims_exclude = prims_to_rigid_prim_set(prims_exclude)

    paths = get_paths_from_rigid_prims(rprims)
    paths_check = get_paths_from_rigid_prims(rprims_check)
    paths_exclude = get_paths_from_rigid_prims(rprims_exclude)

    # Run sanity checks
    assert paths_check.isdisjoint(paths_exclude), \
        f"Paths to check and paths to ignore collisions for should be mutually exclusive! " \
        f"paths_check: {paths_check}, paths_exclude: {paths_exclude}"

    # Determine whether we're checking / filtering any collision from collision set A
    should_check_collisions = len(paths_check) > 0
    should_filter_collisions = len(paths_exclude) > 0

    # Get all collisions from the objects set
    collisions = get_contacts(rprims)

    # Only run the following (expensive) code if we are actively using filtering criteria
    if should_check_collisions or should_filter_collisions:

        # First filter out unnecessary collisions
        if should_filter_collisions:
            # First filter pass, remove the intersection of the main contacts and the contacts from the exclusion set minus
            # the intersection between the exclusion and normal set
            # This filters out any matching collisions in the exclusion set that are NOT an overlap
            # between @rprims and @rprims_exclude
            rprims_exclude_intersect = rprims_exclude.intersection(rprims)
            exclude_disjoint_collisions = get_contacts(rprims_exclude - rprims_exclude_intersect)
            collisions.difference_update(exclude_disjoint_collisions)

            # Second filter pass, we remove collisions that may include self-collisions
            # This is a bit more tricky because we need to actually look at the individual contact pairs to determine
            # whether it's a collision (which may include a self-collision) that should be filtered
            # We do this by grabbing the contacts of the intersection between the exclusion and normal rprims sets,
            # and then making sure the resulting contact pair sets are completely disjoint from the paths intersection
            exclude_intersect_collisions = get_contacts(rprims_exclude_intersect)
            collisions.difference_update({pair for pair in exclude_intersect_collisions if paths.issuperset(set(pair))})

        # Now, we additionally check for explicit collisions, filtering out any that do not meet this criteria
        # This is essentially the inverse of the filter collision process, where we do two passes again, but for each
        # case we look at the union rather than the subtraction of the two sets
        if should_check_collisions:
            # First check pass, keep the intersection of the main contacts and the contacts from the check set minus
            # the intersection between the check and normal set
            # This keeps any matching collisions in the check set that overlap between @rprims and @rprims_check
            rprims_check_intersect = rprims_check.intersection(rprims)
            check_disjoint_collisions = get_contacts(rprims_check - rprims_check_intersect)
            valid_other_collisions = collisions.intersection(check_disjoint_collisions)

            # Second check pass, we additionally keep collisions that may include self-collisions
            # This is a bit more tricky because we need to actually look at the individual contact pairs to determine
            # whether it's a collision (which may include a self-collision) that should be kept
            # We do this by grabbing the contacts of the intersection between the check and normal rprims sets,
            # and then making sure the resulting contact pair sets is strictly a subset of the original set
            # Lastly, we only keep the intersection of this resulting set with the original collision set, so that
            # any previously filtered collisions are respected
            check_intersect_collisions = get_contacts(rprims_check_intersect)
            valid_intersect_collisions = collisions.intersection({pair for pair in check_intersect_collisions if paths.issuperset(set(pair))})

            # Collisions is union of valid other and valid self collisions
            collisions = valid_other_collisions.union(valid_intersect_collisions)

    # Only going into this if it is for logging --> efficiency
    if logging.root.level <= logging.DEBUG:
        for item in collisions:
            logging.debug("linkA:{}, linkB:{}".format(item[0], item[1]))

    return collisions

land_object(obj, pos, quat=None, z_offset=None)

Land the object at the specified position @pos, given a valid position and orientation.

Parameters:

Name Type Description Default
obj BaseObject

Object to place in the environment

required
pos 3-array

Global (x,y,z) location to place the object

required
quat None or 4-array

Optional (x,y,z,w) quaternion orientation when placing the object. If None, a random orientation about the z-axis will be sampled

None
z_offset None or float

Optional additional z_offset to apply

None
Source code in utils/sim_utils.py
def land_object(obj, pos, quat=None, z_offset=None):
    """
    Land the object at the specified position @pos, given a valid position and orientation.

    Args:
        obj (BaseObject): Object to place in the environment
        pos (3-array): Global (x,y,z) location to place the object
        quat (None or 4-array): Optional (x,y,z,w) quaternion orientation when placing the object.
            If None, a random orientation about the z-axis will be sampled
        z_offset (None or float): Optional additional z_offset to apply
    """
    # Make sure sim is playing
    assert og.sim.is_playing(), "Cannot land object while sim is not playing!"

    # Set the object's pose
    quat = T.euler2quat([0, 0, np.random.uniform(0, np.pi * 2)]) if quat is None else quat
    place_base_pose(obj, pos, quat, z_offset)

    # If we're placing a robot, make sure it's reset and not moving
    # Run import here to avoid circular imports
    from omnigibson.robots.robot_base import BaseRobot
    is_robot = isinstance(obj, BaseRobot)
    if is_robot:
        obj.reset()
        obj.keep_still()

    # Check to make sure we landed successfully
    # land for maximum 1 second, should fall down ~5 meters
    land_success = False
    max_simulator_step = int(1.0 / og.sim.get_rendering_dt())
    for _ in range(max_simulator_step):
        # Run a sim step and see if we have any contacts
        og.sim.step()
        land_success = check_collision(prims=obj)
        if land_success:
            # Once we're successful, we can break immediately
            print(f"Landed object {obj.name} successfully!")
            break

    # Print out warning in case we failed to land the object successfully
    if not land_success:
        logging.warning(f"Object {obj.name} failed to land.")

    # Make sure robot isn't moving at the end if we're a robot
    if is_robot:
        obj.reset()
        obj.keep_still()

place_base_pose(obj, pos, quat=None, z_offset=None)

Place the object so that its base (z-min) rests at the location of @pos

Parameters:

Name Type Description Default
obj BaseObject

Object to place in the environment

required
pos 3-array

Global (x,y,z) location to place the base of the robot

required
quat None or 4-array

Optional (x,y,z,w) quaternion orientation when placing the object. If None, the object's current orientation will be used

None
z_offset None or float

Optional additional z_offset to apply

None
Source code in utils/sim_utils.py
def place_base_pose(obj, pos, quat=None, z_offset=None):
    """
    Place the object so that its base (z-min) rests at the location of @pos

    Args:
        obj (BaseObject): Object to place in the environment
        pos (3-array): Global (x,y,z) location to place the base of the robot
        quat (None or 4-array): Optional (x,y,z,w) quaternion orientation when placing the object.
            If None, the object's current orientation will be used
        z_offset (None or float): Optional additional z_offset to apply
    """
    # avoid circular dependency
    from omnigibson.object_states import AABB

    # Make sure AABB is up-to-date before grabbing value
    get_physx_simulation_interface().fetch_results()
    BoundingBoxAPI.clear()
    obj.states[AABB].clear_cache()

    lower, _ = obj.states[AABB].get_value()
    cur_pos = obj.get_position()
    z_diff = cur_pos[2] - lower[2]
    obj.set_position_orientation(pos + np.array([0, 0, z_diff if z_offset is None else z_diff + z_offset]), quat)

prims_to_rigid_prim_set(inp_prims)

Converts prims @inp_prims into its corresponding set of rigid prims

Parameters:

Name Type Description Default
inp_prims list of RigidPrim or EntityPrim

Arbitrary prims

required

Returns:

Type Description

set of RigidPrim: Aggregated set of RigidPrims from @inp_prims

Source code in utils/sim_utils.py
def prims_to_rigid_prim_set(inp_prims):
    """
    Converts prims @inp_prims into its corresponding set of rigid prims

    Args:
        inp_prims (list of RigidPrim or EntityPrim): Arbitrary prims

    Returns:
        set of RigidPrim: Aggregated set of RigidPrims from @inp_prims
    """
    # Avoid circular imports
    from omnigibson.prims.entity_prim import EntityPrim
    from omnigibson.prims.rigid_prim import RigidPrim

    out = set()
    for prim in inp_prims:
        if isinstance(prim, EntityPrim):
            out.update({link for link in prim.links.values()})
        elif isinstance(prim, RigidPrim):
            out.add(prim)
        else:
            raise ValueError(f"Inputted prims must be either EntityPrim or RigidPrim instances "
                             f"when getting collisions! Type: {type(prim)}")
    return out

test_valid_pose(obj, pos, quat=None, z_offset=None)

Test if the object can be placed with no collision.

Parameters:

Name Type Description Default
obj BaseObject

Object to place in the environment

required
pos 3-array

Global (x,y,z) location to place the object

required
quat None or 4-array

Optional (x,y,z,w) quaternion orientation when placing the object. If None, the object's current orientation will be used

None
z_offset None or float

Optional additional z_offset to apply

None

Returns:

Name Type Description
bool

Whether the placed object position is valid

Source code in utils/sim_utils.py
def test_valid_pose(obj, pos, quat=None, z_offset=None):
    """
    Test if the object can be placed with no collision.

    Args:
        obj (BaseObject): Object to place in the environment
        pos (3-array): Global (x,y,z) location to place the object
        quat (None or 4-array): Optional (x,y,z,w) quaternion orientation when placing the object.
            If None, the object's current orientation will be used
        z_offset (None or float): Optional additional z_offset to apply

    Returns:
        bool: Whether the placed object position is valid
    """
    # Make sure sim is playing
    assert og.sim.is_playing(), "Cannot test valid pose while sim is not playing!"

    # Store state before checking object position
    state = og.sim.scene.dump_state(serialized=False)

    # Set the pose of the object
    place_base_pose(obj, pos, quat, z_offset)

    # If we're placing a robot, make sure it's reset and not moving
    # Run import here to avoid circular imports
    from omnigibson.robots.robot_base import BaseRobot
    if isinstance(obj, BaseRobot):
        obj.reset()
        obj.keep_still()

    # Check whether we're in collision after taking a single physics step
    in_collision = check_collision(prims=obj, step_physics=True)

    # Restore state after checking the collision
    og.sim.load_state(state, serialized=False)

    # Valid if there are no collisions
    return not in_collision