Building Worlds with Specifications

Building Worlds with Specifications#

In World Structure Manipulation you built a world by hand: creating bodies, opening a world.modify_world() block, wiring connections, and registering degrees of freedom one statement at a time. That is precise, but the recipe for a single object ends up scattered across many calls, and it is bound to one specific world.

A specification captures that recipe as a single, world-independent object. It describes what an entity is — its geometry, its pose, the connection that attaches it, the semantic meaning it carries — without touching any world. You materialize it later with one method call, and the specification takes care of the bookkeeping: modification blocks, connections, degrees of freedom, and annotation registration.

Two properties make specifications convenient:

Reusable. A specification never binds to a world. Materializing it copies the prototype geometry and pose, so you can materialize the same specification into many worlds, or many times into one world under different names.
Composable. Specifications nest. A body specification carries child specifications; an annotation specification carries nested part specifications; a world specification carries a whole list of objects to materialize.

Used Concepts:

The materialization verbs#

Every specification carries a name and knows how to turn itself into a domain object. There are three distinct contracts, and which one a specification offers tells you what kind of thing it describes:

spawn(world, ...) — used by specifications that describe an entity together with the connection that attaches it to a parent. Spawning materializes the entity, attaches it, and recursively materializes its children, all inside one modification block.
connect(world, child, parent=) — used by connection specifications, which join two already existing entities. A connection is not a kinematic structure entity — it is the attachment between two of them — so there is nothing to spawn and no parent to attach it to.
to_domain_object(...) — materializes an entity in isolation, without attaching it to any world. Use it when you want a free-standing Body to pass somewhere else.

Throughout this guide we need a world with a root to materialize into. World.create_with_root_body() gives us exactly that: a fresh world whose single root body is named map.

import logging
logging.disable(logging.CRITICAL)

import numpy as np

from semantic_digital_twin.world import World
from semantic_digital_twin.spatial_computations.raytracer import RayTracer
from semantic_digital_twin.world_description.connections import (
    FixedConnection,
    PrismaticConnection,
)

world = World.create_with_root_body()
assert world.root.name.name == "map"
print("Root body:", world.root.name)

Root body: map

Body specifications#

A BodySpecification describes a Body. The most direct way to build one is through the shape constructors, mirroring the shapes you already know from Creating Custom Bodies:

BodySpecification.box(name, scale, ...)
BodySpecification.sphere(name, radius, ...)
BodySpecification.cylinder(name, width, height, ...)
BodySpecification.mesh(name, filename, ...)

Calling spawn materializes the body and attaches it to the world root with a FixedConnection by default.

from semantic_digital_twin.specifications.kinematic_structure_entities import (
    BodySpecification,
)
from semantic_digital_twin.world_description.geometry import Scale, Color

world = World.create_with_root_body()

table_top = BodySpecification.box(
    name="table_top", 
    scale=Scale(1.2, 0.8, 0.05), 
    color=Color.from_rgba(red=0.6, green=0.4, blue=0.2, opacity=1.0)
).spawn(world)

assert table_top.name.name == "table_top"
assert isinstance(table_top.parent_connection, FixedConnection)
assert table_top.parent_connection.parent is world.root
print("Spawned", table_top.name, "attached via", type(table_top.parent_connection).__name__)

Spawned table_top attached via FixedConnection

Placing and renaming#

Each shape constructor accepts a parent_T_self: the default placement of the entity in its parent frame. Because a specification is reusable, that placement and the name can also be overridden when you spawn, together with the parent the entity attaches to.

from krrood_spatial_math.spatial_types import HomogeneousTransformationMatrix

# Bake a default pose straight into the specification.
leg = BodySpecification.box(
    name="leg_0",
    scale=Scale(0.05, 0.05, 0.7),
    parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(x=0.55, y=0.35, z=-0.35),
).spawn(world, parent=table_top)

# The same specification is reusable; override the pose (and name) per spawn.
leg_spec = BodySpecification.box("leg", Scale(0.05, 0.05, 0.7))
for index, (x, y) in enumerate([(0.55, -0.35), (-0.55, 0.35), (-0.55, -0.35)], start=1):
    leg_spec.spawn(
        world,
        name=f"leg_{index}",
        parent=table_top,
        parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(x=x, y=y, z=-0.35),
    )

# root + table_top + four legs
assert len(world.bodies) == 6
# The baked-in pose placed leg_0 at its parent-frame offset.
root_T_leg_0 = world.compute_forward_kinematics(world.root, leg)
np.testing.assert_allclose(root_T_leg_0.position.to_np()[:3], [0.55, 0.35, -0.35])
assert leg.parent_connection.parent is table_top
print("Bodies now in the world:", sorted(str(body.name) for body in world.bodies))

