Merge branch 'notebook-test-ci' of github.com:Tigul/pycram-1 into test-test-ci

Author: Tigul

.github/workflows/notebook-test-ci.yml

@@ -26,8 +26,25 @@ jobs:
 
     steps:
 
-      - name: Checkout 🛎
+      - name: Checkout PyCRAM
         uses: actions/checkout@v3
+        with:
+          path: "ros/src/pycram"
+          repository: ${{ github.repository }}
+          ref: ${{ github.ref }}
+          submodules: "recursive"
+
+      # For debugging
+      - name: Setup tmate session
+        uses: mxschmitt/action-tmate@v3
+        if: ${{ github.event_name == 'workflow_dispatch' && inputs.debug_enabled }}
+
+      - name: Update PyCRAM source files
+        run: |
+          rm -rf /opt/ros/overlay_ws/src/pycram/* 
+          cd /opt/ros/overlay_ws/src/pycram
+          rm -rf .git .github .gitignore .gitmodules .readthedocs.yaml
+          cp -r /__w/${{ github.event.repository.name }}/${{ github.event.repository.name }}/ros/src/pycram /opt/ros/overlay_ws/src
 
       # ----------------------------------------------------------------------------------------------------------------
 
@@ -36,6 +53,7 @@ jobs:
           pip3 install --upgrade pip --root-user-action=ignore
           cd /opt/ros/overlay_ws/src/pycram
           pip3 install -r requirements.txt
+          sudo apt-get install -y libpq-dev
           pip3 install -r requirements-resolver.txt
 
       # ----------------------------------------------------------------------------------------------------------------
@@ -48,9 +66,9 @@ jobs:
       - name: Convert Notebooks
         run: |
           cd /opt/ros/overlay_ws/src/pycram/examples
+          rm -rf tmp
           mkdir tmp
           jupytext --to notebook *.md
-          ls .
           mv *.ipynb tmp && cd tmp
 
       - name: Source and start ROS

examples/action_designator.md

@@ -40,6 +40,7 @@ First, we need a BulletWorld with a robot.
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.world_concepts.world_object import Object
 from pycram.datastructures.enums import ObjectType, WorldMode
+from pycram.datastructures.pose import Pose
 
 world = BulletWorld(WorldMode.GUI)
 pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0]))

examples/cram_plan_tutorial.md

@@ -1,4 +1,4 @@
----
+from pycram.robot_description import RobotDescriptionfrom pycram.datastructures.enums import ObjectType---
 jupyter:
   jupytext:
     text_representation:
@@ -16,28 +16,26 @@ jupyter:
 
 In this tutorial we will walk through the capabilities of task trees in pycram.
 
-First we have to import the necessary functionality from pycram.
+Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal.
 
 ```python
-from pycram.bullet_world import BulletWorld
-from pycram.robot_descriptions import robot_description
-import pycram.task
-from pycram.resolver.plans import Arms
+from pycram.worlds.bullet_world import BulletWorld
+from pycram.robot_description import RobotDescription
+import pycram.tasktree
+from pycram.datastructures.enums import Arms, ObjectType
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.process_module import simulated_robot
 from pycram.designators.object_designator import *
+from pycram.datastructures.pose import Pose
+from pycram.world_concepts.world_object import Object
 import anytree
 import pycram.failures
-```
 
-Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal.
-
-```python
 world = BulletWorld()
-robot = Object(robot_description.name, "robot", robot_description.name + ".urdf")
+robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
 robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()
-apartment = Object("apartment", "environment", "apartment.urdf", position=[-1.5, -2.5, 0])
+apartment = Object("apartment", "environment", "apartment.urdf", pose=Pose([-1.5, -2.5, 0]))
 apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()
 table_top = apartment.get_link_position("cooktop")
 # milk = Object("milk", "milk", "milk.stl", position=[table_top[0]-0.15, table_top[1], table_top[2]])
@@ -53,7 +51,7 @@ import numpy as np
 
 
 def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
-    positions = [pos[0] for pos in pose_list[:1000]]
+    positions = [pose.position_as_list() for pose in pose_list[:1000]]
     all_indices = list(range(len(positions)))
     print(len(all_indices))
     pos_idx = np.random.choice(all_indices) if random else all_indices[0]
@@ -98,11 +96,11 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
 
 ```python
 from pycram.costmaps import SemanticCostmap
-from pycram.pose_generator_and_validator import pose_generator
+from pycram.pose_generator_and_validator import PoseGenerator
 
 scm = SemanticCostmap(apartment, "island_countertop")
-poses_list = list(pose_generator(scm, number_of_samples=-1))
-poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x[0]))
+poses_list = list(PoseGenerator(scm, number_of_samples=-1))
+poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x.position_as_list()))
 object_poses = get_n_random_positions(poses_list)
 object_names = ["bowl", "milk", "breakfast_cereal", "spoon"]
 objects = {}
@@ -111,9 +109,9 @@ for obj_name, obj_pose in zip(object_names, object_poses):
     print(obj_name)
     print(obj_pose)
     objects[obj_name] = Object(obj_name, obj_name, obj_name + ".stl",
-                               position=[obj_pose[0][0], obj_pose[0][1], table_top[2]])
-    objects[obj_name].move_base_to_origin_pos()
-    objects[obj_name].original_pose = objects[obj_name].get_position_and_orientation()
+                               pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z]))
+    objects[obj_name].move_base_to_origin_pose()
+    objects[obj_name].original_pose = objects[obj_name].pose
     object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_name]).resolve()
 print(object_poses)
 ```
@@ -125,24 +123,25 @@ import pybullet as p
 
 for link_name in apartment.links.keys():
     world.add_vis_axis(apartment.get_link_pose(link_name))
-    p.addUserDebugText(link_name, apartment.get_link_position(link_name))
+    #p.addUserDebugText(link_name, apartment.get_link_position(link_name), physicsClientId=world.id)
 ```
 
 ```python
 world.remove_vis_axis()
-p.removeAllUserDebugItems()
+#p.removeAllUserDebugItems()
 ```
 
 Finally, we create a plan where the robot parks his arms, walks to the kitchen counter and picks the thingy. Then we
 execute the plan.
 
 ```python
 from pycram.external_interfaces.ik import IKError
+from pycram.datastructures.enums import Grasp
 
 
-@pycram.task.with_tree
+@pycram.tasktree.with_tree
 def plan(obj, obj_desig, torso=0.2, place="countertop"):
-    world.reset_bullet_world()
+    world.reset_world()
     with simulated_robot:
         ParkArmsActionPerformable(Arms.BOTH).perform()
 
@@ -154,7 +153,7 @@ def plan(obj, obj_desig, torso=0.2, place="countertop"):
         ParkArmsActionPerformable(Arms.BOTH).perform()
         good_torsos.append(torso)
         picked_up_arm = pose.reachable_arms[0]
-        PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp="front").perform()
+        PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=Grasp.FRONT).perform()
 
         ParkArmsActionPerformable(Arms.BOTH).perform()
         scm = SemanticCostmapLocation(place, apartment_desig, obj_desig)
@@ -180,7 +179,7 @@ for obj_name in object_names:
         try:
             plan(objects[obj_name], object_desig[obj_name], torso=torso, place="island_countertop")
             done = True
-            objects[obj_name].original_pose = objects[obj_name].get_position_and_orientation()
+            objects[obj_name].original_pose = objects[obj_name].pose
         except (StopIteration, IKError) as e:
             print(type(e))
             print(e)
@@ -195,7 +194,7 @@ Now we get the task tree from its module and render it. Rendering can be done wi
 anytree package. We will use ascii rendering here for ease of displaying.
 
 ```python
-tt = pycram.task.task_tree
+tt = pycram.tasktree.task_tree
 print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))
 ```
 
@@ -205,8 +204,8 @@ tree. Hence, a NoOperation node is the root of any tree. If we re-execute the pl
 tree even though they are not connected.
 
 ```python
-world.reset_bullet_world()
-plan()
+world.reset_world()
+plan(objects["milk"], object_desig["milk"])
 print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))
 ```
 
@@ -216,9 +215,9 @@ reset objects are available. At the end of a with block the old state is restore
 called ``simulation()``.
 
 ```python
-with pycram.task.SimulatedTaskTree() as stt:
-    print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))
-print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))
+with pycram.tasktree.SimulatedTaskTree() as stt:
+    print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
+print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
 ```
 
 Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we
@@ -233,30 +232,30 @@ We can now re-execute this (modified) plan by executing the leaf in pre-ordering
 functionality. This will not append the re-execution to the task tree.
 
 ```python
-world.reset_bullet_world()
+world.reset_world()
 with simulated_robot:
-    [node.code.execute() for node in tt.root.leaves]
-print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))
+    [node.action.perform() for node in tt.root.leaves]
+print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
 ```
 
 Nodes in the task tree contain additional information about the status and time of a task.
 
 ```python
-print(pycram.task.task_tree.children[0])
+print(pycram.tasktree.task_tree.children[0])
 ```
 
 The task tree can also be reset to an empty one by invoking:
 
 ```python
-pycram.task.reset_tree()
-print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))
+pycram.tasktree.task_tree.reset_tree()
+print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
 ```
 
 If a plan fails using the PlanFailure exception, the plan will not stop. Instead, the error will be logged and saved in
 the task tree as a failed subtask. First let's create a simple failing plan and execute it.
 
 ```python
-@pycram.task.with_tree
+@pycram.tasktree.with_tree
 def failing_plan():
     raise pycram.plan_failures.PlanFailure("Oopsie!")
 
@@ -267,8 +266,8 @@ failing_plan()
 We can now investigate the nodes of the tree, and we will see that the tree indeed contains a failed task.
 
 ```python
-print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))
-print(pycram.task.task_tree.children[0])
+print(anytree.RenderTree(pycram.tasktree.task_tree, style=anytree.render.AsciiStyle()))
+print(pycram.tasktree.task_tree.children[0])
 ```
 
 ```python

examples/intro.md

@@ -34,6 +34,10 @@ from pycram.datastructures.enums import ObjectType, WorldMode
 from pycram.datastructures.pose import Pose
 
 world = BulletWorld(mode=WorldMode.GUI)
+
+milk = Object("Milk", ObjectType.MILK, "milk.stl")
+pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
+cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))
 ```
 
 The BulletWorld allows to render images from arbitrary positions. In the following example we render images with the
@@ -72,12 +76,6 @@ Optional:
 * Color
 * Ignore Cached Files
 
-If there is only a filename and no path, PyCRAM will check in the resource directory if there is a matching file.
-
-```python
-milk = Object("Milk", ObjectType.MILK, "milk.stl")
-```
-
 Objects provide methods to change the position and rotation, change the color, attach other objects, set the state of
 joints if the objects has any or get the position and orientation of a link.
 
@@ -88,12 +86,6 @@ methods related to these will not work.
 milk.set_position(Pose([1, 0, 0]))
 ```
 
-To remove an Object from the BulletWorld just call the 'remove' method on the Object.
-
-```python
-milk.remove()
-```
-
 Since everything inside the BulletWorld is an Object, even a complex environment Object like the kitchen can be spawned
 in the same way as the milk.
 
@@ -187,35 +179,33 @@ Allows for geometric reasoning in the BulletWorld. At the moment the following t
 * {meth}`~pycram.world_reasoning.blocking`
 * {meth}`~pycram.world_reasoning.supporting`
 
-To show the geometric reasoning we first spawn a robot as well as the milk Object again.
-
-```python
-import pycram.world_reasoning as btr
-
-milk = Object("Milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 0, 1]))
-pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")
-```
-
 We start with testing for visibility
 
 ```python
+from pycram.world_reasoning import visible
+
 milk.set_position(Pose([1, 0, 1]))
-visible = btr.visible(milk, pr2.get_link_pose("wide_stereo_optical_frame"))
+visible = visible(milk, pr2.get_link_pose("wide_stereo_optical_frame"))
 print(f"Milk visible: {visible}")
 ```
 
 ```python
+from pycram.world_reasoning import contact
+from pycram.datastructures.world import World
+
 milk.set_position(Pose([1, 0, 0.05]))
 
 plane = World.current_world.objects[0]
-contact = btr.contact(milk, plane)
+contact = contact(milk, plane)
 print(f"Milk is in contact with the floor: {contact}")
 ```
 
 ```python
+from pycram.world_reasoning import reachable
+
 milk.set_position(Pose([0.6, -0.5, 0.7]))
 
-reachable = btr.reachable(milk, pr2, "r_gripper_tool_frame")
+reachable = reachable(milk, pr2, "r_gripper_tool_frame")
 print(f"Milk is reachable for the PR2: {reachable}")
 ```
 
@@ -334,14 +324,6 @@ with simulated_robot:
 To get familiar with the PyCRAM Framework we will write a simple pick and place plan. This plan will let the robot grasp
 a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place plan.
 
-```python
-from pycram.designators.object_designator import *
-from pycram.datastructures.enums import Grasp
-
-cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95]))
-
-```
-
 ```python
 from pycram.datastructures.enums import Grasp
 
@@ -351,7 +333,7 @@ robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve()
 with simulated_robot:
     ParkArmsAction([Arms.BOTH]).resolve().perform()
 
-    MoveTorsoAction([0.3]).resolve().perform()
+    MoveTorsoAction([0.5]).resolve().perform()
 
     pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()
     pickup_arm = pickup_pose.reachable_arms[0]