Semantic Annotations

Semantic Annotations#

This exercise introduces creating custom SemanticAnnotations, connecting bodies with a free connection, and querying with the Entity Query Language (EQL).

Your goals:

  • First: Create Cap and Bottle semantic annotations and three cylinder bodies with exact sizes. Annotate them as described.

  • Second: Connect the cap and the large bottle under the world root using a Connection6DoF, positioning the cap perfectly on top of the bottle.

  • Third: Use EQL to query for all Bottle semantic_annotations that have a Cap assigned to them.

0. Setup#

1. Define semantic annotations and create/annotate bodies#

Your goals:

  • Define two custom dataclasses Bottle(SemanticAnnotation) and Cap(SemanticAnnotation).

  • A Cap is the semantic_annotation of a single body.

  • A Bottle is the semantic_annotation of a body and optionally a Cap.

  • Create one body with a smaller cylinder shape (height 2cm, width 3cm) as visual and collision; use it to create a Cap semantic_annotation.

  • Create a second body with a larger cylinder shape (height 30cm, width 8cm) as visual and collision; use it to create a Bottle semantic_annotation. Also assign the Cap semantic_annotation to the Bottle semantic_annotation.

  • Create a third body with a medium cylinder shape (height 15cm, width 4cm) as visual and collision; use it to create a Bottle semantic_annotation. Do not assign a Cap semantic_annotation to the Bottle semantic_annotation.

@dataclass(eq=False)
class Cap(SemanticAnnotation):
    """Semantic annotation declaring that a Body is a bottle cap."""

    body: Body = field(kw_only=True)


@dataclass(eq=False)
class Bottle(SemanticAnnotation):
    """Semantic annotation declaring that a Body is a bottle; may reference a Cap."""

    body: Body = field(kw_only=True)
    cap: Optional[Cap] = field(default=None, kw_only=True)


# Geometries (sizes are in meters)
cap_cylinder = Cylinder(width=0.03, height=0.02, origin=HomogeneousTransformationMatrix())
bottle_large_cylinder = Cylinder(width=0.08, height=0.30, origin=HomogeneousTransformationMatrix())
bottle_medium_cylinder = Cylinder(width=0.04, height=0.15, origin=HomogeneousTransformationMatrix())

# ShapeCollections
cap_shapes = ShapeCollection([cap_cylinder])
bottle_large_shapes = ShapeCollection([bottle_large_cylinder])
bottle_medium_shapes = ShapeCollection([bottle_medium_cylinder])

# Bodies
cap_body = Body(name=PrefixedName("cap_body"), collision=cap_shapes, visual=cap_shapes)
bottle_large_body = Body(
    name=PrefixedName("bottle_large"), collision=bottle_large_shapes, visual=bottle_large_shapes
)
bottle_medium_body = Body(
    name=PrefixedName("bottle_medium"), collision=bottle_medium_shapes, visual=bottle_medium_shapes
)

# Semantic annotations (not added to the world yet)
cap = Cap(body=cap_body)
bottle_large = Bottle(body=bottle_large_body, cap=cap)
bottle_medium = Bottle(body=bottle_medium_body)

2. Connect cap and large bottle under the root and place the cap on top#

Your goals:

  • Connect the cap body and the large bottle body with Connection6DoF connections under the world root.

  • Add the SemanticAnnotations and Connections to the world.

  • Use the exact cylinder parameters to place the cap perfectly on top of the bottle.

# Register bodies and annotations then create free connections under a dedicated root body
with world.modify_world():
    world.add_semantic_annotation(cap)
    world.add_semantic_annotation(bottle_large)
    world.add_semantic_annotation(bottle_medium)

    root_C_bottle_large = Connection6DoF.create_with_dofs(parent=virtual_root, child=bottle_large_body, world=world)
    bottle_large_C_cap = Connection6DoF.create_with_dofs(parent=bottle_large_body, child=cap_body, world=world)
    root_C_bottle_medium = Connection6DoF.create_with_dofs(parent=virtual_root, child=bottle_medium_body, world=world)

    world.add_connection(root_C_bottle_large)
    world.add_connection(bottle_large_C_cap)
    world.add_connection(root_C_bottle_medium)
    
z_offset = bottle_large_cylinder.height / 2.0 + cap_cylinder.height / 2.0
cap_pose = HomogeneousTransformationMatrix.from_xyz_rpy(
    z=z_offset
)
bottle_large_C_cap.origin = cap_pose

3. Query with EQL for Bottles that have a Cap#

Your goals:

  • Build an EQL query that returns all Bottle semantic_annotations in the world that have a Cap assigned.

  • Store the query in a variable named bottles_with_cap_query and the evaluated list in query_result.

bottle = variable(Bottle, domain=world.semantic_annotations)
bottles_with_cap_query = an(
    entity(
        bottle).where(bottle.cap != None
    )
)
query_result = list(bottles_with_cap_query.evaluate())
print(query_result)

[Bottle(name=PrefixedName('None/Bottle'), id=UUID('614855cf-9bff-412f-8c45-4c904770314b'), body=Body(name=PrefixedName('None/bottle_large'), id=UUID('321e0fe2-57dd-42a5-8370-44576ace019e'), index=1), cap=Cap(name=PrefixedName('None/Cap'), id=UUID('fa53113c-0a71-4150-90fb-c08096d48bbe'), body=Body(name=PrefixedName('None/cap_body'), id=UUID('455d7ad4-b23e-4391-b233-00410aaeea86'), index=2)))]