Bodies now in the world: ['leg_0', 'leg_1', 'leg_2', 'leg_3', 'map', 'table_top']

The legs attach to table_top rather than the world root, and the single leg_spec produced independent bodies. The specification was neither consumed nor mutated.

Nesting with child specifications#

Re-attaching children by hand, as above, is fine for ad-hoc placement. When the parent/child structure is fixed, encode it directly in the specification through child_specifications. Spawning the parent then materializes the whole subtree.

world = World.create_with_root_body()

shelf = BodySpecification.box(
    name="shelf",
    scale=Scale(0.8, 0.3, 0.02),
    child_specifications=[
        BodySpecification.box("book", Scale(0.15, 0.2, 0.25)),
        BodySpecification.sphere("ball", 0.06),
    ],
).spawn(world)

book = world.get_body_by_name("book")
assert book.parent_connection.parent is shelf
assert world.get_body_by_name("ball").parent_connection.parent is shelf
print("book and ball are both children of the shelf")

book and ball are both children of the shelf

Other ways to describe geometry#

Beyond primitive shapes, a body specification can describe composite geometry. These mirror the corresponding Body constructors:

BodySpecification.mesh(name, filename, ...) loads the geometry from a mesh file.
BodySpecification.from_event(name, event) builds the geometry from the bounding boxes of a random event — the construction used by semantic annotations with hollow or carved geometry.
BodySpecification.from_3d_points(name, points_3d) builds the convex hull of a point cloud.

from importlib.resources import files
from pathlib import Path

from krrood_spatial_math.spatial_types import Point3

resources = Path(files("semantic_digital_twin")).parent.parent / "resources"

world = World.create_with_root_body()

# A mesh body, loaded from file.
milk_mesh = BodySpecification.mesh(
    name="milk_mesh", filename=str(resources / "stl" / "milk.stl")
).spawn(world)

# A hollow crate: the outer box minus the carved-out interior, expressed as one event.
outer = Scale(0.3, 0.3, 0.3).to_simple_event().as_composite_set()
inner = Scale(0.26, 0.26, 0.3).to_simple_event().as_composite_set()
crate = BodySpecification.from_event("crate", outer - inner).spawn(world)

# The convex hull of a point cloud.
hull = BodySpecification.from_3d_points(
    "hull",
    [Point3(0, 0, 0), Point3(0.2, 0, 0), Point3(0, 0.2, 0), Point3(0, 0, 0.2)],
).spawn(world)

assert len(milk_mesh.collision.shapes) == 1
# Carving the interior out of the crate leaves a composite of boxes.
assert len(crate.collision.shapes) > 1
assert len(hull.collision.shapes) == 1
print("Crate is a composite of", len(crate.collision.shapes), "boxes")

Crate is a composite of 4 boxes

A body specification also exposes body-only fields that the shape constructors leave at their defaults: inertial for inertia properties and visual_shapes for a visual geometry that differs from the collision geometry.

Because a specification carries its geometry, it can be measured before anything is spawned: scale returns the extents of the combined collision geometry — the world-independent counterpart of a spawned entity’s scale.

from semantic_digital_twin.world_description.shape_collection import ShapeCollection
from semantic_digital_twin.world_description.geometry import Box
from semantic_digital_twin.world_description.inertial_properties import Inertial

world = World.create_with_root_body()

detailed = BodySpecification(
    name="detailed_box",
    shapes=Box(scale=Scale(1, 1, 1)).as_shape_collection(),
    visual_shapes=ShapeCollection([Box(scale=Scale(1.1, 1.1, 1.1))]),
    inertial=Inertial(mass=2.0),
).spawn(world)

assert detailed.visual is not detailed.collision
assert len(detailed.visual.shapes) == 1
assert detailed.inertial.mass == 2.0
assert BodySpecification.box("probe", Scale(1.2, 0.8, 0.05)).scale == Scale(1.2, 0.8, 0.05)
print("Distinct visual/collision geometry, mass =", detailed.inertial.mass)

Distinct visual/collision geometry, mass = 2.0

Region specifications#

A RegionSpecification is the Region analogue of BodySpecification. Regions describe abstract spatial areas rather than physical objects (see Regions), so a region specification carries no inertia or visuals — only geometry, a pose, and children. It shares the same shape constructors, including parent_T_self.

from semantic_digital_twin.specifications.kinematic_structure_entities import (
    RegionSpecification,
)

world = World.create_with_root_body()

