Feature rapid neem creation

demos/playingfield/ceo.py

@@ -0,0 +1,99 @@
+import rospy
+
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.datastructures.enums import ObjectType, WorldMode
+from pycram.world_concepts.world_object import Object
+from pycram.datastructures.pose import Pose
+
+from pycram.datastructures.dataclasses import Color
+from demos.playingfield.sandcastle import generic_plan
+from pycram import World
+from pycram.failures import PlanFailure
+from pycram.tasktree import task_tree
+
+import traceback
+import sqlalchemy
+import pycram.orm.base
+from pycram.external_interfaces.procthor import ProcThorInterface
+
+# Many Plans in many environments with many robots
+# Don't know where to store NEEM so I start with the memory db
+engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False)
+session_maker = sqlalchemy.orm.sessionmaker(bind=engine)
+session = session_maker()
+# ToDo: Many Plans missing, but this needs function pointers and I am not comfortable with it
+# plans=[]
+robots = []
+robot_name = "pr2.urdf"
+robots.append(robot_name)
+pycram.orm.base.Base.metadata.create_all(engine)
+session.commit()
+# Set up for ProcThor stuff Example usage
+procThorInterface = ProcThorInterface(base_url="http://procthor.informatik.uni-bremen.de:5000/")
+number_of_test_environment = 5
+
+# Get Environments
+number_of_known_environment = len(
+    procThorInterface.get_all_environments_stored_below_folder(procThorInterface.source_folder))
+print("Number of known Environments:{}".format(number_of_known_environment))
+print("Number of needed Testenvironments:{}".format(number_of_test_environment))
+
+if number_of_known_environment < number_of_test_environment:
+    print("Downloading missing environments...")
+    procThorInterface.download_num_random_environment(number_of_test_environment - number_of_known_environment)
+
+def runWorld():
+    counter = 0
+    works = 0
+    fails = 0
+    known_environments = procThorInterface.get_all_environments_stored_below_folder(procThorInterface.source_folder)
+    world = BulletWorld(WorldMode.GUI)
+    apartment = None
+    milk_pos = Pose([1, -1.78, 0.55], [1, 0, 0, 0])
+    milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]),
+                  color=Color(1, 0, 0, 1))
+    for robot in robots:
+        robot_obj = Object("pr2", ObjectType.ROBOT, robot, pose=Pose([1, 2, 0]))
+        for environment in known_environments:
+            counter += 1
+            print("Trying plan: {} with robot: {} in: {}".format("param_plan", robot, environment["name"]))
+            try:
+                apartment = Object(environment["name"], ObjectType.ENVIRONMENT, environment["storage_place"])
+                generic_plan(world)
+                works += 1
+                print("Successfully!\n Overall successful tries: {}".format(works))
+
+            except PlanFailure as e:
+                traceback.print_exc()
+                fails += 1
+                print("Plan Fail!\n Overall failed tries: {}".format(fails))
+
+            except FileNotFoundError as e2:
+                traceback.print_exc()
+                fails += 1
+                print("Fail!\n Overall failed tries: {}".format(fails))
+
+            finally:
+                try:
+                    process_meta_data = pycram.orm.base.ProcessMetaData()
+                    process_meta_data.description = "CEO Test run number {} robot:{} enviroment:{}".format(counter,
+                                                                                                           robot,
+                                                                                                           environment)
+                    process_meta_data.insert(session)
+                    task_tree.root.insert(session)
+                except Exception as e:
+                    traceback.print_exc()
+                    print("Error while storing the NEEM. This should not happen.")
+                task_tree.reset_tree()
+                if World.current_world is not None and apartment is not None:
+                    world.remove_object(apartment)
+                    milk.set_pose(milk_pos)
+                    print("reseting world")
+    if World.current_world is None:
+        world.remove_object(robot_obj)
+
+    print("Resume:\nOverall successful tries: {}\nOverall failed tries: {}".format(works, fails))
+    World.current_world.exit()
+
+
+runWorld()

demos/playingfield/sandcastle.py

