Merge pull request #118 from LucaKro/world_class_cleanup

World class cleanup

Author: tomsch420

scripts/generate_orm.py

@@ -30,7 +30,7 @@
 import semantic_digital_twin.world_description.world_entity
 from semantic_digital_twin.datastructures.prefixed_name import PrefixedName
 from semantic_digital_twin.spatial_computations.forward_kinematics import (
-    ForwardKinematicsVisitor,
+    ForwardKinematicsManager,
 )
 from semantic_digital_twin.world import (
     ResetStateContextManager,
@@ -73,7 +73,7 @@
     ResetStateContextManager,
     WorldModelUpdateContextManager,
     HasUpdateState,
-    ForwardKinematicsVisitor,
+    ForwardKinematicsManager,
 }
 
 # build the symbol graph

scripts/parse_procthor_files_and_save_to_database.py

@@ -3,6 +3,7 @@
 import os
 import re
 import time
+from typing import List
 
 import tqdm
 from krrood.ormatic.dao import to_dao

src/semantic_digital_twin/adapters/multi_parser.py

@@ -5,6 +5,7 @@
 import numpy
 from typing_extensions import Optional, Set, List
 
+from ..exceptions import WorldEntityNotFoundError
 from ..spatial_types.spatial_types import TransformationMatrix
 from ..world_description.geometry import (
     Box,
@@ -18,36 +19,19 @@
 from ..world_description.shape_collection import ShapeCollection
 from ..world_description.world_entity import KinematicStructureEntity
 
-try:
-    from multiverse_parser import (
-        InertiaSource,
-        UsdImporter,
-        MjcfImporter,
-        UrdfImporter,
-        BodyBuilder,
-        JointBuilder,
-        JointType,
-        Factory,
-        GeomType,
-    )
-    from multiverse_parser.utils import get_relative_transform
-    from pxr import UsdUrdf, UsdGeom, UsdPhysics, Gf  # type: ignore
-except ImportError as e:
-    logging.info(e)
-    InertiaSource = None
-    UsdImporter = None
-    MjcfImporter = None
-    UrdfImporter = None
-    BodyBuilder = None
-    JointBuilder = None
-    JointType = None
-    Factory = None
-    GeomType = None
-    UsdUrdf = None
-    UsdGeom = None
-    UsdPhysics = None
-    Gf = None
-    get_relative_transform = None
+from multiverse_parser import (
+    InertiaSource,
+    UsdImporter,
+    MjcfImporter,
+    UrdfImporter,
+    BodyBuilder,
+    JointBuilder,
+    JointType,
+    Factory,
+    GeomType,
+)
+from multiverse_parser.utils import get_relative_transform
+from pxr import UsdUrdf, UsdGeom, UsdPhysics, Gf  # type: ignore
 
 from ..world_description.connections import (
     RevoluteConnection,
@@ -319,7 +303,7 @@ def parse_dof(
     """
     try:
         return world.get_degree_of_freedom_by_name(free_variable_name)
-    except KeyError:
+    except WorldEntityNotFoundError:
         if joint_builder.type == JointType.CONTINUOUS:
             dof = DegreeOfFreedom(
                 name=PrefixedName(joint_name),

src/semantic_digital_twin/adapters/multi_sim.py

@@ -1224,13 +1224,13 @@ class MultiSimSynchronizer(ModelChangeCallback, ABC):
     """
 
     def _notify(self):
-        for modification in self.world._model_modification_blocks[-1]:
+        for modification in self.world._model_manager.model_modification_blocks[-1]:
             if isinstance(modification, AddKinematicStructureEntityModification):
                 entity = modification.kinematic_structure_entity
                 self.entity_spawner.spawn(simulator=self.simulator, entity=entity)
 
     def stop(self):
-        self.world.model_change_callbacks.remove(self)
+        self.world._model_manager.model_change_callbacks.remove(self)
 
 
 @dataclass

src/semantic_digital_twin/adapters/procthor/procthor_parser.py

@@ -507,7 +507,7 @@ def get_world(self) -> Optional[World]:
                     parent_T_connection_expression=obj_T_child,
                 )
                 body_world.merge_world(
-                    child_world, child_connection, handle_duplicates=True
+                    child_world, child_connection, assign_unique_name_to_duplicates=True
                 )
 
             return body_world
@@ -578,7 +578,9 @@ def parse(self) -> World:
         world = World(name=house_name)
         with world.modify_world():
             world_root = Body(name=PrefixedName(house_name))
-            world.add_kinematic_structure_entity(world_root, handle_duplicates=True)
+            world.add_kinematic_structure_entity(
+                world_root, assign_unique_name_to_duplicates=True
+            )
 
             self.import_rooms(world, house["rooms"])
 
@@ -607,7 +609,9 @@ def import_rooms(world: World, rooms: List[Dict]):
                 child=room_world.root,
                 parent_T_connection_expression=procthor_room.world_T_room,
             )
-            world.merge_world(room_world, room_connection, handle_duplicates=True)
+            world.merge_world(
+                room_world, room_connection, assign_unique_name_to_duplicates=True
+            )
 
     def import_objects(self, world: World, objects: List[Dict]):
         """
@@ -626,7 +630,9 @@ def import_objects(self, world: World, objects: List[Dict]):
                 child=obj_world.root,
                 parent_T_connection_expression=procthor_object.world_T_obj,
             )
-            world.merge_world(obj_world, obj_connection, handle_duplicates=True)
+            world.merge_world(
+                obj_world, obj_connection, assign_unique_name_to_duplicates=True
+            )
 
     def import_walls_and_doors(
         self, world: World, walls: List[Dict], doors: List[Dict]
@@ -647,7 +653,9 @@ def import_walls_and_doors(
                 child=wall_world.root,
                 parent_T_connection_expression=procthor_wall.world_T_wall,
             )
-            world.merge_world(wall_world, wall_connection, handle_duplicates=True)
+            world.merge_world(
+                wall_world, wall_connection, assign_unique_name_to_duplicates=True
+            )
 
     @staticmethod
     def _build_procthor_wall_from_polygon(

src/semantic_digital_twin/adapters/ros/world_fetcher.py

@@ -65,7 +65,8 @@ def get_modifications_as_json(self) -> str:
         :return: JSON string containing all modification blocks.
         """
         modifications_list = [
-            block.to_json() for block in self.world._model_modification_blocks
+            block.to_json()
+            for block in self.world.get_world_model_manager().model_modification_blocks
         ]
         return json.dumps(modifications_list)
 

src/semantic_digital_twin/adapters/ros/world_synchronizer.py

@@ -3,7 +3,7 @@
 from abc import ABC, abstractmethod
 from dataclasses import dataclass, field
 from functools import cached_property
-from typing import ClassVar, Optional, Type, List
+from typing import ClassVar, Optional, Type, List, Dict
 
 import numpy as np
 import rclpy  # type: ignore
@@ -18,6 +18,7 @@
 
 from .messages import MetaData, WorldStateUpdate, Message, ModificationBlock, LoadModel
 from ...callbacks.callback import Callback, StateChangeCallback, ModelChangeCallback
+from ...datastructures.prefixed_name import PrefixedName
 from ...orm.ormatic_interface import *
 from ...world import World
 
@@ -223,6 +224,18 @@ def world_callback(self):
         self.update_previous_world_state()
         self.publish(msg)
 
+    def compute_state_changes(self) -> Dict[PrefixedName, float]:
+        changes = {
+            name: current_state
+            for name, previous_state, current_state in zip(
+                self.world.state.keys(),
+                self.previous_world_state_data,
+                self.world.state.positions,
+            )
+            if not np.allclose(previous_state, current_state)
+        }
+        return changes
+
 
 @dataclass
 class ModelSynchronizer(
@@ -246,7 +259,9 @@ def apply_message(self, msg: ModificationBlock):
     def world_callback(self):
         msg = ModificationBlock(
             meta_data=self.meta_data,
-            modifications=self.world._model_modification_blocks[-1],
+            modifications=self.world.get_world_model_manager().model_modification_blocks[
+                -1
+            ],
         )
         self.publish(msg)
 

src/semantic_digital_twin/adapters/urdf.py

@@ -7,7 +7,7 @@
 from urdf_parser_py import urdf as urdfpy
 
 from ..datastructures.prefixed_name import PrefixedName
-from ..exceptions import ParsingError
+from ..exceptions import ParsingError, WorldEntityNotFoundError
 from ..spatial_types import spatial_types as cas
 from ..spatial_types.derivatives import Derivatives, DerivativeMap
 from ..spatial_types.spatial_types import TransformationMatrix, Vector3
@@ -171,7 +171,6 @@ def parse(self) -> World:
                 joints.append(parsed_joint)
 
             [world.add_connection(joint) for joint in joints]
-            [world.add_kinematic_structure_entity(link) for link in links]
 
         return world
 
@@ -228,7 +227,7 @@ def parse_joint(
 
         try:
             dof = world.get_degree_of_freedom_by_name(dof_name)
-        except KeyError as e:
+        except WorldEntityNotFoundError as e:
             dof = DegreeOfFreedom(
                 name=dof_name,
                 lower_limits=lower_limits,

src/semantic_digital_twin/callbacks/callback.py

@@ -86,12 +86,12 @@ class StateChangeCallback(Callback, ABC):
     """
 
     def __post_init__(self):
-        self.world.state_change_callbacks.append(self)
+        self.world.state.state_change_callbacks.append(self)
         self.update_previous_world_state()
 
     def stop(self):
         try:
-            self.world.state_change_callbacks.remove(self)
+            self.world.state.state_change_callbacks.remove(self)
         except ValueError:
             pass
 
@@ -101,18 +101,6 @@ def update_previous_world_state(self):
         """
         self.previous_world_state_data = np.copy(self.world.state.positions)
 
-    def compute_state_changes(self) -> Dict[PrefixedName, float]:
-        changes = {
-            name: current_state
-            for name, previous_state, current_state in zip(
-                self.world.state.keys(),
-                self.previous_world_state_data,
-                self.world.state.positions,
-            )
-            if not np.allclose(previous_state, current_state)
-        }
-        return changes
-
 
 @dataclass
 class ModelChangeCallback(Callback, ABC):
@@ -121,10 +109,10 @@ class ModelChangeCallback(Callback, ABC):
     """
 
     def __post_init__(self):
-        self.world.model_change_callbacks.append(self)
+        self.world.get_world_model_manager().model_change_callbacks.append(self)
 
     def stop(self):
         try:
-            self.world.model_change_callbacks.remove(self)
+            self.world.get_world_model_manager().model_change_callbacks.remove(self)
         except ValueError:
             pass

src/semantic_digital_twin/collision_checking/trimesh_collision_detector.py

@@ -36,7 +36,7 @@ def sync_world_model(self) -> None:
         """
         Synchronize the collision checker with the current world model
         """
-        if self._last_synced_model == self._world._model_version:
+        if self._last_synced_model == self._world.get_world_model_manager().version:
             return
         bodies_to_be_added = set(self._world.bodies_with_enabled_collision) - set(
             self._collision_objects.keys()
@@ -58,7 +58,7 @@ def sync_world_state(self) -> None:
         """
         Synchronize the collision checker with the current world state
         """
-        if self._last_synced_state == self._world._state_version:
+        if self._last_synced_state == self._world.state.version:
             return
         for body, coll_obj in self._collision_objects.items():
             coll_obj.setTransform(