placement_area = RegionSpecification.box(
    name="placement_area", 
    scale=Scale(0.4, 0.4, 0.01)
).spawn(world)
assert len(placement_area.area.shapes) == 1
print("Spawned region:", placement_area.name)

Spawned region: placement_area

Connection specifications#

By default a spawned body is rigidly fixed to its parent. To give it a degree of freedom — a drawer that slides, a door that swings — pair the body specification with a connection specification. Each connection family is its own specification type that carries exactly the parameters that family needs:

Specification	Connection	Use for
`FixedConnectionSpecification`	`FixedConnection`	A constant relative pose (the default)
`Connection6DoFSpecification`	`Connection6DoF`	A free-floating object that may move and rotate freely
`PrismaticConnectionSpecification`	`PrismaticConnection`	One translational DoF (a sliding drawer)
`RevoluteConnectionSpecification`	`RevoluteConnection`	One rotational DoF (a swinging door)
`ScrewConnectionSpecification`	`ScrewConnection`	One DoF coupling rotation and translation (a bottle cap on its thread)

Every entity specification carries an optional connection_specification; when it is left unset, spawn attaches the entity with a fixed connection. Set it to give the entity a degree of freedom. The active families (Prismatic/Revolute/Screw) require a movement axis, and optionally accept a multiplier, an offset, and dof_limits. A ScrewConnectionSpecification additionally requires a screw_pitch: the distance between adjacent threads along the axis, which is what couples its rotation to its translation.

from semantic_digital_twin.specifications.connections import (
    PrismaticConnectionSpecification,
)
from krrood_spatial_math.spatial_types import Vector3

world = World.create_with_root_body()

drawer = BodySpecification.box(
    name="drawer",
    scale=Scale(0.4, 0.5, 0.2),
    connection_specification=PrismaticConnectionSpecification(axis=Vector3.Z()),
).spawn(world)

assert isinstance(drawer.parent_connection, PrismaticConnection)
print("Drawer is attached by a", type(drawer.parent_connection).__name__)

Drawer is attached by a PrismaticConnection

Warning

An active connection without an axis is rejected at spawn time. Spawning a PrismaticConnectionSpecification() (no axis) raises MissingConnectionAxisError, because the connection cannot generate its degree of freedom without one.

Connecting existing entities#

The connection specifications above are also usable on their own, to join two entities that already exist. Unlike a spawn, which materializes a new entity, connect requires you to supply the child explicitly — a connection joins two pre-existing entities, it does not create one. If you omit parent, the world root is used.

This pairs naturally with to_domain_object, which materializes a free-standing body without attaching it anywhere.

from semantic_digital_twin.specifications.connections import (
    FixedConnectionSpecification,
)

world = World.create_with_root_body()

# Materialize a body in isolation, then attach it with an explicit connection.
free_body = BodySpecification.box("crate", Scale(0.3, 0.3, 0.3)).to_domain_object()
connection = FixedConnectionSpecification().connect(
    world,
    parent=world.root,
    child=free_body,
    parent_T_connection=HomogeneousTransformationMatrix.from_xyz_rpy(x=1, y=2, z=3),
)

assert connection.child is free_body
root_T_crate = world.compute_forward_kinematics(world.root, free_body)
np.testing.assert_allclose(root_T_crate.position.to_np()[:3], [1, 2, 3])
print("Crate position:", root_T_crate.position.to_np()[:3].tolist())

Crate position: [1.0, 2.0, 3.0]

Semantic annotation specifications#

Specifications also describe semantic annotations. There are two ways to build one, trading convenience for control.

Building an annotation specification directly#

SemanticAnnotationWithRootSpecification couples an annotation type with the specification of the body or region it is rooted in. Use it when you want full control over the root geometry (any BodySpecification/RegionSpecification you like), its pose, and its name. Spawning it materializes the root entity, attaches it, registers the annotation, and materializes any children.

from semantic_digital_twin.specifications.semantic_annotations import (
    SemanticAnnotationWithRootSpecification,
)
from semantic_digital_twin.semantic_annotations.semantic_annotations import Milk

world = World.create_with_root_body()

milk = SemanticAnnotationWithRootSpecification(
    name="milk",
    semantic_annotation_type=Milk,
    root_specification=BodySpecification.box(
         name="milk", 
         scale=Scale(0.1, 0.1, 0.2),
         parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(x=0.3, z=0.8)
    ),
).spawn(world)

assert isinstance(milk, Milk)
assert milk in world.semantic_annotations
assert isinstance(milk.root.parent_connection, FixedConnection)
print("Spawned", type(milk).__name__, "rooted via", type(milk.root.parent_connection).__name__)

Spawned Milk rooted via FixedConnection

The root entity is what attaches to the parent, so the connection lives on the root specification. Left unset, the root is fixed to its parent. Because each connection family carries exactly the parameters it uses, offering an axis to a fixed connection is an error rather than a silently ignored argument.

Setting connection_specification on the root specification chooses another connection, so a Milk may rest rigidly or float freely as a Connection6DoF, and a drawer slides on the prismatic connection its creator gives it. That connection is the drawer’s movable_joint.

from semantic_digital_twin.semantic_annotations.semantic_annotations import Drawer

world = World.create_with_root_body()

drawer = SemanticAnnotationWithRootSpecification(
    name="drawer",
    semantic_annotation_type=Drawer,
    root_specification=BodySpecification.box(
        "drawer",
        Scale(0.4, 0.5, 0.2),
        connection_specification=PrismaticConnectionSpecification(axis=Vector3.X()),
    ),
).spawn(world)

assert drawer.movable_joint is drawer.root.parent_connection
assert isinstance(drawer.movable_joint, PrismaticConnection)
print("The drawer slides on a", type(drawer.movable_joint).__name__)

The drawer slides on a PrismaticConnection

Default root specifications and nested parts#

Most annotation classes can build their own geometry from a Scale. get_default_root_kinematic_structure_entity_specification returns a ready-made BodySpecification (or RegionSpecification) with that geometry filled in. This is the easy path: it hides the geometry construction — which for many annotations is a composite shape built from random events (a hollow handle, a carved container case, a wall minus its apertures) — behind a single scale. Reach for it when a default shape is good enough, and build the root specification directly (previous section) when you need a custom root shape.

get_annotation_specification then wraps any root specification into the type’s SemanticAnnotationWithRootSpecification, so building an annotation specification is two composed calls. Both halves are spawnable, so the wrapping step is easy to forget: spawning a root specification on its own yields a bare body or region, with none of the annotation’s semantics attached. Geometry parameters beyond the scale — a handle’s thickness, a container case’s wall_thickness — live on get_default_root_kinematic_structure_entity_specification, whose signature names them, so geometry is described in exactly one place:

Drawer.get_annotation_specification(
    "drawer",
    Drawer.get_default_root_kinematic_structure_entity_specification(
        scale=Scale(0.4, 0.5, 0.6), wall_thickness=0.05
    ),
)

That builder also takes a connection_specification, exactly as BodySpecification.box(...) does, so custom geometry and a custom attachment stay a single expression. Neither call needs a name for the root: a root specification built without one defers naming, and the annotation stamps its own name onto the root entity at spawn time.

get_annotation_specification’s part_specifications argument mounts nested annotations onto part-whole relationship fields, keyed by the field name — spelled out before anything touches a world.

from semantic_digital_twin.semantic_annotations.semantic_annotations import Drawer, Handle

world = World.create_with_root_body()

drawer = Drawer.get_annotation_specification(
    "drawer",
    Drawer.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.4, 0.5, 0.6)),
    part_specifications={
        "handle": Handle.get_annotation_specification(
            "handle", Handle.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 0.05, 0.05))
        ),
    },
).spawn(world)

assert isinstance(drawer.handle, Handle)
assert drawer.handle.root.parent_connection.parent is drawer.root
print("Drawer has a", type(drawer.handle).__name__, "mounted on its root")

Drawer has a Handle mounted on its root

Annotation specifications take the same spawn-time overrides as entity specifications: name, parent, and parent_T_self. Because the annotation stamps its name onto its root, one specification can produce many identically shaped annotations under different names and poses.

handle_specification = Handle.get_annotation_specification(
    "handle", Handle.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 0.05, 0.05))
)
left = handle_specification.spawn(
    world,
    name="left_handle",
    parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(y=-0.2),
)
right = handle_specification.spawn(
    world,
    name="right_handle",
    parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(y=0.2),
)

assert left.name.name == "left_handle" and right.name.name == "right_handle"
assert left.root.name.name == "left_handle"
print("One specification, two handles:", left.name, "and", right.name)

One specification, two handles: left_handle and right_handle

Inert constructor fields#

annotation_kwargs carries the annotation’s inert constructor fields — plain dataclass fields that are neither geometry nor part-whole relationships. They are handed to the annotation constructor unchanged at spawn time. A Table, for example, references the region it supports objects on:

from semantic_digital_twin.semantic_annotations.semantic_annotations import Table

world = World.create_with_root_body()

