Merge pull request #46 from ichumuh/main

implemented _post_init_with_world and _post_init_without_world

Author: tomsch420

src/semantic_world/connections.py

@@ -2,7 +2,7 @@
 
 from abc import ABC, abstractmethod
 from dataclasses import dataclass, field
-from typing import List, Tuple
+from typing import List, Tuple, TYPE_CHECKING
 
 import numpy as np
 
@@ -14,6 +14,8 @@
 from .types import NpMatrix4x4
 from .world_entity import Connection
 
+if TYPE_CHECKING:
+    from .world import World
 
 
 class Has1DOFState:
@@ -22,6 +24,7 @@ class Has1DOFState:
     """
 
     dof: DegreeOfFreedom
+    _world: World
 
     @property
     def position(self) -> float:
@@ -60,6 +63,24 @@ def jerk(self, value: float) -> None:
         self._world.notify_state_change()
 
 
+class HasUpdateState(ABC):
+    """
+    Mixin class for connections that need state updated which are not trivial integrations.
+    Typically needed for connections that use active and passive degrees of freedom.
+    Look at OmniDrive for an example usage.
+    """
+
+    @abstractmethod
+    def update_state(self, dt: float) -> None:
+        """
+        Allows the connection to update the state of its dofs.
+        An integration update for active dofs will have happened before this method is called.
+        Write directly into self._world.state, but don't touch dofs that don't belong to this connection.
+        :param dt: Time passed since last update.
+        """
+        pass
+
+
 @dataclass
 class FixedConnection(Connection):
     """
@@ -72,7 +93,10 @@ class ActiveConnection(Connection):
     """
     Has one or more degrees of freedom that can be actively controlled, e.g., robot joints.
     """
-    active_dofs: List[DegreeOfFreedom] = field(default_factory=list, init=False)
+
+    @property
+    def active_dofs(self) -> List[DegreeOfFreedom]:
+        return []
 
 
 @dataclass
