Skip to content

adjacency

HorizontalAdjacency

Bases: AbsoluteObjectState

State representing the object's horizontal adjacencies in a preset number of directions.

The HorizontalAdjacency state returns adjacency lists for equally spaced coordinate planes. Each plane consists of 2 orthogonal axes, and adjacencies are checked for both the positive and negative directions of each axis.

The value of the state is List[List[AxisAdjacencyList]], where the list dimensions are m.HORIZONTAL_AXIS_COUNT and 2. The first index is used to choose between the different planes, the second index to choose between the orthogonal axes of that plane. Given a plane/axis combo, the item in the list is a AxisAdjacencyList containing adjacencies in both directions of the axis.

If the idea of orthogonal bases is not relevant (and your use case simply requires checking adjacencies in each direction), the flatten_planes() function can be used on the state value to reduce the output to List[AxisAdjacencyList], a list of adjacency lists for all 2 * m.HORIZONTAL_AXIS_COUNT directions.

Source code in object_states/adjacency.py
class HorizontalAdjacency(AbsoluteObjectState):
    """
    State representing the object's horizontal adjacencies in a preset number of directions.

    The HorizontalAdjacency state returns adjacency lists for equally spaced coordinate planes.
    Each plane consists of 2 orthogonal axes, and adjacencies are checked for both the positive
    and negative directions of each axis.

    The value of the state is List[List[AxisAdjacencyList]], where the list dimensions are
    m.HORIZONTAL_AXIS_COUNT and 2. The first index is used to choose between the different planes,
    the second index to choose between the orthogonal axes of that plane. Given a plane/axis combo,
    the item in the list is a AxisAdjacencyList containing adjacencies in both directions of the
    axis.

    If the idea of orthogonal bases is not relevant (and your use case simply requires checking
    adjacencies in each direction), the flatten_planes() function can be used on the state value
    to reduce the output to List[AxisAdjacencyList], a list of adjacency lists for all
    2 * m.HORIZONTAL_AXIS_COUNT directions.
    """

    def _get_value(self):
        coordinate_planes = get_equidistant_coordinate_planes(m.HORIZONTAL_AXIS_COUNT)

        # Flatten the axis dimension and input into compute_adjacencies.
        bodies_by_axis = compute_adjacencies(self.obj, coordinate_planes.reshape(-1, 3), m.MAX_DISTANCE_HORIZONTAL)

        # Now reshape the bodies_by_axis to group by coordinate planes.
        bodies_by_plane = list(zip(bodies_by_axis[::2], bodies_by_axis[1::2]))

        # Return the adjacencies.
        return bodies_by_plane

    def _set_value(self, new_value):
        raise NotImplementedError("HorizontalAdjacency state currently does not support setting.")

    @staticmethod
    def get_dependencies():
        return AbsoluteObjectState.get_dependencies() + [AABB]

VerticalAdjacency

Bases: AbsoluteObjectState

State representing the object's vertical adjacencies. Value is a AxisAdjacencyList object.

Source code in object_states/adjacency.py
class VerticalAdjacency(AbsoluteObjectState):
    """
    State representing the object's vertical adjacencies.
    Value is a AxisAdjacencyList object.
    """

    def _get_value(self):
        # Call the adjacency computation with th Z axis.
        bodies_by_axis = compute_adjacencies(self.obj, np.array([[0, 0, 1]]), m.MAX_DISTANCE_VERTICAL)

        # Return the adjacencies from the only axis we passed in.
        return bodies_by_axis[0]

    def _set_value(self, new_value):
        raise NotImplementedError("VerticalAdjacency state currently does not support setting.")

    @staticmethod
    def get_dependencies():
        return AbsoluteObjectState.get_dependencies() + [AABB]

compute_adjacencies(obj, axes, max_distance)

Given an object and a list of axes, find the adjacent objects in the axes' positive and negative directions.

Parameters:

Name Type Description Default
obj StatefulObject

The object to check adjacencies of.

required
axes 2D-array

(n_axes, 3) array defining the axes to check in. Note that each axis will be checked in both its positive and negative direction.

required

Returns:

Type Description

list of AxisAdjacencyList: List of length len(axes) containing the adjacencies.