surface = RegionSpecification.box("surface", Scale(1, 1, 0.01)).spawn(world)
table = Table.get_annotation_specification(
    "table",
    Table.get_default_root_kinematic_structure_entity_specification(scale=Scale(1, 1, 0.5)),
    annotation_kwargs={"supporting_surface": surface},
).spawn(world)

assert table.supporting_surface is surface
print("Table supports objects on", table.supporting_surface.name)

Table supports objects on surface

To-many parts and geometry that reacts to them#

A list value mounts several parts onto a to-many field, while a single value mounts onto a singular field. The wall/aperture pair below shows two more behaviors at once: an Aperture is region-rooted — its root is a Region, not a Body, so its default root specification is a RegionSpecification — and a Wall cuts its collision geometry around the mounted apertures at spawn time. Each part’s placement in the whole is its root specification’s parent_T_self, which is a plain field you can set after building the specification.

from semantic_digital_twin.semantic_annotations.semantic_annotations import Aperture, Wall
from semantic_digital_twin.world_description.world_entity import Region

world = World.create_with_root_body()

plain_wall = Wall.get_annotation_specification(
    "plain_wall", Wall.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 3, 3))
).spawn(world)

window_left = Aperture.get_annotation_specification(
    "window_left", Aperture.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 0.5, 0.5))
)
window_left.root_specification.parent_T_self = (
    HomogeneousTransformationMatrix.from_xyz_rpy(y=-0.8)
)
window_right = Aperture.get_annotation_specification(
    "window_right", Aperture.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 0.5, 0.5))
)
window_right.root_specification.parent_T_self = (
    HomogeneousTransformationMatrix.from_xyz_rpy(y=0.8)
)

wall = Wall.get_annotation_specification(
    "wall",
    Wall.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 3, 3)),
    part_specifications={"apertures": [window_left, window_right]},
).spawn(world)

assert len(wall.apertures) == 2
assert all(isinstance(aperture.root, Region) for aperture in wall.apertures)
# Cutting the windows out of the wall turns one box into a composite.
assert len(wall.root.collision.shapes) > len(plain_wall.root.collision.shapes)
print(
    "Wall went from", len(plain_wall.root.collision.shapes),
    "shape to", len(wall.root.collision.shapes), "shapes",
)

Wall went from 1 shape to 5 shapes

Joints away from the child’s origin#

A door’s origin sits at its centre, but it swings about the hinge at its edge. The connection moves about its own frame, and connection_T_child places the child relative to that frame, so the door hangs on a single revolute connection without a body for the hinge. parent_T_self stays the pose of the door itself while the hinge is at its zero position.

from semantic_digital_twin.specifications.connections import (
    RevoluteConnectionSpecification,
)
from semantic_digital_twin.semantic_annotations.semantic_annotations import Door
from semantic_digital_twin.world_description.connections import RevoluteConnection

world = World.create_with_root_body()

hinge_T_door = HomogeneousTransformationMatrix.from_xyz_rpy(y=0.5)
door = Door.get_annotation_specification(
    "door",
    Door.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.03, 1, 2)),
    parent_connection_specification=RevoluteConnectionSpecification(
        axis=Vector3.Z(), connection_T_child=hinge_T_door
    ),
).spawn(world)
assert isinstance(door.movable_joint, RevoluteConnection)

door.movable_joint.position = np.pi / 2
world.notify_state_change()

# The door's centre swung a quarter turn around the hinge, not around itself.
expected = hinge_T_door.inverse() @ HomogeneousTransformationMatrix.from_xyz_rpy(yaw=np.pi / 2) @ hinge_T_door
assert np.allclose(door.root.global_transform.to_np(), expected.to_np())
print("Door centre after opening:", door.root.global_transform.position.to_np()[:3])

Door centre after opening: [-0.5 -0.5  0. ]

The specification validates part keys at construction time, so misuse — a list on a singular field, a key that is not a part-whole field, or a part-whole field smuggled in through annotation_kwargs — fails fast, before any world is mutated.

World specifications#

The largest building block is WorldSpecification, which describes an entire scene: an environment, an optional robot, and the objects placed around them. Its environment comes from a world source — usually a parser obtained from a model file with the from_urdf or from_mjcf classmethods, or a map written in code. Calling create_world creates the environment anew every time, so one specification can produce many independent worlds and no two of them share an entity identifier.

import os
from importlib.resources import files
from pathlib import Path

from semantic_digital_twin.specifications.worlds import WorldSpecification

table_urdf = os.path.join(
    Path(files("semantic_digital_twin")).parent.parent, "resources", "urdf", "table.urdf"
)