@@ -81,7 +105,10 @@ class PassiveConnection(Connection):
     Has one or more degrees of freedom that cannot be actively controlled.
     Useful if a transformation is only tracked, e.g., the robot's localization.
     """
-    passive_dofs: List[DegreeOfFreedom] = field(default_factory=list, init=False)
+
+    @property
+    def passive_dofs(self) -> List[DegreeOfFreedom]:
+        return []
 
 
 @dataclass
@@ -170,8 +197,7 @@ def __post_init__(self):
         else:
             self.offset = self.offset
         self.axis = self.axis
-        self.dof = self.dof or self._world.create_degree_of_freedom(name=PrefixedName(str(self.name)))
-        self.active_dofs = [self.dof]
+        self._post_init_world_part()
 
         motor_expression = self.dof.symbols.position * self.multiplier + self.offset
         translation_axis = cas.Vector3(self.axis) * motor_expression
@@ -182,6 +208,18 @@ def __post_init__(self):
         self.origin_expression.reference_frame = self.parent
         self.origin_expression.child_frame = self.child
 
+    def _post_init_with_world(self):
+        self.dof = self.dof or self._world.create_degree_of_freedom(name=PrefixedName(str(self.name)))
+
+    def _post_init_without_world(self):
+        if self.dof is None:
+            raise ValueError("PrismaticConnection cannot be created without a world "
+                             "if the dof is not provided.")
+
+    @property
+    def active_dofs(self) -> List[DegreeOfFreedom]:
+        return [self.dof]
+
     def __hash__(self):
         return hash((self.parent, self.child))
 
@@ -224,8 +262,7 @@ def __post_init__(self):
         else:
             self.offset = self.offset
         self.axis = self.axis
-        self.dof = self.dof or self._world.create_degree_of_freedom(name=PrefixedName(str(self.name)))
-        self.active_dofs = [self.dof]
+        self._post_init_world_part()
 
         motor_expression = self.dof.symbols.position * self.multiplier + self.offset
         rotation_axis = cas.Vector3(self.axis)
@@ -234,6 +271,18 @@ def __post_init__(self):
         self.origin_expression.reference_frame = self.parent
         self.origin_expression.child_frame = self.child
 
+    def _post_init_with_world(self):
+        self.dof = self.dof or self._world.create_degree_of_freedom(name=PrefixedName(str(self.name)))
+
+    def _post_init_without_world(self):
+        if self.dof is None:
+            raise ValueError("RevoluteConnection cannot be created without a world "
+                             "if the dof is not provided.")
+
+    @property
+    def active_dofs(self) -> List[DegreeOfFreedom]:
+        return [self.dof]
+
     def __hash__(self):
         return hash((self.parent, self.child))
 
@@ -268,16 +317,7 @@ class Connection6DoF(PassiveConnection):
 
     def __post_init__(self):
         super().__post_init__()
-        self.x = self.x or self._world.create_degree_of_freedom(name=PrefixedName('x', str(self.name)))
-        self.y = self.y or self._world.create_degree_of_freedom(name=PrefixedName('y', str(self.name)))
-        self.z = self.z or self._world.create_degree_of_freedom(name=PrefixedName('z', str(self.name)))
-        self.qx = self.qx or self._world.create_degree_of_freedom(name=PrefixedName('qx', str(self.name)))
-        self.qy = self.qy or self._world.create_degree_of_freedom(name=PrefixedName('qy', str(self.name)))
-        self.qz = self.qz or self._world.create_degree_of_freedom(name=PrefixedName('qz', str(self.name)))
-        self.qw = self.qw or self._world.create_degree_of_freedom(name=PrefixedName('qw', str(self.name)))
-        self.passive_dofs = [self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw]
-
-        self._world.state[self.qw.name].position = 1.
+        self._post_init_world_part()
         parent_P_child = cas.Point3((self.x.symbols.position,
                                      self.y.symbols.position,
                                      self.z.symbols.position))
@@ -290,6 +330,29 @@ def __post_init__(self):
                                                                                      reference_frame=self.parent,
                                                                                      child_frame=self.child)
 
+    def _post_init_with_world(self):
+        if all(dof is None for dof in self.passive_dofs):
+            self.x = self._world.create_degree_of_freedom(name=PrefixedName('x', str(self.name)))
+            self.y = self._world.create_degree_of_freedom(name=PrefixedName('y', str(self.name)))
+            self.z = self._world.create_degree_of_freedom(name=PrefixedName('z', str(self.name)))
+            self.qx = self._world.create_degree_of_freedom(name=PrefixedName('qx', str(self.name)))
+            self.qy = self._world.create_degree_of_freedom(name=PrefixedName('qy', str(self.name)))
+            self.qz = self._world.create_degree_of_freedom(name=PrefixedName('qz', str(self.name)))
+            self.qw = self._world.create_degree_of_freedom(name=PrefixedName('qw', str(self.name)))
+            self._world.state[self.qw.name].position = 1.
+        elif any(dof is None for dof in self.passive_dofs):
+            raise ValueError("Connection6DoF can only be created "
+                             "if you provide all or none of the passive degrees of freedom")
+
+    def _post_init_without_world(self):
+        if any(dof is None for dof in self.passive_dofs):
+            raise ValueError("Connection6DoF cannot be created without a world "
+                             "if some passive degrees of freedom are not provided.")
+
+    @property
+    def passive_dofs(self) -> List[DegreeOfFreedom]:
+        return [self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw]
+
     @property
     def origin(self) -> NpMatrix4x4:
         return super().origin
@@ -307,24 +370,6 @@ def origin(self, transformation: NpMatrix4x4) -> None:
         self._world.notify_state_change()
 
 
-class HasUpdateState(ABC):
-    """
-    Mixin class for connections that need state updated which are not trivial integrations.
-    Typically needed for connections that use active and passive degrees of freedom.
-    Look at OmniDrive for an example usage.
-    """
-
-    @abstractmethod
-    def update_state(self, dt: float) -> None:
-        """
-        Allows the connection to update the state of its dofs.
-        An integration update for active dofs will have happened before this method is called.
-        Write directly into self._world.state, but don't touch dofs that don't belong to this connection.
-        :param dt: Time passed since last update.
-        """
-        pass
-
-
 @dataclass
 class OmniDrive(ActiveConnection, PassiveConnection, HasUpdateState):
     x: DegreeOfFreedom = field(default=None)
@@ -341,38 +386,7 @@ class OmniDrive(ActiveConnection, PassiveConnection, HasUpdateState):
 
     def __post_init__(self):
         super().__post_init__()
-        stringified_name = str(self.name)
-        lower_translation_limits = DerivativeMap()
-        lower_translation_limits.velocity = -self.translation_velocity_limits
-        upper_translation_limits = DerivativeMap()
-        upper_translation_limits.velocity = self.translation_velocity_limits
-        lower_rotation_limits = DerivativeMap()
-        lower_rotation_limits.velocity = -self.rotation_velocity_limits
-        upper_rotation_limits = DerivativeMap()
-        upper_rotation_limits.velocity = self.rotation_velocity_limits
-
-        self.x = self.x or self._world.create_degree_of_freedom(name=PrefixedName('x', stringified_name))
-        self.y = self.y or self._world.create_degree_of_freedom(name=PrefixedName('y', stringified_name))
-        self.z = self.z or self._world.create_degree_of_freedom(name=PrefixedName('z', stringified_name))
-
-        self.roll = self.roll or self._world.create_degree_of_freedom(name=PrefixedName('roll', stringified_name))
-        self.pitch = self.pitch or self._world.create_degree_of_freedom(name=PrefixedName('pitch', stringified_name))
-        self.yaw = self.yaw or self._world.create_degree_of_freedom(
-            name=PrefixedName('yaw', stringified_name),
-            lower_limits=lower_rotation_limits,
-            upper_limits=upper_rotation_limits)
-
-        self.x_vel = self.x_vel or self._world.create_degree_of_freedom(
-            name=PrefixedName('x_vel', stringified_name),
-            lower_limits=lower_translation_limits,
-            upper_limits=upper_translation_limits)
-        self.y_vel = self.y_vel or self._world.create_degree_of_freedom(
-            name=PrefixedName('y_vel', stringified_name),
-            lower_limits=lower_translation_limits,
-            upper_limits=upper_translation_limits)
-        self.active_dofs = [self.x_vel, self.y_vel, self.yaw]
-        self.passive_dofs = [self.x, self.y, self.z, self.roll, self.pitch]
-
+        self._post_init_world_part()
         odom_T_bf = cas.TransformationMatrix.from_xyz_rpy(x=self.x.symbols.position,
                                                           y=self.y.symbols.position,
                                                           yaw=self.yaw.symbols.position)
@@ -388,6 +402,58 @@ def __post_init__(self):
         self.origin_expression.reference_frame = self.parent
         self.origin_expression.child_frame = self.child
 
+    def _post_init_with_world(self):
+        if all(dof is None for dof in self.dofs):
+            stringified_name = str(self.name)
+            lower_translation_limits = DerivativeMap()
+            lower_translation_limits.velocity = -self.translation_velocity_limits
+            upper_translation_limits = DerivativeMap()
+            upper_translation_limits.velocity = self.translation_velocity_limits
+            lower_rotation_limits = DerivativeMap()
+            lower_rotation_limits.velocity = -self.rotation_velocity_limits
+            upper_rotation_limits = DerivativeMap()
+            upper_rotation_limits.velocity = self.rotation_velocity_limits
+
+            self.x = self._world.create_degree_of_freedom(name=PrefixedName('x', stringified_name))
+            self.y = self._world.create_degree_of_freedom(name=PrefixedName('y', stringified_name))
+            self.z = self._world.create_degree_of_freedom(name=PrefixedName('z', stringified_name))
+
+            self.roll = self._world.create_degree_of_freedom(name=PrefixedName('roll', stringified_name))
+            self.pitch = self._world.create_degree_of_freedom(name=PrefixedName('pitch', stringified_name))
+            self.yaw = self._world.create_degree_of_freedom(
+                name=PrefixedName('yaw', stringified_name),
+                lower_limits=lower_rotation_limits,
+                upper_limits=upper_rotation_limits)
+
+            self.x_vel = self._world.create_degree_of_freedom(
+                name=PrefixedName('x_vel', stringified_name),
+                lower_limits=lower_translation_limits,
+                upper_limits=upper_translation_limits)
+            self.y_vel = self._world.create_degree_of_freedom(
+                name=PrefixedName('y_vel', stringified_name),
+                lower_limits=lower_translation_limits,
+                upper_limits=upper_translation_limits)
+        elif any(dof is None for dof in self.passive_dofs):
+            raise ValueError("OmniDrive can only be created "
+                             "if you provide all or none of the passive degrees of freedom")
+
+    def _post_init_without_world(self):
+        if any(dof is None for dof in self.dofs):
+            raise ValueError("OmniDrive cannot be created without a world "
+                             "if some passive degrees of freedom are not provided.")
+
+    @property
+    def active_dofs(self) -> List[DegreeOfFreedom]:
+        return [self.x_vel, self.y_vel, self.yaw]
+
+    @property
+    def passive_dofs(self) -> List[DegreeOfFreedom]:
+        return [self.x, self.y, self.z, self.roll, self.pitch]
+
+    @property
+    def dofs(self) -> List[DegreeOfFreedom]:
+        return self.active_dofs + self.passive_dofs
+
     def update_state(self, dt: float) -> None:
         state = self._world.state
         state[self.x_vel.name].position = 0