Merge pull request #135 from tomsch420/costmaps

Refactored Costmaps using new probabilistic models and exact inference.

.github/workflows/new-pycram-ci.yml

@@ -60,10 +60,11 @@ jobs:
           pip3 install --upgrade pip --root-user-action=ignore
           cd /opt/ros/overlay_ws/src/pycram
           pip3 install -r requirements.txt
+          pip3 install -r requirements-resolver.txt
 
-      - name: Install pytest, pyjpt & mlflow
+      - name: Install pytest & pyjpt
         run: |
-          pip3 install --ignore-installed pytest pyjpt mlflow
+          pip3 install --ignore-installed pytest pyjpt
 
       - name: Run tests
         run: | 

doc/source/examples.rst

@@ -27,7 +27,7 @@ Misc
     notebooks/bullet_world
     notebooks/minimal_task_tree
     notebooks/pose
-    notebooks/custom_resolver
+    notebooks/improving_actions
     notebooks/language
     notebooks/local_transformer
 

doc/source/index.rst

@@ -1,4 +1,4 @@
-==================================
+    ==================================
 Welcome to pycram's documentation!
 ==================================
 

requirements-resolver.txt

@@ -0,0 +1,5 @@
+-r requirements.txt
+probabilistic_model>=4.0.8
+random_events>=2.0.9
+SQLAlchemy
+tqdm

src/pycram/costmaps.py

@@ -1,6 +1,8 @@
 # used for delayed evaluation of typing until python 3.11 becomes mainstream
 from __future__ import annotations
 
+from dataclasses import dataclass
+
 import numpy as np
 import pybullet as p
 import rospy
@@ -17,6 +19,31 @@
 from .pose import Pose
 
 
+@dataclass
+class Rectangle:
+    """
+    A rectangle that is described by a lower and upper x and y value.
+    """
+    x_lower: float
+    x_upper: float
+    y_lower: float
+    y_upper: float
+
+    def translate(self, x: float, y: float):
+        """Translate the rectangle by x and y"""
+        self.x_lower += x
+        self.x_upper += x
+        self.y_lower += y
+        self.y_upper += y
+
+    def scale(self, x_factor: float, y_factor: float):
+        """Scale the rectangle by x_factor and y_factor"""
+        self.x_lower *= x_factor
+        self.x_upper *= x_factor
+        self.y_lower *= y_factor
+        self.y_upper *= y_factor
+
+
 class Costmap:
     """
     The base class of all Costmaps which implements the visualization of costmaps
@@ -213,6 +240,44 @@ def __add__(self, other: Costmap) -> Costmap:
         else:
             raise ValueError(f"Can only combine two costmaps other type was {type(other)}")
 
+    def partitioning_rectangles(self) -> List[Rectangle]:
+        """
+        Partition the map attached to this costmap into rectangles. The rectangles are axis aligned, exhaustive and
+        disjoint sets.
+
+        :return: A list containing the partitioning rectangles
+        """
+        ocm_map = np.copy(self.map)
+        origin = np.array([self.height / 2, self.width / 2]) * -1
+        rectangles = []
+
+        # for every index pair (i, j) in the occupancy costmap
+        for i in range(0, self.map.shape[0]):
+            for j in range(0, self.map.shape[1]):
+
+                # if this index has not been used yet
+                if ocm_map[i][j] > 0:
+                    curr_width = self._find_consectuive_line((i, j), ocm_map)
+                    curr_pose = (i, j)
+                    curr_height = self._find_max_box_height((i, j), curr_width, ocm_map)
+
+                    # calculate the rectangle in the costmap
+                    x_lower = curr_pose[0]
+                    x_upper = curr_pose[0] + curr_height
+                    y_lower = curr_pose[1]
+                    y_upper = curr_pose[1] + curr_width
+
+                    # mark the found rectangle as occupied
+                    ocm_map[i:i + curr_height, j:j + curr_width] = 0
+
+                    # transform rectangle to map space
+                    rectangle = Rectangle(x_lower, x_upper, y_lower, y_upper)
+                    rectangle.translate(*origin)
+                    rectangle.scale(self.resolution, self.resolution)
+                    rectangles.append(rectangle)
+
+        return rectangles
+
 
 class OccupancyCostmap(Costmap):
     """

src/pycram/designators/actions/actions.py

@@ -1,9 +1,12 @@
 import abc
 import inspect
+
+import numpy as np
+from tf import transformations
 from typing_extensions import Union, Type
 from pycram.designator import ActionDesignatorDescription
 from pycram.designators.motion_designator import *
-from pycram.enums import Arms
+from pycram.enums import Arms, Grasp
 from pycram.task import with_tree
 from dataclasses import dataclass, field
 from ..location_designator import CostmapLocation
@@ -21,7 +24,8 @@
                                       MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction,
                                       LookAtAction as ORMLookAtAction, DetectAction as ORMDetectAction,
                                       TransportAction as ORMTransportAction, OpenAction as ORMOpenAction,
-                                      CloseAction as ORMCloseAction, GraspingAction as ORMGraspingAction, Action)
+                                      CloseAction as ORMCloseAction, GraspingAction as ORMGraspingAction, Action,
+                                      FaceAtAction as ORMFaceAtAction)
 
 
 @dataclass
@@ -523,3 +527,68 @@ def perform(self) -> None:
 
         MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform()
         MoveGripperMotion("close", self.arm, allow_gripper_collision=True).perform()