specification = WorldSpecification.from_urdf(
    table_urdf,
    objects=[
        SemanticAnnotationWithRootSpecification(
            name="milk",
            semantic_annotation_type=Milk,
            root_specification=BodySpecification.box("milk", Scale(0.1, 0.1, 0.2)),
        ),
        BodySpecification.box("cup", Scale(0.07, 0.07, 0.1)),
    ],
)

world = specification.create_world()
assert len(world.get_semantic_annotations_by_type(Milk)) == 1
assert world.get_body_by_name("cup") is not None

# The specification is reusable: each call yields an independent world.
another_world = specification.create_world()
assert world is not another_world
assert len(another_world.get_semantic_annotations_by_type(Milk)) == 1
print("Materialized two independent worlds, each with one milk and one cup")

Materialized two independent worlds, each with one milk and one cup

from_mjcf is the MJCF twin of from_urdf and takes the same arguments:

mjcf_world = WorldSpecification.from_mjcf(
    str(resources / "mjcf" / "table.xml"),
    objects=[BodySpecification.box("cup", Scale(0.07, 0.07, 0.1))],
).create_world()

assert mjcf_world.get_body_by_name("cup") is not None
print("MJCF table world has", len(mjcf_world.bodies), "bodies")

MJCF table world has 8 bodies

Adding a robot#

A world specification can also merge robots into the environment. Each one is described by a RobotSpecification, which bundles a robot’s semantic annotation class with the poses that place it: world_T_odom sets the localization pose, and odom_T_robot_start the robot’s start pose. The robot is parsed from its own description and inserted as world.root -> odom -> connection -> robot. The connection attaching the robot to its odom is the drive determined by the robot’s mobile base, or a fixed connection when the robot has no mobile base.

from semantic_digital_twin.specifications.robots import RobotSpecification
from semantic_digital_twin.robots.pr2 import PR2

world = WorldSpecification.from_urdf(
    file_path=table_urdf,
    robots=[
        RobotSpecification(
            semantic_annotation_type=PR2,
            world_T_odom=HomogeneousTransformationMatrix.from_xyz_rpy(x=1.0),
            odom_T_robot_start=HomogeneousTransformationMatrix.from_xyz_rpy(y=2.0),
        )
    ],
).create_world()

Unknown attribute "type" in /robot[@name='pr2']/link[@name='base_laser_link']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']
Unknown attribute "type" in /robot[@name='pr2']/link[@name='laser_tilt_link']
Unknown tag "material" in /robot[@name='pr2']/link[@name='l_force_torque_link']/collision[1]

---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
Cell In[19], line 13
           world_T_odom=HomogeneousTransformationMatrix.from_xyz_rpy(x=1.0),
           odom_T_robot_start=HomogeneousTransformationMatrix.from_xyz_rpy(y=2.0),
       )
   ],
---> 13 ).create_world()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/specifications/worlds.py:166, in WorldSpecification.create_world(self)
   world = World.create_with_root_body()
for robot_specification in self.robots:
--> 166     robot_specification.spawn(world)
for object_specification in self.objects:
   object_specification.spawn(world)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/specifications/robots.py:97, in RobotSpecification.spawn(self, world)
is_active = issubclass(connection_type, ActiveConnection)
drive_velocity_limits = self._drive_velocity_limits(connection_type)
---> 97 described_robot = self.semantic_annotation_type.from_description()
robot_world = described_robot._world
robot_id = described_robot.id

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/robots/robot_parts.py:1270, in AbstractRobot.from_description(cls)
@classmethod
def from_description(cls) -> Self:
   """
   :return: This robot, in a world holding nothing but it, read from its own
       description: the URDF :meth:`get_ros_file_path` points to, unless the robot
       is described in another format.
   """
   return cls.from_world(
       URDFParser.from_file(
           cls.get_ros_file_path(),
           use_visual_as_collision_backup=cls.uses_visual_as_collision_backup,
-> 1270         ).create_world()
   )

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/adapters/urdf.py:238, in URDFParser.create_world(self)
world = World()
world.name = self.prefix
--> 238 with world.modify_world():
   world.add_kinematic_structure_entity(root)
   main_joints = []

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:314, in WorldModelUpdateContextManager.__exit__(self, exc_type, exc_val, exc_tb)
try:
   if exc_type is None:
--> 314         self.world._notify_model_change(
           publish_changes=self.publish_changes
       )
       run_pending_publications = True
finally:

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:2109, in World._notify_model_change(self, publish_changes, **kwargs)
def _notify_model_change(self, publish_changes: bool = True, **kwargs) -> None:
   """
   Notifies the system of a model change and updates the necessary states, caches,
   and forward kinematics expressions while also triggering registered callbacks
   for model changes.
   """
