Merge pull request #116 from Tigul/pycram-integration

Pycram integration

Author: tomsch420

.github/workflows/build_and_deploy_doc.yml

@@ -78,6 +78,12 @@ jobs:
           rm -f "$ipynb"
         done
 
+    - name: Install Node.js
+      uses: actions/setup-node@v4
+      with:
+        node-version: '18'
+
+
     # Build the book
     - name: Build the book
       run: |

requirements.txt

@@ -22,7 +22,7 @@ pathlib
 probabilistic_model
 python-fcl
 manifold3d
-krrood>=1.0.1
+krrood==1.1.1
 rustworkx_utils
 polytope
 ortools

scripts/generate_orm.py

@@ -11,7 +11,6 @@
 from dataclasses import is_dataclass
 
 import krrood.entity_query_language.orm.model
-import krrood.entity_query_language.symbol_graph
 import trimesh
 from krrood.class_diagrams import ClassDiagram
 from krrood.entity_query_language.predicate import HasTypes, HasType, Symbol

src/semantic_digital_twin/datastructures/prefixed_name.py

@@ -18,7 +18,9 @@ def __str__(self):
         return f"{self.prefix}/{self.name}"
 
     def __eq__(self, other):
-        return str(self) == str(other)
+        if not isinstance(other, type(self)):
+            return False
+        return self.prefix == other.prefix and self.name == other.name
 
     def to_json(self) -> Dict[str, Any]:
         return {**super().to_json(), "name": self.name, "prefix": self.prefix}

src/semantic_digital_twin/pipeline/pipeline.py

@@ -151,9 +151,7 @@ def _apply(self, world: World) -> World:
             if parent_connection is None:
                 return factory.create()
 
-            for entity in world.compute_descendent_child_kinematic_structure_entities(
-                body
-            ):
+            for entity in world.get_kinematic_structure_entities_of_branch(body):
                 world.remove_kinematic_structure_entity(entity)
 
             world.remove_kinematic_structure_entity(body)

src/semantic_digital_twin/semantic_annotations/semantic_annotations.py

@@ -3,11 +3,11 @@
 from dataclasses import dataclass, field
 
 import numpy as np
+
 from probabilistic_model.probabilistic_circuit.rx.helper import uniform_measure_of_event
 from typing_extensions import List
 
 from ..world_description.shape_collection import BoundingBoxCollection
-from ..datastructures.prefixed_name import PrefixedName
 from ..spatial_types import Point3
 from ..datastructures.variables import SpatialVariables
 from ..world_description.world_entity import SemanticAnnotation, Body, Region

src/semantic_digital_twin/spatial_computations/raytracer.py

