Merge pull request #135 from ichumuh/main

Implemented MinimalRobot

Author: LucaKro

src/semantic_digital_twin/robots/minimal_robot.py

@@ -0,0 +1,51 @@
+from __future__ import annotations
+
+from collections import defaultdict
+from dataclasses import dataclass
+from typing import Self
+
+from ..datastructures.prefixed_name import PrefixedName
+from ..robots.abstract_robot import (
+    AbstractRobot,
+)
+from ..world import World
+from ..world_description.world_entity import Body
+
+
+@dataclass
+class MinimalRobot(AbstractRobot):
+    """
+    Creates the bare minimum semantic annotation.
+    Used when you only care that there is a robot.
+    """
+
+    def __hash__(self):
+        return hash(
+            tuple(
+                [self.__class__]
+                + sorted([kse.name for kse in self.kinematic_structure_entities])
+            )
+        )
+
+    def load_srdf(self):
+        pass
+
+    @classmethod
+    def from_world(cls, world: World) -> Self:
+        """
+        Creates a minimal semantic robot annotation from the given world, starting at root_body
+        """
+
+        with world.modify_world():
+            robot = cls(
+                name=PrefixedName(name="generic_robot", prefix=world.name),
+                root=world.root,
+                _world=world,
+            )
+
+            world.add_semantic_annotation(robot, skip_duplicates=True)
+
+            vel_limits = defaultdict(lambda: 1.0)
+            robot.tighten_dof_velocity_limits_of_1dof_connections(new_limits=vel_limits)
+
+        return robot

src/semantic_digital_twin/testing.py

@@ -2,14 +2,17 @@
 import threading
 import time
 
+import pytest
 from krrood.entity_query_language.symbol_graph import SymbolGraph
-from krrood.entity_query_language.symbolic import Variable
 from typing_extensions import Tuple
 
-import pytest
-
 from .adapters.urdf import URDFParser
+from .datastructures.prefixed_name import PrefixedName
+from .spatial_types import TransformationMatrix
+from .spatial_types.derivatives import DerivativeMap
+from .spatial_types.spatial_types import Vector3
 from .utils import rclpy_installed, tracy_installed, hsrb_installed
+from .world import World
 from .world_description.connections import (
     Connection6DoF,
     PrismaticConnection,
@@ -19,13 +22,8 @@
 )
 from .world_description.degree_of_freedom import DegreeOfFreedom
 from .world_description.geometry import Box, Scale, Sphere
-from .datastructures.prefixed_name import PrefixedName
-from .spatial_types import TransformationMatrix
-from .spatial_types.derivatives import DerivativeMap
-from .spatial_types.spatial_types import Vector3
-from .world import World
 from .world_description.shape_collection import ShapeCollection
-from .world_description.world_entity import KinematicStructureEntity, Body
+from .world_description.world_entity import Body
 
 
 @pytest.fixture
@@ -286,6 +284,7 @@ def cleanup_after_test():
     # Teardown: runs after each test
     SymbolGraph().clear()
 
+
 @pytest.fixture()
 def kitchen_world():
     path = os.path.join(

test/test_semantic_annotations/test_semantic_annotations.py

@@ -7,8 +7,13 @@
 )  # You could replace this with numpy's regular assert for better compatibility
 
 from semantic_digital_twin.reasoning.world_reasoner import WorldReasoner
-from semantic_digital_twin.testing import *
+from semantic_digital_twin.robots.minimal_robot import MinimalRobot
+from semantic_digital_twin.robots.pr2 import PR2
 from semantic_digital_twin.semantic_annotations.semantic_annotations import *
+from semantic_digital_twin.testing import *
+from semantic_digital_twin.world_description.world_entity import (
+    KinematicStructureEntity,
+)
 
 try:
     from ripple_down_rules.user_interface.gui import RDRCaseViewer
@@ -131,10 +136,7 @@ def test_aggregate_bodies(kitchen_world):
 
 def test_handle_semantic_annotation_eql(apartment_world):
     with rule_mode():
-        body = let(
-            type_=Body,
-            domain=apartment_world.bodies
-        )
+        body = let(type_=Body, domain=apartment_world.bodies)
         query = infer(entity(Handle(body=body), in_("handle", body.name.name.lower())))
 
     handles = list(query.evaluate())
@@ -251,3 +253,26 @@ def test_semantic_annotation_serde_multiple(apartment_world):
     assert door == door_de2
     assert type(door.handle) == type(door_de2.handle)
     assert type(door.body) == type(door_de2.body)
+
+
+def test_minimal_robot_annotation(pr2_world):
+    urdf_dir = os.path.join(
+        os.path.dirname(os.path.abspath(__file__)), "..", "..", "resources", "urdf"
+    )
+    pr2 = os.path.join(urdf_dir, "pr2_kinematic_tree.urdf")
+    pr2_parser = URDFParser.from_file(file_path=pr2)
+    world_with_pr2 = pr2_parser.parse()
+    with world_with_pr2.modify_world():
+        MinimalRobot.from_world(world_with_pr2)
+        pr2_root = world_with_pr2.root
+        localization_body = Body(name=PrefixedName("odom_combined"))
+        world_with_pr2.add_kinematic_structure_entity(localization_body)
+        c_root_bf = OmniDrive.create_with_dofs(
+            parent=localization_body, child=pr2_root, world=world_with_pr2
+        )
+        world_with_pr2.add_connection(c_root_bf)
+
+    robot = world_with_pr2.get_semantic_annotations_by_type(MinimalRobot)[0]
+    pr2 = PR2.from_world(pr2_world)
+    assert len(robot.bodies) == len(pr2.bodies)
+    assert len(robot.connections) == len(pr2.connections)