Merge pull request #48 from ichumuh/spatial_types_everywhere

Spatial types everywhere

Author: Tigul

examples/graph_of_convex_sets.md

@@ -90,8 +90,8 @@ Let's use graph theory to find a path!
 ```{code-cell} ipython2
 from semantic_world.spatial_types import Point3
 
-start = Point3.from_xyz(-0.75, 0, 0.15)
-goal = Point3.from_xyz(0.75, 0, 0.15)
+start = Point3(-0.75, 0, 0.15)
+goal = Point3(0.75, 0, 0.15)
 path = gcs.path_from_to(start, goal)
 print("A potential path is", [(point.x, point.y) for point in path])
 ```
@@ -145,8 +145,8 @@ This allows the accessing of locations using a sequence of local problems put to
 Finally, let's find a way from here to there:
 
 ```{code-cell} ipython2
-start = Point3.from_xyz(-0.75, 0, 0.15)
-goal = Point3.from_xyz(0.75, 0, 0.15)
+start = Point3(-0.75, 0, 0.15)
+goal = Point3(0.75, 0, 0.15)
 path = gcs.path_from_to(start, goal)
 print("A potential path is", [(point.x, point.y, point.z) for point in path])
 ```

scripts/generate_orm.py

@@ -1,22 +1,16 @@
-import logging
 import os
-import sys
 from enum import Enum
 
-from ormatic.ormatic import logger, ORMatic
-from ormatic.utils import classes_of_module, recursive_subclasses
-from sqlacodegen.generators import TablesGenerator
-from sqlalchemy import create_engine
-from sqlalchemy.orm import registry, Session
-
+import semantic_world.degree_of_freedom
 import semantic_world.geometry
+import semantic_world.views.views
 import semantic_world.world_entity
-from semantic_world.connections import FixedConnection, OmniDrive
-import semantic_world.degree_of_freedom
-from semantic_world.prefixed_name import PrefixedName
+from ormatic.ormatic import ORMatic
+from ormatic.utils import classes_of_module, recursive_subclasses
+from semantic_world.connections import FixedConnection
 from semantic_world.orm.model import *
+from semantic_world.prefixed_name import PrefixedName
 from semantic_world.world import *
-import semantic_world.views.views
 
 # ----------------------------------------------------------------------------------------------------------------------
 # This script generates the ORM classes for the semantic_world package.
@@ -41,34 +35,22 @@
             World, ForwardKinematicsVisitor, Has1DOFState, DegreeOfFreedom}
 classes -= set(recursive_subclasses(Enum))
 
+
 def generate_orm():
     """
     Generate the ORM classes for the pycram package.
     """
-    # Set up logging
-    handler = logging.StreamHandler(sys.stdout)
-    handler.setFormatter(logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s'))
-
-    logger.addHandler(handler)
-    logger.setLevel(logging.INFO)
-
-    mapper_registry = registry()
-    engine = create_engine('sqlite:///:memory:')
-    session = Session(engine)
 
     # Create an ORMatic object with the classes to be mapped
     ormatic = ORMatic(list(classes))
 
     # Generate the ORM classes
     ormatic.make_all_tables()
 
-    # Create the tables in the database
-    mapper_registry.metadata.create_all(session.bind)
-
     path = os.path.abspath(os.path.join(os.getcwd(), '../src/semantic_world/orm/'))
     with open(os.path.join(path, 'ormatic_interface.py'), 'w') as f:
         ormatic.to_sqlalchemy_file(f)
 
 
 if __name__ == '__main__':
-    generate_orm()
+    generate_orm()

src/semantic_world/adapters/multi_parser.py

@@ -9,7 +9,7 @@
                                JointBuilder, JointType)
 from pxr import UsdUrdf
 
-from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection, UnitVector
+from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection
 from ..spatial_types.derivatives import DerivativeMap
 from ..prefixed_name import PrefixedName
 from ..spatial_types import spatial_types as cas