@@ -0,0 +1,154 @@
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.designators.action_designator import *
+from pycram.designators.location_designator import *
+from pycram.designators.object_designator import *
+from pycram.datastructures.enums import ObjectType, WorldMode
+from pycram.datastructures.pose import Pose
+from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.object_descriptors.urdf import ObjectDescription
+from pycram.world_concepts.world_object import Object
+from pycram.datastructures.dataclasses import Color
+from pycram.worlds.multiverse import Multiverse
+
+
+def param_plan(robot_path,environment_path):
+    world = BulletWorld(WorldMode.GUI)
+    # world= Multiverse(simulation='pycram_test')
+    apartment = Object(environment_path[:environment_path.find(".")], ObjectType.ENVIRONMENT, environment_path)
+    robot = Object("pr2", ObjectType.ROBOT, robot_path, pose=Pose([1, 2, 0]))
+
+
+    milk_pos=Pose([1, -1.78, 0.55],[1,0,0,0])
+    milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55],[1,0,0,0]),
+                  color=Color(1, 0, 0, 1))
+
+    pick_pose = Pose([1, -1.78, 0.55])
+
+    robot_desig = BelieveObject(names=["pr2"])
+    apartment_desig = BelieveObject(names=["apartment"])
+
+
+    @with_simulated_robot
+    def move_and_detect(obj_type):
+        NavigateAction(target_locations=[Pose([2, -1.89, 0])]).resolve().perform()
+
+        LookAtAction(targets=[pick_pose]).resolve().perform()
+
+        object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+        return object_desig
+
+
+    with simulated_robot:
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+        MoveTorsoAction([0.25]).resolve().perform()
+
+        milk_desig = move_and_detect(ObjectType.MILK)
+
+        TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
+
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+# param_plan("pr2.urdf","tmp/house_666/house_666.urdf")
+
+
+def param_plan2(robot_path, environment_path):
+    cheatcode={"/home/nleusmann/catkin_ws/src/pycram/resources/apartment.urdf":[Pose([2.62,2.22,1.1]),Pose([4.8, 3.55, 0.8])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_1/house_1.urdf": [Pose([9.16,-1.07,1.20]), Pose([11.82,-6.2,0.8])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_2/house_2.urdf":[Pose([1, -1.78, 0.55]),Pose([2.71,-0.22,0.75])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_3/house_3.urdf":[Pose([6.02, -5.41, 1]),Pose([10.49,-5.83,0.80])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_4/house_4.urdf":[Pose([5.33, -8.81, 1]),Pose([3.78,-8.71,0.75])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_5/house_5.urdf": [Pose([0.5, -6.61, 1]), Pose([4.46, -4.42, 0.85])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_6/house_6.urdf": [Pose([9.73, -9, 0.80]), Pose([7.17, -5.57, 0.85])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_7/house_7.urdf": [Pose([8.77, -2.88, 1]), Pose([9.34, -4.44, 0.80])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_8/house_8.urdf": [Pose([12.28, -7.76, 1]), Pose([10.02, -12.34, 1])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_9/house_9.urdf": [Pose([6.54, -8.51, 1]), Pose([3.5, -7.8, 0.85])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_10/house_10.urdf": [Pose([8.74, -1.3, 1]), Pose([5.74, -2.91, 1])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_11/house_11.urdf": [Pose([7.03, -2.19, 1]), Pose([3.66, -5.84, 0.75],[1,0,0,0]),Pose([6.7, -3.5, 0])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_42/house_42.urdf":[Pose([7.43,-9.37,1.05]),Pose([11.12,-8.35,0.9])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_666/house_666.urdf":[Pose([7.06,-6.82,1]),Pose([1.25,-3.92,0.75],[1,0,0,0]),Pose([7.17,-7.77,0])],
+               "/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_1337/house_1337.urdf":[Pose([2.45,-0.36,0.9]),Pose([0.61,-5.82,0.8],[1,0,0,0]),Pose([2.34,-1.47,0])]}
+    world = BulletWorld(WorldMode.GUI)
+    # world= Multiverse(simulation='pycram_test')
+    print(environment_path+35*'#')
+    apartment = Object(environment_path[:environment_path.find(".")], ObjectType.ENVIRONMENT, environment_path)
+    robot = Object("pr2", ObjectType.ROBOT, robot_path, pose=Pose([1, 2, 0]))
+    milk_pos = cheatcode[environment_path][0]
+    put_down_pos=cheatcode[environment_path][1]
+    move_pos=milk_pos.copy()
+    move_pos.pose.position.x=move_pos.pose.position.x-0.7
+    move_pos.pose.position.z=0
+    milk = Object("milk", ObjectType.MILK, "milk.stl", pose=milk_pos,color=Color(1, 0, 0, 1))
+
+    pick_pose = Pose([1, -1.78, 0.55])
+
+    robot_desig = BelieveObject(names=["pr2"])
+    apartment_desig = BelieveObject(names=["apartment"])
+
+    @with_simulated_robot
+    def move_and_detect(obj_type):
+        NavigateAction(target_locations=[cheatcode[environment_path][2]]).resolve().perform()
+
+        LookAtAction(targets=[milk_pos]).resolve().perform()
+
+        object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+        return object_desig
+
+    with simulated_robot:
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+        MoveTorsoAction([0.25]).resolve().perform()
+
+        milk_desig = move_and_detect(ObjectType.MILK)
+
+        TransportAction(milk_desig, [Arms.LEFT], [put_down_pos]).resolve().perform()
+
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+# param_plan2("pr2.urdf","/home/nleusmann/catkin_ws/src/pycram/resources/tmp/house_6/house_6.urdf")
+
+def loader():
+    for i in range(15):
+        print(i)
+        world = BulletWorld(WorldMode.GUI)
+        # world= Multiverse(simulation='pycram_test')
+        apartment = Object("house6", ObjectType.ENVIRONMENT,"/home/nleusmann/catkin_ws/src/pycram/resources/apartment.urdf")
+        world.remove_object_from_simulator(apartment)
+
+# loader()
+
+
+def generic_plan(inWorld):
+    world = inWorld
+
+    # milk_pos = Pose([1, -1.78, 0.55], [1, 0, 0, 0])
+    # milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]),
+    #               color=Color(1, 0, 0, 1))
+
+    pick_pose = Pose([1, -1.78, 0.55])
+
+    robot_desig = BelieveObject(names=["pr2"])
+    apartment_desig = BelieveObject(names=["apartment"])
+
+    @with_simulated_robot
+    def move_and_detect(obj_type):
+        NavigateAction(target_locations=[Pose([2, -1.89, 0])]).resolve().perform()
+
+        LookAtAction(targets=[pick_pose]).resolve().perform()
+
+        object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
+
+        return object_desig
+
+    with simulated_robot:
+        ParkArmsAction([Arms.BOTH]).resolve().perform()
+
+        MoveTorsoAction([0.25]).resolve().perform()
+
+        milk_desig = move_and_detect(ObjectType.MILK)
+
+        TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
+
+        ParkArmsAction([Arms.BOTH]).resolve().perform()

examples/interface_examples/procthor.md

@@ -0,0 +1,120 @@
+# Procthor Interface - Creating a CEO for testing multible robots in mutliple enviromnents
+
+This Notebook aims to provide an overview of the ProcThor Interface, by giving an easy-to-understand example on how to 
+use it. We will be executing a simple pick and place plan with hard coded values and store the NEEMs of these 
+experiments within a local memory database. 
+
+Please be aware that this Notebook (currently) works only when you are connected to the network at the university in 
+Bremen, otherwise we will not be able to connect to the REST service and the webserver itself.
+
+If you want to check if you are able to run the Notebook simply try to ping the following server.
+
+```batch
+
+ping procthor.informatik.uni-bremen.de
+
+```
+
+If you get a positive answer back from the server everything seems to be all right. 
+
+Now we need to set up the local memory database.
+```python
+import sqlalchemy
+import pycram.orm.base
+
+engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False)
+session_maker = sqlalchemy.orm.sessionmaker(bind=engine)
+session = session_maker()
+pycram.orm.base.Base.metadata.create_all(engine)
+session.commit()
+```
+Time for some more setup, while we can technically use multiple robots  in the different environments we for now stick 
+with only PR2. Afterwards, we will start to set up our ProcThorInterface. Our example will work with 5 different 
+environments.
+```python
+from pycram.external_interfaces.procthor import ProcThorInterface
+from pycram.ros import ros_tools
+pycram_path=ros_tools.get_ros_package_path('pycram')
+
+robots =[]
+robot_name="pr2.urdf"
+robots.append(robot_name)
+
+
+procthor_rest_endpoint = "http://procthor.informatik.uni-bremen.de:5000/"
+source_folder = pycram_path + "resources/tmp/"
+procThorInterface=ProcThorInterface(base_url=procthor_rest_endpoint,source_folder=source_folder)
+number_of_test_environment=5
+```
+First let us check how many different environments are already known.
+
+```python
+number_of_known_environment=len(procThorInterface.get_all_environments_stored_below_folder(procThorInterface.source_folder))
+print("Number of known Environments:{}".format(number_of_known_environment))
+print("Number of needed Testenvironments:{}".format(number_of_test_environment))
+```
+
+Now depending on if we have to get some more, we will download some additional:
+```python
+if number_of_known_environment < number_of_test_environment:
+    print("Downloading missing environments...")
+    procThorInterface.download_num_random_environment(number_of_test_environment-number_of_known_environment)
+```
+
+Now that the preparations are met we can start to run our plan for each robot (even though it is only one) and each
+environment we know:
+```python
+v = 0
+works = 0
+fails = 0
+known_environments = procThorInterface.get_all_environments_stored_below_folder(procThorInterface.source_folder)
+world=BulletWorld(WorldMode.GUI)
+apartment=None
+milk_pos = Pose([1, -1.78, 0.55], [1, 0, 0, 0])
+milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]),
+              color=Color(1, 0, 0, 1))
+for robot in robots:
+    robot_obj=Object("pr2", ObjectType.ROBOT, robot, pose=Pose([1, 2, 0]))
+    for environment in known_environments:
+        v += 1
+        print("Trying plan: {} with robot: {} in: {}".format("param_plan", robot, environment["name"]))
+        try:
+            apartment = Object(environment["name"], ObjectType.ENVIRONMENT,environment["storage_place"])
+            generic_plan(world)
+            works += 1
+            print("Successfully!\n Overall successful tries: {}".format(works))
+
+        except PlanFailure as e:
+            traceback.print_exc()
+            fails += 1
+            print("Plan Fail!\n Overall failed tries: {}".format(fails))
+
+        except FileNotFoundError as e2:
+            traceback.print_exc()
+            fails += 1
+            print("Fail!\n Overall failed tries: {}".format(fails))
+
+        finally:
+            try:
+                process_meta_data = pycram.orm.base.ProcessMetaData()
+                process_meta_data.description = "CEO Test run number {} robot:{} enviroment:{}".format(v, robot,
+                                                                                                       environment)
+                process_meta_data.insert(session)
+                task_tree.root.insert(session)
+            except Exception as e:
+                traceback.print_exc()
+                print("Error while storing the NEEM. This should not happen.")
+            task_tree.reset_tree()
+            if World.current_world is not None and apartment is not None:
+                world.remove_object(apartment)
+                milk.set_pose(milk_pos)
+                print("reseting world")
+if World.current_world is None:
+    world.remove_object(robot_obj)
+
+
+print("Resume:\nOverall successful tries: {}\nOverall failed tries: {}".format(works, fails))
+World.current_world.exit()
+```
+
+

src/pycram/designators/action_designator.py

@@ -924,8 +924,13 @@ class DetectActionPerformable(ActionAbstract):
     """
     orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction)
 
+    object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False)
+
     @with_tree
     def perform(self) -> None:
+        # Store the object's data copy at execution
+        self.object_at_execution = self.object_designator.frozen_copy()
+
         return DetectingMotion(object_type=self.object_designator.obj_type).perform()