-> 2109     self._model_manager.update_model_version_and_notify_callbacks(
       publish_changes=publish_changes, **kwargs
   )
   self.notify_state_change(
       publish_changes=publish_changes, force_republish=True, **kwargs
   )
   for callback in list(self.state.state_change_callbacks):

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/world.py:521, in WorldModelManager.update_model_version_and_notify_callbacks(self, **kwargs)
self.version += 1
for callback in list(self.model_change_callbacks):
--> 521     callback.notify_model_change(**kwargs)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/callbacks/callback.py:170, in ModelChangeCallback.notify_model_change(self, **kwargs)
def notify_model_change(self, **kwargs):
   if not self._is_paused:
--> 170         self.on_model_change(**kwargs)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/collision_detector.py:142, in CollisionDetectorModelUpdater.on_model_change(self, **kwargs)
if self._world.is_empty():
   return
--> 142 self.collision_detector.sync_world_model()
self.compile_collision_fks()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:452, in BulletCollisionDetector.sync_world_model(self)
   return
for body in self._world.bodies_with_collision:
--> 452     self.add_body(body)
self._ordered_bullet_objects = list(self.body_to_bullet_object.values())

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:467, in BulletCollisionDetector.add_body(self, body)
def add_body(self, body: Body):
--> 467     o = create_shape_from_body(body=body, mesh_decomposer=self.mesh_decomposer)
   self.kineverse_world.add_collision_object(o)
   self.body_to_bullet_object[body] = o

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:239, in create_shape_from_body(body, mesh_decomposer)
shapes = []
for collision_id, geometry in enumerate(body.collision):
--> 239     shape = create_shape_from_geometry(
       geometry=geometry, mesh_decomposer=mesh_decomposer
   )
   link_T_geometry = bullet.Transform.from_np(geometry.origin.to_np())
   shapes.append((link_T_geometry, shape))

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:212, in create_shape_from_geometry(geometry, mesh_decomposer)
   shape = create_cylinder_shape(
       diameter=geometry.width, height=geometry.height
   )
case Mesh():
--> 212     shape = load_convex_mesh_shape(
       mesh=geometry,
       single_shape=False,
       scale=geometry.scale,
       mesh_decomposer=mesh_decomposer,
   )
case _:
   raise NotImplementedError()

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:279, in load_convex_mesh_shape(mesh, single_shape, scale, mesh_decomposer)
"""
Loads a convex mesh shape from a mesh.

   (...)    276 :return: the bullet convex shape.
"""
if not mesh.mesh.is_convex and mesh_decomposer is not None:
--> 279     obj_pkg_filename = convert_to_decomposed_obj_and_save_in_tmp(
       mesh=mesh, mesh_decomposer=mesh_decomposer
   )
elif mesh.local_file.suffix.removeprefix(".").lower() in BulletMeshFileFormat:
   obj_pkg_filename = str(mesh.local_file)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:331, in convert_to_decomposed_obj_and_save_in_tmp(mesh, mesh_decomposer, cache_dir, log_path)
       with suppress_stdout_stderr():
           mesh_decomposer.apply_to_mesh_and_save(mesh, unfinished_file_name)
--> 331 return save_in_cache(
   obj_file_name=str(cache_dir / f"{trimesh_quantized_hash(trimesh_obj)}.obj"),
   write=write,
)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:380, in save_in_cache(obj_file_name, write)
with tempfile.NamedTemporaryFile(
   dir=os.path.dirname(obj_file_name), suffix=".obj", delete=False
) as unfinished_file:
   pass
--> 380 write(unfinished_file.name)
logging.info(f'Saved obj to "{obj_file_name}".')
os.replace(unfinished_file.name, obj_file_name)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/collision_checking/pybullet_collision_detector.py:329, in convert_to_decomposed_obj_and_save_in_tmp.<locals>.write(unfinished_file_name)
if not trimesh_obj.is_convex and mesh_decomposer is not None:
   with suppress_stdout_stderr():
--> 329         mesh_decomposer.apply_to_mesh_and_save(mesh, unfinished_file_name)

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/pipeline/mesh_decomposition/vhacd.py:131, in VHACDMeshDecomposer.apply_to_mesh_and_save(self, mesh, output_path)
def apply_to_mesh_and_save(self, mesh: Mesh, output_path: str) -> str:
--> 131     parts = self.apply_to_mesh(mesh)
   trimesh.Scene([p.mesh for p in parts]).export(output_path, file_type="obj")
   return output_path