+
+
+@dataclass
+class FaceAtPerformable(ActionAbstract):
+    """
+    Turn the robot chassis such that is faces the ``pose`` and after that perform a look at action.
+    """
+
+    pose: Pose
+    """
+    The pose to face 
+    """
+
+    orm_class = ORMFaceAtAction
+
+    @with_tree
+    def perform(self) -> None:
+        # get the robot position
+        robot_position = BulletWorld.robot.pose
+
+        # calculate orientation for robot to face the object
+        angle = np.arctan2(robot_position.position.y - self.pose.position.y,
+                           robot_position.position.x - self.pose.position.x) + np.pi
+        orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes="sxyz"))
+
+        # create new robot pose
+        new_robot_pose = Pose(robot_position.position_as_list(), orientation)
+
+        # turn robot
+        NavigateActionPerformable(new_robot_pose).perform()
+
+        # look at target
+        LookAtActionPerformable(self.pose).perform()
+
+
+@dataclass
+class MoveAndPickUpPerformable(ActionAbstract):
+    """
+    Navigate to `standing_position`, then turn towards the object and pick it up.
+    """
+
+    standing_position: Pose
+    """
+    The pose to stand before trying to pick up the object
+    """
+
+    object_designator: ObjectDesignatorDescription.Object
+    """
+    The object to pick up
+    """
+
+    arm: Arms
+    """
+    The arm to use
+    """
+
+    grasp: Grasp
+    """
+    The grasp to use
+    """
+
+    def perform(self):
+        NavigateActionPerformable(self.standing_position).perform()
+        FaceAtPerformable(self.object_designator.pose).perform()
+        PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform()

src/pycram/orm/action_designator.py

@@ -118,3 +118,8 @@ class GraspingAction(ObjectMixin, Action):
     id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)
     arm: Mapped[str]
 
+
+class FaceAtAction(PoseMixin, Action):
+    """ORM Class of pycram.designators.action_designator.FaceAtAction."""
+
+    id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)

src/pycram/orm/queries/queries.py

@@ -0,0 +1,51 @@
+import sqlalchemy.orm
+from sqlalchemy import Select, alias
+from ..task import TaskTreeNode
+from ..action_designator import PickUpAction
+from ..base import Position, RobotState, Pose
+from ..object_designator import Object
+
+
+class Query:
+    """
+    Abstract class for queries
+    """
+
+
+class PickUpWithContext(Query):
+    """
+    Query for pickup actions with context.
+    """
+
+    robot_position = sqlalchemy.orm.aliased(Position)
+    """
+    3D Vector of robot position
+    """
+
+    robot_pose = sqlalchemy.orm.aliased(Pose)
+    """
+    Complete robot pose
+    """
+
+    object_position = sqlalchemy.orm.aliased(Position)
+    """
+    3D Vector for object position
+    """
+
+    relative_x = (robot_position.x - object_position.x).label("relative_x")
+    """
+    Distance on x axis between robot and object
+    """
+
+    relative_y = (robot_position.y - object_position.y).label("relative_y")
+    """
+    Distance on y axis between robot and object
+    """
+
+    def join_statement(self, query: Select):
+        return (query.join(TaskTreeNode).join(TaskTreeNode.action.of_type(PickUpAction))
+                .join(PickUpAction.robot_state).join(self.robot_pose, RobotState.pose)
+                .join(self.robot_position, self.robot_pose.position)
+                .join(Pose.orientation)
+                .join(PickUpAction.object)
+                .join(Object.pose).join(self.object_position, Pose.position))

src/pycram/orm/task.py

@@ -13,22 +13,17 @@ class TaskTreeNode(Base):
     """ORM equivalent of pycram.task.TaskTreeNode."""
 
     id: Mapped[int] = mapped_column(autoincrement=True, primary_key=True, init=False)
-    """id overriden in order to be able to set the remote_side of the parent attribute"""
 
-    code_id: Mapped[int] = mapped_column(ForeignKey("Code.id"), default=None)
-    code: Mapped["Code"] = relationship(init=False)
+    action_id: Mapped[Optional[int]] = mapped_column(ForeignKey(f'{Designator.__tablename__}.id'), default=None)
+    action: Mapped[Optional[Designator]] = relationship(init=False)
+
     start_time: Mapped[datetime.datetime] = mapped_column(default=None)
     end_time: Mapped[Optional[datetime.datetime]] = mapped_column(default=None)
+
     status: Mapped[TaskStatus] = mapped_column(default=None)
     reason: Mapped[Optional[str]] = mapped_column(default=None)
+
     parent_id: Mapped[Optional[int]] = mapped_column(ForeignKey("TaskTreeNode.id"), default=None)
     parent: Mapped["TaskTreeNode"] = relationship(foreign_keys=[parent_id], init=False, remote_side=[id])
 
 
-class Code(Base):
-    """ORM equivalent of pycram.task.Code."""
-
-    function: Mapped[str] = mapped_column(default=None)
-    designator_id: Mapped[Optional[int]] = mapped_column(ForeignKey(f'{Designator.__tablename__}.id'), default=None)
-    designator: Mapped[Designator] = relationship(init=False)
-

src/pycram/resolver/__init__.py

@@ -1 +0,0 @@
-from .location.jpt_location import JPTCostmapLocation