@@ -115,11 +115,11 @@ def parse_joints(self, body_builder: BodyBuilder, world: World) -> list[Connecti
             transform = body_builder.xform.GetLocalTransformation()
             pos = transform.ExtractTranslation()
             quat = transform.ExtractRotationQuat()
-            point_expr = cas.Point3((pos[0], pos[1], pos[2]))
-            quaternion_expr = cas.Quaternion((quat.GetImaginary()[0],
+            point_expr = cas.Point3(pos[0], pos[1], pos[2])
+            quaternion_expr = cas.Quaternion(quat.GetImaginary()[0],
                                               quat.GetImaginary()[1],
                                               quat.GetImaginary()[2],
-                                              quat.GetReal()))
+                                              quat.GetReal())
             origin = cas.TransformationMatrix.from_point_rotation_matrix(point=point_expr,
                                                                          rotation_matrix=quaternion_expr.to_rotation_matrix())
             connection = FixedConnection(parent=parent_body, child=child_body, origin_expression=origin)
@@ -132,11 +132,11 @@ def parse_joint(self, joint_builder: JointBuilder, parent_body: Body, child_body
         joint_name = joint_prim.GetName()
         joint_pos = joint_builder.pos
         joint_quat = joint_builder.quat
-        point_expr = cas.Point3((joint_pos[0], joint_pos[1], joint_pos[2]))
-        quaternion_expr = cas.Quaternion((joint_quat.GetImaginary()[0],
+        point_expr = cas.Point3(joint_pos[0], joint_pos[1], joint_pos[2])
+        quaternion_expr = cas.Quaternion(joint_quat.GetImaginary()[0],
                                           joint_quat.GetImaginary()[1],
                                           joint_quat.GetImaginary()[2],
-                                          joint_quat.GetReal()))
+                                          joint_quat.GetReal())
         origin = cas.TransformationMatrix.from_point_rotation_matrix(point=point_expr,
                                                                      rotation_matrix=quaternion_expr.to_rotation_matrix())
         free_variable_name = PrefixedName(joint_name)
@@ -153,9 +153,10 @@ def parse_joint(self, joint_builder: JointBuilder, parent_body: Body, child_body
         elif joint_builder.type == JointType.FIXED:
             return FixedConnection(parent=parent_body, child=child_body, origin_expression=origin)
         elif joint_builder.type in [JointType.REVOLUTE, JointType.CONTINUOUS, JointType.PRISMATIC]:
-            axis = UnitVector(float(joint_builder.axis.to_array()[0]),
-                              float(joint_builder.axis.to_array()[1]),
-                              float(joint_builder.axis.to_array()[2]))
+            axis = cas.Vector3(float(joint_builder.axis.to_array()[0]),
+                                   float(joint_builder.axis.to_array()[1]),
+                                   float(joint_builder.axis.to_array()[2]),
+                                   reference_frame=parent_body)
             try:
                 dof = world.get_degree_of_freedom_by_name(free_variable_name)
             except KeyError:

src/semantic_world/adapters/urdf.py

@@ -4,10 +4,10 @@
 from ..spatial_types import spatial_types as cas
 from urdf_parser_py import urdf
 
-from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection, UnitVector
+from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection
 from ..prefixed_name import PrefixedName
 from ..spatial_types.derivatives import Derivatives, DerivativeMap
-from ..spatial_types.spatial_types import TransformationMatrix
+from ..spatial_types.spatial_types import TransformationMatrix, Vector3
 from ..utils import suppress_stdout_stderr, hacky_urdf_parser_fix
 from ..world import World, Body, Connection
 from ..geometry import Box, Sphere, Cylinder, Mesh, Scale, Shape, Color
@@ -157,7 +157,8 @@ def parse_joint(self, joint: urdf.Joint, parent: Body, child: Body, world: World
 
         result = connection_type(parent=parent, child=child, origin_expression=parent_T_child,
                                  multiplier=multiplier, offset=offset,
-                                 axis=UnitVector(*map(int, joint.axis)),
+                                 axis=Vector3(*map(int, joint.axis),
+                                                 reference_frame=parent),
                                  dof=dof)
         return result
 

src/semantic_world/connections.py

@@ -2,14 +2,14 @@
 
 from abc import ABC, abstractmethod
 from dataclasses import dataclass, field
-from typing import List, Tuple, TYPE_CHECKING
+from typing import List, TYPE_CHECKING, Union
 
 import numpy as np
 
 from . import spatial_types as cas
 from .degree_of_freedom import DegreeOfFreedom
 from .prefixed_name import PrefixedName
-from .spatial_types.derivatives import Derivatives, DerivativeMap
+from .spatial_types.derivatives import DerivativeMap
 from .spatial_types.math import quaternion_from_rotation_matrix
 from .types import NpMatrix4x4
 from .world_entity import Connection
@@ -111,61 +111,13 @@ def passive_dofs(self) -> List[DegreeOfFreedom]:
         return []
 
 
-@dataclass
-class UnitVector:
-    """
-    Represents a unit vector which is always of size 1.
-    """
-
-    x: float
-    y: float
-    z: float
-
-    def __post_init__(self):
-        self.normalize()
-
-    def normalize(self):
-        length = self.length
-        self.x /= length
-        self.y /= length
-        self.z /= length
-
-    @property
-    def length(self):
-        return np.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
-
-    def __getitem__(self, item: int) -> float:
-        if item == 0:
-            return self.x
-        if item == 1:
-            return self.y
-        if item == 2:
-            return self.z
-        raise IndexError
-
-    def as_tuple(self) -> Tuple[float, float, float]:
-        return self.x, self.y, self.z
-
-    @classmethod
-    def X(cls):
-        return cls(1, 0, 0)
-
-    @classmethod
-    def Y(cls):
-        return cls(0, 1, 0)
-
-    @classmethod
-    def Z(cls):
-        return cls(0, 0, 1)
-
-
 @dataclass
 class PrismaticConnection(ActiveConnection, Has1DOFState):
     """
     Allows the movement along an axis.
     """
 
-    axis: UnitVector = field(kw_only=True)
+    axis: cas.Vector3 = field(kw_only=True)
     """
     Connection moves along this axis, should be a unit vector.
     The axis is defined relative to the local reference frame of the parent body.
@@ -200,7 +152,7 @@ def __post_init__(self):
         self._post_init_world_part()
 
         motor_expression = self.dof.symbols.position * self.multiplier + self.offset
-        translation_axis = cas.Vector3(self.axis) * motor_expression
+        translation_axis = cas.Vector3.from_iterable(self.axis) * motor_expression
         parent_T_child = cas.TransformationMatrix.from_xyz_rpy(x=translation_axis[0],
                                                                y=translation_axis[1],
                                                                z=translation_axis[2])
@@ -230,7 +182,7 @@ class RevoluteConnection(ActiveConnection, Has1DOFState):
     Allows rotation about an axis.
     """
 
-    axis: UnitVector = field(kw_only=True)
+    axis: cas.Vector3 = field(kw_only=True)
     """
     Connection rotates about this axis, should be a unit vector.
     The axis is defined relative to the local reference frame of the parent body.
@@ -265,9 +217,8 @@ def __post_init__(self):
         self._post_init_world_part()
 
         motor_expression = self.dof.symbols.position * self.multiplier + self.offset
-        rotation_axis = cas.Vector3(self.axis)
-        parent_R_child = cas.RotationMatrix.from_axis_angle(rotation_axis, motor_expression)
-        self.origin_expression = self.origin_expression.dot(cas.TransformationMatrix(parent_R_child))
+        parent_R_child = cas.RotationMatrix.from_axis_angle(self.axis, motor_expression)
+        self.origin_expression = self.origin_expression @ cas.TransformationMatrix(parent_R_child)
         self.origin_expression.reference_frame = self.parent
         self.origin_expression.child_frame = self.child
 
@@ -318,13 +269,13 @@ class Connection6DoF(PassiveConnection):
     def __post_init__(self):
         super().__post_init__()
         self._post_init_world_part()
-        parent_P_child = cas.Point3((self.x.symbols.position,
-                                     self.y.symbols.position,
-                                     self.z.symbols.position))
-        parent_R_child = cas.Quaternion((self.qx.symbols.position,
-                                         self.qy.symbols.position,
-                                         self.qz.symbols.position,
-                                         self.qw.symbols.position)).to_rotation_matrix()
+        parent_P_child = cas.Point3(x=self.x.symbols.position,
+                                    y=self.y.symbols.position,
+                                    z=self.z.symbols.position)
+        parent_R_child = cas.Quaternion(x=self.qx.symbols.position,
+                                        y=self.qy.symbols.position,
+                                        z=self.qz.symbols.position,
+                                        w=self.qw.symbols.position).to_rotation_matrix()
         self.origin_expression = cas.TransformationMatrix.from_point_rotation_matrix(point=parent_P_child,
                                                                                      rotation_matrix=parent_R_child,
                                                                                      reference_frame=self.parent,
@@ -354,11 +305,13 @@ 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:
+    def origin(self) -> cas.TransformationMatrix:
         return super().origin
 
     @origin.setter
-    def origin(self, transformation: NpMatrix4x4) -> None:
+    def origin(self, transformation: Union[NpMatrix4x4, cas.TransformationMatrix]) -> None:
+        if isinstance(transformation, cas.TransformationMatrix):
+            transformation = transformation.to_np()
         orientation = quaternion_from_rotation_matrix(transformation)
         self._world.state[self.x.name].position = transformation[0, 3]
         self._world.state[self.y.name].position = transformation[1, 3]

src/semantic_world/exceptions.py

@@ -1,8 +1,10 @@
 from __future__ import annotations
 from typing_extensions import List, TYPE_CHECKING
 
+from .prefixed_name import PrefixedName
+
 if TYPE_CHECKING:
-    from semantic_world.world_entity import View
+    from .world_entity import View
 
 
 class LogicalError(Exception):
@@ -29,3 +31,9 @@ class DuplicateViewError(UsageError):
     def __init__(self, views: List[View]):
         msg = f'Views {views} are duplicates, while views elements should be unique.'
         super().__init__(msg)
+
+
+class ViewNotFoundError(UsageError):
+    def __init__(self, name: PrefixedName):
+        msg = f'View with name {name} not found'
+        super().__init__(msg)

src/semantic_world/geometry.py

@@ -383,7 +383,7 @@ def get_points(self) -> List[Point3]:
 
         :return: A list of Point3 objects representing the corners of the bounding box.
         """
-        return [Point3.from_xyz(x, y, z)
+        return [Point3(x, y, z)
                 for x in (self.min_x, self.max_x)
                 for y in (self.min_y, self.max_y)
                 for z in (self.min_z, self.max_z)]

src/semantic_world/graph_of_convex_sets.py

@@ -198,7 +198,7 @@ def path_from_to(self, start: Point3, goal: Point3) -> Optional[List[Point3]]:
             x_target = intersection.x_interval.center()
             y_target = intersection.y_interval.center()
             z_target = intersection.z_interval.center()
-            result.append(Point3.from_xyz(x_target, y_target, z_target))
+            result.append(Point3(x_target, y_target, z_target))
 
         result.append(goal)
         return result

src/semantic_world/ik_solver.py

@@ -11,8 +11,6 @@
 from .connections import ActiveConnection, PassiveConnection
 from .degree_of_freedom import DegreeOfFreedom
 from .spatial_types import spatial_types as cas
-from .spatial_types.derivatives import Derivatives
-from .types import NpMatrix4x4
 
 if TYPE_CHECKING:
     from .world import World
@@ -137,7 +135,7 @@ class InverseKinematicsSolver:
     Unit is m for the position target or rad for the orientation target.
     """
 
-    def solve(self, root: Body, tip: Body, target: NpMatrix4x4,
+    def solve(self, root: Body, tip: Body, target: cas.TransformationMatrix,
               dt: float = 0.05, max_iterations: int = 200,
               translation_velocity: float = 0.2, rotation_velocity: float = 0.2) -> Dict[DegreeOfFreedom, float]:
         """
@@ -257,7 +255,7 @@ class QPProblem:
     Tip body of the kinematic chain.
     """
 
-    target: NpMatrix4x4
+    target: cas.TransformationMatrix
     """
     Desired tip pose relative to the root body.
     """
@@ -373,7 +371,7 @@ class ConstraintBuilder:
     Tip body of the kinematic chain.
     """
 
-    target: NpMatrix4x4
+    target: cas.TransformationMatrix
     """
     Desired tip pose relative to the root body.
     """
@@ -461,7 +459,7 @@ def _compute_rotation_error(self, root_T_tip: cas.TransformationMatrix) -> Tuple
         """
         rotation_cap = self.max_rotation_velocity * self.dt
 
-        hack = cas.RotationMatrix.from_axis_angle(cas.Vector3((0, 0, 1)), -0.0001)
+        hack = cas.RotationMatrix.from_axis_angle(cas.Vector3.Z(), -0.0001)
         root_R_tip = root_T_tip.to_rotation().dot(hack)
         q_actual = cas.TransformationMatrix(self.target).to_quaternion()
         q_goal = root_R_tip.to_quaternion()