[test] updated orm querying; added docstrings to actions and motions

src/pycram/designators/action_designator.py

@@ -1,6 +1,5 @@
 import itertools
-from typing import Union
-from .motion_designator import *
+from typing_extensions import List, Union, Callable
 from .object_designator import ObjectDesignatorDescription, ObjectPart
 from ..enums import Arms
 from ..designator import ActionDesignatorDescription

src/pycram/designators/actions/actions.py

@@ -1,5 +1,5 @@
 import abc
-from typing import Union
+from typing_extensions import Union
 from pycram.designator import ActionDesignatorDescription
 from pycram.designators.motion_designator import *
 from pycram.enums import Arms
@@ -27,62 +27,36 @@ class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC):
 
     @abc.abstractmethod
     def perform(self) -> None:
-        """Perform the action."""
+        """
+        Perform the action. Will be overwritten by each action.
+        """
         pass
 
     @abc.abstractmethod
     def to_sql(self) -> Action:
-        """Convert this action to its ORM equivalent."""
+        """
+        Convert this action to its ORM equivalent. Will be overwritten by each action.
+        """
         pass
 
     @abc.abstractmethod
     def insert(self, session: Session, **kwargs) -> Action:
-        """Insert this action into the database."""
-        action = super().insert(session)
-        return action
-
-
-@dataclass
-class ParkArmsActionPerformable(ActionAbstract):
-
-    arm: Arms
-    """
-    Entry from the enum for which arm should be parked
-    """
-
-    @with_tree
-    def perform(self) -> None:
-        # create the keyword arguments
-        kwargs = dict()
-        left_poses = None
-        right_poses = None
-
-        # add park left arm if wanted
-        if self.arm in [Arms.LEFT, Arms.BOTH]:
-            kwargs["left_arm_config"] = "park"
-            left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"])
-
-        # add park right arm if wanted
-        if self.arm in [Arms.RIGHT, Arms.BOTH]:
-            kwargs["right_arm_config"] = "park"
-            right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"])
+        """
+        Insert this action into the database.
 
-        MoveArmJointsMotion(left_poses, right_poses).perform()
-
-    def to_sql(self) -> ORMParkArmsAction:
-        return ORMParkArmsAction(self.arm.name)
+        :param session: Session with a database that is used to add and commit the objects
+        :param kwargs: Possible extra keyword arguments
+        :return: The completely instanced ORM object
+        """
 
-    def insert(self, session: Session, **kwargs) -> ORMParkArmsAction:
         action = super().insert(session)
-        session.add(action)
-        session.commit()
         return action
 
 
 @dataclass
 class MoveTorsoActionPerformable(ActionAbstract):
     """
-    Performable Move Torso Action designator.
+    Move the torso of the robot up and down.
     """
 
     position: float
@@ -106,6 +80,9 @@ def insert(self, session: Session, **kwargs) -> ORMMoveTorsoAction:
 
 @dataclass
 class SetGripperActionPerformable(ActionAbstract):
+    """
+    Set the gripper state of the robot.
+    """
 
     gripper: str
     """
@@ -132,6 +109,11 @@ def insert(self, session: Session, *args, **kwargs) -> ORMSetGripperAction:
 
 @dataclass
 class ReleaseActionPerformable(ActionAbstract):
+    """
+    Releases an Object from the robot.
+
+    Note: This action can not ve used yet.
+    """
 
     gripper: str
 
@@ -149,6 +131,11 @@ def insert(self, session: Session, **kwargs) -> ORMParkArmsAction:
 
 @dataclass
 class GripActionPerformable(ActionAbstract):
+    """
+    Grip an object with the robot.
+
+    Note: This action can not be used yet.
+    """
 
     gripper: str
     object_designator: ObjectDesignatorDescription.Object
@@ -165,8 +152,51 @@ def insert(self, session: Session, *args, **kwargs) -> Base:
         raise NotImplementedError()
 
 
+@dataclass
+class ParkArmsActionPerformable(ActionAbstract):
+    """
+    Park the arms of the robot.
+    """
+
+    arm: Arms
+    """
+    Entry from the enum for which arm should be parked
+    """
+
+    @with_tree
+    def perform(self) -> None:
+        # create the keyword arguments
+        kwargs = dict()
+        left_poses = None
+        right_poses = None
+
+        # add park left arm if wanted
+        if self.arm in [Arms.LEFT, Arms.BOTH]:
+            kwargs["left_arm_config"] = "park"
+            left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"])
+
+        # add park right arm if wanted
+        if self.arm in [Arms.RIGHT, Arms.BOTH]:
+            kwargs["right_arm_config"] = "park"
+            right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"])
+
+        MoveArmJointsMotion(left_poses, right_poses).perform()
+
+    def to_sql(self) -> ORMParkArmsAction:
+        return ORMParkArmsAction(self.arm.name)
+
+    def insert(self, session: Session, **kwargs) -> ORMParkArmsAction:
+        action = super().insert(session)
+        session.add(action)
+        session.commit()
+        return action
+
+
 @dataclass
 class PickUpActionPerformable(ActionAbstract):
+    """
+    Let the robot pick up an object.
+    """
 
     object_designator: ObjectDesignatorDescription.Object
     """
@@ -272,6 +302,9 @@ def insert(self, session: Session, **kwargs) -> ORMPickUpAction:
 
 @dataclass
 class PlaceActionPerformable(ActionAbstract):
+    """
+    Places an Object at a position using an arm.
+    """
 
     object_designator: ObjectDesignatorDescription.Object
     """
@@ -326,6 +359,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMPlaceAction:
 
 @dataclass
 class NavigateActionPerformable(ActionAbstract):
+    """
+    Navigates the Robot to a position.
+    """
 
     target_location: Pose
     """
@@ -353,6 +389,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMNavigateAction:
 
 @dataclass
 class TransportActionPerformable(ActionAbstract):
+    """
+    Transports an object to a position using an arm
+    """
 
     object_designator: ObjectDesignatorDescription.Object
     """
@@ -418,6 +457,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMTransportAction:
 
 @dataclass
 class LookAtActionPerformable(ActionAbstract):
+    """
+    Lets the robot look at a position.
+    """
 
     target: Pose
     """
@@ -444,6 +486,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMLookAtAction:
 
 @dataclass
 class DetectActionPerformable(ActionAbstract):
+    """
+    Detects an object that fits the object description and returns an object designator describing the object.
+    """
 
     object_designator: ObjectDesignatorDescription.Object
     """
@@ -471,6 +516,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMDetectAction:
 
 @dataclass
 class OpenActionPerformable(ActionAbstract):
+    """
+    Opens a container like object
+    """
 
     object_designator: ObjectPart.Object
     """
@@ -505,6 +553,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMOpenAction:
 
 @dataclass
 class CloseActionPerformable(ActionAbstract):
+    """
+    Closes a container like object.
+    """
 
     object_designator: ObjectPart.Object
     """
@@ -539,6 +590,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMCloseAction:
 
 @dataclass
 class GraspingActionPerformable(ActionAbstract):
+    """
+    Grasps an object described by the given Object Designator description
+    """
 
     arm: str
     """

src/pycram/designators/motion_designator.py

@@ -1,5 +1,3 @@
-import abc
-import typing
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
 
@@ -17,7 +15,7 @@
                                      OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion,
                                      Motion as ORMMotionDesignator)
 
-from typing import List, Dict, Callable, Optional, get_type_hints, Union, Any
+from typing_extensions import Dict, Optional, get_type_hints, get_args, get_origin
 from ..pose import Pose
 from ..task import with_tree
 
@@ -28,17 +26,15 @@ class BaseMotion(ABC):
     @abstractmethod
     def perform(self):
         """
-        Passes this designator to the process module for execution.
-
-        :return: The return value of the process module if there is any.
+        Passes this designator to the process module for execution. Will be overwritten by each motion.
         """
         pass
         # return ProcessModule.perform(self)
 
     @abstractmethod
     def to_sql(self) -> ORMMotionDesignator:
         """
-        Create an ORM object that corresponds to this description.
+        Create an ORM object that corresponds to this description. Will be overwritten by each motion.
 
         :return: The created ORM object.
         """