File /__w/cognitive_robot_abstract_machine/cognitive_robot_abstract_machine/semantic_digital_twin/src/semantic_digital_twin/pipeline/mesh_decomposition/vhacd.py:112, in VHACDMeshDecomposer.apply_to_mesh(self, mesh)
def apply_to_mesh(self, mesh: Mesh) -> list[Mesh]:
--> 112     decomposed = mesh.mesh.convex_decomposition(
       maxConvexHulls=self.max_convex_hulls,
       resolution=self.resolution,
       minimumVolumePercentErrorAllowed=self.minimum_volume_percent_error_allowed,
       maxRecursionDepth=self.max_recursion_depth,
       shrinkWrap=self.shrink_wrap,
       fillMode=self.fill_mode.value,
       maxNumVerticesPerCH=self.max_vertices_per_convex_hull,
       asyncACD=self.asynchronous,
       minEdgeLength=self.min_edge_length,
       findBestPlane=self.find_best_plane,
   )
   new_geometry = [
       Mesh.from_trimesh(mesh=decomposed_part, origin=mesh.origin)
       for decomposed_part in decomposed
   ]
   return new_geometry

File /opt/ros/cram-env/lib/python3.12/site-packages/trimesh/base.py:3075, in Trimesh.convex_decomposition(self, **kwargs)
def convex_decomposition(self, **kwargs) -> list["Trimesh"]:
   """
   Compute an approximate convex decomposition of a mesh
   using `pip install pyVHACD`.
   (...)   3071     **kwargs : VHACD keyword arguments
   """
   return [
       Trimesh(**kwargs)
-> 3075         for kwargs in decomposition.convex_decomposition(self, **kwargs)
   ]

File /opt/ros/cram-env/lib/python3.12/site-packages/trimesh/decomposition.py:48, in convex_decomposition(mesh, **kwargs)
# the faces are triangulated in a (len(face), ...vertex-index)
# for vtkPolyData
# i.e. so if shaped to four columns the first column is all 3
faces = (
   np.column_stack((np.ones(len(mesh.faces), dtype=np.int64) * 3, mesh.faces))
   .ravel()
   .astype(np.uint32)
)
return [
   {"vertices": v, "faces": f}
---> 48     for v, f in compute_vhacd(mesh.vertices, faces, **kwargs)
]

KeyboardInterrupt: 

Because robots is a list, a world can hold several of them, each with its own localization — including several instances of the same robot. Names are not unique in a world, so two PR2s contribute two joints called pr2/torso_lift_joint. Each robot is therefore annotated while it still owns the world it was parsed into, and its odom body carries its own identifier as a name prefix, so the localization frames stay distinguishable.

two_robot_world = WorldSpecification.from_urdf(
    file_path=table_urdf,
    robots=[
        RobotSpecification(
            semantic_annotation_type=PR2,
            world_T_odom=HomogeneousTransformationMatrix.from_xyz_rpy(x=1.0),
        ),
        RobotSpecification(
            semantic_annotation_type=PR2,
            world_T_odom=HomogeneousTransformationMatrix.from_xyz_rpy(x=-1.0),
        ),
    ],
).create_world()

assert len(two_robot_world.get_semantic_annotations_by_type(PR2)) == 2
print("Two independently localized PR2s share one world")

Putting it together#

The example below assembles a small tabletop scene with a single specification — a table from URDF, a drawer with a handle, and a milk carton placed above the table — and visualizes the result. The milk’s pose is baked into its root body specification through parent_T_self.

world = WorldSpecification.from_urdf(
    file_path=table_urdf,
    objects=[
        Drawer.get_annotation_specification(
            "drawer",
            Drawer.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.4, 0.5, 0.3)),
            part_specifications={
                "handle": Handle.get_annotation_specification(
                    "handle",
                    Handle.get_default_root_kinematic_structure_entity_specification(scale=Scale(0.1, 0.05, 0.05)),
                ),
            },
        ),
        SemanticAnnotationWithRootSpecification(
            name="milk",
            semantic_annotation_type=Milk,
            root_specification=BodySpecification.box(
                "milk",
                Scale(0.1, 0.1, 0.2),
                parent_T_self=HomogeneousTransformationMatrix.from_xyz_rpy(x=0.3, z=0.8),
            ),
        ),
    ],
).create_world()

assert len(world.get_semantic_annotations_by_type(Drawer)) == 1
assert len(world.get_semantic_annotations_by_type(Milk)) == 1
print("Semantic annotations:")
print(*world.semantic_annotations, sep="\n")

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