Suppressed warnings on import.

examples/cram_plans.py

@@ -1,3 +1,5 @@
+import anytree
+
 import pycram.task
 from pycram.resolver.plans import Arms
 from pycram.designators.action_designator import *
@@ -10,12 +12,16 @@
 from pycram.pose_generator_and_validator import pose_generator
 from pycram.external_interfaces.ik import IKError
 from time import sleep
+import tqdm
+import pycram.orm.base
+import pycram.orm.object_designator
+import pycram.orm.motion_designator
+import pycram.orm.action_designator
 
 
 def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
-    positions = [pos[0] for pos in pose_list[:1000]]
+    positions = [pos[0] for pos in pose_list]
     all_indices = list(range(len(positions)))
-    print(len(all_indices))
     pos_idx = np.random.choice(all_indices) if random else all_indices[0]
     all_indices.remove(pos_idx)
     n_positions = np.zeros((n, 3))
@@ -51,7 +57,6 @@ def plan(world, robot_desig, env_desig, obj_desig, torso=0.2, place="countertop"
         MoveTorsoAction.Action(torso).perform()
         location = CostmapLocation(target=obj_desig, reachable_for=robot_desig)
         pose = location.resolve()
-        print()
         NavigateAction.Action(pose.pose).perform()
         ParkArmsAction.Action(Arms.BOTH).perform()
         good_torso.append(torso)
@@ -67,7 +72,6 @@ def plan(world, robot_desig, env_desig, obj_desig, torso=0.2, place="countertop"
         pose = place_location.resolve()
 
         NavigateAction.Action(pose.pose).perform()
-
         PlaceAction.Action(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()
 
         ParkArmsAction.Action(Arms.BOTH).perform()
@@ -78,31 +82,32 @@ def plan(world, robot_desig, env_desig, obj_desig, torso=0.2, place="countertop"
 class CRAMPlan:
     def __init__(self):
         # Define world, robot, and environment
-        self.world = BulletWorld()
+        self.world = BulletWorld("DIRECT")
         self.robot = Object(robot_description.i.name, "robot", robot_description.i.name + ".urdf")
         self.robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()
         self.apartment = Object("apartment", "environment",
-                                "/home/abassi/cram_ws/src/iai_maps/iai_apartment/urdf/apartment.urdf",
+                                "/home/tom_sch/catkin_ws/src/iai_maps/iai_apartment/urdf/apartment.urdf",
                                 position=[-1.5, -2.5, 0])
         self.apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()
         self.table_top = self.apartment.get_link_position("cooktop")
         self.object_names = ["bowl", "milk", "breakfast_cereal", "spoon"]
         sleep(5)
         self.objects = {}
         self.object_desig = {}
-        self.place_objects(inistantiate=True)
+        self.place_objects(instantiate=True)
         self.good_torsos = []
+        self.torso = 0
 
-    def place_objects(self, inistantiate=False):
+    def place_objects(self, instantiate=False):
         # Define and place the objects
         scm = SemanticCostmap(self.apartment, "island_countertop")
+        scm.map[5:40, 16:140] = 0
         poses_list = list(pose_generator(scm, number_of_samples=-1))
+
         poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x[0]))
         object_poses = get_n_random_positions(poses_list)
         for obj_name, obj_pose in zip(self.object_names, object_poses):
-            if inistantiate:
-                print(obj_name)
-                print(obj_pose)
+            if instantiate:
                 self.objects[obj_name] = Object(obj_name, obj_name, obj_name + ".stl",
                                                 position=[obj_pose[0][0], obj_pose[0][1], self.table_top[2]])
                 self.object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_name]).resolve()
@@ -113,34 +118,43 @@ def place_objects(self, inistantiate=False):
             self.objects[obj_name].move_base_to_origin_pos()
             self.objects[obj_name].original_pose = self.objects[obj_name].get_position_and_orientation()
 
-        print(object_poses)
-
+    @with_tree
     def execute_plan(self):
         # Execute plan
         if len(self.object_names) > 0:
-            self.place_objects(inistantiate=False)
+            self.place_objects(instantiate=False)
         else:
-            self.place_objects(inistantiate=True)
+            self.place_objects(instantiate=True)
         for obj_name in self.object_names:
             done = False
-            torso = 0.25 if len(self.good_torsos) == 0 else self.good_torsos[-1]
+            self.torso = 0.25 if len(self.good_torsos) == 0 else self.good_torsos[-1]
             while not done:
                 try:
-                    gt = plan(self.world, self.robot_desig, self.apartment_desig, self.object_desig[obj_name], torso=torso,
+                    gt = plan(self.world, self.robot_desig, self.apartment_desig, self.object_desig[obj_name], torso=self.torso,
                          place="island_countertop")
                     self.good_torsos.extend(gt)
                     done = True
                     self.objects[obj_name].original_pose = self.objects[obj_name].get_position_and_orientation()
                 except (StopIteration, IKError) as e:
-                    print(type(e))
-                    print(e)
-                    print("no solution")
-                    torso += 0.05
-                    if torso > 0.3:
-                        break
-        print(self.good_torsos)
+                    raise PlanFailure()
 
 
 if __name__ == '__main__':
+    exit()
+    engine = sqlalchemy.create_engine("mysql+pymysql://pycrorm@localhost/pycrorm?charset=utf8mb4")
+    session = sqlalchemy.orm.session.Session(bind=engine)
+    pycram.orm.base.Base.metadata.create_all(engine)
     cram_plan = CRAMPlan()
-    cram_plan.execute_plan()
+    for i in tqdm.tqdm(list(range(100000))):
+        try:
+            cram_plan.execute_plan()
+        except PlanFailure:
+            cram_plan.torso += 0.05
+            if cram_plan.torso > 0.3:
+                cram_plan.torso = 0.25
+        finally:
+            pycram.task.task_tree.insert(session)
+            # print(anytree.RenderTree(pycram.task.task_tree))
+            pycram.task.reset_tree()
+
+

src/pycram/__init__.py

@@ -17,9 +17,14 @@
 language -- implementation of the CRAM language.
 process_module -- implementation of process modules.
 """
+
+from . import utils
+
 import logging
 import logging.config
-import pycram.process_modules.available_process_modules
+
+with utils.suppress_stdout_stderr():
+    import pycram.process_modules.available_process_modules
 
 logging.basicConfig(level=logging.WARNING, format='%(levelname)s - %(name)s  - Line:%(lineno)d - %(message)s')
 

src/pycram/bullet_world.py

@@ -8,23 +8,24 @@
 # used for delayed evaluation of typing until python 3.11 becomes mainstream
 from __future__ import annotations
 
-import numpy as np
-import pybullet as p
+import logging
 import os
-import threading
-import time
 import pathlib
-import logging
-import rospkg
 import re
+import threading
+import time
+import xml.etree.ElementTree
 from queue import Queue
+from typing import List, Optional, Dict, Tuple, Callable
+from typing import Union
 
+import numpy as np
+import pybullet as p
+import rospkg
 import rospy
-from typing import List, Optional, Union, Dict
+
 from .event import Event
 from .robot_descriptions.robot_description_handler import InitializedRobotDescription as robot_description
-from typing import List, Optional, Dict, Tuple, Callable, Type
-import xml.etree.ElementTree
 
 
 class BulletWorld:
@@ -299,7 +300,7 @@ def __enter__(self):
         if not BulletWorld.current_bullet_world.is_shadow_world:
             time.sleep(3 / 240)
             while not BulletWorld.current_bullet_world.world_sync.add_obj_queue.empty():
-                time.sleep(5/240)
+                time.sleep(5 / 240)
 
             self.prev_world = BulletWorld.current_bullet_world
             BulletWorld.current_bullet_world.world_sync.pause_sync = True
@@ -458,6 +459,10 @@ def __init__(self, name: str, type: str, path: str,
                 BulletWorld.robot = self
         self.original_pose = [position, orientation]
 
+    def __repr__(self):
+        return self.__class__.__qualname__ + f"(" + ', '.join(
+            [f"{key}={value}" for key, value in self.__dict__.items()]) + ")"
+
     def remove(self) -> None:
         """
         This method removes this object from the BulletWorld it currently
@@ -552,6 +557,10 @@ def set_position_and_orientation(self, position, orientation, base=False) -> Non
         p.resetBasePositionAndOrientation(self.id, position, orientation, self.world.client_id)
         self._set_attached_objects([self])
 
+    @property
+    def pose(self):
+        return self.get_pose()
+
     def move_base_to_origin_pos(self):
         """
         Move the object such that its base becomes at the current origin position.

src/pycram/designators/action_designator.py

@@ -8,12 +8,14 @@
 from .object_designator import ObjectDesignatorDescription
 from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction,
                                      PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction,
-                                     MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction)
-from ..orm.base import Quaternion, Position, Base
+                                     MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction,
+                                     Action as ORMAction)
+from ..orm.base import Quaternion, Position, Base, RobotPosition
 from ..robot_descriptions.robot_description_handler import InitializedRobotDescription as robot_description
 from ..task import with_tree
 from ..enums import Arms
 from ..plan_failures import *
+from ..bullet_world import BulletWorld
 
 
 class ActionDesignatorDescription(DesignatorDescription):
@@ -27,6 +29,10 @@ class Action:
         """
         A single element that fits the description.
         """
+        robot_position: Tuple[List[float], List[float]] = dataclasses.field(init=False)
+
+        def __post_init__(self):
+            self.robot_position = BulletWorld.robot.get_position_and_orientation()
 
         @with_tree
         def perform(self) -> Any:
@@ -42,17 +48,36 @@ def to_sql(self) -> Base:
             """
             raise NotImplementedError(f"{type(self)} has no implementation of to_sql. Feel free to implement it.")
 
-        def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> Base:
+        def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMAction:
             """
             Add and commit this and all related objects to the session.
             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 action: The action to write the robot position primary key in.
             :param args: Possible extra arguments
             :param kwargs: Possible extra keyword arguments
             :return: The completely instanced ORM object
             """
-            raise NotImplementedError(f"{type(self)} has no implementation of insert. Feel free to implement it.")
+
+            # create position and orientation
+            position = Position(*self.robot_position[0])
+            orientation = Quaternion(*self.robot_position[1])
+            session.add_all([position, orientation])
+            session.commit()
+
+            # create robot position object
+            robot_position = RobotPosition()
+            robot_position.position = position.id
+            robot_position.orientation = orientation.id
+            session.add(robot_position)
+            session.commit()
+
+            # create action
+            action = self.to_sql()
+            action.robot_position = robot_position.id
+
+            return action
 
     def __init__(self, grounding_method=None):
         super(ActionDesignatorDescription, self).__init__(grounding_method)
@@ -285,8 +310,7 @@ def to_sql(self) -> ORMPickUpAction:
             return ORMPickUpAction(self.arm, self.grasp)
 
         def insert(self, session: sqlalchemy.orm.session.Session, **kwargs):
-            action = self.to_sql()
-
+            action = super().insert(session)
             # try to create the object designator
             if self.object_designator:
                 od = self.object_designator.insert(session, )
@@ -335,7 +359,7 @@ def to_sql(self) -> ORMPlaceAction:
             return ORMPlaceAction(self.arm)
 
         def insert(self, session, *args, **kwargs) -> ORMPlaceAction:
-            action = self.to_sql()
+            action = super().insert(session)
 
             if self.object_designator:
                 od = self.object_designator.insert(session, )

src/pycram/designators/motion_designator.py

@@ -108,7 +108,7 @@ class PickUpMotion(MotionDesignatorDescription):
     @dataclasses.dataclass
     class Motion(MotionDesignatorDescription.Motion):
         cmd: str
-        object_desig: Object
+        object_desig: ObjectDesignatorDescription.Object
         arm: str
         grasp: str
 

src/pycram/designators/object_designator.py

@@ -1,24 +1,22 @@
 import dataclasses
-from typing import List, Tuple, Union, Optional, Callable
+from typing import List, Union, Optional, Callable
 
 import sqlalchemy.orm
 
-from ..bullet_world import Object
-from ..designator import Designator, DesignatorError, DesignatorDescription
+from ..bullet_world import BulletWorld, Object as BulletWorldObject
+from ..designator import DesignatorDescription
 from ..orm.base import (Position as ORMPosition, Quaternion as ORMQuaternion)
 from ..orm.object_designator import (ObjectDesignator as ORMObjectDesignator,
                                      BelieveObject as ORMBelieveObject,
                                      ObjectPart as ORMObjectPart)
-from ..bullet_world import BulletWorld, Object as BulletWorldObject
 
 
 class ObjectDesignatorDescription(DesignatorDescription):
     """
     Class for object designator descriptions.
     Descriptions hold possible parameter ranges for object designators.
-    :ivar types: Types to cosnider
+    :ivar types: Types to consider
     :ivar names: Names to consider
-    :ivar poses: Poses to consider
     """
 
     @dataclasses.dataclass
@@ -28,7 +26,8 @@ class Object:
         """
         name: str
         type: str
-        pose: Tuple[List[float], List[float]]
+        # pose:
+        # pose: Tuple[List[float], List[float]]
         bullet_world_object: Optional[BulletWorldObject]
 
         def to_sql(self) -> ORMObjectDesignator:
@@ -52,6 +51,15 @@ def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectDesignator
             session.commit()
             return obj
 
+        @property
+        def pose(self):
+            return self.bullet_world_object.get_position_and_orientation()
+
+        def __repr__(self):
+            return self.__class__.__qualname__ + f"(" + ', '.join([f"{f.name}={self.__getattribute__(f.name)}"
+                                                                   for f in dataclasses.fields(self)] +
+                                                                  [f"pose={self.pose}"]) + ')'
+
     def __init__(self, names: Optional[List[str]] = None,
                  types: Optional[List[str]] = None,
                  grounding_method: Optional[Callable] = None):
@@ -78,7 +86,7 @@ def __iter__(self) -> Object:
             if self.types and obj.type not in self.types:
                 continue
 
-            yield self.Object(obj.name, obj.type, obj.get_position_and_orientation(), obj)
+            yield self.Object(obj.name, obj.type, obj)
 
 
 class BelieveObject(ObjectDesignatorDescription):

src/pycram/helper.py

@@ -18,6 +18,7 @@
 from macropy.core.quotes import ast_literal, q
 from .bullet_world import Object as BulletWorldObject
 from .robot_descriptions.robot_description_handler import InitializedRobotDescription as robot_description
+import os
 
 
 class bcolors: