Factories

Factories#

Factories are convenience builders that create consistent worlds and their semantic annotations for you. They are ideal for quickly setting up structured environments such as drawers, containers, and handles without having to wire all bodies, connections, and semantic annotations manually.

Used Concepts:

Create a drawer with a handle#

from krrood.entity_query_language.entity import entity, an, let, symbolic_mode

from semantic_digital_twin.datastructures.prefixed_name import PrefixedName
from semantic_digital_twin.spatial_types.spatial_types import TransformationMatrix
from semantic_digital_twin.semantic_annotations.factories import (
    DrawerFactory,
    ContainerFactory,
    HandleFactory,
    Direction,
    SemanticPositionDescription,
    HorizontalSemanticDirection,
    VerticalSemanticDirection,
)
from semantic_digital_twin.semantic_annotations.semantic_annotations import Drawer, Handle
from semantic_digital_twin.spatial_computations.raytracer import RayTracer
from semantic_digital_twin.world_description.geometry import Scale


# Build a simple drawer with a centered handle
world = DrawerFactory(
    name=PrefixedName("drawer"),
    container_factory=ContainerFactory(name=PrefixedName("container"), direction=Direction.Z, scale=Scale(0.2, 0.4, 0.2),),
    handle_factory=HandleFactory(name=PrefixedName("handle"), scale=Scale(0.05, 0.1, 0.02)),
    semantic_position=SemanticPositionDescription(
        horizontal_direction_chain=[
            HorizontalSemanticDirection.FULLY_CENTER,
        ],
        vertical_direction_chain=[VerticalSemanticDirection.FULLY_CENTER],
    ),
).create()

print(*world.semantic_annotations, sep="\n")
rt = RayTracer(world)
rt.update_scene()
rt.scene.show("jupyter")

Container(name=PrefixedName(name='container', prefix=None), body=Body(name=PrefixedName(name='container', prefix=None), index=0, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))
Handle(name=PrefixedName(name='handle', prefix=None), body=Body(name=PrefixedName(name='handle', prefix=None), index=1, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))
Drawer(name=PrefixedName(name='drawer', prefix=None), container=Container(name=PrefixedName(name='container', prefix=None), body=Body(name=PrefixedName(name='container', prefix=None), index=0, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None)), handle=Handle(name=PrefixedName(name='handle', prefix=None), body=Body(name=PrefixedName(name='handle', prefix=None), index=1, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None)))

/opt/ros/semantic_digital_twin-venv/lib/python3.12/site-packages/probabilistic_model/distributions/uniform.py:56: RuntimeWarning: divide by zero encountered in log
  return -np.log(self.upper - self.lower)

You can query for components of the created furniture using EQL. For example, get all handles:

with symbolic_mode():
    handles = an(entity(let(Handle, world.semantic_annotations)))
print(*handles.evaluate(), sep="\n")

Handle(name=PrefixedName(name='handle', prefix=None), body=Body(name=PrefixedName(name='handle', prefix=None), index=1, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))

Add another handle and filter by context#

# Create an extra handle world and merge it into the existing world at a different pose
useless_handle_world = HandleFactory(name=PrefixedName("useless_handle")).create()
print(useless_handle_world.semantic_annotations)

with world.modify_world():
    world.merge_world_at_pose(
        useless_handle_world,
        TransformationMatrix.from_xyz_rpy(x=1.0, y=1.0),
    )

rt = RayTracer(world)
rt.update_scene()
rt.scene.show("jupyter")

[Handle(name=PrefixedName(name='useless_handle', prefix=None), body=Body(name=PrefixedName(name='useless_handle', prefix=None), index=0, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))]

With two handles in the world, the simple handle query yields multiple results:

with symbolic_mode():
    handles = an(entity(let(Handle, world.semantic_annotations)))
print(*handles.evaluate(), sep="\n")

Handle(name=PrefixedName(name='handle', prefix=None), body=Body(name=PrefixedName(name='handle', prefix=None), index=1, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))
Handle(name=PrefixedName(name='useless_handle', prefix=None), body=Body(name=PrefixedName(name='useless_handle', prefix=None), index=2, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))

We can refine the query to get only the handle that belongs to a drawer:

with symbolic_mode():
    drawer = let(Drawer, world.semantic_annotations)
    handle = let(Handle, world.semantic_annotations)
    result = an(entity(handle, drawer.handle == handle))
print(*result.evaluate(), sep="\n")

Handle(name=PrefixedName(name='handle', prefix=None), body=Body(name=PrefixedName(name='handle', prefix=None), index=1, collision_config=CollisionCheckingConfig(buffer_zone_distance=None, violated_distance=0.0, disabled=None, max_avoided_bodies=1), temp_collision_config=None))
Contents