@@ -51,6 +47,8 @@ def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator:
         Auto-Incrementing primary keys and foreign keys have to be filled by this method.
 
         :param session: Session with a database that is used to add and commit the objects
+        :param args: Possible extra arguments
+        :param kwargs: Possible extra keyword arguments
         :return: The completely instanced ORM motion.
         """
         metadata = ProcessMetaData().insert(session)
@@ -61,6 +59,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator:
         return motion
 
     def __post_init__(self):
+        """
+        Checks if types are missing or wrong
+        """
         right_types = get_type_hints(self)
         attributes = self.__dict__.copy()
 
@@ -72,13 +73,13 @@ def __post_init__(self):
             attribute = attributes[k]
             attribute_type = type(attributes[k])
             right_type = right_types[k]
-            types = typing.get_args(right_type)
+            types = get_args(right_type)
             if attribute is None:
-                if not any([x is type(None) for x in typing.get_args(right_type)]):
+                if not any([x is type(None) for x in get_args(right_type)]):
                     missing.append(k)
             elif attribute_type is not right_type:
                 if attribute_type not in types:
-                    if attribute_type not in [typing.get_origin(x) for x in types if x is not type(None)]:
+                    if attribute_type not in [get_origin(x) for x in types if x is not type(None)]:
                         wrong_type[k] = right_types[k]
                         current_type[k] = attribute_type
         if missing != [] or wrong_type != {}:

src/pycram/resolver/location/database_location.py

@@ -1,6 +1,7 @@
 import numpy as np
 import sqlalchemy.orm
 import sqlalchemy.sql
+from sqlalchemy import select, Select
 from tf import transformations
 import pycram.designators.location_designator
 import pycram.task
@@ -34,25 +35,24 @@ def __init__(self, target, session: sqlalchemy.orm.Session = None,
         super().__init__(target, reachable_for, None, reachable_arm, resolver)
         self.session = session
 
-    def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query:
+    def create_query_from_occupancy_costmap(self) -> Select:
         """
         Create a query that queries all relative robot positions from an object that are not occluded using an
         OccupancyCostmap.
         """
 
         robot_pos = sqlalchemy.orm.aliased(Position)
-        object_pos = sqlalchemy.orm.aliased(Position)
 
         # query all relative robot positions in regard to an objects position
         # make sure to order the joins() correctly
-        query = (self.session.query(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x,
-                                    Position.y).join(TaskTreeNode.code)
-                                               .join(Code.designator.of_type(PickUpAction))
-                                               .join(PickUpAction.robot_state)
-                                               .join(RobotState.pose)
-                                               .join(orm.base.Pose.position)
-                                               .join(PickUpAction.object).filter(Object.type == self.target.type)
-                                                                         .filter(TaskTreeNode.status == "SUCCEEDED"))
+        query = (select(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x, Position.y)
+                 .join(TaskTreeNode.code)
+                 .join(Code.designator.of_type(PickUpAction))
+                 .join(PickUpAction.robot_state)
+                 .join(RobotState.pose)
+                 .join(orm.base.Pose.position)
+                 .join(PickUpAction.object).where(Object.type == self.target.type)
+                                           .where(TaskTreeNode.status == "SUCCEEDED"))
 
         # create Occupancy costmap for the target object
         position, orientation = self.target.pose.to_list()
@@ -94,7 +94,7 @@ def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query:
                                                    robot_pos.y >= rectangle[1][0], robot_pos.y < rectangle[1][1]))
                     # query = self.model.bind({"x": list(rectangle[0]), "y": list(rectangle[1])})
 
-        return query.filter(sqlalchemy.or_(*filters))
+        return query.where(sqlalchemy.or_(*filters))
 
     def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLocation.Location:
         """
@@ -112,7 +112,7 @@ def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLoc
         return result
 
     def __iter__(self) -> JPTCostmapLocation.Location:
-        query = self.create_query_from_occupancy_costmap().limit(200)
-        samples = query.all()
+        statement = self.create_query_from_occupancy_costmap().limit(200)
+        samples = self.session.execute(statement).all()
         for sample in samples:
             yield self.sample_to_location(sample)