Merge pull request #12 from ami-iit/feature/frames

Add frames support

src/rod/__init__.py

@@ -1,6 +1,6 @@
 from . import logging
 from .collision import Collision
-from .common import Pose, Xyz
+from .common import Frame, Pose, Xyz
 from .geometry import (
     Box,
     Capsule,

src/rod/common.py

@@ -70,3 +70,15 @@ def xyz(self) -> List[float]:
     @property
     def rpy(self) -> List[float]:
         return self.pose[3:6]
+
+
+@dataclasses.dataclass
+class Frame(Element):
+
+    name: str = dataclasses.field(metadata=mashumaro.field_options(alias="@name"))
+
+    attached_to: Optional[str] = dataclasses.field(
+        default=None, metadata=mashumaro.field_options(alias="@attached_to")
+    )
+
+    pose: Optional[Pose] = dataclasses.field(default=None)

src/rod/model.py

@@ -3,7 +3,7 @@
 
 import mashumaro
 
-from .common import Pose
+from .common import Frame, Pose
 from .element import Element
 from .joint import Joint
 from .link import Link
@@ -54,6 +54,8 @@ class Model(Element):
 
     model: Optional[Union["Model", List["Model"]]] = dataclasses.field(default=None)
 
+    frame: Optional[Union[Frame, List[Frame]]] = dataclasses.field(default=None)
+
     link: Optional[Union[Link, List[Link]]] = dataclasses.field(default=None)
 
     joint: Optional[Union[Joint, List[Joint]]] = dataclasses.field(default=None)

src/rod/world.py

@@ -3,6 +3,7 @@
 
 import mashumaro.mixins.dict
 
+from .common import Frame
 from .element import Element
 from .model import Model
 from .physics import Physics
@@ -35,3 +36,5 @@ class World(Element):
     scene: Scene = dataclasses.field(default_factory=Scene)
 
     model: Optional[Union[Model, List[Model]]] = dataclasses.field(default=None)
+
+    frame: Optional[Union[Frame, List[Frame]]] = dataclasses.field(default=None)