@@ -205,7 +205,7 @@ def create_camera_rays(
         return self.scene.camera_rays()
 
     def ray_test(
-        self, origin_points: np.ndarray, target_points: np.ndarray
+        self, origin_points: np.ndarray, target_points: np.ndarray, multiple_hits=False
     ) -> Tuple[np.ndarray, np.ndarray, List[Body]]:
         """
         Performs a ray test from the origin point to the target point in the ray tracer scene.
@@ -214,6 +214,8 @@ def ray_test(
         :param target_points: The end point of the ray.
         :return: A tuple containing the points where the ray intersects and the indices of rays that hit the scene as well as the bodies that were.
         """
+        origin_points = np.array(origin_points)
+        target_points = np.array(target_points)
         self.update_scene()
         origin_points = origin_points.reshape((-1, 3))
         target_points = target_points.reshape((-1, 3))
@@ -222,7 +224,7 @@ def ray_test(
 
         ray_directions = target_points - origin_points
         points, index_ray, index_tri = self.scene.to_mesh().ray.intersects_location(
-            origin_points, ray_directions, multiple_hits=False
+            origin_points, ray_directions, multiple_hits=multiple_hits
         )
         bodies = self.scene.triangles_node[index_tri]
 

src/semantic_digital_twin/world.py

@@ -351,13 +351,17 @@ def reset_temporary_collision_config(self):
                 body.reset_temporary_collision_config()
 
     @property
-    def root(self) -> Optional[KinematicStructureEntity]:
+    def root(self):
+        return self._root(self._model_version)
+
+    @lru_cache()
+    def _root(self, model_version) -> Optional[KinematicStructureEntity]:
         """
         The root of the world is the unique node with in-degree 0.
 
+        :param model_version: Version of the model used to manage the cache of return values.
         :return: The root of the world.
         """
-
         if not self.kinematic_structure_entities:
             return None
         possible_roots = [
@@ -490,7 +494,7 @@ def reset_state_context(self) -> ResetStateContextManager:
     def clear_all_lru_caches(self):
         for method_name in dir(self):
             try:
-                method = getattr(self, method_name)
+                method = getattr(type(self), method_name)
                 if hasattr(method, "cache_clear") and callable(method.cache_clear):
                     method.cache_clear()
             except AttributeError:
@@ -517,8 +521,8 @@ def _notify_model_change(self) -> None:
         for model changes.
         """
         # if not self.world_is_being_modified:
-        self.compile_forward_kinematics_expressions()
         self.clear_all_lru_caches()
+        self.compile_forward_kinematics_expressions()
         self.notify_state_change()
         self._model_version += 1
 
@@ -788,7 +792,7 @@ def tree_edge(self, edge: Tuple[int, int, Connection]):
 
         return visitor.connections
 
-    def get_bodies_of_branch(
+    def get_kinematic_structure_entities_of_branch(
         self, root: KinematicStructureEntity
     ) -> List[KinematicStructureEntity]:
         """
@@ -797,22 +801,10 @@ def get_bodies_of_branch(
         :param root: The root body of the branch
         :return: List of all bodies in the subtree rooted at the given body (including the root)
         """
-
-        # Create a custom visitor to collect bodies
-        class BodyCollector(rustworkx.visit.DFSVisitor):
-            def __init__(self, world: World):
-                self.world = world
-                self.bodies = []
-
-            def discover_vertex(self, node_index: int, time: int) -> None:
-                """Called when a vertex is first discovered during DFS traversal"""
-                body = self.world.kinematic_structure[node_index]
-                self.bodies.append(body)
-
-        visitor = BodyCollector(self)
-        rx.dfs_search(self.kinematic_structure, [root.index], visitor)
-
-        return visitor.bodies
+        descendants_indices = rx.descendants(self.kinematic_structure, root.index)
+        return [root] + [
+            self.kinematic_structure[index] for index in descendants_indices
+        ]
 
     def get_semantic_annotation_by_name(
         self, name: Union[str, PrefixedName]
@@ -1171,6 +1163,7 @@ def get_body_by_name(self, name: Union[str, PrefixedName]) -> Body:
             return matches[0]
         raise KeyError(f"Body with name {name} not found")
 
+    @lru_cache(maxsize=None)
     def get_degree_of_freedom_by_name(
         self, name: Union[str, PrefixedName]
     ) -> DegreeOfFreedom:
@@ -1246,24 +1239,6 @@ def compute_child_kinematic_structure_entities(
             self.kinematic_structure.successors(kinematic_structure_entity.index)
         )
 
-    @lru_cache(maxsize=None)
-    def compute_descendent_child_kinematic_structure_entities(
-        self, kinematic_structure_entity: KinematicStructureEntity
-    ) -> List[KinematicStructureEntity]:
-        """
-        Computes all child entities of a given KinematicStructureEntity in the world recursively.
-        :param kinematic_structure_entity: The KinematicStructureEntity for which to compute children.
-        :return: A list of all child KinematicStructureEntities.
-        """
-        children = self.compute_child_kinematic_structure_entities(
-            kinematic_structure_entity
-        )
-        for child in children:
-            children.extend(
-                self.compute_descendent_child_kinematic_structure_entities(child)
-            )
-        return children
-
     @lru_cache(maxsize=None)
     def compute_parent_kinematic_structure_entity(
         self, kinematic_structure_entity: KinematicStructureEntity

src/semantic_digital_twin/world_description/graph_of_convex_sets.py

@@ -14,7 +14,7 @@
 import time
 from functools import reduce
 from operator import or_
-from typing_extensions import List, Optional, Dict
+from typing_extensions import List, Optional, Dict, Sequence
 
 # typing.Self is available starting with Python 3.11
 from typing_extensions import Self
@@ -80,6 +80,20 @@ def __init__(
         self.box_to_index_map = {}
         self.world = world
 
+    def create_subgraph(self, nodes: Sequence[int]) -> Self:
+        """
+        Create a subgraph of the current graph containing only the given nodes.
+
+        :param nodes: The nodes to include in the subgraph.
+        :return: The subgraph.
+        """
+        subgraph = GraphOfConvexSets(self.world, self.search_space)
+        subgraph.graph = self.graph.subgraph(nodes)
+        subgraph.box_to_index_map = {
+            box: index for box, index in self.box_to_index_map.items() if index in nodes
+        }
+        return subgraph
+
     def add_node(self, box: BoundingBox):
         self.box_to_index_map[box] = self.graph.add_node(box)
 
@@ -453,6 +467,31 @@ def free_space_from_world(
             bloat_obstacles=bloat_obstacles,
         )
 
+    @classmethod
+    def obstacles_from_world(
+        cls,
+        world: World,
+        search_space: BoundingBoxCollection,
+        bloat_obstacles: float = 0.0,
+    ) -> Optional[Event]:
+        """
+        Create an event representing the obstacles in the belief state of the robot.
+
+        :param world: The belief state.
+        :param search_space: The search space for the connectivity graph.
+        :param bloat_obstacles: The amount to bloat the obstacles.
+
+        :return: An event representing the obstacles in the search space.
+        """
+
+        view = SemanticEnvironmentAnnotation(root=world.root, _world=world)
+
+        return cls.obstacles_from_semantic_annotations(
+            search_space=search_space,
+            semantic_obstacle_annotation=view,
+            bloat_obstacles=bloat_obstacles,
+        )
+
     @classmethod
     def navigation_map_from_semantic_annotation(
         cls,

src/semantic_digital_twin/world_description/shape_collection.py

@@ -67,9 +67,9 @@ def _transform_to_own_frame(self, shape: Shape):
             # If we don’t have a world, fall back to the owning body/frame
             shape.origin.reference_frame = self.reference_frame
         elif (
-            self.reference_frame is not None
-            and shape.origin.reference_frame != self.reference_frame
-            and self.reference_frame._world is not None
+                self.reference_frame is not None
+                and shape.origin.reference_frame != self.reference_frame
+                and self.reference_frame._world is not None
         ):
             logger.warning(
                 f"Transformed shape {shape} to {self.reference_frame} since it was in a different "
@@ -114,7 +114,7 @@ def combined_mesh(self) -> Trimesh:
         return concatenate(transformed_meshes)
 
     def as_bounding_box_collection_at_origin(
-        self, origin: TransformationMatrix
+            self, origin: TransformationMatrix
     ) -> BoundingBoxCollection:
         """
         Provides the bounding box collection for this entity given a transformation matrix as origin.
@@ -136,7 +136,7 @@ def as_bounding_box_collection_at_origin(
         )
 
     def as_bounding_box_collection_in_frame(
-        self, reference_frame: KinematicStructureEntity
+            self, reference_frame: KinematicStructureEntity
     ) -> BoundingBoxCollection:
         """
         Provides the bounding box collection for this entity in the given reference frame.
@@ -182,9 +182,11 @@ class BoundingBoxCollection(ShapeCollection):
     shapes: List[BoundingBox]
 
     def __post_init__(self):
+        if not self.reference_frame:
+            raise ValueError("BoundingBoxCollection must have a reference frame.")
         for box in self.bounding_boxes:
             assert (
-                box.origin.reference_frame == self.reference_frame
+                    box.origin.reference_frame == self.reference_frame
             ), "All bounding boxes must have the same reference frame."
 
     def __iter__(self) -> Iterator[BoundingBox]:
@@ -209,15 +211,15 @@ def merge(self, other: BoundingBoxCollection) -> BoundingBoxCollection:
         :return: The merged bounding box collection.
         """
         assert (
-            self.reference_frame == other.reference_frame
+                self.reference_frame == other.reference_frame
         ), "The reference frames of the bounding box collections must be the same."
         return BoundingBoxCollection(
             reference_frame=self.reference_frame,
             shapes=self.bounding_boxes + other.bounding_boxes,
         )
 
     def bloat(
-        self, x_amount: float = 0.0, y_amount: float = 0, z_amount: float = 0
+            self, x_amount: float = 0.0, y_amount: float = 0, z_amount: float = 0
     ) -> BoundingBoxCollection:
         """
         Enlarges all bounding boxes in the collection by a given amount in all dimensions.
@@ -235,10 +237,10 @@ def bloat(
 
     @classmethod
     def from_simple_event(
-        cls,
-        reference_frame: KinematicStructureEntity,
-        simple_event: SimpleEvent,
-        keep_surface: bool = False,
+            cls,
+            reference_frame: KinematicStructureEntity,
+            simple_event: SimpleEvent,
+            keep_surface: bool = False,
     ) -> BoundingBoxCollection:
         """
         Create a list of bounding boxes from a simple random event.
@@ -250,9 +252,9 @@ def from_simple_event(
         """
         result = []
         for x, y, z in itertools.product(
-            simple_event[SpatialVariables.x.value].simple_sets,
-            simple_event[SpatialVariables.y.value].simple_sets,
-            simple_event[SpatialVariables.z.value].simple_sets,
+                simple_event[SpatialVariables.x.value].simple_sets,
+                simple_event[SpatialVariables.y.value].simple_sets,
+                simple_event[SpatialVariables.z.value].simple_sets,
         ):
 
             bb = BoundingBox(
@@ -262,7 +264,9 @@ def from_simple_event(
                 x.upper,
                 y.upper,
                 z.upper,
-                TransformationMatrix(reference_frame=reference_frame),
+                TransformationMatrix.from_xyz_quaternion(x.upper - (x.upper - x.lower) / 2, y.upper - (y.upper - y.lower) / 2,
+                                                         z.upper - (z.upper - z.lower) / 2, 0, 0, 0, 1,
+                                                         reference_frame=reference_frame)
             )
             if not keep_surface and (bb.depth == 0 or bb.height == 0 or bb.width == 0):
                 continue
@@ -271,7 +275,7 @@ def from_simple_event(
 
     @classmethod
     def from_event(
-        cls, reference_frame: KinematicStructureEntity, event: Event
+            cls, reference_frame: KinematicStructureEntity, event: Event
     ) -> Self:
         """
         Create a list of bounding boxes from a random event.
@@ -301,7 +305,7 @@ def from_shapes(cls, shapes: ShapeCollection) -> Self:
             return cls(shapes=[])
         for shape in shapes:
             assert (
-                shape.origin.reference_frame == shapes[0].origin.reference_frame
+                    shape.origin.reference_frame == shapes[0].origin.reference_frame
             ), "All shapes must have the same reference frame."
 
         local_bbs = [shape.local_frame_bounding_box for shape in shapes]
@@ -339,5 +343,5 @@ def bounding_box(self) -> BoundingBox:
             max(all_x),
             max(all_y),
             max(all_z),
-            self.reference_frame.global_pose,
+            TransformationMatrix.from_xyz_quaternion(reference_frame=self.reference_frame),
         )