Source code in object_states/adjacency.py
def compute_adjacencies(obj, axes, max_distance):
    """
    Given an object and a list of axes, find the adjacent objects in the axes'
    positive and negative directions.

    Args:
        obj (StatefulObject): The object to check adjacencies of.
        axes (2D-array): (n_axes, 3) array defining the axes to check in.
            Note that each axis will be checked in both its positive and negative direction.

    Returns:
        list of AxisAdjacencyList: List of length len(axes) containing the adjacencies.
    """
    # Get vectors for each of the axes' directions.
    # The ordering is axes1+, axis1-, axis2+, axis2- etc.
    directions = np.empty((len(axes) * 2, 3))
    directions[0::2] = axes
    directions[1::2] = -axes

    # Prepare this object's info for ray casting.
    # Use AABB center instead of position because we cannot get valid position
    # for fixed objects if fixed links are merged.
    aabb_lower, aabb_higher = obj.states[AABB].get_value()
    object_position = (aabb_lower + aabb_higher) / 2.0
    prim_paths = obj.link_prim_paths

    # Prepare the rays to cast.
    ray_starts = np.tile(object_position, (len(directions), 1))
    ray_endpoints = ray_starts + (directions * max_distance)

    # Cast time.
    ray_results = raytest_batch(
        ray_starts,
        ray_endpoints,
        only_closest=False,
        ignore_bodies=prim_paths,
        ignore_collisions=prim_paths
    )

    # Add the results to the appropriate lists
    # For now, we keep our result in the dimensionality of (direction, hit_object_order).
    # We convert the hit link into unique objects encountered
    objs_by_direction = []
    for results in ray_results:
        unique_objs = set()
        for result in results:
            # Check if the inferred hit object is not None, we add it to our set
            obj_prim_path = "/".join(result["rigidBody"].split("/")[:-1])
            obj = og.sim.scene.object_registry("prim_path", obj_prim_path, None)
            if obj is not None:
                unique_objs.add(obj)
        objs_by_direction.append(unique_objs)

    # Reshape so that these have the following indices:
    # (axis_idx, direction-one-or-zero, hit_idx)
    objs_by_axis = [
        AxisAdjacencyList(positive_neighbors, negative_neighbors)
        for positive_neighbors, negative_neighbors in zip(objs_by_direction[::2], objs_by_direction[1::2])
    ]
    return objs_by_axis

get_equidistant_coordinate_planes(n_planes)

Given a number, sample that many equally spaced coordinate planes.

The samples will cover all 360 degrees (although rotational symmetry is assumed, e.g. if you take into account the axis index and the positive/negative directions, only 1/4 of the possible coordinate (1 quadrant, np.pi / 2.0) planes will be sampled: the ones where the first axis' positive direction is in the first quadrant).

Parameters:

Name Type Description Default
n_planes int

number of planes to sample

required

Returns:

Type Description

3D-array: (n_planes, 2, 3) array where the first dimension is the sampled plane index, the second dimension is the axis index (0/1), and the third dimension is the 3-D world-coordinate vector corresponding to the axis.

Source code in object_states/adjacency.py
def get_equidistant_coordinate_planes(n_planes):
    """Given a number, sample that many equally spaced coordinate planes.

    The samples will cover all 360 degrees (although rotational symmetry
    is assumed, e.g. if you take into account the axis index and the
    positive/negative directions, only 1/4 of the possible coordinate (1 quadrant, np.pi / 2.0)
    planes will be sampled: the ones where the first axis' positive direction
    is in the first quadrant).

    Args:
        n_planes (int): number of planes to sample

    Returns:
        3D-array: (n_planes, 2, 3) array where the first dimension
            is the sampled plane index, the second dimension is the axis index
            (0/1), and the third dimension is the 3-D world-coordinate vector
            corresponding to the axis.
    """
    # Compute the positive directions of the 1st axis of each plane.
    first_axis_angles = np.linspace(0, np.pi / 2, n_planes)
    first_axes = np.stack(
        [np.cos(first_axis_angles), np.sin(first_axis_angles), np.zeros_like(first_axis_angles)], axis=1
    )

    # Compute the positive directions of the 2nd axes. These axes are
    # orthogonal to both their corresponding first axes and to the Z axis.
    second_axes = np.cross([0, 0, 1], first_axes)

    # Return the axes in the shape (n_planes, 2, 3)
    return np.stack([first_axes[:, None, :], second_axes[:, None, :]], axis=1)