Fixed free variables in double doors

Author: Tigul

src/semantic_digital_twin/semantic_annotations/factories.py

@@ -267,15 +267,19 @@ def _add_double_door_to_world(
             new_connections.append(new_connection)
             new_dofs.append(new_dof)
 
-        with parent_world.modify_world(), double_door_world.modify_world():
-            for new_dof in new_dofs:
-                parent_world.add_degree_of_freedom(new_dof)
+        with parent_world.modify_world():
+
+            with double_door_world.modify_world():
+                for new_dof in new_dofs:
+                    parent_world.add_degree_of_freedom(new_dof)
 
-            for new_door_world, new_parent_C_left in zip(new_worlds, new_connections):
-                parent_world.merge_world(new_door_world, new_parent_C_left)
+                for new_door_world, new_parent_C_left in zip(
+                    new_worlds, new_connections
+                ):
+                    parent_world.merge_world(new_door_world, new_parent_C_left)
 
-            double_door_world.remove_semantic_annotation(double_door)
-            parent_world.add_semantic_annotation(double_door)
+                double_door_world.remove_semantic_annotation(double_door)
+                parent_world.add_semantic_annotation(double_door)
 
     def _move_door_into_new_world(
         self,
@@ -294,7 +298,7 @@ def _move_door_into_new_world(
         double_door_world = door._world
         door_hinge_kse = door.body.parent_kinematic_structure_entity
         double_door_C_door: RevoluteConnection = door_hinge_kse.parent_connection
-        double_door_T_door = double_door_C_door.origin_expression
+        double_door_T_door = double_door_C_door.parent_T_connection_expression
         parent_T_door = parent_T_double_door @ double_door_T_door
         old_dof = double_door_C_door.dof
 

src/semantic_digital_twin/world_description/degree_of_freedom.py

@@ -18,6 +18,7 @@ class PositionVariable(cas.FloatVariable):
     """
     Describes the position of a degree of freedom.
     """
+
     name: PrefixedName = field(kw_only=True)
     dof: DegreeOfFreedom = field(kw_only=True)
     """ Backreference """
@@ -31,6 +32,7 @@ class VelocityVariable(cas.FloatVariable):
     """
     Describes the velocity of a degree of freedom.
     """
+
     name: PrefixedName = field(kw_only=True)
     dof: DegreeOfFreedom = field(kw_only=True)
     """ Backreference """
@@ -44,6 +46,7 @@ class AccelerationVariable(cas.FloatVariable):
     """
     Describes the acceleration of a degree of freedom.
     """
+
     name: PrefixedName = field(kw_only=True)
     dof: DegreeOfFreedom = field(kw_only=True)
     """ Backreference """
@@ -57,6 +60,7 @@ class JerkVariable(cas.FloatVariable):
     """
     Describes the jerk of a degree of freedom.
     """
+
     name: PrefixedName = field(kw_only=True)
     dof: DegreeOfFreedom = field(kw_only=True)
     """ Backreference """

src/semantic_digital_twin/world_description/world_entity.py

@@ -860,6 +860,7 @@ def add_to_world(self, world: World):
         self._world = world
 
     def __post_init__(self):
+
         self.name = self.name or self._generate_default_name(
             parent=self.parent, child=self.child
         )
@@ -870,6 +871,11 @@ def __post_init__(self):
         if self._connection_T_child_expression is None:
             self._connection_T_child_expression = TransformationMatrix()
 
+        if not self.parent_T_connection_expression.is_constant():
+            raise RuntimeError(
+                f"Parent T matrix must be constant for connection. This one contains free variables: {self.parent_T_connection_expression.free_variables()}"
+            )
+
         if (
             self.parent_T_connection_expression.reference_frame is not None
             and self.parent_T_connection_expression.reference_frame != self.parent