Such speed wow

giskardpy/executor.py

@@ -15,7 +15,7 @@
     AuxiliaryVariableManager,
     AuxiliaryVariable,
 )
-from giskardpy.motion_statechart.context import BuildContext
+from giskardpy.motion_statechart.context import BuildContext, ExecutionContext
 from giskardpy.motion_statechart.motion_statechart import MotionStatechart
 from giskardpy.qp.exceptions import EmptyProblemException
 from giskardpy.qp.qp_controller import QPController
@@ -120,19 +120,30 @@ def build_context(self) -> BuildContext:
             control_cycle_variable=self._control_cycles_variable,
         )
 
+    @property
+    def execution_context(self) -> ExecutionContext:
+        return ExecutionContext(
+            world=self.world,
+            external_collision_data_data=self.collision_scene.get_external_collision_data(),
+            self_collision_data_data=self.collision_scene.get_self_collision_data(),
+            auxiliar_variables_data=self.auxiliary_variable_manager.resolve_auxiliary_variables(),
+            control_cycle_counter=self.control_cycles,
+        )
+
     def tick(self):
         self.control_cycles += 1
         self.collision_scene.sync()
         self.collision_scene.check_collisions()
-        self.motion_statechart.tick(self.build_context)
+        execution_context = self.execution_context
+        self.motion_statechart.tick(execution_context)
         if self.qp_controller is None:
             return
         next_cmd = self.qp_controller.get_cmd(
             world_state=self.world.state.data,
             life_cycle_state=self.motion_statechart.life_cycle_state.data,
-            external_collisions=self.collision_scene.get_external_collision_data(),
-            self_collisions=self.collision_scene.get_self_collision_data(),
-            auxiliary_variables=self.auxiliary_variable_manager.resolve_auxiliary_variables(),
+            external_collisions=execution_context.external_collision_data_data,
+            self_collisions=execution_context.self_collision_data_data,
+            auxiliary_variables=execution_context.auxiliar_variables_data,
         )
         self.world.apply_control_commands(
             next_cmd,
@@ -162,7 +173,7 @@ def _set_velocity_acceleration_jerk_to_zero(self):
     def _compile_qp_controller(self, controller_config: QPControllerConfig):
         ordered_dofs = sorted(
             self.world.active_degrees_of_freedom,
-            key=lambda dof: self.world.state._index[dof.name],
+            key=lambda dof: self.world.state._index[dof.id],
         )
         constraint_collection = (
             self.motion_statechart.combine_constraint_collections_of_nodes()

giskardpy/model/better_pybullet_syncer.py

@@ -98,7 +98,7 @@ def sync_world_model(self) -> None:
         self.objects_in_order = []
 
         for body in sorted(
-            self._world.bodies_with_enabled_collision, key=lambda b: b.name
+            self._world.bodies_with_enabled_collision, key=lambda b: b.id
         ):
             self.add_object(body)
             self.objects_in_order.append(self.body_to_bpb_obj[body])

giskardpy/model/bpb_wrapper.py

@@ -1,6 +1,7 @@
 import os
 import tempfile
 from typing import List, Tuple, Optional
+from uuid import UUID
 
 import giskardpy_bullet_bindings as pb
 import trimesh
@@ -27,8 +28,8 @@
 
 def create_collision(pb_collision: pb.Collision, world: World) -> GiskardCollision:
     collision = GiskardCollision(
-        body_a=world.get_kinematic_structure_entity_by_name(pb_collision.obj_a.name),
-        body_b=world.get_kinematic_structure_entity_by_name(pb_collision.obj_b.name),
+        body_a=world.get_kinematic_structure_entity_by_id(pb_collision.obj_a.name),
+        body_b=world.get_kinematic_structure_entity_by_id(pb_collision.obj_b.name),
         contact_distance_input=pb_collision.contact_distance,
         map_P_pa=pb_collision.map_P_pa,
         map_P_pb=pb_collision.map_P_pb,
@@ -123,7 +124,7 @@ def create_shape_from_link(
     shape = create_compound_shape(shapes_poses=shapes)
     # else:
     #     shape = create_shape_from_geometry(link.collisions[0])
-    return create_object(link.name, shape, pb.Transform.identity())
+    return create_object(link.id, shape, pb.Transform.identity())
 
 
 def create_compound_shape(
@@ -189,7 +190,7 @@ def convert_to_decomposed_obj_and_save_in_tmp(
 
 
 def create_object(
-    name: PrefixedName,
+    name: UUID,
     shape: pb.CollisionShape,
     transform: Optional[pb.Transform] = None,
 ) -> pb.CollisionObject:

giskardpy/model/collision_matrix_manager.py

@@ -4,7 +4,7 @@
 from enum import Enum
 from typing import List, Optional, Set, Dict, Any, Self
 
-from krrood.adapters.json_serializer import SubclassJSONSerializer
+from krrood.adapters.json_serializer import SubclassJSONSerializer, to_json, from_json
 
 from giskardpy.utils.utils import JsonSerializableEnum
 from semantic_digital_twin.adapters.world_entity_kwargs_tracker import (
@@ -40,24 +40,20 @@ def to_json(self) -> Dict[str, Any]:
             **super().to_json(),
             "type_": self.type_.to_json(),
             "distance": self.distance,
-            "body_group1_names": [body.name.to_json() for body in self.body_group1],
-            "body_group2_names": [body.name.to_json() for body in self.body_group2],
+            "body_group1_ids": [to_json(body.id) for body in self.body_group1],
+            "body_group2_ids": [to_json(body.id) for body in self.body_group2],
         }
 
     @classmethod
     def _from_json(cls, data: Dict[str, Any], **kwargs) -> Self:
         tracker = KinematicStructureEntityKwargsTracker.from_kwargs(kwargs)
         body_group1 = [
-            tracker.get_kinematic_structure_entity(
-                PrefixedName.from_json(name, **kwargs)
-            )
-            for name in data["body_group1_names"]
+            tracker.get_kinematic_structure_entity(from_json(id_))
+            for id_ in data["body_group1_ids"]
         ]
         body_group2 = [
-            tracker.get_kinematic_structure_entity(
-                PrefixedName.from_json(name, **kwargs)
-            )
-            for name in data["body_group2_names"]
+            tracker.get_kinematic_structure_entity(from_json(id_))
+            for id_ in data["body_group2_ids"]
         ]
         return cls(
             type_=CollisionAvoidanceTypes.from_json(data["type_"], **kwargs),

giskardpy/model/collisions.py

@@ -263,7 +263,7 @@ def transform_self_collision(
         new_link_a, new_link_b = world.compute_chain_reduced_to_controlled_connections(
             link_a, link_b
         )
-        if new_link_a.name > new_link_b.name:
+        if new_link_a.id > new_link_b.id:
             collision = collision.reverse()
             new_link_a, new_link_b = new_link_b, new_link_a
         collision.body_a = new_link_a

giskardpy/motion_statechart/auxilary_variable_manager.py

@@ -61,12 +61,17 @@ def create_vector3(name: PrefixedName, provider: Callable[[], List[float]]):
 @dataclass
 class AuxiliaryVariableManager:
     variables: List[AuxiliaryVariable] = field(default_factory=list)
+    data: np.ndarray = field(default_factory=lambda: np.array([], dtype=np.float64))
+
+    def add_variable(self, variable: AuxiliaryVariable):
+        self.variables.append(variable)
+        self.data = np.append(self.data, 0.0)
 
     def create_float_variable(
         self, name: PrefixedName, provider: Callable[[], float] = None
     ) -> AuxiliaryVariable:
         v = AuxiliaryVariable(name=name, provider=provider)
-        self.variables.append(v)
+        self.add_variable(v)
         return v
 
     def create_point3(
@@ -81,7 +86,9 @@ def create_point3(
         z = AuxiliaryVariable(
             name=PrefixedName("z", str(name)), provider=lambda: provider()[2]
         )
-        self.variables.extend([x, y, z])
+        self.add_variable(x)
+        self.add_variable(y)
+        self.add_variable(z)
         return Point3(x, y, z)
 
     def create_transformation_matrix(
@@ -94,9 +101,11 @@ def create_transformation_matrix(
                     name=PrefixedName(f"t[{row},{column}]", str(name)),
                     provider=lambda r=row, c=column: provider()[r, c],
                 )
-                self.variables.append(auxiliary_variable)
+                self.add_variable(auxiliary_variable)
                 transformation_matrix[row, column] = auxiliary_variable
         return transformation_matrix
 
     def resolve_auxiliary_variables(self) -> np.ndarray:
-        return np.array([v.resolve() for v in self.variables])
+        for i, v in enumerate(self.variables):
+            self.data[i] = v.resolve()
+        return self.data

giskardpy/motion_statechart/context.py

@@ -1,5 +1,6 @@
 from dataclasses import dataclass
 
+import numpy as np
 from typing_extensions import Self
 
 from giskardpy.model.collision_world_syncer import CollisionWorldSynchronizer
@@ -19,9 +20,6 @@ class BuildContext:
     qp_controller_config: QPControllerConfig
     control_cycle_variable: AuxiliaryVariable
 
-    def to_execution_context(self):
-        return ExecutionContext(world=self.world)
-
     @classmethod
     def empty(cls) -> Self:
         return cls(
@@ -36,3 +34,7 @@ def empty(cls) -> Self:
 @dataclass
 class ExecutionContext:
     world: World
+    external_collision_data_data: np.ndarray
+    self_collision_data_data: np.ndarray
+    auxiliar_variables_data: np.ndarray
+    control_cycle_counter: int

giskardpy/motion_statechart/goals/collision_avoidance.py

@@ -515,7 +515,7 @@ def add_self_collision_avoidance_constraints(self, context: BuildContext):
                             body_a_original, body_b_original
                         )
                     )
-                    if body_b.name < body_a.name:
+                    if body_b.id < body_a.id:
                         body_a, body_b = body_b, body_a
                     counter[body_a, body_b] = max(
                         [

giskardpy/motion_statechart/motion_statechart.py

@@ -7,6 +7,7 @@
 import numpy as np
 import rustworkx as rx
 from krrood.adapters.json_serializer import SubclassJSONSerializer
+from line_profiler.explicit_profiler import profile
 from typing_extensions import List, MutableMapping, ClassVar, Self, Type
 
 import semantic_digital_twin.spatial_types.spatial_types as cas
@@ -177,12 +178,19 @@ def compile(self):
             parameters=[self.observation_symbols(), self.life_cycle_symbols()],
             sparse=False,
         )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=0, numpy_array=self.motion_statechart.observation_state.data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=1, numpy_array=self.data
+        )
 
     def __getitem__(self, node: MotionStatechartNode) -> LifeCycleValues:
         return LifeCycleValues(super().__getitem__(node))
 
-    def update_state(self, observation_state: np.ndarray):
-        self.data = self._compiled_updater(observation_state, self.data)
+    @profile
+    def update_state(self):
+        np.copyto(self.data, self._compiled_updater.evaluate())
 
     def __str__(self) -> str:
         return str(
@@ -228,24 +236,29 @@ def compile(self, context: BuildContext):
             ],
             sparse=False,
         )
-
-    def update_state(
-        self,
-        life_cycle_state: np.ndarray,
-        world_state: np.ndarray,
-        external_collision_data: np.ndarray,
-        self_collision_data: np.ndarray,
-        auxiliar_variables: np.ndarray,
-    ):
-        self.data = self._compiled_updater(
-            self.data,
-            life_cycle_state,
-            world_state,
-            external_collision_data,
-            self_collision_data,
-            auxiliar_variables,
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=0, numpy_array=self.data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=1, numpy_array=self.motion_statechart.life_cycle_state.data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=2, numpy_array=context.world.state.data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=3, numpy_array=context.collision_scene.external_collision_data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=4, numpy_array=context.collision_scene.self_collision_data
+        )
+        self._compiled_updater.bind_args_to_memory_view(
+            arg_idx=5, numpy_array=context.auxiliary_variable_manager.data
         )
 
+    @profile
+    def update_state(self):
+        np.copyto(self.data, self._compiled_updater.evaluate())
+
 
 @dataclass(repr=False, eq=False)
 class StateHistoryItem:
@@ -520,25 +533,17 @@ def combine_constraint_collections_of_nodes(self) -> ConstraintCollection:
             )
         return combined_constraint_collection
 
-    def _update_observation_state(self, context: BuildContext):
-        self.observation_state.update_state(
-            life_cycle_state=self.life_cycle_state.data,
-            world_state=context.world.state.data,
-            external_collision_data=context.collision_scene.get_external_collision_data(),
-            self_collision_data=context.collision_scene.get_self_collision_data(),
-            auxiliar_variables=context.auxiliary_variable_manager.resolve_auxiliary_variables(),
-        )
+    def _update_observation_state(self, context: ExecutionContext):
+        self.observation_state.update_state()
         for node in self.nodes:
             if self.life_cycle_state[node] == LifeCycleValues.RUNNING:
-                observation_overwrite = node.on_tick(
-                    context=context.to_execution_context()
-                )
+                observation_overwrite = node.on_tick(context=context)
                 if observation_overwrite is not None:
                     self.observation_state[node] = observation_overwrite
 
     def _update_life_cycle_state(self, context: ExecutionContext):
         previous = self.life_cycle_state.data.copy()
-        self.life_cycle_state.update_state(self.observation_state.data)
+        self.life_cycle_state.update_state()
         self._trigger_life_cycle_callbacks(
             previous, self.life_cycle_state.data, context
         )
@@ -573,18 +578,18 @@ def _trigger_life_cycle_callbacks(
                 case _:
                     pass
 
-    def tick(self, context: BuildContext):
+    def tick(self, context: ExecutionContext):
         """
         Executes a single tick of the motion statechart.
         First the observation state is updated, then the life cycle state is updated.
         :param context: The context required to execute the tick.
         """
         self._update_observation_state(context)
-        self._update_life_cycle_state(context.to_execution_context())
+        self._update_life_cycle_state(context)
         self._raise_if_cancel_motion()
         self.history.append(
             next_item=StateHistoryItem(
-                control_cycle=context.control_cycle_variable.evaluate(),
+                control_cycle=context.control_cycle_counter,
                 life_cycle_state=self.life_cycle_state,
                 observation_state=self.observation_state,
             )

test/conftest.py

@@ -125,7 +125,7 @@ def from_world(cls, world: World) -> Self:
                 root=world.get_body_by_name("bot"),
                 _world=world,
             )
-            world.add_semantic_annotation(boxbot, skip_duplicates=True)
+            world.add_semantic_annotation(boxbot)
 
 
 @pytest.fixture()

test/test_motion_statechart/test_json_parsing.py

@@ -170,7 +170,7 @@ def test_executing_json_parsed_statechart():
         )
         world.add_degree_of_freedom(dof)
         root_C_tip = RevoluteConnection(
-            parent=root, child=tip, axis=Vector3.Z(), dof_name=dof.name
+            parent=root, child=tip, axis=Vector3.Z(), dof_id=dof.id
         )
         world.add_connection(root_C_tip)
 
@@ -179,7 +179,7 @@ def test_executing_json_parsed_statechart():
         )
         world.add_degree_of_freedom(dof)
         root_C_tip2 = RevoluteConnection(
-            parent=root, child=tip2, axis=Vector3.Z(), dof_name=dof.name
+            parent=root, child=tip2, axis=Vector3.Z(), dof_id=dof.id
         )
         world.add_connection(root_C_tip2)