diff --git a/.github/workflows/reusable-sphinx-build.yml b/.github/workflows/reusable-jupyter-book-build.yml similarity index 70% rename from .github/workflows/reusable-sphinx-build.yml rename to .github/workflows/reusable-jupyter-book-build.yml index c6353b831..d3005a079 100644 --- a/.github/workflows/reusable-sphinx-build.yml +++ b/.github/workflows/reusable-jupyter-book-build.yml @@ -1,4 +1,4 @@ -name: Build Sphinx documentation 📖 +name: Build Jupyter-Book documentation 📖 on: push: @@ -18,30 +18,35 @@ defaults: jobs: test-and-build: - name: Build Sphinx documentation - runs-on: ubuntu-20.04 + name: Build Jupyter-Book documentation + runs-on: ubuntu-22.04 steps: - name: Checkout 🛎 uses: actions/checkout@v3 + - name: Install PyCRAM dependencies 🍼 + uses: py-actions/py-dependency-install@v4 + with: + path: "requirements-setuptools.txt" + # ---------------------------------------------------------------------------------------------------------------- - - name: Install PyCRAM dependencies 🍼 + - name: Install Jupyter-Book dependencies 🍼 uses: py-actions/py-dependency-install@v4 with: path: "requirements.txt" # ---------------------------------------------------------------------------------------------------------------- - - name: Install Sphinx dependencies 📚 + - name: Install Jupyter-Book dependencies 📚 uses: py-actions/py-dependency-install@v4 with: path: "doc/requirements.txt" # ---------------------------------------------------------------------------------------------------------------- - - name: Build Sphinx documentation 📝 + - name: Build Jupyter-Book documentation 📝 working-directory: ./doc run: | - sudo apt install pandoc - make html \ No newline at end of file + cd source + jupyter-book build . \ No newline at end of file diff --git a/.readthedocs.yaml b/.readthedocs.yaml index 162d99dd6..8a83fb6de 100644 --- a/.readthedocs.yaml +++ b/.readthedocs.yaml @@ -1,13 +1,31 @@ +# .readthedocs.yaml +# Read the Docs configuration file +# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details + +# Required version: 2 +# Set the OS, Python version and other tools you might need build: - os: "ubuntu-20.04" + os: ubuntu-22.04 tools: - python: "3.8" + python: "3.9" + apt_packages: + - graphviz + - python3-catkin-pkg + jobs: + pre_build: + # Generate the Sphinx configuration for this Jupyter Book so it builds. + - "jupyter-book config sphinx doc/source" python: - install: - - requirements: doc/requirements.txt + install: + - requirements: doc/requirements.txt +# - method: pip +# path: . +# extra_requirements: +# - catkin_pkg +# - sphinx -submodules: - exclude: all \ No newline at end of file +sphinx: + builder: html \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index c2ff9a272..5042795d3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -6,7 +6,7 @@ FROM $FROM_IMAGE as builder ARG OVERLAY_WS=/opt/ros/overlay_ws WORKDIR $OVERLAY_WS/src -RUN sudo apt-get update && apt-get install python3-pip python3-vcstool git -y +RUN sudo apt-get update && apt-get install python3-pip python3-vcstool git default-jre -y RUN vcs import --input https://raw.githubusercontent.com/cram2/pycram/dev/pycram-https.rosinstall --recursive --skip-existing $OVERLAY_WS/src RUN sudo apt-get update && apt-get install python3-pip -y && pip3 install pip --upgrade RUN pip3 install --no-cache-dir -r $OVERLAY_WS/src/pycram/requirements.txt diff --git a/README.md b/README.md index c243da496..9b51bbf7c 100644 --- a/README.md +++ b/README.md @@ -29,18 +29,19 @@ The plan that both robots execute is a relativly simple pick and place plan: The code for this plan can be seen below. ``` -from pycram.bullet_world import BulletWorld, Object +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object from pycram.process_module import simulated_robot from pycram.designators.motion_designator import * from pycram.designators.location_designator import * from pycram.designators.action_designator import * from pycram.designators.object_designator import * -from pycram.enums import ObjectType +from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode -world = BulletWorld() +world = BulletWorld(WorldMode.GUI) kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", position=[1.4, 1, 0.95]) +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) cereal_desig = ObjectDesignatorDescription(names=["cereal"]) kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) @@ -56,7 +57,7 @@ with simulated_robot: NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() - PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=["front"]).resolve().perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() @@ -69,6 +70,8 @@ with simulated_robot: PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() + +world.exit() ``` diff --git a/binder/README.md b/binder/README.md index b10281a0b..d9d6ece00 100644 --- a/binder/README.md +++ b/binder/README.md @@ -99,8 +99,7 @@ RUN cd pycram \ && cd src/neem_interface_python \ && git clone https://github.com/benjaminalt/neem-interface.git src/neem-interface -RUN pip install --requirement ${PYCRAM_WS}/src/pycram/requirements.txt --user -RUN pip install --requirement ${PYCRAM_WS}/src/pycram/src/neem_interface_python/requirements.txt --user \ +RUN pip install --requirement ${PYCRAM_WS}/src/pycram/requirements.txt --user \ && pip cache purge ``` @@ -448,15 +447,3 @@ with simulated_robot: arms=["left"], grasps=["left", "right"]).resolve().perform() ``` - - - - - - - - - - - - diff --git a/binder/pycram-http.rosinstall b/binder/pycram-http.rosinstall index 4ad9ec467..15a9e5238 100644 --- a/binder/pycram-http.rosinstall +++ b/binder/pycram-http.rosinstall @@ -7,18 +7,10 @@ repositories: type: git url: http://github.com/code-iai/iai_robots.git version: master - pr2_common: - type: git - url: https://github.com/PR2/pr2_common.git - version: b34703bcca2b07cadbc3777d3c504c232a0c0c28 kdl_ik_services: type: git url: https://github.com/cram2/kdl_ik_service.git verison: master - pr2_kinematics: - type: git - url: https://github.com/PR2/pr2_kinematics.git - version: kinetic-devel orocos_kinematics_dynamics: type: git url: https://github.com/orocos/orocos_kinematics_dynamics.git diff --git a/config/__init__.py b/config/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py new file mode 100644 index 000000000..6463cf151 --- /dev/null +++ b/config/multiverse_conf.py @@ -0,0 +1,72 @@ +import datetime + +from typing_extensions import Type + +from .world_conf import WorldConfig +from pycram.description import ObjectDescription +from pycram.helper import find_multiverse_resources_path +from pycram.object_descriptors.mjcf import ObjectDescription as MJCF + + +class MultiverseConfig(WorldConfig): + # Multiverse Configuration + resources_path = find_multiverse_resources_path() + """ + The path to the Multiverse resources directory. + """ + + # Multiverse Socket Configuration + HOST: str = "tcp://127.0.0.1" + SERVER_HOST: str = HOST + SERVER_PORT: str = 7000 + BASE_CLIENT_PORT: int = 9000 + + # Multiverse Client Configuration + READER_MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) + """ + The maximum wait time for the data in seconds. + """ + + # Multiverse Simulation Configuration + simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) + simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) + """ + The time step of the simulation in seconds and the frequency of the simulation in Hz. + """ + + simulation_wait_time_factor: float = 1.0 + """ + The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for + the time taken by the simulation to process the request, this depends on the computational power of the machine + running the simulation. + """ + + use_static_mode: bool = True + """ + If True, the simulation will always be in paused state unless the simulate() function is called, this behaves + similar to bullet_world which uses the bullet physics engine. + """ + + use_controller: bool = False + use_controller = use_controller and not use_static_mode + """ + Only used when use_static_mode is False. This turns on the controller for the robot joints. + """ + + default_description_type: Type[ObjectDescription] = MJCF + """ + The default description type for the objects. + """ + + use_physics_simulator_state: bool = True + """ + Whether to use the physics simulator state when restoring or saving the world state. + """ + + clear_cache_at_start = False + + let_pycram_move_attached_objects = False + let_pycram_handle_spawning = False + + position_tolerance = 2e-2 + prismatic_joint_position_tolerance = 2e-2 diff --git a/config/world_conf.py b/config/world_conf.py new file mode 100644 index 000000000..76db4b330 --- /dev/null +++ b/config/world_conf.py @@ -0,0 +1,93 @@ +import math +import os + +from typing_extensions import Tuple, Type +from pycram.description import ObjectDescription +from pycram.object_descriptors.urdf import ObjectDescription as URDF + + +class WorldConfig: + + """ + A class to store the configuration of the world, this can be inherited to create a new configuration class for a + specific world (e.g. multiverse has MultiverseConfig which inherits from this class). + """ + + resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') + resources_path = os.path.abspath(resources_path) + """ + Global reference for the resources path, this is used to search for the description files of the robot and + the objects. + """ + + cache_dir_name: str = 'cached' + """ + The name of the cache directory. + """ + + cache_dir: str = os.path.join(resources_path, cache_dir_name) + """ + Global reference for the cache directory, this is used to cache the description files of the robot and the objects. + """ + + clear_cache_at_start: bool = True + """ + Whether to clear the cache directory at the start. + """ + + prospection_world_prefix: str = "prospection_" + """ + The prefix for the prospection world name. + """ + + simulation_frequency: int = 240 + """ + The simulation frequency (Hz), used for calculating the equivalent real time in the simulation. + """ + + update_poses_from_sim_on_get: bool = True + """ + Whether to update the poses from the simulator when getting the object poses. + """ + + default_description_type: Type[ObjectDescription] = URDF + """ + The default description type for the objects. + """ + + use_physics_simulator_state: bool = False + """ + Whether to use the physics simulator state when restoring or saving the world state. + Currently with PyBullet, this causes a bug where ray_test does not work correctly after restoring the state using the + simulator, so it is recommended to set this to False in PyBullet. + """ + + let_pycram_move_attached_objects: bool = True + let_pycram_handle_spawning: bool = True + let_pycram_handle_world_sync: bool = True + """ + Whether to let PyCRAM handle the movement of attached objects, the spawning of objects, + and the world synchronization. + """ + + position_tolerance: float = 1e-2 + orientation_tolerance: float = 10 * math.pi / 180 + prismatic_joint_position_tolerance: float = 1e-2 + revolute_joint_position_tolerance: float = 5 * math.pi / 180 + """ + The acceptable error for the position and orientation of an object/link, and the joint positions. + """ + + use_percentage_of_goal: bool = False + acceptable_percentage_of_goal: float = 0.5 + """ + Whether to use a percentage of the goal as the acceptable error. + """ + + raise_goal_validator_error: bool = False + """ + Whether to raise an error if the goals are not achieved. + """ + @classmethod + def get_pose_tolerance(cls) -> Tuple[float, float]: + return cls.position_tolerance, cls.orientation_tolerance diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 92542cbec..f06fc3d38 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -8,10 +8,12 @@ from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object from pycram.datastructures.dataclasses import Color +from pycram.ros.viz_marker_publisher import VizMarkerPublisher extension = ObjectDescription.get_file_extension() world = BulletWorld(WorldMode.GUI) + robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") @@ -49,15 +51,15 @@ def move_and_detect(obj_type): milk_desig = move_and_detect(ObjectType.MILK) - TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) - TransportAction(cereal_desig, ["right"], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() + TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() bowl_desig = move_and_detect(ObjectType.BOWL) - TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) @@ -74,10 +76,10 @@ def move_and_detect(obj_type): spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() - pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right" - PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform() + pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT + PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() - ParkArmsAction([Arms.LEFT if pickup_arm == "left" else Arms.RIGHT]).resolve().perform() + ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform() CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() @@ -94,3 +96,5 @@ def move_and_detect(obj_type): PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() + +world.exit() diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py new file mode 100644 index 000000000..6e71ec112 --- /dev/null +++ b/demos/pycram_multiverse_demo/demo.py @@ -0,0 +1,100 @@ +from pycram.datastructures.dataclasses import Color +from pycram.datastructures.enums import ObjectType, Arms, Grasp +from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ + LookAtAction, DetectAction, OpenAction, PickUpAction, CloseAction, PlaceAction +from pycram.designators.location_designator import CostmapLocation, AccessingLocation +from pycram.designators.object_designator import BelieveObject, ObjectPart +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse import Multiverse + + +@with_simulated_robot +def move_and_detect(obj_type: ObjectType, pick_pose: Pose): + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[pick_pose]).resolve().perform() + + object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + + return object_desig + +world = Multiverse(simulation_name='pycram_test') +extension = ObjectDescription.get_file_extension() +robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + +milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]), + color=Color(1, 0, 0, 1)) + +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), + color=Color(0, 0, 1, 1)) +apartment.attach(spoon, 'cabinet10_drawer1') + +robot_desig = BelieveObject(names=[robot.name]) +apartment_desig = BelieveObject(names=[apartment.name]) + +with simulated_robot: + + # Transport the milk + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() + + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + + # Find and navigate to the drawer containing the spoon + handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve()) + drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), + robot_desig=robot_desig.resolve()).resolve() + + NavigateAction([drawer_open_loc.pose]).resolve().perform() + + OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + spoon.detach(apartment) + + # Detect and pickup the spoon + LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform() + + spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() + + pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT + PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() + + ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform() + + CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.15]).resolve().perform() + + # Find a pose to place the spoon, move and then place it + spoon_target_pose = Pose([2.35, 2.6, 0.95], [0, 0, 0, 1]) + placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() + + NavigateAction([placing_loc.pose]).resolve().perform() + + PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + +world.exit() + + +def debug_place_spoon(): + robot.set_pose(Pose([1.66, 2.56, 0.01], [0.0, 0.0, -0.04044101807153309, 0.9991819274072855])) + spoon.set_pose(Pose([1.9601757566599975, 2.06718019258626, 1.0427727691273496], + [0.11157527804553227, -0.7076243776942466, 0.12773644958149588, 0.685931554070963])) + robot.attach(spoon, 'r_gripper_tool_frame') + pickup_arm = Arms.RIGHT + spoon_desig = BelieveObject(names=[spoon.name]) + return pickup_arm, spoon_desig diff --git a/demos/pycram_ur5_demo/demo.py b/demos/pycram_ur5_demo/demo.py index 42d6709d3..491ad704f 100644 --- a/demos/pycram_ur5_demo/demo.py +++ b/demos/pycram_ur5_demo/demo.py @@ -7,9 +7,9 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.datastructures.world import Object from pycram.datastructures.pose import Pose -from pycram.ros.force_torque_sensor import ForceTorqueSensor -from pycram.ros.joint_state_publisher import JointStatePublisher -from pycram.ros.tf_broadcaster import TFBroadcaster +from pycram.ros_utils.force_torque_sensor import ForceTorqueSensor +from pycram.ros_utils.joint_state_publisher import JointStatePublisher +from pycram.ros_utils.tf_broadcaster import TFBroadcaster SCRIPT_DIR = os.path.abspath(os.path.dirname(__file__)) PYCRAM_DIR = os.path.join(SCRIPT_DIR, os.pardir, os.pardir) diff --git a/demos/pycram_virtual_building_demos/setup/launch_robot.py b/demos/pycram_virtual_building_demos/setup/launch_robot.py index eaa9a08a8..2e51c45e0 100644 --- a/demos/pycram_virtual_building_demos/setup/launch_robot.py +++ b/demos/pycram_virtual_building_demos/setup/launch_robot.py @@ -3,6 +3,9 @@ import rospy import rospkg +from pycram.ros.logging import loginfo +from pycram.ros.ros_tools import create_ros_pack + def launch_pr2(): """ @@ -39,14 +42,14 @@ def launch_robot(launch_file, package='pycram', launch_folder='/launch/'): :param launch_folder: Location of the launch file inside the package """ - rospath = rospkg.RosPack() + rospath = create_ros_pack() uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [rospath.get_path(package) + launch_folder + launch_file]) launch.start() - rospy.loginfo(f'{launch_file} started') + loginfo(f'{launch_file} started') # Wait for ik server to launch time.sleep(2) diff --git a/doc/Makefile b/doc/Makefile deleted file mode 100644 index 9530909a4..000000000 --- a/doc/Makefile +++ /dev/null @@ -1,233 +0,0 @@ -# Makefile for Sphinx documentation -# - -# You can set these variables from the command line. -SPHINXOPTS = -SPHINXBUILD = sphinx-build -PAPER = -BUILDDIR = build -APIDOCDIR = source/autoapi - -# User-friendly check for sphinx-build -ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) - $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don\'t have Sphinx installed, grab it from http://sphinx-doc.org/) -endif - -# Internal variables. -PAPEROPT_a4 = -D latex_paper_size=a4 -PAPEROPT_letter = -D latex_paper_size=letter -ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source -# the i18n builder cannot share the environment and doctrees with the others -I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source - -.PHONY: help -help: - @echo "Please use \`make ' where is one of" - @echo " html to make standalone HTML files" - @echo " dirhtml to make HTML files named index.html in directories" - @echo " singlehtml to make a single large HTML file" - @echo " pickle to make pickle files" - @echo " json to make JSON files" - @echo " htmlhelp to make HTML files and a HTML help project" - @echo " qthelp to make HTML files and a qthelp project" - @echo " applehelp to make an Apple Help Book" - @echo " devhelp to make HTML files and a Devhelp project" - @echo " epub to make an epub" - @echo " epub3 to make an epub3" - @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" - @echo " latexpdf to make LaTeX files and run them through pdflatex" - @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" - @echo " text to make text files" - @echo " man to make manual pages" - @echo " texinfo to make Texinfo files" - @echo " info to make Texinfo files and run them through makeinfo" - @echo " gettext to make PO message catalogs" - @echo " changes to make an overview of all changed/added/deprecated items" - @echo " xml to make Docutils-native XML files" - @echo " pseudoxml to make pseudoxml-XML files for display purposes" - @echo " linkcheck to check all external links for integrity" - @echo " doctest to run all doctests embedded in the documentation (if enabled)" - @echo " coverage to run coverage check of the documentation (if enabled)" - @echo " dummy to check syntax errors of document sources" - -.PHONY: clean -clean: - rm -rf $(BUILDDIR) - rm -rf $(APIDOCDIR) - -.PHONY: html -html: - make clean - $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html - @echo - @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." - -.PHONY: dirhtml -dirhtml: - $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml - @echo - @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." - -.PHONY: singlehtml -singlehtml: - $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml - @echo - @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." - -.PHONY: pickle -pickle: - $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle - @echo - @echo "Build finished; now you can process the pickle files." - -.PHONY: json -json: - $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json - @echo - @echo "Build finished; now you can process the JSON files." - -.PHONY: htmlhelp -htmlhelp: - $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp - @echo - @echo "Build finished; now you can run HTML Help Workshop with the" \ - ".hhp project file in $(BUILDDIR)/htmlhelp." - -.PHONY: qthelp -qthelp: - $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp - @echo - @echo "Build finished; now you can run "qcollectiongenerator" with the" \ - ".qhcp project file in $(BUILDDIR)/qthelp, like this:" - @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/pyrap.qhcp" - @echo "To view the help file:" - @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/pyrap.qhc" - -.PHONY: applehelp -applehelp: - $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp - @echo - @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." - @echo "N.B. You won't be able to view it unless you put it in" \ - "~/Library/Documentation/Help or install it in your application" \ - "bundle." - -.PHONY: devhelp -devhelp: - $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp - @echo - @echo "Build finished." - @echo "To view the help file:" - @echo "# mkdir -p $$HOME/.local/share/devhelp/pyrap" - @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/pyrap" - @echo "# devhelp" - -.PHONY: epub -epub: - $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub - @echo - @echo "Build finished. The epub file is in $(BUILDDIR)/epub." - -.PHONY: epub3 -epub3: - $(SPHINXBUILD) -b epub3 $(ALLSPHINXOPTS) $(BUILDDIR)/epub3 - @echo - @echo "Build finished. The epub3 file is in $(BUILDDIR)/epub3." - -.PHONY: latex -latex: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo - @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." - @echo "Run \`make' in that directory to run these through (pdf)latex" \ - "(use \`make latexpdf' here to do that automatically)." - -.PHONY: latexpdf -latexpdf: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo "Running LaTeX files through pdflatex..." - $(MAKE) -C $(BUILDDIR)/latex all-pdf - @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." - -.PHONY: latexpdfja -latexpdfja: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo "Running LaTeX files through platex and dvipdfmx..." - $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja - @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." - -.PHONY: text -text: - $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text - @echo - @echo "Build finished. The text files are in $(BUILDDIR)/text." - -.PHONY: man -man: - $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man - @echo - @echo "Build finished. The manual pages are in $(BUILDDIR)/man." - -.PHONY: texinfo -texinfo: - $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo - @echo - @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." - @echo "Run \`make' in that directory to run these through makeinfo" \ - "(use \`make info' here to do that automatically)." - -.PHONY: info -info: - $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo - @echo "Running Texinfo files through makeinfo..." - make -C $(BUILDDIR)/texinfo info - @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." - -.PHONY: gettext -gettext: - $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale - @echo - @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." - -.PHONY: changes -changes: - $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes - @echo - @echo "The overview file is in $(BUILDDIR)/changes." - -.PHONY: linkcheck -linkcheck: - $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck - @echo - @echo "Link check complete; look for any errors in the above output " \ - "or in $(BUILDDIR)/linkcheck/output.txt." - -.PHONY: doctest -doctest: - $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest - @echo "Testing of doctests in the sources finished, look at the " \ - "results in $(BUILDDIR)/doctest/output.txt." - -.PHONY: coverage -coverage: - $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage - @echo "Testing of coverage in the sources finished, look at the " \ - "results in $(BUILDDIR)/coverage/python.txt." - -.PHONY: xml -xml: - $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml - @echo - @echo "Build finished. The XML files are in $(BUILDDIR)/xml." - -.PHONY: pseudoxml -pseudoxml: - $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml - @echo - @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." - -.PHONY: dummy -dummy: - $(SPHINXBUILD) -b dummy $(ALLSPHINXOPTS) $(BUILDDIR)/dummy - @echo - @echo "Build finished. Dummy builder generates no files." diff --git a/doc/README.md b/doc/README.md index 486b9bf9b..529f686a6 100644 --- a/doc/README.md +++ b/doc/README.md @@ -7,17 +7,24 @@ the instructions below. ## Building the documentation -The documentation uses sphinx as engine. -Building sphinx based documentations requires `pandoc `_ -to be installed. Pandoc can be installed via the Ubunutu package manager: +The documentation uses jupyter-book as engine. Building the documentation requires Python 3.9 or higher to avoid +dependency conflicts. On Ubuntu 20.04 you can install Python 3.9 with the following commands. ~~~ -sudo apt install pandoc +apt-get install python3.9 ~~~ -After installing pandoc, install sphinx on your device. +It is recommended to create a virtual environment to avoid conflicts with the system python interpreter. ~~~ -sudo apt install python3-sphinx +apt-get install python3.9-virtualenv +virtualenv -p python3.9 --system-site-packages build-doc ~~~ + +Activate the virtual environment. +~~~ +source build-doc/bin/activate +~~~ + + Install the requirements in your python interpreter. ~~~ @@ -26,10 +33,11 @@ pip install -r requirements.txt Run pycram and build the docs. ~~~ -make html +cd doc/source +jupyter-book build . ~~~ Show the index. ~~~ -firefox build/html/index.html +firefox _build/html/index.html ~~~ \ No newline at end of file diff --git a/doc/make.bat b/doc/make.bat deleted file mode 100644 index fc599585b..000000000 --- a/doc/make.bat +++ /dev/null @@ -1,190 +0,0 @@ -@ECHO OFF - -REM Command file for Sphinx documentation - -if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-build -) -set BUILDDIR=_build -set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . -set I18NSPHINXOPTS=%SPHINXOPTS% . -if NOT "%PAPER%" == "" ( - set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% - set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% -) - -if "%1" == "" goto help - -if "%1" == "help" ( - :help - echo.Please use `make ^` where ^ is one of - echo. html to make standalone HTML files - echo. dirhtml to make HTML files named index.html in directories - echo. singlehtml to make a single large HTML file - echo. pickle to make pickle files - echo. json to make JSON files - echo. htmlhelp to make HTML files and a HTML help project - echo. qthelp to make HTML files and a qthelp project - echo. devhelp to make HTML files and a Devhelp project - echo. epub to make an epub - echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter - echo. text to make text files - echo. man to make manual pages - echo. texinfo to make Texinfo files - echo. gettext to make PO message catalogs - echo. changes to make an overview over all changed/added/deprecated items - echo. linkcheck to check all external links for integrity - echo. doctest to run all doctests embedded in the documentation if enabled - goto end -) - -if "%1" == "clean" ( - for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i - del /q /s %BUILDDIR%\* - goto end -) - -if "%1" == "html" ( - %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/html. - goto end -) - -if "%1" == "dirhtml" ( - %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. - goto end -) - -if "%1" == "singlehtml" ( - %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. - goto end -) - -if "%1" == "pickle" ( - %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can process the pickle files. - goto end -) - -if "%1" == "json" ( - %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can process the JSON files. - goto end -) - -if "%1" == "htmlhelp" ( - %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can run HTML Help Workshop with the ^ -.hhp project file in %BUILDDIR%/htmlhelp. - goto end -) - -if "%1" == "qthelp" ( - %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can run "qcollectiongenerator" with the ^ -.qhcp project file in %BUILDDIR%/qthelp, like this: - echo.^> qcollectiongenerator %BUILDDIR%\qthelp\PRAC.qhcp - echo.To view the help file: - echo.^> assistant -collectionFile %BUILDDIR%\qthelp\PRAC.ghc - goto end -) - -if "%1" == "devhelp" ( - %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. - goto end -) - -if "%1" == "epub" ( - %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The epub file is in %BUILDDIR%/epub. - goto end -) - -if "%1" == "latex" ( - %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. - goto end -) - -if "%1" == "text" ( - %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The text files are in %BUILDDIR%/text. - goto end -) - -if "%1" == "man" ( - %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The manual pages are in %BUILDDIR%/man. - goto end -) - -if "%1" == "texinfo" ( - %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. - goto end -) - -if "%1" == "gettext" ( - %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The message catalogs are in %BUILDDIR%/locale. - goto end -) - -if "%1" == "changes" ( - %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes - if errorlevel 1 exit /b 1 - echo. - echo.The overview file is in %BUILDDIR%/changes. - goto end -) - -if "%1" == "linkcheck" ( - %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck - if errorlevel 1 exit /b 1 - echo. - echo.Link check complete; look for any errors in the above output ^ -or in %BUILDDIR%/linkcheck/output.txt. - goto end -) - -if "%1" == "doctest" ( - %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest - if errorlevel 1 exit /b 1 - echo. - echo.Testing of doctests in the sources finished, look at the ^ -results in %BUILDDIR%/doctest/output.txt. - goto end -) - -:end diff --git a/doc/requirements.txt b/doc/requirements.txt index 1d73d4587..06dc928b3 100644 --- a/doc/requirements.txt +++ b/doc/requirements.txt @@ -1,9 +1,6 @@ -sphinx==6.1.3 -sphinxcontrib-bibtex -sphinxcontrib-packages -sphinx-paramlinks -sphinx-numfig -sphinx-autoapi -sphinx-rtd-theme -nbsphinx -jinja2==3.1.4 +jupyter-book +matplotlib +numpy +ghp-import +sphinx-proof +sphinx-autoapi \ No newline at end of file diff --git a/doc/source/_config.yml b/doc/source/_config.yml new file mode 100644 index 000000000..4cf29bde6 --- /dev/null +++ b/doc/source/_config.yml @@ -0,0 +1,69 @@ +####################################################################################### +# A default configuration that will be loaded for all jupyter books +# See the documentation for help and more options: +# https://jupyterbook.org/customize/config.html + +####################################################################################### +# Book settings +title : PyCRAM # The title of the doc. Will be placed in the left navbar. +author : Jonas Dech # The author of the doc +copyright : "2024" # Copyright year to be placed in the footer +logo : "../images/pycram_logo.png" # A path to the doc logo + +# Force re-execution of notebooks on each build. +# See https://jupyterbook.org/content/execute.html +execute: + execute_notebooks: auto + +# Define the name of the latex output file for PDF builds +latex: + latex_documents: + targetname: doc.tex + use_jupyterbook_latex: false + +# Add a bibtex file so that we can create citations +bibtex_bibfiles: + - references.bib + +# Information about where the doc exists on the web +repository: + url: https://github.com/cram2/pycram # Online location of your doc + path_to_book: docs # Optional path to your doc, relative to the repository root + branch: dev # Which branch of the repository should be used when creating links (optional) + +# Add GitHub buttons to your doc +# See https://jupyterbook.org/customize/config.html#add-a-link-to-your-repository +html: + use_issues_button: true + use_repository_button: true + show_navbar_depth: 0 + + +sphinx: + extra_extensions: + - sphinx_proof + - 'sphinx.ext.autodoc' + - 'sphinx.ext.autosummary' + - 'autoapi.extension' + config: + suppress_warnings: ["mystnb.unknown_mime_type"] + autosummary_generate: False + html_js_files: + - https://cdnjs.cloudflare.com/ajax/libs/require.js/2.3.4/require.min.js + autoapi_dirs: ['../../src'] + autoapi_add_toctree_entry: True + mathjax_path: https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js + +parse: + myst_enable_extensions: + - amsmath + - colon_fence + - deflist + - dollarmath + - html_admonition + - html_image + - linkify + - replacements + - smartquotes + - substitution + - tasklist \ No newline at end of file diff --git a/doc/source/_toc.yml b/doc/source/_toc.yml new file mode 100644 index 000000000..9f02b49a9 --- /dev/null +++ b/doc/source/_toc.yml @@ -0,0 +1,54 @@ +# Table of contents +# Learn more at https://jupyterbook.org/customize/toc.html + +format: jb-book +root: index.rst +parts: + - caption: Getting Started + chapters: + - file: installation.rst + + - caption: Concepts + chapters: + - file: ros_utils.rst + - file: new_robot.rst + - file: notebooks.rst + - file: designators.rst + - file: knowledge.rst + + - caption: Trouble Shooting + chapters: + - file: troubleshooting.rst + - file: remarks.rst + + + - caption: Examples + chapters: + - file: notebooks/intro + - file: notebooks/bullet_world + - file: notebooks/language + - file: notebooks/local_transformer + - file: designator_example.rst + sections: + - file: notebooks/action_designator + - file: notebooks/object_designator + - file: notebooks/location_designator + - file: notebooks/motion_designator + - file: orm + sections: + - file: notebooks/orm_example + - file: notebooks/orm_querying_examples + - file: notebooks/migrate_neems + - file: datastructures.rst + sections: + - file: notebooks/pose + - file: notebooks/robot_description + - file: external_interfaces.rst + sections: + - file: notebooks/interface_examples/giskard.md + - file: notebooks/interface_examples/robokudo.md + - file: notebooks/ontology + + - caption: API + chapters: + - file: autoapi/pycram/index.rst \ No newline at end of file diff --git a/doc/source/datastructures.rst b/doc/source/datastructures.rst new file mode 100644 index 000000000..99ec8a595 --- /dev/null +++ b/doc/source/datastructures.rst @@ -0,0 +1,3 @@ +============= +Datastructure +============= \ No newline at end of file diff --git a/doc/source/designator_example.rst b/doc/source/designator_example.rst new file mode 100644 index 000000000..0f9297c4e --- /dev/null +++ b/doc/source/designator_example.rst @@ -0,0 +1,3 @@ +=========== +Designators +=========== diff --git a/doc/source/designators.rst b/doc/source/designators.rst index 08ec4d8f4..b2a4b21c2 100644 --- a/doc/source/designators.rst +++ b/doc/source/designators.rst @@ -1,5 +1,3 @@ -.. _designators: - =========== Designators =========== @@ -22,7 +20,7 @@ poses. .. code-block:: python - poses = [[[1, 0, 0], [0, 0, 0, 1]], [[1.2, 0.2, 0], [0, 0, 1, 0]]] + poses = [Pose([1, 0, 0], [0, 0, 0, 1]), Pose([1.2, 0.2, 0], [0, 0, 1, 0])] NavigateAction(target_locations=poses) This is a description of an action which moves the robot to a pose in the environment. @@ -45,7 +43,7 @@ action looks like this: .. code-block:: python - NavigateActionPerformable(robot_position=((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)), target_location=[[1, 0, 0], [0, 0, 0, 1]]) + NavigateActionPerformable(robot_position=(Pose([0.0, 0.0, 0.0] [0.0, 0.0, 0.0, 1.0]), target_location=Pose([1, 0, 0], [0, 0, 0, 1])) A visual representation of the whole idea of designator and designator descriptions can be @@ -68,15 +66,15 @@ Object Designator Object designators represent objects in (simulated) world. The description of object designators can take names and types that the object should match. -The :meth:`~pycram.designators.object_designator.ObjectDesignatorDescription.ground` method returns an object with all +The :meth:`~pycram.designator.ObjectDesignatorDescription.ground` method returns an object with all its data attached that matches the description. -The :meth:`~pycram.designators.object_designator.ObjectDesignatorDescription.__iter__` method iterates over all objects +The :meth:`~pycram.designator.ObjectDesignatorDescription.__iter__` method iterates over all objects that match the description. Contributing Object Designators ------------------------------- Object Designators should always be part of an object designator description. -The general class structure is seen in :mod:`~pycram.designators.object_designator.ObjectDesignatorDescription`. +The general class structure is seen in :mod:`~pycram.designator.ObjectDesignatorDescription`. New object description need to inherit from the general object description. If the object they ground to differs from the base object, a `dataclass `_. should be created inside the new description. The dataclass is one element that matches the description. @@ -94,7 +92,7 @@ resolving the description to a single designator one parameter out of the given Motion Designator ================= Motion designators describe atomic actions that are executable for an agent. In contrast to action -designators there is no failure handling or other action designators. Furthermore, the :meth:`~pycram.designators.motion_designator.MotionDesignatorDescription.Motion.perform` +designators there is no failure handling or other action designators. Furthermore, the :meth:`~pycram.designator.MotionDesignatorDescription.Motion.perform` method passes the resolved motion designator to the respective Process Module for execution on the robot. Another difference to action designator is that motion designators only take a single parameter instead of a @@ -113,17 +111,17 @@ Creating your own Designator ============================ Creating your own designator is fairly easy, you only need to extend the base class of the respective description. - - :mod:`~pycram.designators.action_designator.ActionDesignatorDescription` - - :mod:`~pycram.designators.object_designator.ObjectDesignatorDescription` - - :mod:`~pycram.designators.location_designator.LocationDesignatorDescription` - - :mod:`~pycram.designators.motion_designator.MotionDesignatorDescription` + - :mod:`~pycram.designator.ActionDesignatorDescription` + - :mod:`~pycram.designator.ObjectDesignatorDescription` + - :mod:`~pycram.designator.LocationDesignatorDescription` + - :mod:`~pycram.designator.BaseMotion` Afterwards you need to implement your own ``ground`` method which is the default resolver and for location and object designator it makes sense to also implement a ``__iter__`` method. The ``ground`` and ``__iter__`` methods should return the designator sub-class so you also need to implement these with the parameter your designator needs. The sub-class can already contain some parameters, this is usually the case if the parameter is the same for every designator -of this type. For example, :mod:`pycram.designators.location_designator.LocationDesignatorDescription.Location` +of this type. For example, :class:`~pycram.designator.LocationDesignatorDescription.Location` contains a ``pose`` parameter since every location designator contains a resolved pose. For action and motion designator the sub-class is also the place where the ``perform`` method is written which contains diff --git a/doc/source/examples.rst b/doc/source/examples.rst index 055406048..e637d5599 100644 --- a/doc/source/examples.rst +++ b/doc/source/examples.rst @@ -4,40 +4,6 @@ Examples All types of examples -Intro Example -============= - -.. nbgallery:: - notebooks/intro - - -Designator -========== - -.. nbgallery:: - notebooks/location_designator - notebooks/object_designator - notebooks/action_designator - notebooks/motion_designator - -Misc -==== - -.. nbgallery:: - notebooks/bullet_world - notebooks/minimal_task_tree - notebooks/pose - notebooks/improving_actions - notebooks/language - notebooks/local_transformer - -Interface Examples -================== - -.. nbgallery:: - notebooks/interface_examples/giskard - notebooks/interface_examples/robokudo - Object Relational Mapping ========================= @@ -60,15 +26,3 @@ to fill out the description. For this, check the documentation of :meth:`pycram. ORM Examples ------------ - -.. nbgallery:: - - notebooks/orm_example - notebooks/migrate_neems - notebooks/orm_querying_examples - -Ontology -========== - -.. nbgallery:: - notebooks/ontology \ No newline at end of file diff --git a/doc/source/external_interfaces.rst b/doc/source/external_interfaces.rst new file mode 100644 index 000000000..ebb39477a --- /dev/null +++ b/doc/source/external_interfaces.rst @@ -0,0 +1,3 @@ +================== +External Interface +================== \ No newline at end of file diff --git a/doc/source/index.rst b/doc/source/index.rst index 89f46a9e7..3e956cc1e 100644 --- a/doc/source/index.rst +++ b/doc/source/index.rst @@ -52,19 +52,19 @@ The code for this plan can be seen below. .. code-block:: python - from pycram.world.bullet_world import BulletWorld - from pycram.world_concepts.world_concepts import Object + from pycram.worlds.bullet_world import BulletWorld + from pycram.world_concepts.world_object import Object from pycram.process_module import simulated_robot from pycram.designators.motion_designator import * from pycram.designators.location_designator import * from pycram.designators.action_designator import * from pycram.designators.object_designator import * - from pycram.datastructures.enums import ObjectType, Arms, Grasps + from pycram.datastructures.enums import ObjectType, Arms, Grasp, WorldMode - world = BulletWorld() + world = BulletWorld(WorldMode.GUI) kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") - cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", position=[1.4, 1, 0.95]) + cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) cereal_desig = ObjectDesignatorDescription(names=["cereal"]) kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) @@ -80,7 +80,7 @@ The code for this plan can be seen below. NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() - PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasps.FRONT]).resolve().perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() @@ -96,39 +96,20 @@ The code for this plan can be seen below. world.exit() -Tutorials ---------- -There are a handful of tutorials to get you started on using PyCRAM. These tutorials are: +Citing PyCRAM +============= - * `Setup your Python REPL `_ - * `Interact with the BulletWorld `_ - * `Add your own robot `_ +If you want to cite PyCRAM in your work, you can use the following bibtex entry: +.. code-block:: bibtex - -Authors -------- - - * **Jonas Dech** - * **Andy Augsten** - * **Dustin Augsten** - * **Christopher Pollok** - * **Thomas Lipps** - * **Benjamin Alt** - -.. toctree:: - :maxdepth: 2 - :caption: Contents: - - installation - designators - resolvers - new_robot - examples - troubleshooting - ros_utils - remarks + @software{dech2024pycram, + author = {Dech, Jonas}, + title = {PyCRAM: A Python framework for cognition-enbabled robtics}, + url = {https://github.com/cram2/pycram}, + version = {0.2.0}, + } diff --git a/doc/source/installation.rst b/doc/source/installation.rst index 2b1ebecc4..a36b3f00a 100644 --- a/doc/source/installation.rst +++ b/doc/source/installation.rst @@ -1,5 +1,3 @@ -.. _installation: - ============ Installation ============ @@ -70,7 +68,7 @@ Now you can install PyCRAM into your ROS workspace. cd .. catkin_make source devel/setup.bash - echo "~/workspace/ros/devel/setup.bash" >> ~/.bashrc + echo "source ~/workspace/ros/devel/setup.bash" >> ~/.bashrc The cloning and setting up can take several minutes. After the command finishes you should see a number of repositories in your ROS workspace. @@ -107,8 +105,19 @@ Then install the Python packages in the requirements.txt file .. code-block:: shell sudo pip3 install -r requirements.txt - sudo pip3 install -r src/neem_interface_python/requirements.txt +This installs the packages into ``/usr/local/lib``. If you prefer to not clutter your system-wide python installation, +you can also install the packages into the catkin workspace as follows: + +.. code-block:: shell + + # install packages into catkin workspace instead of ~/.local + export PYTHONUSERBASE=~/workspace/ros/devel + # don't install packages that are available in system + export PIP_IGNORE_INSTALLED=0 + + pip3 install -r requirements.txt + pip3 install -r src/neem_interface_python/requirements.txt Building your ROS workspace =========================== @@ -147,20 +156,21 @@ IK solver. Building the documentation ========================== -The documentation uses sphinx as engine. -Building sphinx based documentations requires pandoc -to be installed. Pandoc can be installed via the package manager of Ubuntu. +The documentation uses jupyter-book as engine. +Building the documentation requires Python 3.9 to avoid dependency conflicts. +To install Python 3.9 on Ubuntu 20.04, use the following commands: .. code-block:: shell - sudo apt install pandoc + sudo apt install python3.9 -After installing pandoc, install sphinx on your device. +It is recommended to use a virtual environment to avoid conflicts with the system Python. .. code-block:: shell - sudo apt install python3-sphinx - + apt-get install python3-virtualenv + virtualenv -p python3.9 --system-site-packages build-doc + source build-doc/bin/activate Install the requirements in your python interpreter. @@ -175,14 +185,14 @@ Run pycram and build the docs. cd ~/workspace/ros roslaunch pycram ik_and_description.launch - cd src/pycram/doc - make html + cd src/pycram/doc/source + jupyter-book build . Show the index. .. code-block:: - firefox build/html/index.html + firefox _build/html/index.html @@ -282,7 +292,7 @@ To verify that it works, you can execute any Testcase. **Useful tips** - `Keyboard shortcuts `_ - - `Keymap `_ + - `Keymap `_, which can be configured in **Settings | Keymap**. The default is GNOME. - `Python interpreter `_ - `Python virtual environment `_ @@ -297,6 +307,8 @@ To verify that it works, you can execute any Testcase. - **Double Shift**: Quick file search +- **Alt + Shift + 1**: Reveal/Select current file in Project View + - **Ctrl F/R**: Find/Replace text in current file - **Ctrl Shift F/R**: Find/Replace text in the whole project, module, directory, scope diff --git a/doc/source/new_robot.rst b/doc/source/new_robot.rst index 04d95d7c9..a28c4dbe8 100644 --- a/doc/source/new_robot.rst +++ b/doc/source/new_robot.rst @@ -16,8 +16,9 @@ descriptions of the cameras mounted on the robot. An overview of the different components of the robot description as well as how these are created can be found in the following example: -.. nbgallery:: - notebooks/robot_description.ipynb +:ref:`Robot Description example` + + -------------------------------- diff --git a/doc/source/notebooks.rst b/doc/source/notebooks.rst new file mode 100644 index 000000000..2a6928c5f --- /dev/null +++ b/doc/source/notebooks.rst @@ -0,0 +1,58 @@ +========================= +Jupyter Notebook Examples +========================= + +PyCRAM uses Jupyter Notebooks as a means of providing code examples and tutorials. These examples are located in the +examples directory. + +These Notebooks are stored as Myst Markdown files, which provide the advantage of better verison control and easier +integration with Sphinx documentation. However, the Notebooks can be run as usual in Jupyter Notebook. + + +================ +Running Examples +================ +Despite the examples being stored as Myst Markdown files, they can still be run as Jupyter Notebooks. To run an example, +simply open the Notebook in Jupyter Notebook and run the cells as you would with any other Notebook. + +To run Jupyter Notebook, you need to have Jupyter Notebook installed. You can install Jupyter Notebook using pip: + +.. code-block:: bash + + pip install jupyter + +After installing Jupyter Notebook, you can run it by executing the following command in the terminal: + +.. code-block:: bash + + jupyter notebook + +Afterwards, a new website will open in your browser, where you can navigate to the examples directory and open the +desired Notebook. + + +=============== +Adding Examples +=============== +If you want to add a new example, you can do so by creating a new Notebook in the examples directory. After creating +and testing the example you need to convert it to the Myst Markdown format. For this step we use the +`jupytext `_ tool. To convert a Notebook to Myst Markdown, run the following +command in the examples directory of the project: + +.. code-block:: bash + + jupytext --to markdown your_notebook.ipynb + +------------------------------- +Linking directly to source code +------------------------------- +Since the examples are stored as Myst Markdown files, you can link directly to the source code documentation. For +example, to link to the source code of the class `PickUpAction` you can use: + +.. code-block:: markdown + + {class}`~pycram.designators.action_designator.PickUpAction` + + +Alternatively, you can use `{meth}`, `{func}`, `{attr}`, `{class}`, `{mod}` and `{obj}` to link to methods, functions, +attributes, classes, modules and objects, respectively. diff --git a/doc/source/orm.md b/doc/source/orm.md new file mode 100644 index 000000000..7bfc5c879 --- /dev/null +++ b/doc/source/orm.md @@ -0,0 +1 @@ +# ORM \ No newline at end of file diff --git a/doc/source/remarks.rst b/doc/source/remarks.rst index 902ae7b1b..0debe0d8a 100644 --- a/doc/source/remarks.rst +++ b/doc/source/remarks.rst @@ -27,28 +27,9 @@ that will display in the drop down menu of jupyter. After that, select the corre everything should work now. Refer `here `_ for details. -Adding Notebooks to the Documentation -===================================== - -Adding notebooks to the documentation is done with the -`nbsphinx `_ extension. If they are outside of the doc folder -please put a symbolic link in the doc/source/notebooks folder, such that no duplication is done. Sphinx will -automatically copy them during the build process. Use relative symbolic links since absolute paths won't work for -different machines. Dirty Terminals =============== If your terminal gets polluted by PyBullet complaining about incomplete URDF descriptions, you need to first fix your URDF files by inserting the missing tags and second delete the `resources/cached` folder. - -Missing pr2_arm_kinematics -========================== - -Aptitudes autoremove likes to also remove the arm kinematics. Reinstall the missing libraries with - -.. code-block:: shell - - sudo apt-get install ros-noetic-moveit - -Then rebuild your workspace. diff --git a/doc/source/resolvers.rst b/doc/source/resolvers.rst index 0303fa7cb..db7774b3f 100644 --- a/doc/source/resolvers.rst +++ b/doc/source/resolvers.rst @@ -1,3 +1,4 @@ +================ Custom Resolvers ================ @@ -14,5 +15,3 @@ Tutorial -------- A tutorial for custom resolver creation is found in the notebook below. -.. nbgallery:: - notebooks/improving_actions \ No newline at end of file diff --git a/doc/source/ros_utils.rst b/doc/source/ros_utils.rst index c926b6360..d4e4ba158 100644 --- a/doc/source/ros_utils.rst +++ b/doc/source/ros_utils.rst @@ -7,6 +7,8 @@ PyCRAM provides a number of utils to interact with the ROS network. The utils ar * A TF Broadcaster * A Joint State Publisher * A Simulated Force Torque Sensor +* A Visualisation Marker Publisher +* A Robot State Updater These site will go over all utils what they do and how to use them. All ROS utils presented here will publish continuously in a new thread. You can either stop the publishing by calling the diff --git a/doc/source/troubleshooting.rst b/doc/source/troubleshooting.rst index 5a1136717..768818778 100644 --- a/doc/source/troubleshooting.rst +++ b/doc/source/troubleshooting.rst @@ -108,4 +108,3 @@ real_robot environments. This is also explained in the `Action Designator Exampl with simulated_robot: NavigateAction([Pose()]).resolve().perform() - diff --git a/docker/Dockerfile b/docker/Dockerfile index e85fb2af8..1cd5716e5 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -5,7 +5,7 @@ FROM $FROM_IMAGE as cacher ARG OVERLAY_WS WORKDIR $OVERLAY_WS/src -RUN apt-get update && apt-get install python3-pip python3-vcstool git -y && pip3 install pip --upgrade +RUN apt-get update && apt-get install python3-pip python3-vcstool git default-jre -y && pip3 install pip --upgrade RUN pip3 install rosdep && rosdep init RUN vcs import --input https://raw.githubusercontent.com/cram2/pycram/dev/pycram-https.rosinstall --recursive --skip-existing $OVERLAY_WS/src @@ -18,4 +18,4 @@ RUN pip3 install --upgrade pip WORKDIR $OVERLAY_WS/src/pycram RUN pip3 install -r requirements.txt -EXPOSE 11311 \ No newline at end of file +EXPOSE 11311 diff --git a/examples/action_designator.ipynb b/examples/action_designator.ipynb deleted file mode 100644 index 5d2a0abc9..000000000 --- a/examples/action_designator.ipynb +++ /dev/null @@ -1,738 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "53c84222", - "metadata": {}, - "source": [ - "# Action Designator\n", - "This example will show the different kinds of Action Designators that are available. We will see how to create Action Designators and what they do.\n", - "\n", - "\n", - "Action Designators are high-level descriptions of actions which the robot should execute. \n", - "\n", - "Action Designators are created from an Action Designator Description, which describes the type of action as well as the parameter for this action. Parameter are given as a list of possible parameters.\n", - "For example, if you want to describe the robot moving to a table you would need a ```NavigateAction``` and a list of poses that are near the table. The Action Designator Description will then pick one of the poses and return a performable Action Designator which contains the picked pose. \n", - "\n" - ] - }, - { - "cell_type": "markdown", - "id": "c2241da6", - "metadata": {}, - "source": [ - "## Navigate Action\n", - "We will start with a simple example of the ```NavigateAction```. \n", - "\n", - "First, we need a BulletWorld with a robot." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "40ddbb51", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:47.931938Z", - "start_time": "2024-06-28T08:38:47.202525Z" - } - }, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld\n", - "from pycram.world_concepts.world_object import Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "world = BulletWorld(WorldMode.GUI)\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" - ] - }, - { - "cell_type": "markdown", - "id": "7621bace", - "metadata": {}, - "source": [ - "To move the robot we need to create a description and resolve it to an actual Designator. The description of navigation only needs a list of possible poses." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "5534efa5", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:48.479005Z", - "start_time": "2024-06-28T08:38:47.933443Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1719565018.682599]: Ontology [http://www.ease-crc.org/ont/SOMA-HOME.owl#]'s name: SOMA-HOME has been loaded\n", - "[INFO] [1719565018.683494]: - main namespace: SOMA-HOME\n", - "[INFO] [1719565018.684191]: - loaded ontologies:\n", - "[INFO] [1719565018.684669]: http://www.ease-crc.org/ont/SOMA-HOME.owl#\n", - "[INFO] [1719565018.685104]: http://www.ease-crc.org/ont/DUL.owl#\n", - "[INFO] [1719565018.685512]: http://www.ease-crc.org/ont/SOMA.owl#\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import NavigateAction\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "pose = Pose([1, 0, 0], [0, 0, 0, 1])\n", - "\n", - "# This is the Designator Description\n", - "navigate_description = NavigateAction(target_locations=[pose])\n", - "\n", - "# This is the performable Designator\n", - "navigate_designator = navigate_description.resolve()" - ] - }, - { - "cell_type": "markdown", - "id": "3d381bf6", - "metadata": {}, - "source": [ - "What we now did was: create the pose where we want to move the robot, create a description describing a navigation with a list of possible poses (in this case the list contains only one pose) and create an action designator from the description. The action designator contains the pose picked from the list of possible poses and can be performed." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "2450a3ff", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:48.998209Z", - "start_time": "2024-06-28T08:38:48.480029Z" - } - }, - "outputs": [], - "source": [ - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " navigate_designator.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "12d45cab", - "metadata": {}, - "source": [ - "Every designator that is performed needs to be in an environment that specifies where to perform the designator either on the real robot or the simulated one. This environment is called ```simulated_robot``` similar there is also a ```real_robot``` environment. \n", - "\n", - "There are also decorators which do the same thing but for whole methods, they are called ```with_real_robot``` and ```with_simulated_robot```." - ] - }, - { - "cell_type": "markdown", - "id": "8e46f0ef", - "metadata": {}, - "source": [ - "## Move Torso\n", - "This action designator moves the torso up or down, specifically it sets the torso joint to a given value.\n", - "\n", - "We start again by creating a description and resolving it to a designator. Afterwards, the designator is performed in a ```simulated_robot``` environment. " - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "210d1e00", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:49.516973Z", - "start_time": "2024-06-28T08:38:48.999223Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import MoveTorsoAction\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "torso_pose = 0.2\n", - "\n", - "torso_desig = MoveTorsoAction([torso_pose]).resolve()\n", - "\n", - "with simulated_robot:\n", - " torso_desig.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "ea823840", - "metadata": {}, - "source": [ - "## Set Gripper\n", - "As the name implies, this action designator is used to open or close the gripper. \n", - "\n", - "The procedure is similar to the last time, but this time we will shorten it a bit." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "c3aafa52", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:50.042242Z", - "start_time": "2024-06-28T08:38:49.517822Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import SetGripperAction\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import GripperState, Arms\n", - "\n", - "gripper = Arms.RIGHT\n", - "motion = GripperState.OPEN\n", - "\n", - "with simulated_robot:\n", - " SetGripperAction(grippers=[gripper], motions=[motion]).resolve().perform()" - ] - }, - { - "cell_type": "markdown", - "id": "30797081", - "metadata": {}, - "source": [ - "## Park Arms\n", - "Park arms is used to move one or both arms into the default parking position." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "af0f1507", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:50.568722Z", - "start_time": "2024-06-28T08:38:50.043663Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import ParkArmsAction\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import Arms\n", - "\n", - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()" - ] - }, - { - "cell_type": "markdown", - "id": "1998c17a", - "metadata": {}, - "source": [ - "## Pick Up and Place\n", - "Since these two are dependent on each other, meaning you can only place something when you picked it up beforehand, they will be shown together. \n", - "\n", - "These action designators use object designators, which will not be further explained in this tutorial so please check the example on object designators for more details. \n", - "\n", - "To start we need an environment in which we can pick up and place things as well as an object to pick up." - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "0ccc84d4", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:50.735382Z", - "start_time": "2024-06-28T08:38:50.569774Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "\n", - "world.reset_world()" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "41fd7a83", - "metadata": { - "ExecuteTime": { - "end_time": "2024-06-28T08:38:56.010350Z", - "start_time": "2024-06-28T08:38:50.737096Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1719565072.914777]: Waiting for IK service: /pr2_right_arm_kinematics/get_ik\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import PickUpAction, PlaceAction, ParkArmsAction, MoveTorsoAction, NavigateAction\n", - "from pycram.designators.object_designator import BelieveObject\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import Arms, Grasp\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "milk_desig = BelieveObject(names=[\"milk\"])\n", - "arm = Arms.RIGHT\n", - "\n", - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " \n", - " MoveTorsoAction([0.3]).resolve().perform()\n", - " \n", - " NavigateAction([Pose([0.78, 1, 0.0], \n", - " [0.0, 0.0, 0.014701099828940344, 0.9998919329926708])]).resolve().perform()\n", - " \n", - " PickUpAction(object_designator_description=milk_desig, \n", - " arms=[arm], \n", - " grasps=[Grasp.RIGHT]).resolve().perform()\n", - " \n", - " NavigateAction([Pose([-1.90, 0.78, 0.0], \n", - " [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]).resolve().perform()\n", - " \n", - " PlaceAction(object_designator_description=milk_desig, \n", - " target_locations=[Pose([-1.20, 1.0192, 0.9624], \n", - " #[0.0, 0.0, 0.6339889056055381, 0.7733421413379024])], \n", - " [0, 0, 0, 1])],\n", - " arms=[arm]).resolve().perform()" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "29c836b1", - "metadata": {}, - "outputs": [], - "source": [ - "world.reset_world()" - ] - }, - { - "cell_type": "markdown", - "id": "0147c458", - "metadata": {}, - "source": [ - "## Look At\n", - "Look at lets the robot look at a specific point, for example if it should look at an object for detecting. " - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "89ac737d", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import LookAtAction\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "target_location = Pose([1, 0, 0.5], [0, 0, 0, 1])\n", - "with simulated_robot:\n", - " LookAtAction(targets=[target_location]).resolve().perform()" - ] - }, - { - "cell_type": "markdown", - "id": "fec0c378", - "metadata": {}, - "source": [ - "## Detect\n", - "Detect is used to detect objects in the field of vision (FOV) of the robot. We will use the milk used in the pick up/place example, if you didn't execute that example you can spawn the milk with the following cell. The detect designator will return a resolved instance of an ObjectDesignatorDescription. " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "59dc7f2c", - "metadata": {}, - "outputs": [], - "source": [ - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "a57a5ab2", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ObjectDesignatorDescription.Object(name=milk, obj_type=ObjectType.MILK, world_object=Object(_saved_states={}, \n", - "id=4, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719565030\n", - " nsecs: 374893903\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719565081\n", - " nsecs: 656339883\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719565030\n", - " nsecs: 374893903\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719565081\n", - " nsecs: 656339883\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>, pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719565081\n", - " nsecs: 656339883\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0)\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import DetectAction, LookAtAction, ParkArmsAction, NavigateAction\n", - "from pycram.designators.object_designator import BelieveObject\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "milk_desig = BelieveObject(names=[\"milk\"])\n", - "\n", - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " \n", - " NavigateAction([Pose([0, 1, 0], [0, 0, 0, 1])]).resolve().perform()\n", - " \n", - " LookAtAction(targets=[milk_desig.resolve().pose]).resolve().perform()\n", - " \n", - " obj_desig = DetectAction(milk_desig).resolve().perform()\n", - " \n", - " print(obj_desig)" - ] - }, - { - "cell_type": "markdown", - "id": "0a6f5da9", - "metadata": {}, - "source": [ - "## Transporting\n", - "Transporting can transport an object from its current position to another target position. It is similar to the Pick and Place plan used in the Pick-up and Place example. Since we need an Object which we can transport we spawn a milk, you don't need to do this if you already have spawned it in a previous example." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "9473f6f4", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "eadf97ac", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.designators.object_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "\n", - "milk_desig = BelieveObject(names=[\"milk\"])\n", - "\n", - "description = TransportAction(milk_desig,\n", - " [Arms.LEFT],\n", - " [Pose([-1.35, 0.78, 0.95],\n", - " [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])])\n", - "with simulated_robot:\n", - " MoveTorsoAction([0.2]).resolve().perform()\n", - " description.resolve().perform()" - ] - }, - { - "cell_type": "markdown", - "id": "a90e1e40", - "metadata": {}, - "source": [ - "## Opening\n", - "Opening allows the robot to open a drawer, the drawer is identified by an ObjectPart designator which describes the handle of the drawer that should be grasped. \n", - "\n", - "For the moment this designator works only in the apartment environment, therefore we remove the kitchen and spawn the apartment." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "d1619edc", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen.remove()" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "0748a5b2", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n", - "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n" - ] - } - ], - "source": [ - "apartment = Object(\"apartment\", ObjectType.ENVIRONMENT, \"apartment.urdf\")" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "42baa2dc", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.designators.object_designator import *\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "apartment_desig = BelieveObject(names=[\"apartment\"]).resolve()\n", - "handle_deisg = ObjectPart(names=[\"handle_cab10_t\"], part_of=apartment_desig)\n", - "\n", - "with simulated_robot:\n", - " MoveTorsoAction([0.25]).resolve().perform()\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " NavigateAction([Pose([1.7474915981292725, 2.6873629093170166, 0.0],\n", - " [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()\n", - " OpenAction(handle_deisg, [Arms.RIGHT]).resolve().perform()" - ] - }, - { - "cell_type": "markdown", - "id": "645b3b21", - "metadata": {}, - "source": [ - "## Closing\n", - "Closing lets the robot close an open drawer, like opening the drawer is identified by an ObjectPart designator describing the handle to be grasped. \n", - "\n", - "This action designator only works in the apartment environment for the moment, therefore we remove the kitchen and spawn the apartment. Additionally, we open the drawer such that we can close it with the action designator." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "0acd9e2d", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen.remove()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "63728b78", - "metadata": {}, - "outputs": [], - "source": [ - "apartment = Object(\"apartment\", ObjectType.ENVIRONMENT, \"apartment.urdf\")\n", - "apartment.set_joint_state(\"cabinet10_drawer_top_joint\", 0.4)" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "b82707b1", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.designators.object_designator import *\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "apartment_desig = BelieveObject(names=[\"apartment\"]).resolve()\n", - "handle_deisg = ObjectPart(names=[\"handle_cab10_t\"], part_of=apartment_desig)\n", - "\n", - "with simulated_robot:\n", - " MoveTorsoAction([0.25]).resolve().perform()\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " NavigateAction([Pose([1.7474915981292725, 2.8073629093170166, 0.0],\n", - " [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()\n", - " CloseAction(handle_deisg, [Arms.RIGHT]).resolve().perform()" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "a34f16df", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "shutdown request: [/pycram] Reason: new node registered with same name\n" - ] - } - ], - "source": [ - "world.exit()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/action_designator.md b/examples/action_designator.md new file mode 100644 index 000000000..4988e4f8b --- /dev/null +++ b/examples/action_designator.md @@ -0,0 +1,333 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + + + +# Action Designator + +This example will show the different kinds of Action Designators that are available. We will see how to create Action +Designators and what they do. + +Action Designators are high-level descriptions of actions which the robot should execute. + +Action Designators are created from an Action Designator Description, which describes the type of action as well as the +parameter for this action. Parameter are given as a list of possible parameters. +For example, if you want to describe the robot moving to a table you would need a +{meth}`~pycram.designators.action_designator.NavigateAction` and a list of poses that are near the table. The Action +Designator Description will then pick one of the poses and return a performable Action Designator which contains the +picked pose. + + + + +## Navigate Action + +We will start with a simple example of the {meth}`~pycram.designators.action_designator.NavigateAction`. + +First, we need a BulletWorld with a robot. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import ObjectType, WorldMode + +world = BulletWorld(WorldMode.GUI) +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +``` + +To move the robot we need to create a description and resolve it to an actual Designator. The description of navigation +only needs a list of possible poses. + +```python +from pycram.designators.action_designator import NavigateAction +from pycram.datastructures.pose import Pose + +pose = Pose([1, 0, 0], [0, 0, 0, 1]) + +# This is the Designator Description +navigate_description = NavigateAction(target_locations=[pose]) + +# This is the performable Designator +navigate_designator = navigate_description.resolve() +``` + +What we now did was: create the pose where we want to move the robot, create a description describing a navigation with +a list of possible poses (in this case the list contains only one pose) and create an action designator from the +description. The action designator contains the pose picked from the list of possible poses and can be performed. + +```python +from pycram.process_module import simulated_robot + +with simulated_robot: + navigate_designator.perform() +``` + +Every designator that is performed needs to be in an environment that specifies where to perform the designator either +on the real robot or the simulated one. This environment is called {meth}`~pycram.process_module.simulated_robot` similar there is also +a {meth}`~pycram.process_module.real_robot` environment. + +There are also decorators which do the same thing but for whole methods, they are called {meth}`~pycram.process_module.with_real_robot` +and {meth}`~pycram.process_module.with_simulated_robot`. + +## Move Torso + +This action designator moves the torso up or down, specifically it sets the torso joint to a given value. + +We start again by creating a description and resolving it to a designator. Afterwards, the designator is performed in +a {meth}`~pycram.process_module.simulated_robot` environment. + +```python +from pycram.designators.action_designator import MoveTorsoAction +from pycram.process_module import simulated_robot + +torso_pose = 0.2 + +torso_desig = MoveTorsoAction([torso_pose]).resolve() + +with simulated_robot: + torso_desig.perform() +``` + +## Set Gripper + +As the name implies, this action designator is used to open or close the gripper. + +The procedure is similar to the last time, but this time we will shorten it a bit. + +```python +from pycram.designators.action_designator import SetGripperAction +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import GripperState, Arms + +gripper = Arms.RIGHT +motion = GripperState.OPEN + +with simulated_robot: + SetGripperAction(grippers=[gripper], motions=[motion]).resolve().perform() +``` + +## Park Arms + +Park arms is used to move one or both arms into the default parking position. + +```python +from pycram.designators.action_designator import ParkArmsAction +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import Arms + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() +``` + +## Pick Up and Place + +Since these two are dependent on each other, meaning you can only place something when you picked it up beforehand, they +will be shown together. + +These action designators use object designators, which will not be further explained in this tutorial so please check +the example on object designators for more details. + +To start we need an environment in which we can pick up and place things as well as an object to pick up. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + +world.reset_world() +``` + +```python +from pycram.designators.action_designator import PickUpAction, PlaceAction, ParkArmsAction, MoveTorsoAction, + +NavigateAction +from pycram.designators.object_designator import BelieveObject +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import Arms, Grasp +from pycram.datastructures.pose import Pose + +milk_desig = BelieveObject(names=["milk"]) +arm = Arms.RIGHT + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.3]).resolve().perform() + + NavigateAction([Pose([0.78, 1, 0.0], + [0.0, 0.0, 0.014701099828940344, 0.9998919329926708])]).resolve().perform() + + PickUpAction(object_designator_description=milk_desig, + arms=[arm], + grasps=[Grasp.RIGHT]).resolve().perform() + + NavigateAction([Pose([-1.90, 0.78, 0.0], + [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]).resolve().perform() + + PlaceAction(object_designator_description=milk_desig, + target_locations=[Pose([-1.20, 1.0192, 0.9624], + # [0.0, 0.0, 0.6339889056055381, 0.7733421413379024])], + [0, 0, 0, 1])], + arms=[arm]).resolve().perform() +``` + +```python +world.reset_world() +``` + +## Look At + +Look at lets the robot look at a specific point, for example if it should look at an object for detecting. + +```python +from pycram.designators.action_designator import LookAtAction +from pycram.process_module import simulated_robot +from pycram.datastructures.pose import Pose + +target_location = Pose([1, 0, 0.5], [0, 0, 0, 1]) +with simulated_robot: + LookAtAction(targets=[target_location]).resolve().perform() +``` + +## Detect + +Detect is used to detect objects in the field of vision (FOV) of the robot. We will use the milk used in the pick +up/place example, if you didn't execute that example you can spawn the milk with the following cell. The detect +designator will return a resolved instance of an ObjectDesignatorDescription. + +```python +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +``` + +```python +from pycram.designators.action_designator import DetectAction, LookAtAction, ParkArmsAction, NavigateAction +from pycram.designators.object_designator import BelieveObject +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot +from pycram.datastructures.pose import Pose + +milk_desig = BelieveObject(names=["milk"]) + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + NavigateAction([Pose([0, 1, 0], [0, 0, 0, 1])]).resolve().perform() + + LookAtAction(targets=[milk_desig.resolve().pose]).resolve().perform() + + obj_desig = DetectAction(milk_desig).resolve().perform() + + print(obj_desig) +``` + +## Transporting + +Transporting can transport an object from its current position to another target position. It is similar to the Pick and +Place plan used in the Pick-up and Place example. Since we need an Object which we can transport we spawn a milk, you +don't need to do this if you already have spawned it in a previous example. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +``` + +```python +from pycram.designators.action_designator import * +from pycram.designators.object_designator import * +from pycram.process_module import simulated_robot +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms + +milk_desig = BelieveObject(names=["milk"]) + +description = TransportAction(milk_desig, + [Arms.LEFT], + [Pose([-1.35, 0.78, 0.95], + [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]) +with simulated_robot: + MoveTorsoAction([0.2]).resolve().perform() + description.resolve().perform() +``` + +## Opening + +Opening allows the robot to open a drawer, the drawer is identified by an ObjectPart designator which describes the +handle of the drawer that should be grasped. + +For the moment this designator works only in the apartment environment, therefore we remove the kitchen and spawn the +apartment. + +```python +kitchen.remove() +``` + +```python +apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") +``` + +```python +from pycram.designators.action_designator import * +from pycram.designators.object_designator import * +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot +from pycram.datastructures.pose import Pose + +apartment_desig = BelieveObject(names=["apartment"]).resolve() +handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig) + +with simulated_robot: + MoveTorsoAction([0.25]).resolve().perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + NavigateAction([Pose([1.7474915981292725, 2.6873629093170166, 0.0], + [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform() + OpenAction(handle_deisg, [Arms.RIGHT]).resolve().perform() +``` + +## Closing + +Closing lets the robot close an open drawer, like opening the drawer is identified by an ObjectPart designator +describing the handle to be grasped. + +This action designator only works in the apartment environment for the moment, therefore we remove the kitchen and spawn +the apartment. Additionally, we open the drawer such that we can close it with the action designator. + +```python +kitchen.remove() +``` + +```python +apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") +apartment.set_joint_state("cabinet10_drawer_top_joint", 0.4) +``` + +```python +from pycram.designators.action_designator import * +from pycram.designators.object_designator import * +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot +from pycram.datastructures.pose import Pose + +apartment_desig = BelieveObject(names=["apartment"]).resolve() +handle_deisg = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig) + +with simulated_robot: + MoveTorsoAction([0.25]).resolve().perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + NavigateAction([Pose([1.7474915981292725, 2.8073629093170166, 0.0], + [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform() + CloseAction(handle_deisg, [Arms.RIGHT]).resolve().perform() +``` + +```python +world.exit() +``` diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb deleted file mode 100644 index 76aea07ab..000000000 --- a/examples/bullet_world.ipynb +++ /dev/null @@ -1,566 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "bf7aa008", - "metadata": {}, - "source": [ - "# Bullet World\n", - "This Notebook will show you the basics of working with the PyCRAM BulletWorld.\n", - "\n", - "First we need to import and create a BulletWorld." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "06ccb17f", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-01T16:28:16.307401805Z", - "start_time": "2024-02-01T16:28:15.499235438Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Failed to import Giskard messages\n", - "Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "pybullet build time: Nov 28 2023 23:51:11\n" - ] - } - ], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import ObjectType, WorldMode\n", - "\n", - "world = BulletWorld(mode=WorldMode.GUI)" - ] - }, - { - "cell_type": "markdown", - "id": "fcd82896", - "metadata": {}, - "source": [ - "This new window is the BulletWorld, PyCRAMs internal physics simulation. You can use the mouse to move the camera around:\n", - "\n", - " * Press the left mouse button to rotate the camera\n", - " * Press the right mouse button to move the camera \n", - " * Press the middle mouse button (scroll wheel) and move the mouse up or down to zoom\n", - " \n", - "At the moment the BulletWorld only contains a floor, this is spawned by default when creating the BulletWorld. Furthermore, the gravity is set to 9.8 m^2, which is the same gravitation as the one on earth. \n", - " " - ] - }, - { - "cell_type": "markdown", - "id": "2daa8ee9", - "metadata": {}, - "source": [ - "To close the BulletWorld again please use the ```exit``` method since it will also terminate threads running in the background" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "5c650abe", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "85097a03", - "metadata": {}, - "source": [ - "To spawn new things in the BulletWorld we need to import the Object class and create and instance of it. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "5352b30b", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-01T16:14:50.629562309Z", - "start_time": "2024-02-01T16:14:50.621645448Z" - } - }, - "outputs": [], - "source": [ - "from pycram.world_concepts.world_object import Object\n", - "\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([0, 0, 1]))" - ] - }, - { - "cell_type": "markdown", - "id": "39581961", - "metadata": {}, - "source": [ - "As you can see this spawns a milk floating in the air. What we did here was create a new Object which has the name \"milk\" as well as the type ```ObjectType.MILK ``` , is spawned from the file \"milk.stl\" and is at the position [0, 0, 1]. \n", - "\n", - "The type of an Object can either be from the enum ObjectType or a string. However, it is recommended to use the enum since this would make for a more consistent naming of types which makes it easier to work with types. But since the types of the enum might not fit your case you can also use strings. \n", - "\n", - "The first three of these parameters are required while the position is optional. As you can see it was sufficient to only specify the filename for PyCRAM to spawn the milk mesh. When only providing a filename, PyCRAM will search in its resource directory for a matching file and use it. \n", - "\n", - "For a complete list of all parameters that can be used to crate an Object please check the documentation. \n", - "\n", - "\n", - "\n", - "Since the Object is spawned, we can now interact with it. First we want to move it around and change its orientation" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "a0812110", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:29:23.208599020Z", - "start_time": "2024-01-25T18:29:23.206209707Z" - } - }, - "outputs": [], - "source": [ - "milk.set_position(Pose([1, 1, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "ada6535a", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:29:23.675871190Z", - "start_time": "2024-01-25T18:29:23.673804147Z" - } - }, - "outputs": [], - "source": [ - "milk.set_orientation(Pose(orientation=[1,0, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "d4fabab5", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:29:24.180713247Z", - "start_time": "2024-01-25T18:29:24.175368603Z" - } - }, - "outputs": [], - "source": [ - "milk.set_pose(Pose([0, 0, 1], [0, 0, 0, 1]))" - ] - }, - { - "cell_type": "markdown", - "id": "9fb6d22f", - "metadata": {}, - "source": [ - "In the same sense as setting the position or orientation, you can also get the position and orientation." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "1543b134", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:29:58.869360443Z", - "start_time": "2024-01-25T18:29:58.827502541Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Position: \n", - "x: 0.0\n", - "y: 0.0\n", - "z: 1.0\n", - "Orientation: \n", - "x: 0.0\n", - "y: 0.0\n", - "z: 0.0\n", - "w: 1.0\n", - "Pose: \n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706207364\n", - " nsecs: 174507617\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "print(f\"Position: \\n{milk.get_position()}\")\n", - "\n", - "print(f\"Orientation: \\n{milk.get_orientation()}\")\n", - "\n", - "print(f\"Pose: \\n{milk.get_pose()}\")" - ] - }, - { - "cell_type": "markdown", - "id": "a7e5eb21", - "metadata": {}, - "source": [ - "## Attachments\n", - "You can attach Objects to each other simply by calling the attach method on one of them and providing the other as parameter. Since attachments are bi-directional it doesn't matter on which Object you call the method. \n", - "\n", - "First we need another Object" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "bf7206d8", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:02.056106044Z", - "start_time": "2024-01-25T18:30:02.001333744Z" - } - }, - "outputs": [], - "source": [ - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "6e227cbe", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:03.288290549Z", - "start_time": "2024-01-25T18:30:03.283992204Z" - } - }, - "outputs": [], - "source": [ - "milk.attach(cereal)" - ] - }, - { - "cell_type": "markdown", - "id": "0ee3fc9c", - "metadata": {}, - "source": [ - "Now since they are attached to each other, if we move one of them the other will move in conjunction." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "f3b96872", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:05.029258354Z", - "start_time": "2024-01-25T18:30:05.026329993Z" - } - }, - "outputs": [], - "source": [ - "milk.set_position(Pose([1,1,1]))" - ] - }, - { - "cell_type": "markdown", - "id": "e988f6fd", - "metadata": {}, - "source": [ - "In the same way the Object can also be detached, just call the detach method on one of the two attached Objects." - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "03222f62", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:06.737363899Z", - "start_time": "2024-01-25T18:30:06.734694119Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Removing constraint with id: 1\n" - ] - } - ], - "source": [ - "cereal.detach(milk)" - ] - }, - { - "cell_type": "markdown", - "id": "45ce1a82", - "metadata": {}, - "source": [ - "## Links and Joints\n", - "Objects spawned from mesh files do not have links or joints, but if you spawn things from a URDF like a robot they will have a lot of links and joints. Every Object has two dictionaries as attributes, namely ```links``` and ```joints``` which contain every link or joint as key and a unique id, used by PyBullet, as value. \n", - "\n", - "We will see this at the example of the PR2:" - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "aa315f79", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:13.548802172Z", - "start_time": "2024-01-25T18:30:13.540787905Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{'base_link': , 'base_footprint': , 'base_bellow_link': , 'base_laser_link': , 'fl_caster_rotation_link': , 'fl_caster_l_wheel_link': , 'fl_caster_r_wheel_link': , 'fr_caster_rotation_link': , 'fr_caster_l_wheel_link': , 'fr_caster_r_wheel_link': , 'bl_caster_rotation_link': , 'bl_caster_l_wheel_link': , 'bl_caster_r_wheel_link': , 'br_caster_rotation_link': , 'br_caster_l_wheel_link': , 'br_caster_r_wheel_link': , 'torso_lift_link': , 'l_torso_lift_side_plate_link': , 'r_torso_lift_side_plate_link': , 'torso_lift_motor_screw_link': , 'imu_link': , 'head_pan_link': , 'head_tilt_link': , 'head_plate_frame': , 'sensor_mount_link': , 'high_def_frame': , 'high_def_optical_frame': , 'double_stereo_link': , 'wide_stereo_link': , 'wide_stereo_optical_frame': , 'wide_stereo_l_stereo_camera_frame': , 'wide_stereo_l_stereo_camera_optical_frame': , 'wide_stereo_r_stereo_camera_frame': , 'wide_stereo_r_stereo_camera_optical_frame': , 'narrow_stereo_link': , 'narrow_stereo_optical_frame': , 'narrow_stereo_l_stereo_camera_frame': , 'narrow_stereo_l_stereo_camera_optical_frame': , 'narrow_stereo_r_stereo_camera_frame': , 'narrow_stereo_r_stereo_camera_optical_frame': , 'projector_wg6802418_frame': , 'projector_wg6802418_child_frame': , 'laser_tilt_mount_link': , 'laser_tilt_link': , 'r_shoulder_pan_link': , 'r_shoulder_lift_link': , 'r_upper_arm_roll_link': , 'r_upper_arm_link': , 'r_forearm_roll_link': , 'r_elbow_flex_link': , 'r_forearm_link': , 'r_wrist_flex_link': , 'r_wrist_roll_link': , 'r_gripper_palm_link': , 'r_gripper_led_frame': , 'r_gripper_motor_accelerometer_link': , 'r_gripper_tool_frame': , 'r_gripper_motor_slider_link': , 'r_gripper_motor_screw_link': , 'r_gripper_l_finger_link': , 'r_gripper_r_finger_link': , 'r_gripper_l_finger_tip_link': , 'r_gripper_r_finger_tip_link': , 'r_gripper_l_finger_tip_frame': , 'l_shoulder_pan_link': , 'l_shoulder_lift_link': , 'l_upper_arm_roll_link': , 'l_upper_arm_link': , 'l_forearm_roll_link': , 'l_elbow_flex_link': , 'l_forearm_link': , 'l_wrist_flex_link': , 'l_wrist_roll_link': , 'l_gripper_palm_link': , 'l_gripper_led_frame': , 'l_gripper_motor_accelerometer_link': , 'l_gripper_tool_frame': , 'l_gripper_motor_slider_link': , 'l_gripper_motor_screw_link': , 'l_gripper_l_finger_link': , 'l_gripper_r_finger_link': , 'l_gripper_l_finger_tip_link': , 'l_gripper_r_finger_tip_link': , 'l_gripper_l_finger_tip_frame': , 'l_forearm_cam_frame': , 'l_forearm_cam_optical_frame': , 'r_forearm_cam_frame': , 'r_forearm_cam_optical_frame': }\n" - ] - } - ], - "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "print(pr2.links)" - ] - }, - { - "cell_type": "markdown", - "id": "472d5bf6", - "metadata": {}, - "source": [ - "For links there are similar methods available as for the pose. However, you can only **get** the position and orientation of a link. " - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "d7899a0a", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:26.283263579Z", - "start_time": "2024-01-25T18:30:26.259577324Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Position: \n", - "x: -0.05000000447034836\n", - "y: 0.0\n", - "z: 0.7906750440597534\n", - "Orientation: \n", - "x: 0.0\n", - "y: 0.0\n", - "z: 0.0\n", - "w: 1.0\n", - "Pose: \n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706207426\n", - " nsecs: 257559537\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.05000000447034836\n", - " y: 0.0\n", - " z: 0.7906750440597534\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n", - "\n", - "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n", - "\n", - "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")" - ] - }, - { - "cell_type": "markdown", - "id": "7b7a16fa", - "metadata": {}, - "source": [ - "Methods available for joints are:\n", - "\n", - " * ```get_joint_state```\n", - " * ```set_joint_state```\n", - " * ```get_joint_limits```\n", - " \n", - "We will see how these methods work at the example of the torso_lift_joint:" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "1706dcdd", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:51.633276335Z", - "start_time": "2024-01-25T18:30:51.590393104Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Joint limits: (0.0, 0.33)\n", - "Current Joint state: 0.0\n", - "New Joint state: 0.2\n" - ] - } - ], - "source": [ - "print(f\"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}\")\n", - "\n", - "print(f\"Current Joint state: {pr2.get_joint_position('torso_lift_joint')}\")\n", - "\n", - "pr2.set_joint_position(\"torso_lift_joint\", 0.2)\n", - "\n", - "print(f\"New Joint state: {pr2.get_joint_position('torso_lift_joint')}\")" - ] - }, - { - "cell_type": "markdown", - "id": "959b16e5", - "metadata": {}, - "source": [ - "## Misc Methods\n", - "There are a few methods that don't fit any category but could be helpful anyway. The first two are ```get_color``` and ```set_color```, as the name implies they can be used to get or set the color for specific links or the whole Object. " - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "2c8ed9b2", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:57.200886492Z", - "start_time": "2024-01-25T18:30:57.198278714Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Pr2 forearm color: Color(R=0.699999988079071, G=0.699999988079071, B=0.699999988079071, A=1.0)\n" - ] - } - ], - "source": [ - "print(f\"Pr2 forearm color: {pr2.get_link_color('r_forearm_link')}\")" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "37656d79", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:30:59.531448130Z", - "start_time": "2024-01-25T18:30:59.523750241Z" - } - }, - "outputs": [], - "source": [ - "pr2.set_link_color(\"r_forearm_link\", [1, 0, 0])" - ] - }, - { - "cell_type": "markdown", - "id": "911a49f7", - "metadata": {}, - "source": [ - "Lastly, there is ```get_AABB```, AABB stands for *A*xis *A*ligned *B*ounding *B*ox. This method returns two points in world coordinates which span a rectangle representing the AABB." - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "63433513", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-25T18:31:05.700993547Z", - "start_time": "2024-01-25T18:31:05.677436085Z" - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "AxisAlignedBoundingBox(min_x=-0.0015000000000000005, min_y=-0.0015000000000000005, min_z=0.06949999999999999, max_x=0.0015000000000000005, max_y=0.0015000000000000005, max_z=0.0725)" - ] - }, - "execution_count": 16, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "pr2.get_axis_aligned_bounding_box()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/bullet_world.md b/examples/bullet_world.md new file mode 100644 index 000000000..b8903f248 --- /dev/null +++ b/examples/bullet_world.md @@ -0,0 +1,181 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Bullet World + +This Notebook will show you the basics of working with the PyCRAM BulletWorld. + +First we need to import and create a BulletWorld. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType, WorldMode + +world = BulletWorld(mode=WorldMode.GUI) +``` + +This new window is the BulletWorld, PyCRAMs internal physics simulation. You can use the mouse to move the camera +around: + +* Press the left mouse button to rotate the camera +* Press the right mouse button to move the camera +* Press the middle mouse button (scroll wheel) and move the mouse up or down to zoom + +At the moment the BulletWorld only contains a floor, this is spawned by default when creating the BulletWorld. +Furthermore, the gravity is set to 9.8 $m^2$, which is the same gravitation as the one on earth. + +To spawn new things in the BulletWorld we need to import the Object class and create and instance of it. + +```python +from pycram.world_concepts.world_object import Object + +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([0, 0, 1])) +``` + + +As you can see this spawns a milk floating in the air. What we did here was create a new Object which has the name " +milk" as well as the type {attr}`~pycram.datastructures.enums.ObjectType.MILK`, is spawned from the file "milk.stl" and is at the position [0, 0, 1]. + +The type of an Object can either be from the enum ObjectType or a string. However, it is recommended to use the enum +since this would make for a more consistent naming of types which makes it easier to work with types. But since the +types of the enum might not fit your case you can also use strings. + +The first three of these parameters are required while the position is optional. As you can see it was sufficient to +only specify the filename for PyCRAM to spawn the milk mesh. When only providing a filename, PyCRAM will search in its +resource directory for a matching file and use it. + +For a complete list of all parameters that can be used to crate an Object please check the documentation. + +Since the Object is spawned, we can now interact with it. First we want to move it around and change its orientation + + +```python +milk.set_position(Pose([1, 1, 1])) +``` + +```python +milk.set_orientation(Pose(orientation=[1, 0, 0, 1])) +``` + +```python +milk.set_pose(Pose([0, 0, 1], [0, 0, 0, 1])) +``` + +In the same sense as setting the position or orientation, you can also get the position and orientation. + +```python +print(f"Position: \n{milk.get_position()}") + +print(f"Orientation: \n{milk.get_orientation()}") + +print(f"Pose: \n{milk.get_pose()}") +``` + +## Attachments + +You can attach Objects to each other simply by calling the attach method on one of them and providing the other as +parameter. Since attachments are bi-directional it doesn't matter on which Object you call the method. + +First we need another Object + +```python +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1, 0, 1])) +``` + +```python +milk.attach(cereal) +``` + +Now since they are attached to each other, if we move one of them the other will move in conjunction. + +```python +milk.set_position(Pose([1, 1, 1])) +``` + +In the same way the Object can also be detached, just call the detach method on one of the two attached Objects. + +```python +cereal.detach(milk) +``` + +## Links and Joints + +Objects spawned from mesh files do not have links or joints, but if you spawn things from a URDF like a robot they will +have a lot of links and joints. Every Object has two dictionaries as attributes, namely ```links``` and ```joints``` +which contain every link or joint as key and a unique id, used by PyBullet, as value. + +We will see this at the example of the PR2: + +```python +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +print(pr2.links) +``` + +For links there are similar methods available as for the pose. However, you can only **get** the position and +orientation of a link. + +```python +print(f"Position: \n{pr2.get_link_position('torso_lift_link')}") + +print(f"Orientation: \n{pr2.get_link_orientation('torso_lift_link')}") + +print(f"Pose: \n{pr2.get_link_pose('torso_lift_link')}") +``` + +Methods available for joints are: + +* {meth}`~pycram.world_concepts.world_object.Object.get_joint_position` +* {meth}`~pycram.world_concepts.world_object.Object.set_joint_position` +* {meth}`~pycram.world_concepts.world_object.Object.get_joint_limits` + +We will see how these methods work at the example of the torso_lift_joint: + +```python +print(f"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}") + +print(f"Current Joint state: {pr2.get_joint_position('torso_lift_joint')}") + +pr2.set_joint_position("torso_lift_joint", 0.2) + +print(f"New Joint state: {pr2.get_joint_position('torso_lift_joint')}") +``` + +## Misc Methods + +There are a few methods that don't fit any category but could be helpful anyway. The first two are {meth}`~pycram.description.Link.get_color` +and {meth}`~pycram.description.Link.set_color`, as the name implies they can be used to get or set the color for specific links or the whole +Object. + +```python +print(f"Pr2 forearm color: {pr2.get_link_color('r_forearm_link')}") +``` + +```python +pr2.set_link_color("r_forearm_link", [1, 0, 0]) +``` + +Lastly, there is {meth}`~pycram.description.Link.get_axis_aligned_bounding_box`, AABB stands for *A*xis *A*ligned *B*ounding *B*ox. This method returns two points in +world coordinates which span a rectangle representing the AABB. + +```python +pr2.get_axis_aligned_bounding_box() +``` + +To close the BulletWorld again please use the {meth}`~pycram.datastructures.world.World.exit` method since it will also terminate threads running in the +background + +```python +world.exit() +``` diff --git a/examples/cram_plan_tutorial.ipynb b/examples/cram_plan_tutorial.ipynb deleted file mode 100644 index d7d35c54d..000000000 --- a/examples/cram_plan_tutorial.ipynb +++ /dev/null @@ -1,763 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# TaskTree Tutorial\n", - "\n", - "In this tutorial we will walk through the capabilities of task trees in pycram.\n", - "\n", - "First we have to import the necessary functionality from pycram." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T16:11:39.975025Z", - "start_time": "2023-04-20T16:11:39.974924Z" - }, - "collapsed": true - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "pybullet build time: Sep 20 2021 20:33:29\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - } - ], - "source": [ - "from pycram.bullet_world import BulletWorld\n", - "from pycram.robot_descriptions import robot_description\n", - "import pycram.task\n", - "from pycram.resolver.plans import Arms\n", - "from pycram.designators.action_designator import *\n", - "from pycram.designators.location_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.designators.object_designator import *\n", - "import anytree\n", - "import pycram.plan_failures" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T16:11:55.868162Z", - "start_time": "2023-04-20T16:11:55.868029Z" - } - }, - "outputs": [], - "source": [ - "world = BulletWorld()\n", - "robot = Object(robot_description.name, \"robot\", robot_description.name + \".urdf\")\n", - "robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve()\n", - "apartment = Object(\"apartment\", \"environment\", \"/home/abassi/cram_ws/src/iai_maps/iai_apartment/urdf/apartment.urdf\", position=[-1.5, -2.5, 0])\n", - "apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve()\n", - "table_top = apartment.get_link_position(\"cooktop\")\n", - "# milk = Object(\"milk\", \"milk\", \"milk.stl\", position=[table_top[0]-0.15, table_top[1], table_top[2]])\n", - "# milk.set_position(position=milk.get_position(), base=True)\n", - "# cereal = Object(\"cereal\", \"cereal\", \"breakfast_cereal.stl\", position=table_top)\n", - "# cereal.set_position(position=[table_top[0]-0.1, table_top[1] + 0.5, table_top[2]], base=True)\n", - "# milk_desig = ObjectDesignator(ObjectDesignatorDescription(name=\"milk\", type=\"milk\"))\n", - "# cereal_desig = ObjectDesignator(ObjectDesignatorDescription(name=\"cereal\", type=\"cereal\"))" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T16:40:40.071476Z", - "start_time": "2023-04-20T16:40:40.071396Z" - } - }, - "outputs": [], - "source": [ - "import numpy as np\n", - "\n", - "def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):\n", - " positions = [pos[0] for pos in pose_list[:1000]]\n", - " all_indices = list(range(len(positions)))\n", - " print(len(all_indices))\n", - " pos_idx = np.random.choice(all_indices) if random else all_indices[0]\n", - " all_indices.remove(pos_idx)\n", - " n_positions = np.zeros((n,3))\n", - " for i in range(n):\n", - " n_positions[i,:] = positions[pos_idx]\n", - " found_count = 1\n", - " found_indices = [pos_idx]\n", - " for i in range(len(positions)-1):\n", - " pos_idx = np.random.choice(all_indices) if random else all_indices[i]\n", - " diff = np.absolute(np.linalg.norm(n_positions - positions[pos_idx], axis=1))\n", - " # print(diff)\n", - " min_diff = np.min(diff)\n", - " # print(min_diff)\n", - " if min_diff >= dist:\n", - " # print(\"found\")\n", - " n_positions[found_count,:] = positions[pos_idx]\n", - " found_indices.append(pos_idx)\n", - " found_count += 1\n", - " all_indices.remove(pos_idx)\n", - " if found_count == n:\n", - " break\n", - " found_poses = [pose_list[i] for i in found_indices]\n", - " # found_positions = [positions[i] for i in found_indices]\n", - " # for i in range(len(found_positions)):\n", - " # print(found_poses[i][0])\n", - " # print(found_positions[i])\n", - " # assert np.allclose(found_positions[i],found_poses[i][0])\n", - " # for i in range(len(found_poses)):\n", - " # for j in range(i+1,len(found_poses)):\n", - " # pos1 = np.array(found_poses[i][0])\n", - " # pos2 = np.array(found_poses[j][0])\n", - " # diff = np.absolute(np.linalg.norm(pos1 - pos2))\n", - " # print(diff)\n", - " # assert diff >= dist\n", - " return found_poses\n", - "\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": 29, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T16:40:54.196536Z", - "start_time": "2023-04-20T16:40:54.196396Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "1000\n", - "bowl\n", - "([1.4647347927093506, -1.3561391830444336, 0.921999990940094], [0.0, 0.0, 0.755535559475718, 0.6551076387645878])\n", - "milk\n", - "([1.3690669536590576, 1.3640167713165283, 0.921999990940094], [-0.0, 0.0, 0.7419867057523215, -0.670414594476297])\n", - "breakfast_cereal\n", - "([0.9295452237129211, 1.664717197418213, 0.921999990940094], [-0.0, 0.0, 0.6296505072306592, -0.7768785225144107])\n", - "spoon\n", - "([1.6883658170700073, 0.9235076308250427, 0.921999990940094], [-0.0, 0.0, 0.866722037404449, -0.4987914492826445])\n", - "[([1.4647347927093506, -1.3561391830444336, 0.921999990940094], [0.0, 0.0, 0.755535559475718, 0.6551076387645878]), ([1.3690669536590576, 1.3640167713165283, 0.921999990940094], [-0.0, 0.0, 0.7419867057523215, -0.670414594476297]), ([0.9295452237129211, 1.664717197418213, 0.921999990940094], [-0.0, 0.0, 0.6296505072306592, -0.7768785225144107]), ([1.6883658170700073, 0.9235076308250427, 0.921999990940094], [-0.0, 0.0, 0.866722037404449, -0.4987914492826445])]\n" - ] - } - ], - "source": [ - "from pycram.costmaps import SemanticCostmap\n", - "from pycram.pose_generator_and_validator import pose_generator\n", - "scm = SemanticCostmap(apartment,\"island_countertop\")\n", - "poses_list = list(pose_generator(scm, number_of_samples=-1))\n", - "poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x[0]))\n", - "object_poses = get_n_random_positions(poses_list)\n", - "object_names = [\"bowl\", \"milk\", \"breakfast_cereal\", \"spoon\"]\n", - "objects = {}\n", - "object_desig = {}\n", - "for obj_name, obj_pose in zip(object_names, object_poses):\n", - " print(obj_name)\n", - " print(obj_pose)\n", - " objects[obj_name] = Object(obj_name, obj_name, obj_name+\".stl\", position=[obj_pose[0][0], obj_pose[0][1], table_top[2]])\n", - " objects[obj_name].move_base_to_origin_pos()\n", - " objects[obj_name].original_pose = objects[obj_name].get_position_and_orientation()\n", - " object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_name]).resolve()\n", - "print(object_poses)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "If You want to visualize all apartment frames" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T15:55:24.448172Z", - "start_time": "2023-04-20T15:55:24.448016Z" - } - }, - "outputs": [], - "source": [ - "import pybullet as p\n", - "for link_name in apartment.links.keys():\n", - " world.add_vis_axis(apartment.get_link_pose(link_name))\n", - " p.addUserDebugText(link_name, apartment.get_link_position(link_name))" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-14T18:10:34.708807Z", - "start_time": "2023-04-14T18:10:23.815102Z" - } - }, - "outputs": [], - "source": [ - "world.remove_vis_axis()\n", - "p.removeAllUserDebugItems()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "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." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-20T15:54:48.890670Z", - "start_time": "2023-04-20T15:54:48.890550Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "\n", - "\n", - "no solution\n", - "\n", - "\n", - "\n", - "no solution\n", - "\n", - "\n", - "\n", - "no solution\n", - "\n", - "\n", - "Position ((1.0674792528152466, -0.059463053941726685, -0.052117444574832916), (-3.810604887188873e-10, 5.370975886265228e-10, 0.9858646988868713, 0.16754356026649475)) in frame 'torso_lift_link' is not reachable for end effector\n", - "no solution\n", - "\n", - "\n", - "no solution\n", - "[0.25, 0.3, 0.3, 0.3]\n" - ] - } - ], - "source": [ - "from pycram.external_interfaces.ik import IKError\n", - "@pycram.task.with_tree\n", - "def plan(obj, obj_desig, torso=0.2, place=\"countertop\"):\n", - " world.reset_bullet_world()\n", - " with simulated_robot:\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - "\n", - " MoveTorsoActionPerformable(torso).perform()\n", - " location = CostmapLocation(target=obj_desig, reachable_for=robot_desig)\n", - " pose = location.resolve()\n", - " print()\n", - " NavigateActionPerformable(pose.pose).perform()\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - " good_torsos.append(torso)\n", - " picked_up_arm = pose.reachable_arms[0]\n", - " PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=\"front\").perform()\n", - "\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - " scm = SemanticCostmapLocation(place, apartment_desig, obj_desig)\n", - " pose_island = scm.resolve()\n", - "\n", - " place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, reachable_arm=picked_up_arm)\n", - " pose = place_location.resolve()\n", - "\n", - " NavigateActionPerformable(pose.pose).perform()\n", - "\n", - " PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()\n", - "\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - "\n", - "good_torsos = []\n", - "for obj_name in object_names:\n", - " done = False\n", - " torso = 0.25 if len(good_torsos) == 0 else good_torsos[-1]\n", - " while not done:\n", - " try:\n", - " plan(objects[obj_name], object_desig[obj_name], torso=torso, place=\"island_countertop\")\n", - " done = True\n", - " objects[obj_name].original_pose = objects[obj_name].get_position_and_orientation()\n", - " except (StopIteration,IKError) as e:\n", - " print(type(e))\n", - " print(e)\n", - " print(\"no solution\")\n", - " torso += 0.05\n", - " if torso > 0.3:\n", - " break\n", - "print(good_torsos)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Now we get the task tree from its module and render it. Rendering can be done with any render method described in the anytree package. We will use ascii rendering here for ease of displaying." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:49:56.924918Z", - "start_time": "2023-04-13T10:49:56.916067Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n", - "+-- plan()\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - " |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - " +-- .park_arms(arms = Arms.BOTH)\n" - ] - } - ], - "source": [ - "tt = pycram.task.task_tree\n", - "print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node. This is done because several, not connected, plans that get executed after each other should still appear in the task tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same tree even though they are not connected." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:04.750336Z", - "start_time": "2023-04-13T10:50:00.022503Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Parking arms Arms.BOTH.\n", - "Moving to [0.7199999690055847, 1.0399999618530273, 0.0]. Orientation: [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606].\n", - "(0.7199999690055847, 1.0399999618530273, 0.0)\n", - "Parking arms Arms.BOTH.\n", - "Picking up ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}) with left.\n", - "Not attached to anything!\n", - "Parking arms Arms.BOTH.\n", - "Moving to [-1.9075000286102295, 0.7792000770568848, 0.0]. Orientation: [0.0, 0.0, 0.16439898301071468, 0.9863939245479175].\n", - "(-1.9075000286102295, 0.7792000770568848, 0.0)\n", - "Placing ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}) with left at [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]].\n", - "Parking arms Arms.BOTH.\n", - "no_operation()\n", - "|-- plan()\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - "| |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - "| +-- .park_arms(arms = Arms.BOTH)\n", - "+-- plan()\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - " |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - " +-- .park_arms(arms = Arms.BOTH)\n" - ] - } - ], - "source": [ - "world.reset_bullet_world()\n", - "plan()\n", - "print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly reset objects are available. At the end of a with block the old state is restored. The root for such things is then called ``simulation()``." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:08.965958Z", - "start_time": "2023-04-13T10:50:08.940548Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "simulation()\n", - "no_operation()\n", - "|-- plan()\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - "| |-- .park_arms(arms = Arms.BOTH)\n", - "| |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - "| |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - "| +-- .park_arms(arms = Arms.BOTH)\n", - "+-- plan()\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - " |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - " +-- .park_arms(arms = Arms.BOTH)\n" - ] - } - ], - "source": [ - "with pycram.task.SimulatedTaskTree() as stt:\n", - " print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))\n", - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we would write:" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:11.726747Z", - "start_time": "2023-04-13T10:50:11.717057Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n", - "+-- plan()\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - " |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - " +-- .park_arms(arms = Arms.BOTH)\n" - ] - } - ], - "source": [ - "tt.root.children = (tt.root.children[0],)\n", - "print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree functionality. This will not append the re-execution to the task tree." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:18.110775Z", - "start_time": "2023-04-13T10:50:14.041891Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Parking arms Arms.BOTH.\n", - "Moving to [0.7199999690055847, 1.0399999618530273, 0.0]. Orientation: [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606].\n", - "(0.7199999690055847, 1.0399999618530273, 0.0)\n", - "Parking arms Arms.BOTH.\n", - "Picking up ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}) with left.\n", - "Not attached to anything!\n", - "Parking arms Arms.BOTH.\n", - "Moving to [-1.9075000286102295, 0.7792000770568848, 0.0]. Orientation: [0.0, 0.0, 0.16439898301071468, 0.9863939245479175].\n", - "(-1.9075000286102295, 0.7792000770568848, 0.0)\n", - "Placing ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}) with left at [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]].\n", - "Parking arms Arms.BOTH.\n", - "no_operation()\n", - "+-- plan()\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [0.7199999690055847, 1.0399999618530273, 0.0], orientation = [-0.0, 0.0, 0.03442140918758331, -0.9994074077119606])\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .pick_up(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), grasp = front)\n", - " |-- .park_arms(arms = Arms.BOTH)\n", - " |-- .navigate(target = [-1.9075000286102295, 0.7792000770568848, 0.0], orientation = [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])\n", - " |-- .place(arm = left, object_desig = ObjectDesignator({'resolver': 'grounding', 'type': 'milk', 'name': 'milk', 'object': , 'pose': ((1.3, 1.0, 0.9), (0.0, 0.0, 0.0, 1.0))}), target = [[-1.2074999809265137, 1.019200086593628, 0.9439613818399623], [0.0, 0.0, 0.6339889056055381, 0.7733421413379024]])\n", - " +-- .park_arms(arms = Arms.BOTH)\n" - ] - } - ], - "source": [ - "world.reset_bullet_world()\n", - "with simulated_robot:\n", - " [node.code.execute() for node in tt.root.leaves]\n", - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Nodes in the task tree contain additional information about the status and time of a task." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:21.074256Z", - "start_time": "2023-04-13T10:50:21.063201Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Code: plan() \n", - " start_time: 2023-04-13 10:49:43.184007 \n", - " Status: TaskStatus.SUCCEEDED \n", - " end_time: 2023-04-13 10:49:47.980515 \n", - " \n" - ] - } - ], - "source": [ - "print(pycram.task.task_tree.children[0])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The task tree can also be reset to an empty one by invoking:" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:23.996623Z", - "start_time": "2023-04-13T10:50:23.988659Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n" - ] - } - ], - "source": [ - "pycram.task.reset_tree()\n", - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "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." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:27.502622Z", - "start_time": "2023-04-13T10:50:27.496138Z" - } - }, - "outputs": [], - "source": [ - "@pycram.task.with_tree\n", - "def failing_plan():\n", - " raise pycram.plan_failures.PlanFailure(\"Oopsie!\")\n", - "\n", - "failing_plan()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can now investigate the nodes of the tree, and we will see that the tree indeed contains a failed task." - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:31.140127Z", - "start_time": "2023-04-13T10:50:31.132328Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n", - "+-- failing_plan()\n", - "Code: failing_plan() \n", - " start_time: 2023-04-13 10:50:27.493840 \n", - " Status: TaskStatus.FAILED \n", - " end_time: 2023-04-13 10:50:27.494815 \n", - " \n" - ] - } - ], - "source": [ - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))\n", - "print(pycram.task.task_tree.children[0])" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-13T10:50:34.484428Z", - "start_time": "2023-04-13T10:50:34.256310Z" - } - }, - "outputs": [ - { - "ename": "NameError", - "evalue": "name 'world' is not defined", - "output_type": "error", - "traceback": [ - "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m", - "\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)", - "Cell \u001B[0;32mIn[1], line 1\u001B[0m\n\u001B[0;32m----> 1\u001B[0m \u001B[43mworld\u001B[49m\u001B[38;5;241m.\u001B[39mexit()\n", - "\u001B[0;31mNameError\u001B[0m: name 'world' is not defined" - ] - } - ], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 1 -} diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md new file mode 100644 index 000000000..7ca106a34 --- /dev/null +++ b/examples/cram_plan_tutorial.md @@ -0,0 +1,276 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# TaskTree Tutorial + +In this tutorial we will walk through the capabilities of task trees in pycram. + +First we have to import the necessary functionality from pycram. + +```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.designators.action_designator import * +from pycram.designators.location_designator import * +from pycram.process_module import simulated_robot +from pycram.designators.object_designator import * +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_desig = ObjectDesignatorDescription(names=['pr2']).resolve() +apartment = Object("apartment", "environment", "apartment.urdf", position=[-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]]) +# milk.set_position(position=milk.get_position(), base=True) +# cereal = Object("cereal", "cereal", "breakfast_cereal.stl", position=table_top) +# cereal.set_position(position=[table_top[0]-0.1, table_top[1] + 0.5, table_top[2]], base=True) +# milk_desig = ObjectDesignator(ObjectDesignatorDescription(name="milk", type="milk")) +# cereal_desig = ObjectDesignator(ObjectDesignatorDescription(name="cereal", type="cereal")) +``` + +```python +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]] + 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)) + for i in range(n): + n_positions[i, :] = positions[pos_idx] + found_count = 1 + found_indices = [pos_idx] + for i in range(len(positions) - 1): + pos_idx = np.random.choice(all_indices) if random else all_indices[i] + diff = np.absolute(np.linalg.norm(n_positions - positions[pos_idx], axis=1)) + # print(diff) + min_diff = np.min(diff) + # print(min_diff) + if min_diff >= dist: + # print("found") + n_positions[found_count, :] = positions[pos_idx] + found_indices.append(pos_idx) + found_count += 1 + all_indices.remove(pos_idx) + if found_count == n: + break + found_poses = [pose_list[i] for i in found_indices] + # found_positions = [positions[i] for i in found_indices] + # for i in range(len(found_positions)): + # print(found_poses[i][0]) + # print(found_positions[i]) + # assert np.allclose(found_positions[i],found_poses[i][0]) + # for i in range(len(found_poses)): + # for j in range(i+1,len(found_poses)): + # pos1 = np.array(found_poses[i][0]) + # pos2 = np.array(found_poses[j][0]) + # diff = np.absolute(np.linalg.norm(pos1 - pos2)) + # print(diff) + # assert diff >= dist + return found_poses + + + +``` + +```python +from pycram.costmaps import SemanticCostmap +from pycram.pose_generator_and_validator import pose_generator + +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])) +object_poses = get_n_random_positions(poses_list) +object_names = ["bowl", "milk", "breakfast_cereal", "spoon"] +objects = {} +object_desig = {} +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() + object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_name]).resolve() +print(object_poses) +``` + +If You want to visualize all apartment frames + +```python +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)) +``` + +```python +world.remove_vis_axis() +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 + + +@pycram.task.with_tree +def plan(obj, obj_desig, torso=0.2, place="countertop"): + world.reset_bullet_world() + with simulated_robot: + ParkArmsActionPerformable(Arms.BOTH).perform() + + MoveTorsoActionPerformable(torso).perform() + location = CostmapLocation(target=obj_desig, reachable_for=robot_desig) + pose = location.resolve() + print() + NavigateActionPerformable(pose.pose).perform() + 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() + + ParkArmsActionPerformable(Arms.BOTH).perform() + scm = SemanticCostmapLocation(place, apartment_desig, obj_desig) + pose_island = scm.resolve() + + place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, + reachable_arm=picked_up_arm) + pose = place_location.resolve() + + NavigateActionPerformable(pose.pose).perform() + + PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, + arm=picked_up_arm).perform() + + ParkArmsActionPerformable(Arms.BOTH).perform() + + +good_torsos = [] +for obj_name in object_names: + done = False + torso = 0.25 if len(good_torsos) == 0 else good_torsos[-1] + while not done: + 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() + except (StopIteration, IKError) as e: + print(type(e)) + print(e) + print("no solution") + torso += 0.05 + if torso > 0.3: + break +print(good_torsos) +``` + +Now we get the task tree from its module and render it. Rendering can be done with any render method described in the +anytree package. We will use ascii rendering here for ease of displaying. + +```python +tt = pycram.task.task_tree +print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) +``` + +As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node. +This is done because several, not connected, plans that get executed after each other should still appear in the task +tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same +tree even though they are not connected. + +```python +world.reset_bullet_world() +plan() +print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) +``` + +Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can +be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly +reset objects are available. At the end of a with block the old state is restored. The root for such things is then +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())) +``` + +Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we +would write: + +```python +tt.root.children = (tt.root.children[0],) +print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle())) +``` + +We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree +functionality. This will not append the re-execution to the task tree. + +```python +world.reset_bullet_world() +with simulated_robot: + [node.code.execute() for node in tt.root.leaves] +print(anytree.RenderTree(pycram.task.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]) +``` + +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())) +``` + +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 +def failing_plan(): + raise pycram.plan_failures.PlanFailure("Oopsie!") + + +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]) +``` + +```python +world.exit() +``` diff --git a/examples/improving_actions.ipynb b/examples/improving_actions.ipynb deleted file mode 100644 index dfecbb6d9..000000000 --- a/examples/improving_actions.ipynb +++ /dev/null @@ -1,11269 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "source": [ - "# Improving Actions using Probabilities\n", - "\n", - "In this tutorial we will look at probabilistic specifications of actions and especially at an advance plan to pick up objects.\n", - "After this tutorial you will know:\n", - "- Why are probabilities useful for robotics \n", - "- How to use probabilistic models to specify actions\n", - "- How to use probabilistic machine learning to improve actions" - ], - "metadata": { - "collapsed": false - }, - "id": "e982bdf776b7192d" - }, - { - "cell_type": "markdown", - "source": [ - "Let's start by importing all the necessary modules." - ], - "metadata": { - "collapsed": false - }, - "id": "bbbaaaf31c3a9e8b" - }, - { - "cell_type": "code", - "source": [ - "import numpy as np\n", - "import os\n", - "import random\n", - "\n", - "import pandas as pd\n", - "import sqlalchemy.orm\n", - "\n", - "import plotly\n", - "plotly.offline.init_notebook_mode()\n", - "import plotly.graph_objects as go\n", - "import tqdm\n", - "\n", - "from probabilistic_model.learning.jpt.jpt import JPT\n", - "from probabilistic_model.learning.jpt.variables import infer_variables_from_dataframe\n", - "from random_events.product_algebra import Event, SimpleEvent\n", - "\n", - "import pycram.orm.base\n", - "from pycram.designators.action_designator import MoveTorsoActionPerformable\n", - "from pycram.plan_failures import PlanFailure\n", - "from pycram.designators.object_designator import ObjectDesignatorDescription\n", - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.robot_descriptions import robot_description\n", - "from pycram.datastructures.enums import ObjectType, WorldMode\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.ros.viz_marker_publisher import VizMarkerPublisher\n", - "from pycram.process_module import ProcessModule, simulated_robot\n", - "from pycram.designators.specialized_designators.probabilistic.probabilistic_action import MoveAndPickUp, Arms, Grasp\n", - "from pycram.tasktree import task_tree, reset_tree\n", - "\n", - "ProcessModule.execution_delay = False\n", - "np.random.seed(69)\n", - "random.seed(69)" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:34:29.951034Z", - "start_time": "2024-06-19T12:34:28.064833Z" - } - }, - "id": "690c193e24875d2b", - "outputs": [ - { - "data": { - "text/html": [ - " \n", - " " - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Failed to import Giskard messages, the real robot will not be available\n", - "Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" - ] - } - ], - "execution_count": 1 - }, - { - "cell_type": "markdown", - "source": [ - "Next, we connect to a database where we can store and load robot experiences." - ], - "metadata": { - "collapsed": false - }, - "id": "8aa86e4b5256e444" - }, - { - "cell_type": "code", - "id": "initial_id", - "metadata": { - "collapsed": true, - "ExecuteTime": { - "end_time": "2024-06-19T12:34:29.973138Z", - "start_time": "2024-06-19T12:34:29.951995Z" - } - }, - "source": [ - "pycrorm_uri = os.getenv('PYCRORM_URI')\n", - "pycrorm_uri = \"mysql+pymysql://\" + pycrorm_uri\n", - "engine = sqlalchemy.create_engine('sqlite:///:memory:')\n", - "session = sqlalchemy.orm.sessionmaker(bind=engine)()\n", - "pycram.orm.base.Base.metadata.create_all(engine)" - ], - "outputs": [], - "execution_count": 2 - }, - { - "cell_type": "markdown", - "source": [ - "Now we construct an empty world with just a floating milk, where we can learn about PickUp actions." - ], - "metadata": { - "collapsed": false - }, - "id": "9883ac8ebb551df0" - }, - { - "cell_type": "code", - "source": [ - "world = BulletWorld(WorldMode.DIRECT)\n", - "print(world.prospection_world)\n", - "robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + \".urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "viz_marker_publisher = VizMarkerPublisher()\n", - "milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:34:30.600015Z", - "start_time": "2024-06-19T12:34:29.974200Z" - } - }, - "id": "a66e49b86c6c92f1", - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - } - ], - "execution_count": 3 - }, - { - "cell_type": "markdown", - "source": [ - "Next, we create a default, probabilistic model that describes how to pick up objects. We visualize the default policy. The default policy tries to pick up the object by standing close to it, but not too close. " - ], - "metadata": { - "collapsed": false - }, - "id": "2522d93e72798903" - }, - { - "cell_type": "code", - "source": [ - "fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value])\n", - "print(world.current_world)\n", - "p_xy = fpa.policy.marginal([fpa.variables.relative_x, fpa.variables.relative_y])\n", - "fig = go.Figure(p_xy.root.plot(), p_xy.root.plotly_layout())\n", - "fig.update_layout(title=\"Marginal View of relative x and y position of the robot with respect to the object.\")\n", - "fig.show()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:34:31.756867Z", - "start_time": "2024-06-19T12:34:30.610906Z" - } - }, - "id": "24bc79646157a9bf", - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1718800471.201020]: Ontology [http://www.ease-crc.org/ont/SOMA-HOME.owl#]'s name: SOMA-HOME has been loaded\n", - "[INFO] [1718800471.201809]: - main namespace: SOMA-HOME\n", - "[INFO] [1718800471.202345]: - loaded ontologies:\n", - "[INFO] [1718800471.202893]: http://www.ease-crc.org/ont/SOMA-HOME.owl#\n", - "[INFO] [1718800471.203402]: http://www.ease-crc.org/ont/DUL.owl#\n", - "[INFO] [1718800471.203924]: http://www.ease-crc.org/ont/SOMA.owl#\n", - "\n" - ] - }, - { - "data": { - "application/vnd.plotly.v1+json": { - "data": [ - { - "marker": { - "color": [ - 0.028123421067445347, - 0.2755062855709545, - 0.19261245548012446, - 0.023357284762837844, - 0.022925385706912067, - 0.20539928192965615, - 0.19999097528974533, - 0.29997662293270067, - 0.23058605351570072, - 0.023759640394324377, - 0.28120106909430026, - 0.2898871518898078, - 0.16021338341664013, - 0.08301152363384078, - 0.2745464688619452, - 0.1648576364730594, - 0.11876806915419744, - 0.29880435653760784, - 0.2200240711835459, - 0.14633699966688954, - 0.2186362012562861, - 0.10951483099803036, - 0.16094492303344998, - 0.20337098152735764, - 0.04253450604267415, - 0.11298992334832025, - 0.02564370534082973, - 0.2281798816316696, - 0.22083926415750366, - 0.09642787767163971, - 0.1570007051511029, - 0.040840020182190274, - 0.18931742811762442, - 0.08444541054368347, - 0.20812905568119786, - 0.29179011132491367, - 0.03350002278973128, - 0.07043399030009607, - 0.09511453291822053, - 0.08519271421953988, - 0.13834272159182348, - 0.20090927188546515, - 0.25896919463915186, - 0.20790228245963885, - 0.2644197927469763, - 0.042372678051849605, - 0.15110380199918677, - 0.28514170189880195, - 0.2072429276607405, - 0.30019324229825095, - 0.23597226385804906, - 0.1566197904856368, - 0.29154289561789953, - 0.05617865128555986, - 0.028118048571823828, - 0.22506908825491007, - 0.10203918347154615, - 0.16199476750930256, - 0.14650564339227665, - 0.30783719765497014, - 0.0791696154613557, - 0.16207668291053307, - 0.024103853765979222, - 0.01644187633366254, - 0.11823518086476531, - 0.010877695885739053, - 0.2320031207444976, - 0.03370150587200022, - 0.10173129401490916, - 0.03082329484874193, - 0.004499517381517722, - 0.15779997105636903, - 0.2527336251435179, - 0.16125444360748512, - 0.20802251012955775, - 0.09119654734685995, - 0.1624679179860548, - 0.01468587797328038, - 0.2315691607334984, - 0.00852393581027553, - 0.16884033481773297, - 0.020902780977931232, - 0.09967954733480336, - 0.15447246000374193, - 0.11875564750209706, - 0.10054471719948066, - 0.05706388308874062, - 0.01273478100710033, - 0.1211634652985465, - 0.3208946087071062, - 0.07552032856746504, - 0.12101748641387873, - 0.18789949216972093, - 0.11426763361198095, - 0.010478357386090311, - 0.003457661383213099, - 0.1706049149427473, - 0.03674006949146705, - 0.21735851434305115, - 0.07620196719775184, - 0.22381773690769083, - 0.2113992190558357, - 0.030729308146801707, - 0.1126929972895314, - 0.22018616000476218, - 0.2130669406355888, - 0.06101554957276635, - 0.18855788259298356, - 0.00212371023623854, - 0.020298793894667343, - 0.3085826979003413, - 0.048619986207660545, - 0.10432884461863415, - 0.20622047756648057, - 0.10207251239688503, - 0.06786904344068295, - 0.23087394257390784, - 0.2480695045754658, - 0.2880713076699791, - 0.29182391411632763, - 0.13331152907545357, - 0.05821626357280414, - 0.15334716033636178, - 0.12538898964966025, - 0.23931877654971728, - 0.23515311131553626, - 0.16565170644966762, - 0.12138104750472718, - 0.14906296809019243, - 0.08363351754786681, - 0.1612263895990386, - 0.13852124499113389, - 0.05988723423853299, - 0.09297303579024307, - 0.17315284274547874, - 0.042441310973739294, - 0.2846210384961489, - 0.28359939267850065, - 0.02186578609332628, - 0.3006881221165481, - 0.2821052747398781, - 0.01632656672201673, - 0.31116432120591386, - 0.13085557662670322, - 0.015741071028361395, - 0.03373996431561978, - 0.1441408747160869, - 0.02800142656134721, - 0.22452077186570632, - 0.19029119674515288, - 0.0162689306609825, - 0.30338847675832126, - 0.24538700379296555, - 0.1855609170349976, - 0.047583782992911466, - 0.27163551265941505, - 0.25276814097890626, - 0.09174043453266721, - 0.005726819440010488, - 0.13932511181959698, - 0.08468252908083551, - 0.11338566578646163, - 0.23055597596492902, - 0.1786990104397984, - 0.0015392765236733762, - 0.009866656815473671, - 0.10739266725910065, - 0.2047213559298833, - 0.20050097851059134, - 0.2238969108824862, - 0.12942408376984532, - 0.2288166240686834, - 0.11873333891955713, - 0.16876744940159008, - 0.1416662995756706, - 0.14974603820400492, - 0.1307254057268261, - 0.20758842264313612, - 0.13681745117601102, - 0.014084015875901256, - 0.1405970357613048, - 0.20546319585528955, - 0.04854095757266072, - 0.26545600711403294, - 0.012923271077006502, - 0.10511171322479447, - 0.13201348822142903, - 0.06931922704793507, - 0.07851572949937144, - 0.05296046775970428, - 0.2968935668111134, - 0.21573998737258523, - 0.12229293245821063, - 0.1553881314358735, - 0.26737075169437613, - 0.29614854974624943, - 0.28009161797271703, - 0.3046228210412597, - 0.21358130309717582, - 0.16856126581988756, - 0.22047139383648978, - 0.2979943542092814, - 0.06824557597764569, - 0.06661893349207189, - 0.2310489343760546, - 0.1731726929217221, - 0.14229980199849412, - 0.035155320857756515, - 0.02491650247759577, - 0.13890371628208442, - 0.12958697204542946, - 0.2697373807469447, - 0.10724129793262586, - 0.00646709525210731, - 0.27335083201293375, - 0.24124686194452585, - 0.13563671118989257, - 0.04688632705079162, - 0.01321232906164481, - 0.2114476634000545, - 0.16394368806826706, - 0.24877686976957125, - 0.1069459970049361, - 0.26960026385808344, - 0.14509261420956837, - 0.31502561360194115, - 0.0317479810938003, - 0.2226734327749007, - 0.11523160978203954, - 0.04892394437504112, - 0.24207074037848073, - 0.2688568773713261, - 0.07264654543222429, - 0.14024970353068142, - 0.3203531805567701, - 0.09527699719198537, - 0.009184572683621576, - 0.2116325772128736, - 0.12456668760835099, - 0.030014491332143112, - 0.2849207224316112, - 0.18673420702102225, - 0.18672442851384705, - 0.0888006039990118, - 0.2998139206904707, - 0.16525503060658556, - 0.2553460893420262, - 0.2593927405639267, - 0.10843946440895365, - 0.13816072535607712, - 0.30974182445117754, - 0.2316933676178697, - 0.11677268904533945, - 0.2661606565295555, - 0.2867605929458641, - 0.17662339489159942, - 0.022194030356256084, - 0.11684916294060413, - 0.10012374952668006, - 0.12249498646054295, - 0.01811070003429157, - 0.11909024957522203, - 0.2669179366428269, - 0.15463104926585952, - 0.030689530327413003, - 0.020551636514646258, - 0.2714444005773994, - 0.07296913745064378, - 0.12207741644900312, - 0.12928955372947107, - 0.1884314020002067, - 0.08303767610772822, - 0.0921185737425186, - 0.2003652946548403, - 0.3020075308187433, - 0.14203245742495316, - 0.24275265956722408, - 0.05415504072526139, - 0.044959054800388025, - 0.29852833722568145, - 0.24415863720798847, - 0.23571074920037385, - 0.2699013815313577, - 0.06426006834592154, - 0.03021033434506747, - 0.07065158093364375, - 0.0730009624538837, - 0.27169529760987343, - 0.032665996520076034, - 0.09645041885087632, - 0.25484475997656464, - 0.25066563162412897, - 0.19485181268316387, - 0.10388045127014824, - 0.10996608832283507, - 0.20875702031812415, - 0.26780687112062546, - 0.24974561786104366, - 0.1974341234177424, - 0.2503575465147905, - 0.11114277389221333, - 0.13240508463384762, - 0.020748415681073516, - 0.2923007440143002, - 0.2899134785729763, - 0.0607232900231758, - 0.27877439543051374, - 0.11133108748704656, - 0.05873957872151918, - 0.01781148242144561, - 0.03438180540530011, - 0.3208433394167513, - 0.007424692120030026, - 0.3056121044006439, - 0.2819784866335039, - 0.008692288391355356, - 0.05029181666055738, - 0.037806702508597106, - 0.2164012203959834, - 0.10192012639538946, - 0.2313404137782167, - 0.28460135571207634, - 0.18001981879409232, - 0.18073830751560893, - 0.17653705968010702, - 0.19346159889970016, - 0.09674345547549391, - 0.17465892936542154, - 0.2618587525298051, - 0.25851726122586577, - 0.022916157964885327, - 0.019949874992270293, - 0.09327196252118125, - 0.07276172533316123, - 0.27375062819548296, - 0.05510984153611012, - 0.1293146833064055, - 0.19059175884337387, - 0.2677478060857984, - 0.016366289814942706, - 0.1341439843008419, - 0.07452868462153403, - 0.07820874988500262, - 0.04137179645643563, - 0.28076485905912496, - 0.23626655703749258, - 0.2539820183679605, - 0.08883511817484889, - 0.2362938545586217, - 0.20094148220797883, - 0.02919215405312, - 0.12083368572984259, - 0.29911081651628285, - 0.12558545696534582, - 0.14478357511533466, - 0.22545750815115173, - 0.24205939131051157, - 0.11248116913081921, - 0.04779283301956207, - 0.25116382417336064, - 0.162485655260129, - 0.18953461865651036, - 0.08526166688247475, - 0.26031765302498727, - 0.08649249919953039, - 0.08025592436011254, - 0.20684857915336172, - 0.2772571194947606, - 0.1343823551870644, - 0.11741488130140872, - 0.014485447688006104, - 0.09162866603378181, - 0.029440989266279395, - 0.04028912265523717, - 0.16203221838259893, - 0.1895600374542604, - 0.004954701690106474, - 0.27786461372290516, - 0.024125646090002572, - 0.05142944717227691, - 0.13429459690285417, - 0.29929451318676753, - 0.07247097192948655, - 0.08376960687633923, - 0.02042657995498393, - 0.3120578857173682, - 0.29484361192275454, - 0.23169294352529804, - 0.011450142718382276, - 0.28102900878547926, - 0.06619728044221725, - 0.0552656996467748, - 0.08121548363294644, - 0.2590531644820921, - 0.28091337837697, - 0.10704117483700802, - 0.2035125827763747, - 0.056947455604692936, - 0.09207210085575995, - 0.03478928343065315, - 0.14327969166511878, - 0.01719802437271844, - 0.0731872370803158, - 0.17200655969637602, - 0.020327280509541246, - 0.12460985125616934, - 0.2599294629360969, - 0.1558011461096509, - 0.04153475044820962, - 0.1996082847058631, - 0.03203260382585345, - 0.17991186605714532, - 0.10451950257605737, - 0.13542358970955365, - 0.20152221112848617, - 0.1418025616616222, - 0.2651722211105163, - 0.24496510144862788, - 0.12581083020931919, - 0.028589000111226632, - 0.06095243700448755, - 0.3129607699271889, - 0.3181154925809186, - 0.1660582486961708, - 0.29239962642524486, - 0.15197977425478362, - 0.08898277184470965, - 0.17479817552413998, - 0.08464023853918376, - 0.25993282449418514, - 0.1687434086227523, - 0.11325586306122637, - 0.040729371289557975, - 0.32063714444096875, - 0.27370125006515517, - 0.10418291451621987, - 0.28020716681507873, - 0.31308281956835177, - 0.04614020018862072, - 0.13159760924826977, - 0.10839443790865029, - 0.1504717975899796, - 0.24640068845772953, - 0.24265820362509244, - 0.2512821514920696, - 0.1473573145296826, - 0.1085773789316734, - 0.1553955907545821, - 0.25004648448322164, - 0.08644169315734422, - 0.1773516751185147, - 0.1658301613441365, - 0.07521615553039471, - 0.12513598576027313, - 0.15136768280423885, - 0.06084777752146193, - 0.009202913064737814, - 0.31274454248076244, - 0.12581732326769135, - 0.18222658052174573, - 0.025218030639663984, - 0.05844358514799253, - 0.02214923715549926, - 0.2999674550825955, - 0.13450774138678495, - 0.1618853392409212, - 0.301833479394922, - 0.14636041826962506, - 0.27570496457656957, - 0.13273974036783628, - 0.055481483783639894, - 0.1894414070268974, - 0.2057389172757516, - 0.18790187683541063, - 0.17241880705941, - 0.2391908077376081, - 0.1988764537982996, - 0.28532811415826287, - 0.2712414375094775, - 0.018501440562407497, - 0.04578391770372277, - 0.20011494279776787, - 0.17087491116508208, - 0.1717881963131874, - 0.03032382575723609, - 0.14348559238764197, - 0.06392271394786911, - 0.1997456918762171, - 0.009819295009970894, - 0.2820610655798064, - 0.15463502953348382, - 0.03784484024214461, - 0.29745776996343004, - 0.03009034669682088, - 0.21132529643403308, - 0.06173489339043606, - 0.23000204161652593, - 0.05821881289045437, - 0.13680448105072177, - 0.1251397900105803, - 0.05550050939187808, - 0.01829329357488195, - 0.2690386352620283, - 0.08444565468247273, - 0.03597182101264137, - 0.18396754706251625, - 0.24731689207785154, - 0.21852709728757572, - 0.060095920984171276, - 0.16144569706422301, - 0.17565736137393764, - 0.20658016541663834, - 0.2000647836567879, - 0.11742455772049513, - 0.12894055709820337, - 0.26309750612573635, - 0.26655307996949523, - 0.046843875152815316, - 0.1563460256636438, - 0.08093704029970482, - 0.20075545973796424, - 0.06901247640774133, - 0.007216125126364098, - 0.19762480360773896, - 0.2880451884326343, - 0.2780958505329447, - 0.28867421242163377, - 0.13307465551503792, - 0.07341936615002101, - 0.1602937052076167, - 0.27727276367034187, - 0.23099015161865635, - 0.11855212505050938, - 0.04549731671124006, - 0.196133573029593, - 0.12768728339725594, - 0.17534547474113193, - 0.048951043635571866, - 0.1365919931508789, - 0.13431604555532056, - 0.15619282070310725, - 0.15504580272120447, - 0.146176232881962, - 0.3069597613153599, - 0.26179915952519867, - 0.26234126706687194, - 0.2153257808232647, - 0.29170069590623315, - 0.16518029779129695, - 0.03387841985028342, - 0.08046457153055628, - 0.14366729996416971, - 0.044475558976217966, - 0.058789769265013725, - 0.23080700697573375, - 0.18188322376230415, - 0.18617730236060967, - 0.24869177275961804, - 0.19741614141459404, - 0.08926375417017998, - 0.04945135120980477, - 0.1292485912045233, - 0.26512491812241845, - 0.3202063131803518, - 0.14094432155547248, - 0.3120207026984458, - 0.006677134998614324, - 0.2532623096357512, - 0.07157646638731181, - 0.12249945216335671, - 0.17451954424434288, - 0.24953497234472263, - 0.1448828106080705, - 0.046526476738971104, - 0.08709517115631074, - 0.0589201710584767, - 0.15641629808277394, - 0.23471598366414326, - 0.06186424970344842, - 0.05190238681437093, - 0.13750673610147504, - 0.19470965943097607, - 0.18571081795292294, - 0.20445994981711374, - 0.12917792088994054, - 0.18077804500064376, - 0.02963834342433223, - 0.21267173990596155, - 0.05571235574614556, - 0.21101350335649846, - 0.016099964775211136, - 0.1409021509393946, - 0.2670627097735382, - 0.2684941171660789, - 0.009622002137382088, - 0.10924782964983297, - 0.12258215282446647, - 0.16885184332003017, - 0.11759443836240387, - 0.20316196280320234, - 0.2132289182252676, - 0.13223963103575584, - 0.22201250413375018, - 0.020989967557495152, - 0.08500869976877663, - 0.2973934953640685, - 0.11501480763502009, - 0.2785404078395268, - 0.14971582146124485, - 0.04200813462797168, - 0.14174564311314494, - 0.1512837180088186, - 0.20890509369668636, - 0.08242526199021062, - 0.059112148276822096, - 0.05759789951830418, - 0.29701737432662245, - 0.0843850135901416, - 0.18427066213723942, - 0.13772125375435365, - 0.0010459344067679306, - 0.003939180000223809, - 0.18081215156869143, - 0.08583188312323818, - 0.24648227533626269, - 0.28639877646852063, - 0.23671451965705967, - 0.22924214919706248, - 0.20295093222727134, - 0.1301852237205805, - 0.003508156038645622, - 0.21643168073002023, - 0.2594419391102938, - 0.02626700962804169, - 0.27297416461747015, - 0.1381867224201822, - 0.008877927210102615, - 0.007968320422991247, - 0.11011317336872967, - 0.013668591006575066, - 0.13075815353742348, - 0.19890770617524933, - 0.25551438293267265, - 0.07775416098207918, - 0.30732139071576775, - 0.06416048523056678, - 0.13115033963786513, - 0.03227040104154454, - 0.3191048410163939, - 0.20545305943232142, - 0.19803470543894283, - 0.2121981170840059, - 0.23311031510896937, - 0.13598005660720508, - 0.2069322781260865, - 0.07821647155413726, - 0.1767738997072744, - 0.02293688487258753, - 0.15018345049582651, - 0.058143236700217635, - 0.09702848639787105, - 0.27152075095610845, - 0.006408869078351341, - 0.047737495004824784, - 0.123317737019775, - 0.25623074554457725, - 0.1181130529289701, - 0.2642332081741031, - 0.1205415601693278, - 0.12399030014959761, - 0.14079279694293578, - 0.31307977008530785, - 0.15201371983453446, - 0.04302141306101524, - 0.13805994428943463, - 0.11255968069489855, - 0.2870904296558023, - 0.17565442056076166, - 0.06428380333813162, - 0.13095102085666793, - 0.08482116253299629, - 0.2851359907612209, - 0.14637315903599335, - 0.017030273924824264, - 0.24608950515132388, - 0.030713245995478516, - 0.0601732470549814, - 0.3023953664463326, - 0.06904547461318145, - 0.09648832390138108, - 0.29582999953433947, - 0.005423583232896728, - 0.05590704908683021, - 0.1634170622134225, - 0.3155781593369998, - 0.2562905634113047, - 0.14042659308835764, - 0.0891438117585867, - 0.13401112204639948, - 0.2203908381229101, - 0.18248548620354416, - 0.01359861726098872, - 0.19385174955392923, - 0.015363482778288145, - 0.25963707622661714, - 0.12388347957327742, - 0.16876007319537806, - 0.137820105767107, - 0.12132404332499629, - 0.06554911208160769, - 0.19612806344238884, - 0.2897619799208934, - 0.1518857093432718, - 0.10822758035115856, - 0.22849047579409648, - 0.050798598176004656, - 0.2299978552760817, - 0.0478383302045268, - 0.10670369664798394, - 0.15923396938256215, - 0.22423445977536458, - 0.10705802544112321, - 0.05761008283316787, - 0.04181948700355519, - 0.18918773584545387, - 0.23900622321886802, - 0.16344446665547965, - 0.08108266113674752, - 0.21412493879769154, - 0.009890187640209328, - 0.20063283069573315, - 0.12816261094259848, - 0.1368666104663216, - 0.11216782822917253, - 0.22154158103056115, - 0.03816417837004799, - 0.22084868183861317, - 0.2252937259207474, - 0.2745413654048875, - 0.053293910548268625, - 0.2567426845469148, - 0.2146282500085843, - 0.15633392096950605, - 0.12481797563345329, - 0.13604587630320716, - 0.0426325554577188, - 0.23667819090494036, - 0.1915684783036557, - 0.20399124220113607, - 0.018360236896145285, - 0.12560908967928297, - 0.229914368987592, - 0.20655958399669985, - 0.06111447731562556, - 0.03833055428203391, - 0.13754412140220998, - 0.20897462784580165, - 0.011770443680182329, - 0.1642450952344869, - 0.1439383793591831, - 0.20243957762203155, - 0.15151038886315288, - 0.17571914148880224, - 0.00971513443470036, - 0.25291245483413227, - 0.19760243276543485, - 0.10432829947076358, - 0.021504561475608485, - 0.2458611967320093, - 0.253768599228114, - 0.02728985136471895, - 0.3150880365703088, - 0.17648481051552511, - 0.0031812859818763036, - 0.20192960236404497, - 0.014141539146770112, - 0.2051148431306037, - 0.0923819005611092, - 0.2840125796706265, - 0.11744506965266965, - 0.05666529374228956, - 0.12431878987581009, - 0.170115459520518, - 0.05296462749595469, - 0.28911954811892404, - 0.24758599420459543, - 0.16893377959488678, - 0.20005945241418369, - 0.30513707248354904, - 0.2549872603654682, - 0.022969482458268606, - 0.05425096837519992, - 0.2504734129539533, - 0.14287507305524294, - 0.08217656584173445, - 0.052158672360793024, - 0.16476863948648132, - 0.22092168927037456, - 0.3099469865322952, - 0.21459598608073066, - 0.20391885672713103, - 0.04640102134757639, - 0.022613977872659747, - 0.26575233678971044, - 0.13050024982560207, - 0.22811335012900458, - 0.0451100254070365, - 0.21546460997888653, - 0.30005534358413316, - 0.209140558237376, - 0.2901128256659546, - 0.012914039408906843, - 0.06562220527068537, - 0.23994745930559683, - 0.03089520599670031, - 0.17550144623386504, - 0.10091931578614015, - 0.0441617371747382, - 0.13818083694005814, - 0.10472189773140865, - 0.21423844637662087, - 0.22397808032814723, - 0.018928729268300984, - 0.05592905585718967, - 0.07069386174392771, - 0.2114554111071868, - 0.26483172070276356, - 0.17881191547364209, - 0.21091489455752208, - 0.2993309141189388, - 0.21862228581299262, - 0.013209169727666172, - 0.30173771569287194, - 0.25859950170944485, - 0.22721141653052104, - 0.1719207791653972, - 0.08236803551065623, - 0.23033284888120661, - 0.010796131092842219, - 0.2921484040298793, - 0.29569150712620085, - 0.2439789836194617, - 0.2588024749488275, - 0.10513672155776148, - 0.26131558155435936, - 0.15527377855493454, - 0.20967741880161653, - 0.13520936413561196, - 0.31159796664749956, - 0.2793205202048684, - 0.23939296299194138, - 0.0983696097344998, - 0.027198342270921286, - 0.2732467349796888, - 0.31014057336397893, - 0.20279929140323075, - 0.30207355995310026, - 0.27800128117178213, - 0.021310832772757013, - 0.09882566727545974, - 0.024463109607915878, - 0.24298504100006457, - 0.10352108060933139, - 0.2716075917305972, - 0.08502687331949908, - 0.22304004318884738, - 0.2177551469871992, - 0.24576609837716987, - 0.24636758129815756, - 0.05608975070588209, - 0.30462635934388443, - 0.3062058785673454, - 0.1894263160140458, - 0.26093529843540286, - 0.30917052744866763, - 0.1529585532415057, - 0.29991244605854017, - 0.06307000671221523, - 0.30156079772530414, - 0.15893604136322562, - 0.1485765443841899, - 0.24106823723893733, - 0.02548952266600863, - 0.2880576656515833, - 0.10130300071104655, - 0.24384721626573225, - 0.18892775790370048, - 0.23363267937350132, - 0.2914252896419818, - 0.10644258861160726, - 0.2966816821959808, - 0.2923758095035957, - 0.29471150563335563, - 0.09094827382055706, - 0.19527808880905992, - 0.10924732798250873, - 0.24073369194512304, - 0.021500093901445746, - 0.16630571670055036, - 0.3042276811788364, - 0.12276442314336096, - 0.08004062106486226, - 0.2600834331213411, - 0.1413860857538514, - 0.31610071473146434, - 0.13708996921923633, - 0.2984078972398175, - 0.04965209554359336, - 0.14060451352870673, - 0.19127332744604184, - 0.24532297221154925, - 0.29611045546303105, - 0.31530064810837655, - 0.136457455477333, - 0.165143385963172, - 0.31393405700480304, - 0.05752244806754432, - 0.21006750805869562, - 0.3054159457034637, - 0.2559461165448927, - 0.25607447986194487, - 0.1792797863678326, - 0.3050569777874923, - 0.07553726354760644, - 0.21527071734348763, - 0.19307782550806393, - 0.17155171258414115, - 0.2567376681220612, - 0.3220097931718553, - 0.24198701154057228, - 0.17771792310091275, - 0.1489229686917102, - 0.31928359295782216, - 0.28613003308337565, - 0.2848224094058011, - 0.058345431453868085, - 0.2578470671068484, - 0.17470516529555838, - 0.26599094361740006, - 0.2742464041489417, - 0.2137881583411134, - 0.2518120811109862, - 0.29942178953102816, - 0.22843618221854997, - 0.17747349192437817, - 0.17711517705368834, - 0.2646410193264032, - 0.2911923701349646, - 0.2722445390788712, - 0.1647079624706379, - 0.13338850186413284, - 0.0692557177959392, - 0.22207293423985922, - 0.044463521493856635, - 0.13977400491140662, - 0.24297368117675497, - 0.31286599101321655, - 0.2139949242404099, - 0.21024384011311067, - 0.11694052204467564, - 0.23220953591101595, - 0.3064746026982342, - 0.02256032489556911, - 0.30369445657399574, - 0.07291586897847283, - 0.25399031619728146, - 0.29585306669745526, - 0.16863921634017126, - 0.3186043466210699, - 0.10658866761628137, - 0.2131054882852513, - 0.26850075941828344, - 0.2884476928604199, - 0.19007790747426398, - 0.264840572617916, - 0.1878005999066394, - 0.2363577270653103, - 0.060329663678228194, - 0.22029135072083939, - 0.2553540874993936, - 0.10280343062456218, - 0.19755298094129195, - 0.25553309483824876, - 0.1823996432233547, - 0.1436686248000445, - 0.11971698304597757, - 0.30886091683355094, - 0.3164169062111376, - 0.23922668430819347, - 0.1689134074064798, - 0.3026566654183515, - 0.1676606164659271, - 0.29308545757380744, - 0.23701855117823176, - 0.2604336587833792, - 0.3106589116507101, - 0.29693434101593347, - 0.32005366796948437, - 0.08110098429165676, - 0.3193856443581172 - ] - }, - "mode": "markers", - "name": "Samples", - "x": [ - -1.5696327789480555, - -0.319161041164713, - -0.7037508429235464, - -0.49647265319727407, - -0.8364372069270214, - -0.6549050829327184, - -0.6730064873322712, - -0.20863105116594155, - -0.6055612177006341, - -1.5328560721257058, - -0.2940068200571684, - -0.36738569085252837, - -0.8551608079713421, - -0.6570931250466215, - -0.43520423652324447, - -0.8424367586785109, - -0.5967489828276259, - -0.30417485293268937, - -0.6299519188253683, - -0.8811876008175503, - -0.3046273060281897, - -0.9639506421860431, - -0.4305522911298151, - -0.7100762563174903, - -0.5919693658610833, - -0.9262289983157213, - -1.5797678632201277, - -0.375593090223646, - -0.5825653101279508, - -0.7208234891544728, - -0.4489748669222336, - -1.4220028980929318, - -0.755267318200301, - -1.1536676474710332, - -0.6840580628171089, - -0.3629303817243719, - -1.4833970417152602, - -1.1994267084896038, - -0.2400026661400186, - -1.1681243836115338, - -0.908735487152637, - -0.7184441441379994, - -0.5058096275401923, - -0.697402752215401, - -0.20999399722591558, - -0.8360583602818987, - -0.38559645571488466, - -0.392641665876604, - -0.6307711169716815, - -0.3416856993040956, - -0.3383952349061352, - -0.875379275871185, - -0.2248425995625818, - -1.3265714610516925, - -1.1677287084827614, - -0.6203588989931668, - -0.30662765990225893, - -0.5602494677617743, - -0.914697227006307, - -0.3067867700086469, - -0.4835821449886224, - -0.8271580405996515, - -1.286455555643974, - -1.0409690772600215, - -1.0244906073147353, - -0.9077996522092113, - -0.6072669187880991, - -1.0424855461903249, - -1.0379595079374921, - -1.515331958494378, - -0.698873547388849, - -0.7823270984654109, - -0.31860003878994075, - -0.8449200339478316, - -0.6956087600141532, - -0.6814809979715937, - -0.7491131253198249, - -1.6379330583755674, - -0.6086571833055305, - -1.8375576050260456, - -0.6286241270020342, - -1.0558311397187181, - -1.1035851121287832, - -0.5813634807592303, - -1.0204234230261875, - -0.2675670026264853, - -1.1050094638180452, - -1.8078688367488331, - -0.5662676236809416, - -0.20852235376882444, - -0.6930938059104285, - -0.7000023375925584, - -0.24443777107067932, - -0.9632363673505486, - -0.8424741877580568, - -0.9469669504148497, - -0.8222652022566409, - -0.8213384708620832, - -0.6229014729278547, - -1.0675551545841562, - -0.4598081329291661, - -0.6421370819043899, - -1.2661867640584248, - -0.9782990455261611, - -0.64773981061225, - -0.24867703381393394, - -1.2886009535977787, - -0.7158088634272443, - -1.6012758177677435, - -0.49080800765805516, - -0.2326029060488203, - -1.2314544331554624, - -0.3319872912611299, - -0.6868748447956776, - -0.7639024733732167, - -0.5704578015106015, - -0.25357040121497726, - -0.497904495297595, - -0.40015445425798263, - -0.3840554052962895, - -0.574839573807819, - -1.2410155754523786, - -0.8276918245487551, - -0.6639918533506817, - -0.3920279452994202, - -0.33103069360392956, - -0.6263555601674113, - -1.0114999005556158, - -0.8072740850388584, - -0.335937859499499, - -0.8466520928209553, - -0.25902530129731843, - -0.22365190640382765, - -1.0909342855504711, - -0.695494492892813, - -0.2602987271879546, - -0.3102341650114874, - -0.3661885970395811, - -1.2981988430841342, - -0.21843611820722475, - -0.4149445604674899, - -1.3133070261673276, - -0.26936827905020516, - -0.973842959207491, - -1.0640130612438115, - -1.015148184247882, - -0.8758342939972003, - -1.4926902512404294, - -0.32936408798523586, - -0.5796066622930753, - -0.5137095973207513, - -0.25732232046855735, - -0.3268356051648147, - -0.6191036522698776, - -0.6296349880315258, - -0.361052751510218, - -0.28725246992148107, - -0.606221108839936, - -0.4664773513265265, - -0.33561693772826845, - -0.7070202467310683, - -0.5840786497137318, - -0.5722058596101709, - -0.7986051704799302, - -1.7694469533085908, - -0.3294998585383055, - -1.0451542097284892, - -0.666025845171171, - -0.4810747298789097, - -0.611555451562051, - -0.6552097925450119, - -0.6241717326657591, - -1.0222411425538758, - -0.7302965553211677, - -0.9223369202012656, - -0.8973505878363648, - -0.8268243708191414, - -0.3845933258199158, - -0.9456471653980288, - -1.2640882069393702, - -0.8240221334083859, - -0.616357264975989, - -0.2498302395597905, - -0.4157692693454854, - -0.6871417550348244, - -1.072517190942976, - -0.9216001928325814, - -0.7843468185453977, - -1.2084404747791415, - -1.2064595577565165, - -0.3364537028559997, - -0.6325690725692085, - -0.4444690643390548, - -0.6909354092727906, - -0.3357145385184098, - -0.36164996753241424, - -0.42544459289045206, - -0.20276945905726065, - -0.4437669533491271, - -0.44790752461082384, - -0.40310060429040007, - -0.35199099727294736, - -0.4441346646423242, - -0.2521001620670765, - -0.6156724082667631, - -0.3629577600466717, - -0.9222185118811213, - -1.352655458188702, - -1.5749551150357481, - -0.24511620836179027, - -0.971917102974481, - -0.47376891556960216, - -0.23634772290033337, - -0.9265568498230535, - -0.3654129640084707, - -0.438316182841528, - -0.3215029993380871, - -0.9835192828720243, - -0.7462884868590206, - -0.2719222240757492, - -0.8509686337055516, - -0.2880087942444184, - -0.8815846613117513, - -0.4699378335968014, - -0.42580430367589234, - -0.2639437840869308, - -0.9170934492048961, - -0.6458952024063404, - -0.982154167580608, - -1.3717367043387656, - -0.2004257353883267, - -0.21721640492654426, - -1.0311346555835252, - -0.2323693924640495, - -0.2000612168351382, - -0.21225733522441942, - -1.4846658916532722, - -0.40472006530825544, - -0.9959896090939582, - -0.49871524387837457, - -0.4095102349649716, - -0.6717456609131005, - -0.6600804339343114, - -0.41138267467275, - -0.3077285777068107, - -0.2570671058836985, - -0.5104811720416601, - -0.4827484858950292, - -0.7987028326718215, - -0.5665158752226204, - -0.27210699499341945, - -0.5980566147347742, - -1.003466959659439, - -0.31154202330349157, - -0.24677690319138418, - -0.7885455349300793, - -1.6427714124240782, - -0.9011203189982329, - -0.8623942033460361, - -0.8087392155537116, - -1.4855217234870721, - -0.5771588046923404, - -0.42297301190811165, - -0.6031026644139628, - -0.8290546657649632, - -0.5629573409306975, - -0.32124763623837954, - -1.0334218730900244, - -0.7689787817184935, - -0.2460146900032458, - -0.7085729912586546, - -1.0799305623842883, - -0.2620239799321787, - -0.7236162250536223, - -0.21680670655932294, - -0.46880283516647414, - -0.5356163519396823, - -0.4753658051713492, - -1.337567465445126, - -0.3406617870345225, - -0.5359494759478894, - -0.5511613958928456, - -0.31031404769398857, - -1.1073501442874532, - -0.9409265441996243, - -1.2501438735627135, - -0.7110628319698971, - -0.43839777271126723, - -1.5287951514515281, - -1.1076823847628872, - -0.4234910210586943, - -0.2589001493268236, - -0.6050377789686205, - -0.7920260119499969, - -0.490797728111933, - -0.21114627692372492, - -0.41365417751376315, - -0.424698852664819, - -0.48085750616742406, - -0.5386500121334966, - -0.3226716991111056, - -0.8099991410013294, - -1.349532828826143, - -0.3485057544367568, - -0.392431585451937, - -1.3031501780428085, - -0.3548159082748625, - -0.2512472718674896, - -0.8990726744945937, - -1.0710737803677752, - -1.1556170215788863, - -0.22508861154963583, - -0.8254777456077546, - -0.3147349916664917, - -0.3048384439398312, - -0.7459474394493287, - -1.3802843127186282, - -0.5656476773842796, - -0.6617844892789734, - -0.5216407217308772, - -0.2076021199498098, - -0.40599580239051747, - -0.5168744652781844, - -0.7368206530208155, - -0.7702637567413472, - -0.6919807294749483, - -1.1007528445891555, - -0.6877085961074693, - -0.4804383526851259, - -0.5038067954666486, - -1.0643021566365838, - -1.5014830002419026, - -0.781364626017522, - -1.209638955481054, - -0.42126365452895786, - -1.2644586139928669, - -0.8439280277690027, - -0.7571695635995837, - -0.4100175864354335, - -1.6141211778869031, - -0.765594688714129, - -0.6938170359357051, - -1.1957295675118267, - -1.023856396839748, - -0.3005657491070823, - -0.4974821964586805, - -0.41723152212360026, - -0.5867604475631873, - -0.5936879661336978, - -0.7186113031375087, - -0.9286414743885536, - -0.6654895143894859, - -0.30223643854849364, - -0.6358510528819052, - -0.24718653292640835, - -0.3908661234643796, - -0.29789322806743623, - -0.9840998834656477, - -1.3735904845262612, - -0.29178256776656636, - -0.22864577597245067, - -0.23072508452221036, - -0.7117099179311469, - -0.24863177136108253, - -1.1650378801413706, - -1.0838749468692694, - -0.26332111643238576, - -0.43091783465356026, - -0.4728669956218612, - -0.6610707969732641, - -1.320130923291471, - -1.0551116035265522, - -0.5248002741844938, - -1.2698528882922873, - -0.3868924697739869, - -0.5338878133735893, - -1.1378706056234955, - -0.25751757296861766, - -0.970734612162809, - -0.8647192091652992, - -0.8718446989336481, - -0.33376677070373795, - -0.299194163103962, - -0.8946405005726561, - -1.6624578037646025, - -0.22765456886770594, - -0.3304739251766039, - -0.25270425332440405, - -1.5885410178453652, - -0.3407061075188583, - -1.2690058717949846, - -0.5623543310417253, - -1.1944174825201288, - -0.3974231173072942, - -0.20576749792549387, - -1.071950232343708, - -0.3053108625864956, - -1.276585542279544, - -1.1084872131083718, - -0.6850574758639265, - -0.9266230134706483, - -1.6863382352083902, - -0.5260897706456319, - -0.7778573927128758, - -1.0644960133690031, - -0.7375698057558463, - -0.5097392546749847, - -0.26782541289212747, - -1.1812105008096627, - -0.5916919213447234, - -0.9515318463151268, - -0.7881898555180059, - -1.0777535061145365, - -0.8946608674383422, - -0.4839538408403289, - -0.7831102556885365, - -0.4750528951782947, - -0.567478828129574, - -0.387882610882089, - -1.5520350122828377, - -1.2750323735313538, - -0.2382051158962066, - -0.23117180564981957, - -0.6550327810933327, - -0.2425381572098855, - -0.5264594889994424, - -1.1522543276102337, - -0.5196409209685934, - 0.3279860998951509, - 0.28370134622463156, - 0.24717598076248576, - 0.3260294649792164, - 1.3338393649293885, - 0.2219556553797932, - 0.39188928862758415, - 0.21739290120364094, - 0.4132199941965392, - 0.20512705708906248, - 0.5361282548606832, - 0.744042147746849, - 0.8825597633438976, - 0.6121500181458531, - 0.3898032807269675, - 0.28350755514742737, - 0.42579035702912243, - 0.6827800469070412, - 0.6381218803044458, - 0.8735229758492928, - 0.503518505352145, - 0.2591772417206193, - 0.523105195809637, - 0.841478685512702, - 0.8010991956708534, - 0.9168107806440077, - 0.29373460833970455, - 0.6445708438598138, - 1.4193090002793172, - 0.23284244154369374, - 0.9213202974203683, - 0.7155848204073335, - 0.6071113043574593, - 0.6902596696649355, - 1.4710317043668986, - 0.30073841859628536, - 0.8438789448758597, - 0.7527323377695969, - 0.29490948753910917, - 0.39689826323991323, - 0.35999578530215737, - 0.8818055761527458, - 1.3173286271136162, - 0.7603921417296388, - 0.6897218932187155, - 0.24153414962559505, - 0.6559927512874946, - 0.3801615652701374, - 0.5051552043088097, - 0.2803843033566373, - 0.44656528150345337, - 1.6676685571142817, - 1.1445476229805995, - 0.7244629546537222, - 0.5063680861118414, - 0.4726145686544781, - 0.8037252013927677, - 0.596971655061172, - 0.3586539856847922, - 0.6353502290741736, - 1.209773272374557, - 0.41025199796416, - 0.8758122565133468, - 1.24313708366167, - 0.2820034360964468, - 1.2820317637831813, - 0.4734662750496369, - 0.5540656737667065, - 0.47765378941774095, - 1.0605959558231124, - 0.618910718400868, - 0.4754687711552713, - 0.6145819588497254, - 0.3935249877583668, - 0.476294634820412, - 0.8917045695797277, - 0.6802955922481103, - 0.5482617208037022, - 0.5473722133485324, - 0.26086684462536675, - 1.0153858120601302, - 0.6463754473564187, - 0.7542722966419699, - 0.7021804675848633, - 0.3690005258630817, - 0.8176569795905397, - 0.23071731462566772, - 0.24754032982946425, - 0.40349081985549806, - 0.468378757208491, - 0.8776409978724693, - 1.094429257810337, - 0.6572269436908217, - 1.2278611112722269, - 1.8508670498590807, - 0.24716156080003085, - 0.31830822044244317, - 0.43307476861651395, - 0.36343525844914604, - 0.7713904430132136, - 0.9746390673136994, - 0.7785716894482491, - 0.4455389875376321, - 0.25089109215135025, - 0.4245885733885096, - 0.7442182765180619, - 0.22292827445504582, - 0.9603830001674456, - 0.632494074711971, - 1.2891932067014376, - 0.9482862834437653, - 0.9452272252471765, - 0.8237743180694621, - 0.8165734437718702, - 0.9030465605519181, - 0.24539426703666803, - 0.41505481385141646, - 0.5005194518144461, - 0.6332969443932666, - 0.21808345627612946, - 0.8291398466820252, - 1.415022668898805, - 0.36031434631668363, - 0.2825832998523382, - 1.2546368885436916, - 1.1813965727650175, - 0.28512902177016547, - 0.5454558924698361, - 0.36587207383412357, - 0.407345655139629, - 0.7010398778486446, - 1.1500466951323047, - 1.134675300902595, - 0.6599838509098535, - 0.49071469915742905, - 0.22329725207655918, - 0.720425239250981, - 0.20570515799880626, - 1.7098319463300276, - 0.4783638974155945, - 1.2337211035409361, - 0.7650601753549181, - 0.5711405294814179, - 0.5508906105308078, - 0.9175863734290612, - 1.1025329004660596, - 0.5944441701228608, - 1.1218859667756287, - 0.28211944498578906, - 0.5638414551549339, - 0.669323740750292, - 0.6435037505284065, - 0.7429587925164312, - 0.7292835473491007, - 0.6533388038508788, - 0.60531671375061, - 0.2701282685654021, - 0.20172368093191498, - 1.5496001601845313, - 0.6810019197978308, - 1.2231948068004659, - 0.6869044336969143, - 1.672868213286874, - 0.8353403945128639, - 0.22502983359720238, - 0.23617406486473655, - 1.610550346521808, - 1.0392275940264215, - 0.9987180584061282, - 0.8333685819573866, - 0.7786675045874928, - 0.5233885157797282, - 0.5837505471759789, - 0.9524454222401467, - 0.6484726638818059, - 1.6641128259113966, - 0.9052331839470863, - 0.35823597107186117, - 0.3708227453519214, - 0.43731830958900675, - 0.8813917000759536, - 1.164153997966817, - 0.23366419952123224, - 0.6088810381522086, - 0.5973530648545637, - 1.1644417335314392, - 1.2039453247911214, - 1.2299489687425536, - 0.272967379288491, - 1.069873272397645, - 0.2969646589670813, - 0.6098179790554855, - 1.6407428320306199, - 1.0380164229725397, - 0.40873525327793564, - 1.1273525594016616, - 0.3004012169459323, - 0.3975226115438568, - 0.2774813908334842, - 0.6192986797156649, - 0.5721387143726717, - 0.976961449012314, - 1.190677216250485, - 0.2305476383502376, - 0.5083915038138925, - 1.171596957392141, - 0.20757044547678252, - 0.9063307798777671, - 0.9875280546081155, - 1.8577547673782675, - 1.0443286279676485, - 1.7436505834381695, - 0.8293029657402088, - 0.7233021646390216, - 0.2521145407879583, - 0.6249502651648267, - 0.28366243490982307, - 1.1750926080062731, - 0.7675183713811359, - 0.774522692223976, - 0.21207040849332173, - 0.7005527654778194, - 0.6354051723666104, - 0.556808956770568, - 0.5945132124006043, - 0.7317538001042706, - 0.25175976358007396, - 0.5549135274586992, - 0.7101506586654142, - 1.4588050867352786, - 0.5726438138719193, - 0.4878070728069157, - 0.8853364096630997, - 0.41521521663696126, - 1.487810218101714, - 0.7034519355295965, - 0.42151061073219354, - 0.3735249083300581, - 1.025041350719793, - 0.3517748518282005, - 0.2441633172206933, - 0.9868676720071381, - 0.4056990192697675, - 0.20520538393437646, - 0.6160541929956556, - 1.3016408545915423, - 0.6779888593630862, - 0.7470748888438492, - 0.40010994719822246, - 0.7552492144935012, - 1.2801463166092026, - 0.9092939012232458, - 0.652943655011229, - 0.3782882325049173, - 0.4610837073548369, - 0.33245648092521607, - 0.5168864148223111, - 1.5476345526410462, - 1.1217933188650633, - 0.2652418229644539, - 1.0727642462022045, - 1.098460262437152, - 0.2356930932560543, - 0.5983295987551805, - 0.3840157784653527, - 0.3065941152200136, - 0.2618248453895985, - 0.3885312744882427, - 0.46613032658929066, - 0.3835582783942505, - 0.9347190958211085, - 0.6023530721586746, - 0.7236734438999763, - 1.2647171517659894, - 0.3912622751992866, - 0.6703458220529684, - 0.25904668588683366, - 0.9345908167971683, - 0.7743910243904557, - 0.6825644967128222, - 0.9364620609447017, - 0.8029219377900626, - 0.6672796424262755, - 0.3821025148533498, - 0.7754238346672292, - 0.9512251206738984, - 0.6251577634441883, - 1.1429208840705756, - 0.6165008305569726, - 1.3636279549028218, - 0.6045427235143528, - 0.3041575255523084, - 0.44470362584818174, - 0.9271610654212511, - 1.0779859062491297, - 1.435719235101117, - 0.2965219881777894, - 0.21943429266245473, - 0.3220501310502539, - 0.8914604746170638, - 0.6739092416841906, - 1.0818722383495285, - 0.5720845748257763, - 0.4107387519988422, - 0.7026368016780461, - 0.8370739015750036, - 0.5466268543158539, - 0.8634249761539101, - 0.29125822081382474, - 0.515711337217874, - 0.2500038416960823, - 0.33831864700863457, - 0.20096288066948204, - 0.6744287804660148, - 0.527684944918767, - 0.5727567991660929, - 0.8484304075546483, - 0.5881262968822073, - 0.26758883705028985, - 0.7389913750276278, - 0.27362296751181014, - 0.5701085324296198, - 0.5591035714579102, - 0.5326817293026397, - 0.7017967442151225, - 0.9815476238596504, - 1.4734897416164254, - 0.9481651173209934, - 0.6929440255532457, - 1.8023394450031693, - 0.4972094703131539, - 0.9134699047550131, - 0.6899868004170266, - 0.8935442390057852, - 0.7295903338754363, - 1.8632015417570251, - 0.5355780437838616, - 0.7218747384485589, - 0.980987116868595, - 1.5149873020362843, - 0.5568120714662872, - 0.4937383084328216, - 1.1610650020967885, - 0.2521983839076295, - 0.715756395162246, - 1.8257522798129735, - 0.5267665117814609, - 1.6808345819749932, - 0.5260108433801477, - 0.9273956618592435, - 0.38643463932513994, - 0.7662343269421142, - 0.9471423523489559, - 0.8023077546441674, - 0.36246654915827947, - 0.22110272344294113, - 0.39353168674900735, - 0.45131106671646565, - 0.5100585461573698, - 0.3219263850047293, - 0.2789530093301241, - 0.5027114552696956, - 0.2643058134398305, - 0.6959319612452143, - 0.5298850741728258, - 0.7403792009602088, - 0.6031640652306075, - 0.7805405810008067, - 0.32054124478580537, - 0.4288731187256566, - 0.24291051727328436, - 0.4231125924666791, - 0.543890282339062, - 1.4090312803272869, - 1.164540978232025, - 0.3987728155548715, - 0.3422298339057126, - 0.6171900813594045, - 1.1348939169620702, - 0.6714726292229867, - 0.3442627542174186, - 0.3339229748915118, - 0.27028095517962764, - 1.7730069584660355, - 0.23713597560917313, - 0.5426170417538481, - 1.4898747717852159, - 0.6187326860928656, - 1.0708540827307065, - 1.4209169328760949, - 0.8308482106965753, - 1.0590634971504715, - 0.20857342984306476, - 0.5476536998103853, - 1.591047910061486, - 0.2630514465765809, - 0.49876652338734634, - 0.31293699935147784, - -0.19318078350770412, - -0.0592680772209378, - -0.1091285240040694, - -0.17613013582463843, - 0.011250560254200317, - -0.1571743272846896, - -0.04878718545327888, - 0.031819505461751094, - -0.15662817498082662, - -0.08858696611852603, - 0.14111230574387237, - -0.15897953392129113, - -0.12078534281011068, - 0.17407458448955035, - -0.02409844627211349, - 0.1541810211235763, - -0.06029068746030678, - 0.131390663448839, - -0.005278223669663982, - 0.07605783294621278, - -0.08682297621892131, - 0.11598599258574936, - -0.11530549907919374, - 0.11104810595582874, - -0.16831035882287596, - 0.03370588092894643, - -0.19777773459793693, - -0.18116187784493698, - 0.11363335944679195, - -0.030035526670591523, - -0.18094516439036304, - -0.12103761242975244, - -0.054084927157600673, - 0.012786443863606873, - -0.19936416172985516, - 0.17606841359956, - 0.019707731127929642, - -0.18947887331818872, - 0.12269983679077782, - -0.008122792556036984, - 0.05244926156067574, - -0.029719203041510583, - -0.13207781893727746, - 0.1356408752677492, - 0.18532488793303692, - -0.17730529030205788, - -0.028079340608739047, - 0.145190213296223, - -0.023824591404100374, - 0.0932144648785701, - 0.10939859146694383, - -0.0948693872910583, - -0.06727416494765781, - -0.17416888034722114, - -0.03340641038710354, - 0.15513284445391118, - 0.1634621779386963, - 0.07732097597318352, - 0.004054187554678183, - 0.19008418576689765, - 0.06452928747959344, - 0.18448068609330787, - -0.1060946170930679, - -0.04481030251237116, - -0.14755587741230816, - 0.002491303920360491, - -0.1522733050369443, - -0.04121956727704986, - -0.05225263871885123, - 0.10921807651038455, - 0.03505783930587905, - 0.0038576222722824173, - -0.03485063366063629, - 0.1270627646880857, - 0.17997194622261173, - 0.05247938080147335, - -0.191056405730227, - 0.15906814253786036, - -0.14760873676795855, - 0.03823182587183629, - 0.18881964040244606, - -0.009145438209414339, - -0.05277292326510956, - 0.09411353172618458, - 0.08144309581199065, - -0.17583911014945874, - 0.0014661540916668099, - 0.18025229364922743, - 4.441065024742982E-4, - -0.13258724742617872, - -0.02540157216720304, - -0.018289677077484433, - -0.15447114374719198, - 0.19591447242382196, - -0.18558556830413037, - 0.10137859739587837, - 0.03058673882990356, - -0.06908332465572643, - 0.052009238267383615, - 0.039836767928889426, - 0.07113760520608924, - -0.11927656760812745, - -0.0838947122693098, - 0.12820925489701646, - -0.18002703768957923, - 0.042097727554874856, - -0.09560573888983673, - -0.12859682911678288, - -0.09247544846497209, - 0.007232044255733563, - 0.16221811451733717, - -0.19485291788708317, - -0.046600710774268, - 0.023978019017412443, - -0.039109878480868505, - 0.19599634196159174, - -0.17352979312027728, - 0.026844917413715, - 0.06364832974163001, - -0.008898177985178873, - -0.05940954195168736, - 0.008602923834989193, - 0.15722576325162582, - 0.022335337591913314, - -0.15931423816071263, - 0.14488086939206574, - 0.0185248365598922, - -0.16540159164955534, - 0.19791336076244054, - 0.07367186136326567, - -0.08881372149000946, - 0.18110051785829465, - -0.06096200797327724, - 0.1683103601602918, - -0.00646684187390373, - -0.17835969348519867, - 0.007715383818712402, - -0.021834179044233338, - 0.07791125018062676, - 0.07148066724034398, - -0.047989721905967245, - 0.07688567034556415, - 0.04204946664206943, - 0.098318935503801, - -0.15987118085368104, - -0.157359542585154, - -0.10868562025882644, - -0.13370625673050082, - -0.12982596020696216, - 0.15414381819839285, - 0.1406194246424957, - -0.13542966448516702, - 0.06417610935586696, - -0.15670403690856102, - 0.1327698470039879, - 0.008108816718807288, - 0.09013822820949219, - 0.13398446477135645, - -0.006082520677326981, - -0.18261800992037447, - -0.09919707849094019, - 0.15000376575879476, - -0.13728385301180426, - -0.0036070960784663876, - 0.11809576615421916, - 0.11076586493597416, - 0.07447795415523303, - -0.047869329435025386, - -0.19205059443591913, - -0.070147747557739, - 0.04739535797568771, - -0.04182393164656708, - -0.08572193022329572, - 0.02914912393100945 - ], - "y": [ - -0.15301437997105, - 0.3213860088445152, - 0.2604184401383524, - 1.5576807016334187, - 1.4113437343499209, - 0.2643987730152312, - 0.2693570240046678, - 0.27664366390303397, - 0.128197757695641, - -0.5532875229363937, - 0.3134536780469422, - -0.1389216278074129, - 0.12631943551576397, - 0.9864138929707987, - 0.13870804147155538, - -0.09476917096627427, - -0.8309520077639467, - -0.17734657010826652, - 0.18217246081989877, - 0.2477083239449639, - 0.5861366886027292, - 0.4455360330998765, - 0.7465424913935751, - -0.06725387834193752, - -1.3126384867258571, - 0.48843052955589694, - 0.28947977995589413, - 0.5025522246579026, - -0.29486558699218574, - -0.8575437776340076, - -0.7522861506609577, - -0.30332894799633114, - -0.09954565940239457, - 0.23813618583007165, - 0.13292629440480808, - -0.12653051566539117, - -0.33429070860521876, - 0.36119632875241037, - 1.1004908566153773, - 0.11971920664299252, - -0.261206620420543, - 0.06890406086763841, - 0.1058805271511468, - -0.01801133275133588, - -0.44958618439677533, - -1.1739956684718513, - -0.8106241561844381, - 0.1288776404870241, - -0.3033138796503666, - 0.050868998035187005, - -0.4955174802666303, - 0.060441311436597436, - 0.3130843912267881, - -0.18824060323665345, - -1.0600760235244828, - 0.15003533853206955, - 1.050897600784574, - 0.6498612766105015, - 0.0053116940268050725, - 0.008601971528667372, - 1.1037748445009197, - 0.22694312434043787, - 0.9931885765999081, - 1.392951917765181, - 0.03881335772992313, - 1.6164617310678966, - -0.09077029599766913, - -1.1042846305848077, - -0.3522332684692525, - -0.3150579399879033, - -1.9573882605752102, - 0.3878134427341167, - 0.4357994109717254, - -0.16398110876660435, - 0.04738499466843363, - 0.9199618423768696, - -0.41486401546044577, - -0.6738339014081826, - -0.09176619303037488, - 0.5516040652708741, - 0.5473950888908113, - 1.2919371399131465, - -0.062470527397032044, - 0.6676554916603052, - -0.07369680043157802, - 1.06844209185999, - 0.7473592636904324, - -0.10505492527223181, - -0.8402163840106307, - -0.09575416906560955, - -1.0094465235944963, - 0.7333654319253654, - 0.7267091252154091, - -0.39673104335605397, - -1.6627352449119213, - 1.9200063321334118, - 0.09106411543210338, - -1.2431002877517296, - 0.23282784474031498, - -0.592200889621595, - -0.448895995883034, - 0.2401557184800756, - -0.891805694598972, - 0.3768645308457176, - 0.09860500857702481, - -0.6327057148346267, - 0.22834627218232276, - 0.2682831008076155, - -1.5831974814678826, - 1.603842015206769, - -0.19408553918036156, - -0.6505754946205049, - 1.032472634344269, - 0.15172426318735754, - 0.783921376773656, - -1.131707144283875, - 0.5635542330157127, - 0.249289644046372, - 0.020773721927530273, - -0.010678927441218806, - -0.7750057623025229, - 0.46848560629227304, - 0.3255534534752694, - 0.742601627070257, - 0.43849836275663673, - 0.5039295791517683, - -0.5670577240173875, - 0.04118714841603517, - -0.40951545840160586, - 1.1333422111431362, - -0.15535086162780576, - -0.9086502992553623, - -1.29664167779368, - 0.31830182466844364, - 0.431133276578208, - -1.4169987825174262, - -0.27633193309916615, - -0.20519853451056635, - -1.0264111300141177, - -0.2645283617596661, - -0.09645660234082613, - -1.1428885309127002, - 0.1043205346127748, - -0.036039209416391056, - 1.3911743600911466, - -1.128961613554662, - 0.29306373336504926, - -0.5132090575053336, - -0.548920987009858, - 0.4891459741451133, - 1.6645144743997877, - -0.20623849763721394, - 0.4627149186611974, - 0.4659298511997271, - -1.2509320334995127, - 0.29823304697508085, - -0.4569207471241272, - 0.9681404452984049, - -1.9649431940857787, - -0.8799513142352064, - 0.9407317571508749, - 0.8670757360200358, - -0.23632595242411472, - 0.01703165973719882, - -1.5038311552698806, - -1.850954334753884, - -0.23435860032964273, - -0.24191514782345194, - 0.5399177321923979, - -0.19641126079821236, - -0.7289504952856043, - 0.03538239650747473, - 0.043658877503287646, - -0.4023784206049981, - 0.13994104816634134, - 0.097899551998586, - 0.5167452529381545, - 0.5833470424744641, - -0.1042451667788273, - -1.2573001459101594, - 0.44592915090648944, - -0.3443834780588313, - -1.3707443613424664, - -0.26353361515804474, - -1.6710983643081743, - -0.13581728692951592, - 0.3025097751477138, - 0.9848050234917216, - -0.01165413069327169, - 0.6314053942897084, - 0.13110443361832674, - -0.22258847311349325, - 0.9054232042201434, - -0.5481289810193638, - -0.34988948346979587, - -0.0459488372842332, - -0.08742609471319074, - 0.25213759701018373, - 0.5126563181387723, - -0.7041610260939984, - 0.5152718989702177, - -0.05282970253502626, - 1.1846497241336522, - 1.2494878410069068, - -0.04561952253573246, - 0.7333045228204672, - 0.12385508886080089, - -0.6590223131490576, - 0.3574202455378356, - 0.9109885950197133, - -0.12160621189680039, - -0.043097253241535745, - -1.0453810395600032, - -1.760263768045159, - -0.281922820971706, - 0.3818656459531349, - 0.9002327095885992, - -1.0043539723224533, - 1.6388002505624364, - 0.6291613234403036, - -0.009620882316767238, - -0.4735585765344223, - -0.6117578435307456, - -0.07733857488486075, - 0.8155253632526942, - 0.03798566375051011, - 1.2348577448661946, - -0.029602309683815976, - -0.3349511462317723, - 0.227672259317406, - -0.5425520305835457, - -0.42708433611223917, - 0.6891434279953931, - 0.9090342211705729, - -0.11964369374354021, - 1.1054056246740076, - -1.184062995035656, - 0.5523705317727369, - -0.08322657621163727, - -1.4742340304156052, - 0.06207715476034241, - 0.37793477539033893, - -0.3980240136295996, - 1.0808024905430538, - -0.16094808158409804, - 0.8063434521064029, - 0.14336078703193145, - 0.17992719004038082, - 0.7068556287309752, - -0.7578952826943709, - 0.11824482013604995, - 0.14379380989081653, - -0.23788237605313656, - 0.37763030822077254, - -0.32282602695072293, - 0.16715072955760985, - 0.15887705003181812, - -0.5008607127912784, - -0.6882096353561602, - 0.60136009119467, - 0.8488141514364567, - 0.8430713428015675, - -0.24065692133856867, - -0.6472931363825261, - 1.3086290447547462, - 1.5760398596235383, - 0.34176864140286967, - 0.6824703916394234, - 0.6540484760898516, - -0.9493078853009523, - -0.28801644201812293, - -0.488076558511294, - -1.1099684779589567, - 7.518920094929756E-4, - 0.25749966755643205, - -0.8049419092040209, - 0.21174333704124756, - 1.2672566726295291, - -0.4784603154828383, - -0.09406310009168195, - -0.19673122033483514, - 0.23953889390593958, - -0.35973842367050585, - 0.6592423082796158, - 1.2370272252742944, - -0.056062374940619566, - -1.0137456308070254, - -0.16397976776976225, - -0.014624983707894244, - -0.166654831375075, - -0.32212413766275816, - 0.4823007224906907, - -0.43064269605171107, - 0.7437857526274558, - -0.9395258060016881, - 0.6618228136442154, - 0.24977452069982947, - 0.35064396989400015, - -0.5541969530122689, - 0.10358215386747288, - -1.0044055599519428, - 0.5308469888615068, - -0.9849134140769973, - -0.15660045332655323, - -0.013345500114138705, - -0.13869202818023543, - 0.25974220951030286, - -1.023781938157092, - 0.970730525172316, - -1.3404181467659124, - 0.975082031404889, - 0.046317734198942674, - 1.7713098129191707, - -0.04887126172422783, - 0.29833860620553276, - 1.7620696232399091, - 0.027129747296123042, - -1.3679617989809034, - 0.09311435768004381, - 0.9630496902115169, - 0.5803225496758074, - 0.08855020935786416, - 0.6029403106885801, - 0.28949669323927235, - -0.23859191354615641, - 0.2825717836403466, - -0.200109144023236, - -0.43357233542394846, - -0.15854628256061082, - -0.12239046901192145, - 1.2486720346003892, - -0.7590053379723941, - -0.8232332280411517, - 0.2708259804824738, - -0.18461456556146413, - -0.464305159749146, - 0.499299334045749, - 0.017991108674833934, - 0.25613130179770716, - 0.6505015627528846, - 0.5819915980295804, - 1.0154787153052467, - 0.18604421335836183, - -1.0261003144606073, - 0.3096867648042249, - 0.3336375247791076, - -0.3352872896658244, - -0.9963399574269691, - -0.07887713426690576, - -0.06593362553156626, - -1.2599556405041694, - 0.7658144583259151, - -0.17777003976270447, - 0.7658133976772592, - -0.8873679937301202, - 0.5028470409689616, - 0.49582223663049385, - 0.36403952750135454, - 0.26483328669756695, - 0.4609996760558505, - -0.8251633577235368, - -0.7252292214754474, - 0.9335455057697211, - -0.4472614391170916, - -0.07995137562743608, - -0.51357345804906, - -0.6499524183188709, - -0.11457615483234394, - 0.8363437398237723, - 0.7880576914963741, - -1.1865382370445867, - -0.439042724304974, - -1.4717200672939095, - 0.7177431986617044, - 0.7656906704533467, - 0.5422372067621204, - -1.7113376940428948, - 0.3609901465311741, - 1.3031354746535622, - -1.0657452599244759, - 0.40449243518502437, - 0.1045697881153259, - -1.2045994930309303, - -0.7715641903459555, - -0.20775042556209217, - -0.1695519857689059, - -0.16764569315283906, - -0.5607947618771214, - 0.9286002479879858, - -0.2631057402119721, - -0.1440413014890081, - -1.2228525381964466, - -0.004913621533659078, - -0.32982531327185455, - -0.3786509703414725, - -0.03837066232371396, - 0.6440656975350038, - 0.3898249894548805, - -0.26916036792086256, - 1.343558723373756, - 0.018285663274745984, - 0.3677390673478808, - 1.119808771457701, - -0.2667741424814469, - 1.2956263221087354, - 0.674215318506462, - 0.05931926150631952, - 0.8387205964219164, - 0.8378391827096728, - -0.42107835495028445, - -1.2048220084877328, - 0.10026382221011161, - 0.11329995098969674, - -0.3390244016985369, - -0.5325901388958182, - -0.5060294739800626, - -0.13307299219507446, - 0.024803278884042494, - -0.9157132898844885, - -0.24885431209964806, - 0.2965835261826779, - 0.1447192320405867, - 0.08892848565989808, - -0.5313723868217886, - 0.29464719986537435, - -0.7230879192144544, - -0.08735198130864887, - 0.6245834355340425, - -1.130388655392741, - -0.42761372311712476, - 0.7964219647324434, - 0.9938905545043052, - 0.5811077726440276, - 0.06472074465833187, - -0.24114058656961873, - -1.0631817150216425, - -0.13222407372542483, - 0.18766543635088076, - -1.3056205739925941, - -0.6248324661916649, - -0.5992283935142848, - -0.6597422681884305, - -0.40602654711756225, - -0.5017297706692982, - -0.3404252265736823, - 0.6039157552432116, - -0.8538738534200835, - 0.12143610256287654, - -0.22042108929901216, - -1.1389118572645214, - 0.609909426081653, - 0.06864883104538085, - -0.9282441580657186, - 0.3921991085919522, - -0.8472138441468573, - -1.1401387252072426, - -1.2608747244152902, - -0.15543770029604703, - 0.3743007484568634, - -0.32626781094654833, - -1.4925212910474333, - -1.1310361781491753, - -0.7496838327803892, - 0.1721789714726251, - -0.45826821031052195, - -0.41263652995570066, - 0.16406127495576725, - 0.8247198015247821, - -0.2735596642022021, - 0.3972300774688153, - -0.2689965222276732, - 0.038564130778401394, - -0.1464203444246901, - 0.7276706260841134, - -0.4934675760953334, - -0.449420485174567, - -0.5252595579028144, - -0.3025029371376858, - 0.14605920763611197, - -0.3532650091249646, - 0.8305666929661976, - 0.004969127991753145, - -0.6530105409230131, - 0.673896393099993, - -1.3288548637282398, - -0.7079218846206845, - -1.2399418292152982, - -0.3507823398214642, - 1.4407853696005044, - -0.11546811859598995, - -0.12509348299111042, - 0.8030434123983735, - -0.22128031356928438, - 0.8808821285684807, - 0.49617963600619575, - -1.1806460542330321, - -0.39689022705040417, - 0.7966748714482194, - 0.722606353506844, - 0.8765051029365909, - -1.195683903573502, - -1.6620369258828132, - 0.045294661869221736, - -0.76975187777211, - 1.3335011080683947, - -0.5553493481216075, - -0.1160877348972047, - 0.6072904147385466, - 0.8347531756510136, - -0.5672681168308594, - 0.2937706515374221, - -0.004150177814565539, - -0.6236670264505197, - -0.6240225786432583, - 0.9545581179839779, - -0.43584659717012325, - -0.2745631812535526, - -1.3257315916652592, - -0.03791826878294506, - 0.4819919167542062, - 0.2995469507076062, - 0.2860646186104262, - -0.6494122122031096, - -0.690147840282516, - -0.24356864544039084, - -0.09079418914826366, - 0.1624168516047696, - -0.5812139660880296, - 0.760035607054712, - 0.3749420151426396, - 0.016014457887083076, - -0.5643064453842966, - -0.9317376164970161, - -1.2050929368345655, - 0.7037543908438333, - 0.22769167115648328, - -0.5069085287897545, - 0.5205029941850191, - 0.08670625616422449, - -0.17353126848448092, - -0.3067047099386662, - 0.3365269923539015, - -0.15315717910831203, - -0.19190665880445926, - -0.2896809257588623, - 0.0599771328535715, - -0.22482441960241104, - 0.31697716619165, - -0.17103421816930314, - 0.5465306924217281, - 1.1428559707372168, - 0.8811411064443713, - -0.6743216268658035, - -0.59506089649305, - -0.5485277819840544, - -0.5682219603768915, - -0.6805889712382598, - 0.37632223998851305, - 0.21677725036576914, - -0.09777659346833067, - 0.7970481537575317, - -0.7255665946908553, - -0.05254674417132981, - -0.07025667819913191, - -0.5969805065448943, - -0.19590566074647858, - 1.0007741466160196, - -0.2459784815710585, - 0.17586665639316487, - 0.6560027062426063, - -0.5792451459988985, - -0.026146519703195915, - 0.07663690170023768, - 0.8764489950667644, - -1.0016974926903055, - -0.6992507154658733, - -0.8316554387119782, - 0.2178756313353997, - -1.1184035259204212, - -1.2084292803370484, - -0.590002124773795, - -0.14282564510481024, - -0.41559699959762186, - -0.37011180002407107, - 0.9431877931102849, - -0.7653762008554557, - -0.18284861199379782, - 0.015802399595516978, - 0.554396695473939, - 0.0018305571779806944, - 0.49644540204546433, - 0.42178658245793554, - 0.4308610628185202, - -0.41851641272833745, - 0.9827707750319898, - 0.22394244861513934, - -0.13245368871023935, - 0.015437722510435816, - 0.6709682228992988, - -0.485617271776588, - -0.34732140298797815, - 0.1788721180237173, - -0.022642423363683342, - 0.10221103058706356, - 0.7493747137166218, - 0.019339267521172647, - 0.9701509108187862, - -0.0543390892494502, - 0.19537880581826175, - -0.8547779642811562, - 0.9028455108668682, - 0.6586881361307687, - -0.35362755064199997, - -0.23653046755059973, - 0.5429939017855365, - 0.5074471541933775, - -0.23550234327305383, - 0.49370491148266066, - 0.7205341803382498, - 0.7257089435130264, - -1.7569335458261317, - 1.837202358509002, - 0.6776725000876546, - -0.3169366679950562, - -0.47563873227500364, - 0.09139907380135963, - 0.5290674828689154, - 0.07384755039357216, - 0.42831791371401273, - -0.018742285067405846, - 1.7750944743117651, - 0.6271650752453236, - -0.08226981836939772, - 1.087570680353463, - 0.413884687937685, - -0.2715177087888687, - 1.6324776918051396, - 0.5450107076300342, - 0.1778544670819083, - -0.41029897604449994, - 0.5125137174881665, - 0.08806946066415734, - 0.4657496509241864, - 1.039061249079823, - 0.12411578497763291, - 0.5305968494152972, - 0.5986104191737679, - -1.3227725979564595, - -0.11519382873558076, - 0.08815563817333279, - -0.36274116384957583, - -0.39522982324005884, - -0.1371328688075758, - -0.6130182085343819, - 0.6542083925376304, - 1.075338969041045, - -0.3802337559081979, - -0.7502696790851888, - 0.6956900837603146, - 1.234057484759378, - 0.681863631880364, - -0.21753376058186938, - 1.3238206490043756, - -1.2096286235192464, - -0.9117717548890075, - -0.37170409591014675, - -0.03756551865528295, - 0.35097799997484797, - -0.9859781253843698, - -0.17219767812760167, - -0.8437311193588898, - 0.1876057466674505, - -0.6482820626863774, - -0.606462114317217, - -0.6606105639325159, - 0.7363142771684608, - 0.062273256414571675, - 0.29127866226720067, - 0.147244290234244, - 0.3494630378896125, - 0.9782038099985576, - -0.16641804730710122, - 0.7905730518821413, - -1.6965386200308787, - -0.22560208927742653, - 0.06242150777424035, - 0.6841893580296609, - -0.20414366047284738, - -0.661947666011661, - -0.21846333104985902, - -0.28004817925450937, - -1.9429380230750941, - -1.2855366927074412, - -0.7959005163824101, - -0.028364550621987904, - 0.3556610956104884, - -0.8135114110245536, - 1.0892177465221557, - -0.22834315274422026, - -0.2559911125648778, - -0.3055966045890318, - 1.2705453910792117, - -0.6352820814274245, - -1.6255479733958955, - -0.4442670364252169, - 0.36188636689091386, - 0.3091984810981612, - -0.6572059302986681, - 0.3851453994627402, - 0.9981382057179816, - -0.3158056813555188, - 0.09327926572036427, - -0.4464893404969738, - 0.4844561018680667, - -0.03803186975167783, - 0.7678712612559632, - 0.0749661408909717, - -0.3105716916856579, - -0.8878262627567984, - 0.8129432660604093, - 0.46185459360177283, - -0.5392258875476569, - -0.7797512197550032, - -0.1706195509283849, - 0.7022094114106948, - -0.5469206353660323, - 0.7896667742044485, - 0.7959850880367, - 0.05522538822114961, - 1.5368059480831398, - -0.44159471800426553, - -0.8954088464612937, - -0.6411379726570363, - 0.6348812862307636, - -0.35263574107192797, - -1.19848368960449, - -0.5843407051682407, - -0.3746998767435111, - 0.38230788148928163, - -1.3166174833326925, - 0.48508014292007967, - 0.001240401562760917, - 0.7023656744309447, - 0.8178363823357205, - 0.4369632356090235, - -1.3134886620105286, - -0.5342827158821787, - -0.1496983441171279, - 0.6563662007329267, - 1.6088978070879218, - -0.823402547518619, - -0.3198537552824771, - 0.025604628918326634, - -0.8646263121870907, - 0.07964811124636033, - -0.028307986265926222, - -0.03713369187390699, - -0.3312764539206307, - -0.6893374555678627, - 0.14124668697940945, - 0.19297651590943418, - -0.06846661476725889, - -0.3501059720252227, - -0.280282490645595, - -0.06225703851753191, - -0.1280739846202859, - -0.4624768146236727, - -0.6784626597925163, - 0.0946380560784403, - -0.20871712439560097, - -1.0812845846778996, - 0.08548142810804381, - 0.3717810509790422, - 1.1546022177556179, - -0.4882223764890888, - -0.5911360108244468, - 0.47276393720842336, - -0.661641435090182, - 0.15941429699113396, - 0.6860593302392283, - -0.9431450708804671, - 0.5976757436626472, - 0.7455895659983497, - -1.3435968446806092, - -0.04534013658763317, - -0.32912539922752426, - 0.6588601205294702, - 0.6492395540500334, - 0.15870508910573292, - 0.17271591858295113, - -1.6185608212508753, - -1.1600952733170988, - 0.14011498684921228, - 0.5600278436814841, - 1.0252249101692579, - 1.1225992685160449, - -0.7851587009979921, - 0.491958064123449, - 0.16839355474511863, - 0.52533968878768, - 0.45850233102244836, - -0.03274997127812013, - 1.1614815045174753, - 0.2866681209232796, - 0.9139293079894977, - 0.11400407324792101, - -0.8524520297054505, - -0.009540195650290217, - 0.035768703546328176, - 0.6076611504935259, - -0.28362208002668965, - -0.3491338708001264, - 1.258417885985821, - 0.22115593262073827, - -0.41645566224880415, - -0.5227674052484298, - -0.250426376135792, - -0.1299981185458199, - -0.4526490817486374, - 0.22547604298223112, - 0.642783536337899, - -0.33509761868101845, - 0.5929999099500157, - -1.3154782738257698, - 1.1474477711539817, - 0.6097700064086156, - -0.4553567361662137, - -0.7961884032706887, - -0.6785274031440033, - -0.30197956555303945, - -0.6605233431068976, - -1.7939197714609465, - -0.3344022158290913, - -0.5171751733837753, - -0.6110236171821302, - -0.8178519091627887, - -1.1801072215789237, - -0.5991354240176784, - -1.8520222038375813, - -0.34088020365627936, - -0.3658766732691894, - -0.5503719689441744, - -0.5138706915202661, - -1.0729576229692417, - -0.507943421117049, - -0.8790839063013742, - -0.6860427846650597, - -0.9505188095487928, - -0.2622099040443325, - -0.4231684748899264, - -0.5633190970706533, - -1.1108083460917109, - -1.5752749355351545, - -0.42493337856689284, - -0.2717075317521898, - -0.7145937796008616, - -0.2834633382215218, - -0.4260133266886562, - -1.6618130089430732, - -1.1091628942370608, - -1.608369005812391, - -0.5475063582352403, - -1.0879335992300185, - -0.42837202259584367, - -1.1686490350737677, - -0.6452486957618453, - -0.6615444072163892, - -0.5643577635769275, - -0.5472604284057888, - -1.3335710083585874, - -0.2652016062849905, - -0.26089908563630576, - -0.7609038150604371, - -0.48827298147433834, - -0.2988353656362835, - -0.8859499156500548, - -0.32909655565880425, - -1.2924850686198488, - -0.3320628260022395, - -0.8514261657669897, - -0.9063922529750973, - -0.5609072520822797, - -1.5996173932679036, - -0.3932225311134939, - -1.0980121634453077, - -0.5395269375535947, - -0.7604172819787173, - -0.5796377498585633, - -0.3711110829487401, - -1.0743134791167757, - -0.33065186597196733, - -0.38172893542150016, - -0.3384953324518427, - -1.1453266792811734, - -0.7393295631765994, - -1.0574593213279724, - -0.5821020687264945, - -1.6600274083483861, - -0.8418556005906928, - -0.29973632054205934, - -0.9905079600901129, - -1.199364342401775, - -0.47566601099965483, - -0.9203076448654368, - -0.21427543225599788, - -0.9495599443547348, - -0.29941441262677015, - -1.3851494307091914, - -0.9354288418411322, - -0.7491344236210977, - -0.5608516792875113, - -0.3195484980833957, - -0.2650177516028934, - -0.9355529949860277, - -0.8467283394035142, - -0.23874850335922454, - -1.3307676986201595, - -0.6899273309985843, - -0.2796913337217296, - -0.4903200584995291, - -0.49381457862393835, - -0.7902771175103981, - 0.3198891373276655, - 1.2224420571901917, - 0.6701953692678642, - 0.7477184387669606, - 0.820864099116711, - 0.5113525826652402, - 0.20528735700177222, - 0.5643062235285118, - 0.7817645773552057, - 0.9047440400858373, - 0.22032478083878332, - 0.3883035761723866, - 0.4041603773604231, - 1.3256431713635248, - 0.4950573882457931, - 0.7891107256768725, - 0.4879849305613825, - 0.45734149982933403, - 0.6762011441254779, - 0.5066216683422085, - 0.3029807660366283, - 0.6259278309087696, - 0.8005565651344673, - 0.8042908454116239, - 0.49179205270167775, - 0.386917375588718, - 0.43856873644674976, - 0.8479919408413905, - 0.9513759598737783, - 1.2509864127661456, - 0.64839351496744, - 1.4148273331192363, - 0.9190030545717754, - 0.5704230952842605, - 0.2647647032612503, - 0.6519305841454981, - 0.686861567877312, - 1.0167444890758688, - 0.6132546161462723, - 0.2584881244269224, - 1.6454515566487125, - 0.3275123357337015, - 1.2362793861663945, - 0.5304314785882482, - 0.3627633280194873, - 0.8307166518393406, - 0.24092526319262303, - 1.0701025038429335, - 0.6606187195380192, - 0.45403459382476014, - 0.38397506954553157, - 0.7472963199469107, - 0.47726341597163224, - 0.7514134031954369, - 0.5819303289421425, - 1.3059854779410622, - 0.651685212098664, - 0.5065134398644039, - 1.0831974075756816, - 0.7332739059485318, - 0.5218106449764864, - 0.7743456734072737, - 0.9253198107959261, - 1.0026382000602738, - 0.28466129300496035, - 0.21024124978558695, - 0.5722803087001817, - 0.833285045952612, - 0.31179724942954024, - 0.8303928633070755, - 0.3711495441593481, - 0.5944184507269621, - 0.473847643811217, - 0.28310229715602026, - 0.3577790100763111, - 0.2313552354194825, - 1.1919395258218959, - 0.23772719618893487 - ], - "type": "scatter" - }, - { - "marker": { - "color": "#00CC96" - }, - "mode": "markers", - "name": "Expectation", - "x": [ - 4.53615413392031E-17 - ], - "y": [ - 1.0149609892054728E-17 - ], - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "140226884469712", - "line": { - "color": "#AB63FA" - }, - "mode": "lines", - "name": "Mode", - "showlegend": true, - "x": [ - -0.2, - -0.2, - -0.2, - -0.2, - -0.2, - null, - 0.2, - 0.2, - 0.2, - 0.2, - 0.2, - null - ], - "y": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - null, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - null - ], - "type": "scatter" - } - ], - "layout": { - "title": { - "text": "Marginal View of relative x and y position of the robot with respect to the object." - }, - "xaxis": { - "title": { - "text": "relative_x" - } - }, - "yaxis": { - "title": { - "text": "relative_y" - } - }, - "template": { - "data": { - "histogram2dcontour": [ - { - "type": "histogram2dcontour", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "choropleth": [ - { - "type": "choropleth", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "histogram2d": [ - { - "type": "histogram2d", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "heatmap": [ - { - "type": "heatmap", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "heatmapgl": [ - { - "type": "heatmapgl", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "contourcarpet": [ - { - "type": "contourcarpet", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "contour": [ - { - "type": "contour", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "surface": [ - { - "type": "surface", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "mesh3d": [ - { - "type": "mesh3d", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "scatter": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scatter" - } - ], - "parcoords": [ - { - "type": "parcoords", - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterpolargl": [ - { - "type": "scatterpolargl", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "bar": [ - { - "error_x": { - "color": "#f2f5fa" - }, - "error_y": { - "color": "#f2f5fa" - }, - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "bar" - } - ], - "scattergeo": [ - { - "type": "scattergeo", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterpolar": [ - { - "type": "scatterpolar", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "histogram": [ - { - "marker": { - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "histogram" - } - ], - "scattergl": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scattergl" - } - ], - "scatter3d": [ - { - "type": "scatter3d", - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scattermapbox": [ - { - "type": "scattermapbox", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterternary": [ - { - "type": "scatterternary", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scattercarpet": [ - { - "type": "scattercarpet", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "carpet": [ - { - "aaxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "baxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "type": "carpet" - } - ], - "table": [ - { - "cells": { - "fill": { - "color": "#506784" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "header": { - "fill": { - "color": "#2a3f5f" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "type": "table" - } - ], - "barpolar": [ - { - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "barpolar" - } - ], - "pie": [ - { - "automargin": true, - "type": "pie" - } - ] - }, - "layout": { - "autotypenumbers": "strict", - "colorway": [ - "#636efa", - "#EF553B", - "#00cc96", - "#ab63fa", - "#FFA15A", - "#19d3f3", - "#FF6692", - "#B6E880", - "#FF97FF", - "#FECB52" - ], - "font": { - "color": "#f2f5fa" - }, - "hovermode": "closest", - "hoverlabel": { - "align": "left" - }, - "paper_bgcolor": "rgb(17,17,17)", - "plot_bgcolor": "rgb(17,17,17)", - "polar": { - "bgcolor": "rgb(17,17,17)", - "angularaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "radialaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "ternary": { - "bgcolor": "rgb(17,17,17)", - "aaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "baxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "caxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "coloraxis": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "colorscale": { - "sequential": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "sequentialminus": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "diverging": [ - [ - 0, - "#8e0152" - ], - [ - 0.1, - "#c51b7d" - ], - [ - 0.2, - "#de77ae" - ], - [ - 0.3, - "#f1b6da" - ], - [ - 0.4, - "#fde0ef" - ], - [ - 0.5, - "#f7f7f7" - ], - [ - 0.6, - "#e6f5d0" - ], - [ - 0.7, - "#b8e186" - ], - [ - 0.8, - "#7fbc41" - ], - [ - 0.9, - "#4d9221" - ], - [ - 1, - "#276419" - ] - ] - }, - "xaxis": { - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "automargin": true, - "zerolinewidth": 2 - }, - "yaxis": { - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "automargin": true, - "zerolinewidth": 2 - }, - "scene": { - "xaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - }, - "yaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - }, - "zaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - } - }, - "shapedefaults": { - "line": { - "color": "#f2f5fa" - } - }, - "annotationdefaults": { - "arrowcolor": "#f2f5fa", - "arrowhead": 0, - "arrowwidth": 1 - }, - "geo": { - "bgcolor": "rgb(17,17,17)", - "landcolor": "rgb(17,17,17)", - "subunitcolor": "#506784", - "showland": true, - "showlakes": true, - "lakecolor": "rgb(17,17,17)" - }, - "title": { - "x": 0.05 - }, - "updatemenudefaults": { - "bgcolor": "#506784", - "borderwidth": 0 - }, - "sliderdefaults": { - "bgcolor": "#C8D4E3", - "borderwidth": 1, - "bordercolor": "rgb(17,17,17)", - "tickwidth": 0 - }, - "mapbox": { - "style": "dark" - } - } - } - }, - "config": { - "plotlyServerURL": "https://plot.ly" - } - }, - "text/html": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "execution_count": 4 - }, - { - "cell_type": "markdown", - "source": [ - "Next, we will perform pick up tasks using the default policy and observe the success rate.\n", - "The robot will now experiment with the behaviour specified by the default policy and observe his success rate in doing so.\n", - "After finishing the experiments, we insert the results into the database." - ], - "metadata": { - "collapsed": false - }, - "id": "b5cb56317f506b04" - }, - { - "cell_type": "code", - "source": [ - "pycram.orm.base.ProcessMetaData().description = \"Experimenting with Pick Up Actions\"\n", - "fpa.sample_amount = 500\n", - "print(world.current_world)\n", - "with simulated_robot:\n", - " print(world.current_world)\n", - " fpa.batch_rollout()\n", - "task_tree.insert(session)\n", - "reset_tree()\n", - "session.commit()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:18.971938Z", - "start_time": "2024-06-19T12:34:31.757653Z" - } - }, - "id": "7461b696b899f603", - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - " 0%| | 1/500 [00:00<00:58, 8.54it/s, Success Probability=0]" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1718800471.819477]: Waiting for IK service: /pr2_left_arm_kinematics/get_ik\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "100%|██████████| 500/500 [00:42<00:00, 11.66it/s, Success Probability=0.148]\n", - "Inserting TaskTree into database: 100%|██████████| 4873/4873 [00:04<00:00, 1124.99it/s]\n" - ] - } - ], - "execution_count": 5 - }, - { - "cell_type": "markdown", - "source": [ - "Let's query the data that is needed to learn a pick up action and have a look at it." - ], - "metadata": { - "collapsed": false - }, - "id": "2122db4e86143db9" - }, - { - "cell_type": "code", - "source": [ - "samples = pd.read_sql(fpa.query_for_database(), engine)\n", - "samples" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:18.985317Z", - "start_time": "2024-06-19T12:35:18.972729Z" - } - }, - "id": "5d9cd8fe3195d669", - "outputs": [ - { - "data": { - "text/plain": [ - " arm grasp relative_x relative_y\n", - "0 left front -0.414498 0.464266\n", - "1 right front -0.660237 -0.222640\n", - "2 left right -0.579790 -0.067381\n", - "3 left right -0.309193 -0.553711\n", - "4 right right -0.564946 -0.297454\n", - ".. ... ... ... ...\n", - "69 right right -0.167057 0.345697\n", - "70 left front -0.094622 0.572538\n", - "71 left right 0.169038 0.451932\n", - "72 right left 0.088937 0.682180\n", - "73 right right 0.027124 0.305716\n", - "\n", - "[74 rows x 4 columns]" - ], - "text/html": [ - "
\n", - "\n", - "\n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - " \n", - "
armgrasprelative_xrelative_y
0leftfront-0.4144980.464266
1rightfront-0.660237-0.222640
2leftright-0.579790-0.067381
3leftright-0.309193-0.553711
4rightright-0.564946-0.297454
...............
69rightright-0.1670570.345697
70leftfront-0.0946220.572538
71leftright0.1690380.451932
72rightleft0.0889370.682180
73rightright0.0271240.305716
\n", - "

74 rows × 4 columns

\n", - "
" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "execution_count": 6 - }, - { - "cell_type": "markdown", - "source": [ - "We can now learn a probabilistic model from the data. We will use the JPT algorithm to learn a model from the data." - ], - "metadata": { - "collapsed": false - }, - "id": "b51a7ee5d60ae120" - }, - { - "cell_type": "code", - "source": [ - "variables = infer_variables_from_dataframe(samples, scale_continuous_types=False)\n", - "model = JPT(variables, min_samples_leaf=25)\n", - "model.fit(samples)\n", - "model = model.probabilistic_circuit" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:18.993419Z", - "start_time": "2024-06-19T12:35:18.985971Z" - } - }, - "id": "8cfff78b3abd589b", - "outputs": [], - "execution_count": 7 - }, - { - "cell_type": "code", - "source": [ - "arm, grasp, relative_x, relative_y = model.variables" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:18.998232Z", - "start_time": "2024-06-19T12:35:18.994937Z" - } - }, - "id": "77642e8f22ac880a", - "outputs": [], - "execution_count": 8 - }, - { - "cell_type": "markdown", - "source": [ - "Let's have a look at how the model looks like. We will visualize the model density when we condition on grasping the object from the front with the left arm." - ], - "metadata": { - "collapsed": false - }, - "id": "9d6f0b2705e5ad5a" - }, - { - "cell_type": "code", - "source": [ - "event = SimpleEvent({arm:Arms.LEFT, grasp: Grasp.FRONT}).as_composite_set()\n", - "conditional_model, conditional_probability = model.conditional(event)\n", - "p_xy = conditional_model.marginal([relative_x, relative_y])\n", - "fig = go.Figure(p_xy.plot(), p_xy.plotly_layout())\n", - "fig.show()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:19.052051Z", - "start_time": "2024-06-19T12:35:18.999301Z" - } - }, - "id": "825dceb35326faf7", - "outputs": [ - { - "data": { - "application/vnd.plotly.v1+json": { - "data": [ - { - "legendgroup": "1", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - -0.7239851799129747, - -0.2675667293707046, - null, - -0.2675667293707046, - 0.21043272769243637, - null, - 0.21043272769243637, - 0.6802107054679791, - null - ], - "xaxis": "x", - "y": [ - 0.9737652891034544, - 0.9737652891034544, - null, - 0.5811257181848325, - 0.5811257181848325, - null, - 0.5912958693659719, - 0.5912958693659719, - null - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "1", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.7239851799129747, - -0.2675667293707046, - -0.2675667293707046, - 0.21043272769243637, - 0.21043272769243637, - 0.6802107054679791 - ], - "xaxis": "x", - "y": [ - 0.0, - 0.4444444444444444, - 0.4444444444444444, - 0.7222222222222222, - 0.7222222222222222, - 1.0 - ], - "yaxis": "y", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "1", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - -0.7239851799129747, - -0.7239851799129747, - -0.2675667293707046, - -0.2675667293707046, - null - ], - "xaxis": "x", - "y": [ - 0, - 1.0224535535586274, - 1.0224535535586274, - 0, - null - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "1", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - -0.10457966991274159, - -0.10457966991274159 - ], - "xaxis": "x", - "y": [ - 0, - 1.0224535535586274 - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - -0.6979180764986803, - -0.44438768097407855, - null, - -0.44438768097407855, - 0.08272590549147685, - null, - 0.08272590549147685, - 0.6808958390540965, - null - ], - "xaxis": "x2", - "y": [ - 1.2052028512135755, - 1.2052028512135755, - null, - 0.737770565726636, - 0.737770565726636, - null, - 0.5108173086128014, - 0.5108173086128014, - null - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.6979180764986803, - -0.44438768097407855, - -0.44438768097407855, - 0.08272590549147685, - 0.08272590549147685, - 0.6808958390540965 - ], - "xaxis": "x2", - "y": [ - 0.0, - 0.3055555555555556, - 0.3055555555555556, - 0.6944444444444444, - 0.6944444444444444, - 1.0 - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "2", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - -0.6979180764986803, - -0.6979180764986803, - -0.44438768097407855, - -0.44438768097407855, - null - ], - "xaxis": "x2", - "y": [ - 0, - 1.2654629937742543, - 1.2654629937742543, - 0, - null - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - -0.12817762498549254, - -0.12817762498549254 - ], - "xaxis": "x2", - "y": [ - 0, - 1.2654629937742543 - ], - "yaxis": "y2", - "type": "scatter" - } - ], - "layout": { - "annotations": [ - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.22, - "xanchor": "center", - "xref": "paper", - "y": 1.0, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.76, - "xanchor": "center", - "xref": "paper", - "y": 1.0, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "P(Leaf = 0) = 1.0", - "textangle": 90, - "x": 0.98, - "xanchor": "left", - "xref": "paper", - "y": 0.5, - "yanchor": "middle", - "yref": "paper" - } - ], - "height": 300, - "template": { - "data": { - "barpolar": [ - { - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "barpolar" - } - ], - "bar": [ - { - "error_x": { - "color": "#f2f5fa" - }, - "error_y": { - "color": "#f2f5fa" - }, - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "bar" - } - ], - "carpet": [ - { - "aaxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "baxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "type": "carpet" - } - ], - "choropleth": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "choropleth" - } - ], - "contourcarpet": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "contourcarpet" - } - ], - "contour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "contour" - } - ], - "heatmapgl": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "heatmapgl" - } - ], - "heatmap": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "heatmap" - } - ], - "histogram2dcontour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "histogram2dcontour" - } - ], - "histogram2d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "histogram2d" - } - ], - "histogram": [ - { - "marker": { - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "histogram" - } - ], - "mesh3d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "mesh3d" - } - ], - "parcoords": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "parcoords" - } - ], - "pie": [ - { - "automargin": true, - "type": "pie" - } - ], - "scatter3d": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatter3d" - } - ], - "scattercarpet": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattercarpet" - } - ], - "scattergeo": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattergeo" - } - ], - "scattergl": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scattergl" - } - ], - "scattermapbox": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattermapbox" - } - ], - "scatterpolargl": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolargl" - } - ], - "scatterpolar": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolar" - } - ], - "scatter": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scatter" - } - ], - "scatterternary": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterternary" - } - ], - "surface": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "surface" - } - ], - "table": [ - { - "cells": { - "fill": { - "color": "#506784" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "header": { - "fill": { - "color": "#2a3f5f" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "type": "table" - } - ] - }, - "layout": { - "annotationdefaults": { - "arrowcolor": "#f2f5fa", - "arrowhead": 0, - "arrowwidth": 1 - }, - "autotypenumbers": "strict", - "coloraxis": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "colorscale": { - "diverging": [ - [ - 0, - "#8e0152" - ], - [ - 0.1, - "#c51b7d" - ], - [ - 0.2, - "#de77ae" - ], - [ - 0.3, - "#f1b6da" - ], - [ - 0.4, - "#fde0ef" - ], - [ - 0.5, - "#f7f7f7" - ], - [ - 0.6, - "#e6f5d0" - ], - [ - 0.7, - "#b8e186" - ], - [ - 0.8, - "#7fbc41" - ], - [ - 0.9, - "#4d9221" - ], - [ - 1, - "#276419" - ] - ], - "sequential": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "sequentialminus": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - }, - "colorway": [ - "#636efa", - "#EF553B", - "#00cc96", - "#ab63fa", - "#FFA15A", - "#19d3f3", - "#FF6692", - "#B6E880", - "#FF97FF", - "#FECB52" - ], - "font": { - "color": "#f2f5fa" - }, - "geo": { - "bgcolor": "rgb(17,17,17)", - "lakecolor": "rgb(17,17,17)", - "landcolor": "rgb(17,17,17)", - "showlakes": true, - "showland": true, - "subunitcolor": "#506784" - }, - "hoverlabel": { - "align": "left" - }, - "hovermode": "closest", - "mapbox": { - "style": "dark" - }, - "paper_bgcolor": "rgb(17,17,17)", - "plot_bgcolor": "rgb(17,17,17)", - "polar": { - "angularaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "bgcolor": "rgb(17,17,17)", - "radialaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "scene": { - "xaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - }, - "yaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - }, - "zaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - } - }, - "shapedefaults": { - "line": { - "color": "#f2f5fa" - } - }, - "sliderdefaults": { - "bgcolor": "#C8D4E3", - "bordercolor": "rgb(17,17,17)", - "borderwidth": 1, - "tickwidth": 0 - }, - "ternary": { - "aaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "baxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "bgcolor": "rgb(17,17,17)", - "caxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "title": { - "x": 0.05 - }, - "updatemenudefaults": { - "bgcolor": "#506784", - "borderwidth": 0 - }, - "xaxis": { - "automargin": true, - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "zerolinewidth": 2 - }, - "yaxis": { - "automargin": true, - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "zerolinewidth": 2 - } - } - }, - "title": { - "text": "Joint Probability Tree over 2 variables" - }, - "width": 1200, - "xaxis": { - "anchor": "y", - "domain": [ - 0.0, - 0.44 - ], - "title": { - "text": "relative_x" - } - }, - "yaxis": { - "anchor": "x", - "domain": [ - 0.0, - 1.0 - ] - }, - "xaxis2": { - "anchor": "y2", - "domain": [ - 0.54, - 0.98 - ], - "title": { - "text": "relative_y" - } - }, - "yaxis2": { - "anchor": "x2", - "domain": [ - 0.0, - 1.0 - ] - } - }, - "config": { - "plotlyServerURL": "https://plot.ly" - } - }, - "text/html": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "execution_count": 9 - }, - { - "cell_type": "markdown", - "source": [ - "Let's make a monte carlo estimate on the success probability of the new model." - ], - "metadata": { - "collapsed": false - }, - "id": "f6e58eaa8d627dcc" - }, - { - "cell_type": "code", - "source": [ - "fpa.policy = model\n", - "fpa.sample_amount = 100\n", - "with simulated_robot:\n", - " fpa.batch_rollout()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:30.994949Z", - "start_time": "2024-06-19T12:35:19.053104Z" - } - }, - "id": "6bfc00e5d93a19af", - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "100%|██████████| 100/100 [00:11<00:00, 8.38it/s, Success Probability=0.41]\n" - ] - } - ], - "execution_count": 10 - }, - { - "cell_type": "markdown", - "source": [ - "We can see, that our new and improved model has a success probability of 60% as opposed to the 30% from the standard policy." - ], - "metadata": { - "collapsed": false - }, - "id": "c270cd173a43b342" - }, - { - "cell_type": "markdown", - "source": [ - "Next, we put the learned model to the test in a complex environment, where the milk is placed in a difficult to access area." - ], - "metadata": { - "collapsed": false - }, - "id": "98bfa937581a9c47" - }, - { - "cell_type": "code", - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"apartment.urdf\")\n", - "\n", - "milk.set_pose(Pose([0.5, 3.15, 1.04]))\n", - "milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground()\n", - "fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP], policy=model)\n", - "fpa.sample_amount = 200\n" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:33.576063Z", - "start_time": "2024-06-19T12:35:30.995741Z" - } - }, - "id": "90d7cb5e1e1cc7fd", - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n" - ] - } - ], - "execution_count": 11 - }, - { - "cell_type": "code", - "source": [ - "p_xy = model.marginal([relative_x, relative_y])\n", - "fig = go.Figure(p_xy.plot(), p_xy.plotly_layout())\n", - "fig.show()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:35:49.058900Z", - "start_time": "2024-06-19T12:35:49.015756Z" - } - }, - "id": "78161f8e4a14782e", - "outputs": [ - { - "data": { - "application/vnd.plotly.v1+json": { - "data": [ - { - "legendgroup": "1", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - -0.7239851799129747, - -0.2675667293707046, - null, - -0.2675667293707046, - 0.21043272769243637, - null, - 0.21043272769243637, - 0.6802107054679791, - null - ], - "xaxis": "x", - "y": [ - 0.9737652891034544, - 0.9737652891034544, - null, - 0.5811257181848325, - 0.5811257181848325, - null, - 0.5912958693659719, - 0.5912958693659719, - null - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "1", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.7239851799129747, - -0.2675667293707046, - -0.2675667293707046, - 0.21043272769243637, - 0.21043272769243637, - 0.6802107054679791 - ], - "xaxis": "x", - "y": [ - 0.0, - 0.4444444444444444, - 0.4444444444444444, - 0.7222222222222222, - 0.7222222222222222, - 1.0 - ], - "yaxis": "y", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "1", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - -0.7239851799129747, - -0.7239851799129747, - -0.2675667293707046, - -0.2675667293707046, - null - ], - "xaxis": "x", - "y": [ - 0, - 1.0224535535586274, - 1.0224535535586274, - 0, - null - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "1", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - -0.10457966991274159, - -0.10457966991274159 - ], - "xaxis": "x", - "y": [ - 0, - 1.0224535535586274 - ], - "yaxis": "y", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - -0.6979180764986803, - -0.44438768097407855, - null, - -0.44438768097407855, - 0.08272590549147685, - null, - 0.08272590549147685, - 0.6808958390540965, - null - ], - "xaxis": "x2", - "y": [ - 1.2052028512135755, - 1.2052028512135755, - null, - 0.737770565726636, - 0.737770565726636, - null, - 0.5108173086128014, - 0.5108173086128014, - null - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.6979180764986803, - -0.44438768097407855, - -0.44438768097407855, - 0.08272590549147685, - 0.08272590549147685, - 0.6808958390540965 - ], - "xaxis": "x2", - "y": [ - 0.0, - 0.3055555555555556, - 0.3055555555555556, - 0.6944444444444444, - 0.6944444444444444, - 1.0 - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "2", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - -0.6979180764986803, - -0.6979180764986803, - -0.44438768097407855, - -0.44438768097407855, - null - ], - "xaxis": "x2", - "y": [ - 0, - 1.2654629937742543, - 1.2654629937742543, - 0, - null - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "legendgroup": "2", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - -0.12817762498549254, - -0.12817762498549254 - ], - "xaxis": "x2", - "y": [ - 0, - 1.2654629937742543 - ], - "yaxis": "y2", - "type": "scatter" - }, - { - "legendgroup": "3", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - -0.6602365724496425, - 0.02795449267080552, - null, - 0.02795449267080552, - 0.27616057598567767, - null, - 0.27616057598567767, - 0.6253591615690668, - null - ], - "xaxis": "x3", - "y": [ - 0.6500642680885906, - 0.6500642680885906, - null, - 1.0602395042953165, - 1.0602395042953165, - null, - 0.8289657981486859, - 0.8289657981486859, - null - ], - "yaxis": "y3", - "type": "scatter" - }, - { - "legendgroup": "3", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.6602365724496425, - 0.02795449267080552, - 0.02795449267080552, - 0.27616057598567767, - 0.27616057598567767, - 0.6253591615690668 - ], - "xaxis": "x3", - "y": [ - 0.0, - 0.4473684210526316, - 0.4473684210526316, - 0.7105263157894737, - 0.7105263157894737, - 1.0 - ], - "yaxis": "y3", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "3", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - 0.02795449267080552, - 0.02795449267080552, - 0.27616057598567767, - 0.27616057598567767, - null - ], - "xaxis": "x3", - "y": [ - 0, - 1.1132514795100823, - 1.1132514795100823, - 0, - null - ], - "yaxis": "y3", - "type": "scatter" - }, - { - "legendgroup": "3", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - 0.029066742676668283, - 0.029066742676668283 - ], - "xaxis": "x3", - "y": [ - 0, - 1.1132514795100823 - ], - "yaxis": "y3", - "type": "scatter" - }, - { - "legendgroup": "4", - "line": { - "color": "#636EFA" - }, - "mode": "lines", - "name": "Probability Density Function", - "x": [ - 0.090237002916839, - 0.8035425895689983, - null, - -0.7117316495858472, - -0.31771421153121704, - null, - -0.31771421153121704, - 0.090237002916839, - null - ], - "xaxis": "x4", - "y": [ - 0.516498201508182, - 0.516498201508182, - null, - 0.8014606542374061, - 0.8014606542374061, - null, - 0.7740863674384764, - 0.7740863674384764, - null - ], - "yaxis": "y4", - "type": "scatter" - }, - { - "legendgroup": "4", - "line": { - "color": "#EF553B" - }, - "mode": "lines", - "name": "Cumulative Distribution Function", - "x": [ - -0.7117316495858472, - -0.31771421153121704, - -0.31771421153121704, - 0.090237002916839, - 0.090237002916839, - 0.8035425895689983 - ], - "xaxis": "x4", - "y": [ - 0.0, - 0.31578947368421056, - 0.31578947368421056, - 0.6315789473684211, - 0.6315789473684211, - 1.0000000000000002 - ], - "yaxis": "y4", - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "4", - "line": { - "color": "#AB63FA" - }, - "mode": "lines+markers", - "name": "Mode", - "x": [ - -0.7117316495858472, - -0.7117316495858472, - -0.31771421153121704, - -0.31771421153121704, - null - ], - "xaxis": "x4", - "y": [ - 0, - 0.8415336869492764, - 0.8415336869492764, - 0, - null - ], - "yaxis": "y4", - "type": "scatter" - }, - { - "legendgroup": "4", - "line": { - "color": "#00CC96" - }, - "marker": { - "color": "#00CC96" - }, - "mode": "lines+markers", - "name": "Expectation", - "x": [ - -0.03381792818388927, - -0.03381792818388927 - ], - "xaxis": "x4", - "y": [ - 0, - 0.8415336869492764 - ], - "yaxis": "y4", - "type": "scatter" - } - ], - "layout": { - "annotations": [ - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.22, - "xanchor": "center", - "xref": "paper", - "y": 1.0, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.76, - "xanchor": "center", - "xref": "paper", - "y": 1.0, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.22, - "xanchor": "center", - "xref": "paper", - "y": 0.375, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "NygaDistribution", - "x": 0.76, - "xanchor": "center", - "xref": "paper", - "y": 0.375, - "yanchor": "bottom", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "P(Leaf = 0) = 0.4864864864864865", - "textangle": 90, - "x": 0.98, - "xanchor": "left", - "xref": "paper", - "y": 0.8125, - "yanchor": "middle", - "yref": "paper" - }, - { - "font": { - "size": 16 - }, - "showarrow": false, - "text": "P(Leaf = 1) = 0.5135135135135135", - "textangle": 90, - "x": 0.98, - "xanchor": "left", - "xref": "paper", - "y": 0.1875, - "yanchor": "middle", - "yref": "paper" - } - ], - "height": 600, - "template": { - "data": { - "barpolar": [ - { - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "barpolar" - } - ], - "bar": [ - { - "error_x": { - "color": "#f2f5fa" - }, - "error_y": { - "color": "#f2f5fa" - }, - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "bar" - } - ], - "carpet": [ - { - "aaxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "baxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "type": "carpet" - } - ], - "choropleth": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "choropleth" - } - ], - "contourcarpet": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "contourcarpet" - } - ], - "contour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "contour" - } - ], - "heatmapgl": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "heatmapgl" - } - ], - "heatmap": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "heatmap" - } - ], - "histogram2dcontour": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "histogram2dcontour" - } - ], - "histogram2d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "histogram2d" - } - ], - "histogram": [ - { - "marker": { - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "histogram" - } - ], - "mesh3d": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "type": "mesh3d" - } - ], - "parcoords": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "parcoords" - } - ], - "pie": [ - { - "automargin": true, - "type": "pie" - } - ], - "scatter3d": [ - { - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatter3d" - } - ], - "scattercarpet": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattercarpet" - } - ], - "scattergeo": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattergeo" - } - ], - "scattergl": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scattergl" - } - ], - "scattermapbox": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scattermapbox" - } - ], - "scatterpolargl": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolargl" - } - ], - "scatterpolar": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterpolar" - } - ], - "scatter": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scatter" - } - ], - "scatterternary": [ - { - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "type": "scatterternary" - } - ], - "surface": [ - { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "type": "surface" - } - ], - "table": [ - { - "cells": { - "fill": { - "color": "#506784" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "header": { - "fill": { - "color": "#2a3f5f" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "type": "table" - } - ] - }, - "layout": { - "annotationdefaults": { - "arrowcolor": "#f2f5fa", - "arrowhead": 0, - "arrowwidth": 1 - }, - "autotypenumbers": "strict", - "coloraxis": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "colorscale": { - "diverging": [ - [ - 0, - "#8e0152" - ], - [ - 0.1, - "#c51b7d" - ], - [ - 0.2, - "#de77ae" - ], - [ - 0.3, - "#f1b6da" - ], - [ - 0.4, - "#fde0ef" - ], - [ - 0.5, - "#f7f7f7" - ], - [ - 0.6, - "#e6f5d0" - ], - [ - 0.7, - "#b8e186" - ], - [ - 0.8, - "#7fbc41" - ], - [ - 0.9, - "#4d9221" - ], - [ - 1, - "#276419" - ] - ], - "sequential": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "sequentialminus": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - }, - "colorway": [ - "#636efa", - "#EF553B", - "#00cc96", - "#ab63fa", - "#FFA15A", - "#19d3f3", - "#FF6692", - "#B6E880", - "#FF97FF", - "#FECB52" - ], - "font": { - "color": "#f2f5fa" - }, - "geo": { - "bgcolor": "rgb(17,17,17)", - "lakecolor": "rgb(17,17,17)", - "landcolor": "rgb(17,17,17)", - "showlakes": true, - "showland": true, - "subunitcolor": "#506784" - }, - "hoverlabel": { - "align": "left" - }, - "hovermode": "closest", - "mapbox": { - "style": "dark" - }, - "paper_bgcolor": "rgb(17,17,17)", - "plot_bgcolor": "rgb(17,17,17)", - "polar": { - "angularaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "bgcolor": "rgb(17,17,17)", - "radialaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "scene": { - "xaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - }, - "yaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - }, - "zaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "gridwidth": 2, - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3" - } - }, - "shapedefaults": { - "line": { - "color": "#f2f5fa" - } - }, - "sliderdefaults": { - "bgcolor": "#C8D4E3", - "bordercolor": "rgb(17,17,17)", - "borderwidth": 1, - "tickwidth": 0 - }, - "ternary": { - "aaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "baxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "bgcolor": "rgb(17,17,17)", - "caxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "title": { - "x": 0.05 - }, - "updatemenudefaults": { - "bgcolor": "#506784", - "borderwidth": 0 - }, - "xaxis": { - "automargin": true, - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "zerolinewidth": 2 - }, - "yaxis": { - "automargin": true, - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "zerolinewidth": 2 - } - } - }, - "title": { - "text": "Joint Probability Tree over 2 variables" - }, - "width": 1200, - "xaxis": { - "anchor": "y", - "domain": [ - 0.0, - 0.44 - ], - "title": { - "text": "relative_x" - } - }, - "yaxis": { - "anchor": "x", - "domain": [ - 0.625, - 1.0 - ] - }, - "xaxis2": { - "anchor": "y2", - "domain": [ - 0.54, - 0.98 - ], - "title": { - "text": "relative_y" - } - }, - "yaxis2": { - "anchor": "x2", - "domain": [ - 0.625, - 1.0 - ] - }, - "xaxis3": { - "anchor": "y3", - "domain": [ - 0.0, - 0.44 - ], - "title": { - "text": "relative_x" - } - }, - "yaxis3": { - "anchor": "x3", - "domain": [ - 0.0, - 0.375 - ] - }, - "xaxis4": { - "anchor": "y4", - "domain": [ - 0.54, - 0.98 - ], - "title": { - "text": "relative_y" - } - }, - "yaxis4": { - "anchor": "x4", - "domain": [ - 0.0, - 0.375 - ] - } - }, - "config": { - "plotlyServerURL": "https://plot.ly" - } - }, - "text/html": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "execution_count": 13 - }, - { - "cell_type": "markdown", - "source": [ - "Let's look at the density of the relative x and y position of the robot with respect to the milk. We can see that he would like to access the object from the right front area." - ], - "metadata": { - "collapsed": false - }, - "id": "eaaf99db302255e0" - }, - { - "cell_type": "code", - "source": [ - "grounded_model = fpa.ground_model()\n", - "p_xy = grounded_model.marginal([relative_x, relative_y]).simplify()\n", - "fig = go.Figure(p_xy.plot(), p_xy.plotly_layout())\n", - "fig.update_layout(title=\"Marginal View of relative x and y position with respect to the milk\",\n", - " xaxis_range=[-1, 1], yaxis_range=[-1, 1])\n", - "fig.show()" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:36:03.666867Z", - "start_time": "2024-06-19T12:36:02.249618Z" - } - }, - "id": "a36ee203bb45f8b1", - "outputs": [ - { - "data": { - "application/vnd.plotly.v1+json": { - "data": [ - { - "marker": { - "color": [ - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 2.9638954031177898, - 9.606454216997486, - 4.764711598791809, - 7.728607001909592, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 7.728607001909592, - 7.728607001909592, - 4.764711598791809, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 4.841742618205678, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 2.9638954031177898, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 2.9638954031177898, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 4.841742618205678, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.764711598791809, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 4.764711598791809, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 4.841742618205678, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 4.841742618205678, - 4.841742618205678, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 7.728607001909592, - 9.606454216997486, - 9.606454216997486, - 9.606454216997486, - 7.728607001909592, - 9.606454216997486, - 4.764711598791809, - 9.606454216997486 - ] - }, - "mode": "markers", - "name": "Samples", - "x": [ - 0.42988536038074243, - 0.3352085786691254, - 0.6795031850168607, - 0.5563286261003058, - 0.332674252616063, - 0.41566146740057963, - 0.4339411072883757, - 0.4819259407996226, - 0.4177364638436398, - 0.3168130149449634, - 0.5119372166551864, - 0.3825749930663972, - 0.5768675587373966, - 0.47581401660812717, - 0.5334511197089785, - 0.3707087009950383, - 0.5185625519481485, - 0.5825126668818157, - 0.4201225805824894, - 0.4617531916160809, - 0.3826220655191214, - 0.4423628441484706, - 0.5310571102430116, - 0.45076919170851715, - 0.5802635096593237, - 0.3228416121106089, - 0.5046619694246298, - 0.4504745711048834, - 0.4061529750155864, - 0.33851349534656566, - 0.44479429225061085, - 0.6545410771187099, - 0.4775344174742533, - 0.33674380170196533, - 0.5491096790868414, - 0.5770107883899371, - 0.5265169456354307, - 0.5596307165737172, - 0.3738833312993278, - 0.5454066275424398, - 0.6747665849446265, - 0.5135803416300453, - 0.6043123618458159, - 0.675939593038509, - 0.35051209079592505, - 0.33480874333163396, - 0.6237651868442495, - 0.393412910970007, - 0.47285030942243356, - 0.6328069376336507, - 0.38611773387322534, - 0.6163777451030035, - 0.5410058193300686, - 0.3870049167734881, - 0.5728102503729785, - 0.3849320533736915, - 0.300381552794172, - 0.3182027128116637, - 0.4159036686833495, - 0.5628593934659156, - 0.41458021169567844, - 0.4351708810036276, - 0.32342603028545774, - 0.42735525778245764, - 0.4528121286252873, - 0.5880155737545596, - 0.6508225188191974, - 0.6540708733327127, - 0.47934672694905955, - 0.4574043173749577, - 0.606686907714979, - 0.4281734410354604, - 0.4880352790018819, - 0.4489869166046141, - 0.596383386365013, - 0.5367691026985135, - 0.3456943906407819, - 0.318817568083893, - 0.3642802008282772, - 0.4265900284082115, - 0.3559469466574688, - 0.36735869134333393, - 0.3470111715946449, - 0.44254070575718196, - 0.4698125155548615, - 0.3435787335649153, - 0.6755187202112338, - 0.47682143137103106, - 0.4542082576533417, - 0.6765600276764783, - 0.5290940102464754, - 0.6106842115354754, - 0.5510529267165485, - 0.4187851418043004, - 0.30251101327428737, - 0.6193219626834523, - 0.508197232411394, - 0.5986044991981104, - 0.4172245852191982, - 0.48026599305734924, - 0.549216698432597, - 0.43271078532462176, - 0.4199388006388523, - 0.43571230310179054, - 0.5967579808176309, - 0.35822996691071185, - 0.5299122007392687, - 0.6335312894410241, - 0.6121343015472896, - 0.520155060284157, - 0.3272587265400469, - 0.5908834789007653, - 0.40641611314143644, - 0.676514162967734, - 0.39427108049767035, - 0.5416985599938915, - 0.49012765868128, - 0.3512418870844141, - 0.30044148504195567, - 0.42606151472577064, - 0.47350205984666127, - 0.6155389400192397, - 0.6128708139810786, - 0.5795019070520877, - 0.5457492059003386, - 0.6059924107147013, - 0.45072624474242123, - 0.3961053198569739, - 0.4330495925215986, - 0.42143285380201156, - 0.6479328709330588, - 0.31286618741233146, - 0.5478082736978018, - 0.6721985175749965, - 0.5475173838394669, - 0.31867846839465663, - 0.532535654667347, - 0.6021533530756122, - 0.380706184155436, - 0.3663052136225272, - 0.5553437379785602, - 0.39173367353904487, - 0.5346630907341318, - 0.411912773451267, - 0.6375830482653931, - 0.48226299084299623, - 0.4777813596641106, - 0.33701357679688954, - 0.5717769121016307, - 0.40491926159066466, - 0.671041898370738, - 0.5282238142260749, - 0.46208784464157493, - 0.31529449825259714, - 0.4200064591611488, - 0.45561999229308997, - 0.5853637469615267, - 0.5552913053797582, - 0.3955519144251796, - 0.555861509236689, - 0.5326458332046095, - 0.6200404188914119, - 0.522445328857812, - 0.5495109227529287, - 0.4287022319624, - 0.6105701194424951, - 0.6100546953813801, - 0.40613145880374646, - 0.44126089321396256, - 0.5847684584554669, - 0.37320980026587797, - 0.42887007462642046, - 0.4678246928271108, - 0.3399142271037192, - 0.3190133015202772, - 0.47234135796901017, - 0.4647885200248082, - 0.5480445110095962, - 0.5153351445905848, - 0.6019917912805319, - 0.3139530222508948, - 0.475261708854133, - 0.6318662581366353, - 0.6148235094502389, - 0.5352783905757497, - 0.5674534129216564, - 0.3463658492579674, - 0.5012027373244504, - 0.5623758169852204, - 0.5343274280253134, - 0.40430935084723396, - 0.605897132776865, - 0.38885389600448084, - 0.5092068211667973, - 0.42551952480415656, - 0.36353754700466046, - 0.3678154100678811, - 0.4781891482945349, - 0.3497337838652511, - 0.3735894347880035, - 0.6184692638796306, - 0.34940554621436937, - 0.6015033766372097, - 0.6015007552987262, - 0.3306849819297444, - 0.3962676024643479, - 0.5336985688047055, - 0.5320696389481485, - 0.5175435116843274, - 0.34351683518450604, - 0.334066992107907, - 0.4786695762815546, - 0.31354850632349945, - 0.6338913660945051, - 0.5490158189595506, - 0.44985477025024007, - 0.3350756156284119, - 0.4060486341641222, - 0.48031479664857357, - 0.5579395340675665, - 0.5792457441396219, - 0.467452645577601, - 0.4762731917443016, - 0.5070979055508753, - 0.49649719608597853, - 0.6169282753618126, - 0.3477578012779851, - 0.6098967161994884, - 0.3516543952194628, - 0.5398277060461714, - 0.306280945824414, - 0.3025721982225473, - 0.43359814069077873, - 0.32791904447732856, - 0.4026529586121036, - 0.6193468599910799, - 0.33935074875389176, - 0.585849003702591, - 0.41939868356659066, - 0.33868597993010396, - 0.5429558432268379, - 0.5282023364778556, - 0.6207819968245387, - 0.6211487250315986, - 0.3238625252593595, - 0.32005204278380567, - 0.5333643967710875, - 0.5139701834351342, - 0.5519470742364343, - 0.391845381957949, - 0.3115178864868103, - 0.5148645217545065, - 0.6747896712033955, - 0.45824834365238176, - 0.3979291282835638, - 0.3484201822753145, - 0.35705996142807755, - 0.371299730656985, - 0.3262668478517395, - 0.6098560872825985, - 0.49494623592510467, - 0.5805155307145371, - 0.6261592379782239, - 0.6710493981322669, - 0.6662685225185536, - 0.4505728389539816, - 0.47239226531250134, - 0.5346805849604419, - 0.42980762976006537, - 0.3808149786885326, - 0.30547804467635886, - 0.35636713532058734, - 0.6132257199471679, - 0.3341536531047724, - 0.35608447973229945, - 0.3205744610154208, - 0.5624456992611704, - 0.4789632227518386, - 0.37563041858068824, - 0.459255608847875, - 0.4280075256284893, - 0.5870041823521392, - 0.4736832842801935, - 0.5381324251107927, - 0.6058206843544843, - 0.33426993572576325, - 0.31941919149458065, - 0.544366878768604, - 0.38012156085300053, - 0.587715403310906, - 0.358440697825147, - 0.3031964457711464, - 0.6040272109784965, - 0.4841409173048818, - 0.32575233754098226, - 0.3182599557167663, - 0.6411413637137657, - 0.6639294074403644, - 0.6626165156236263, - 0.5438877504777022, - 0.37120882064031757, - 0.3050276712161679, - 0.45108741897666804, - 0.6031170537661832, - 0.4220889618515189, - 0.5031936956568175, - 0.32874049879911627, - 0.5561934980766559, - 0.5207781413572871, - 0.382012632670682, - 0.3545979767928034, - 0.545870992960017, - 0.409719890022918, - 0.42141198951363523, - 0.5928469566268351, - 0.3848800388091498, - 0.6343204034712275, - 0.6284584356820836, - 0.5691249308484246, - 0.3462691931429879, - 0.5759296158370806, - 0.31672007257874063, - 0.4848428364618186, - 0.368102911154588, - 0.42348324999434017, - 0.6032194576170853, - 0.42513457756221795, - 0.5531730965809161, - 0.5254543002151939, - 0.4007406313202368, - 0.36583612097547913, - 0.6056466550716171, - 0.4135669856122696, - 0.566486010275784, - 0.5947681998858121, - 0.32758306226185263, - 0.3081681872582423, - 0.5662147796682845, - 0.5644367795723102, - 0.6003030165954573, - 0.5783881511604269, - 0.31869009486481, - 0.5804379023646179, - 0.5915919343687432, - 0.4498152600368026, - 0.44550121284921945, - 0.47751918176117525, - 0.6057387946187691, - 0.5424521859196773, - 0.6379407679384073, - 0.6151200357537998, - 0.6616809882738683, - 0.5581051802388629, - 0.312914616255999, - 0.44114763434928883, - 0.3174460748654514, - 0.36246165781378115, - 0.4916987160783909, - 0.34977221022999966, - 0.5511089014567276, - 0.48865505346211324, - 0.38560250567779353, - 0.4370743109051154, - 0.3995106830059925, - 0.6696851598404556, - 0.5642404431493762, - 0.4806783536535858, - 0.5367148732300164, - 0.3470091619673478, - 0.4778445977097037, - 0.5428659655235208, - 0.619780655448282, - 0.30386246120343363, - 0.4910631698500436, - 0.5660463526131025, - 0.463339057623153, - 0.3047233703790016, - 0.4958145198814963, - 0.5093818076137747, - 0.6800775366326994, - 0.5869373951504173, - 0.578502063128332, - 0.3378384916208629, - 0.5574257510329792, - 0.5362177040071909, - 0.3097739744516343, - 0.5980619890549369, - 0.32136967854118464, - 0.6081637678715899, - 0.5328113130101821, - 0.32651933963261437, - 0.5307585874644716, - 0.5737949215469555, - 0.5880524789296148, - 0.3478575863592595, - 0.42031635105096427, - 0.5996468520986225, - 0.41367151398643515, - 0.6332658415765766, - 0.38113870486471835, - 0.3606034472934354, - 0.607104835464799, - 0.4064557669008798, - 0.564965205929413, - 0.31725624284474413, - 0.6772897801234353, - 0.5241826632336644, - 0.3390675969078796, - 0.5422818497529251, - 0.3310933623588305, - 0.3979372819598015, - 0.34509278337930194, - 0.44423860240725377, - 0.4403370265572064, - 0.40227882983599733, - 0.6189723998102774, - 0.3635194968219591, - 0.46116818928388836, - 0.518482465750081, - 0.5086706599604813, - 0.3242892861363404, - 0.46970996372918583, - 0.5792879672453817, - 0.3398705898052808, - 0.4254875197368586, - 0.42195647576196776, - 0.5138771626934819, - 0.6258018799274729, - 0.40286253229026875, - 0.4114693022771978, - 0.38214485365387935, - 0.6032413662210361, - 0.4479834373030994, - 0.5589717053512879, - 0.5648003898371132, - 0.6164344377658376, - 0.32103674585448805, - 0.3549732062528493, - 0.3917690294330709, - 0.4111259832757658, - 0.4867526441064, - 0.6678720035690955, - 0.5684773243226523, - 0.6721143691316314, - 0.613131318855549, - 0.44550668167634844, - 0.31973108505766085, - 0.5982381767533393, - 0.32631990374802133, - 0.4271498298212263, - 0.6528393882420612, - 0.5074723729293898, - 0.4165122996186811, - 0.3041264780913426, - 0.3743254779798378, - 0.32025277323634554, - 0.3876138290683849, - 0.5698936204710108, - 0.4635144169228198, - 0.4221650958100128, - 0.5774941001923917, - 0.473574446624317, - 0.6314020296869871, - 0.5844023255808881, - 0.37627041771614433, - 0.5840379374345654, - 0.4383591856168229, - 0.4925632169953053, - 0.35388263528939407, - 0.5058070018325906, - 0.44722356790021006, - 0.4917761559009847, - 0.3148714016165515, - 0.3589130092515802, - 0.3466099663277886, - 0.33799207817345517, - 0.3596328022519932, - 0.35619905797041773, - 0.553682301531407, - 0.6255251070608925, - 0.3092261516047041, - 0.4773604462731471, - 0.3850584624032678, - 0.6247071150502739, - 0.4833012785019752, - 0.46963579378516807, - 0.42632668064283535, - 0.3290909995631735, - 0.32240083444598117, - 0.4500755764321057, - 0.39787256411717375, - 0.559397369683763, - 0.5567508878674507, - 0.5868883722760263, - 0.4178112675154927, - 0.6528239309051813, - 0.565517809381186, - 0.5708806111353355, - 0.42964945749928274, - 0.6660583381029961, - 0.33684560209742304, - 0.3983922799986928, - 0.3426828629395711, - 0.5640204900547066, - 0.5911067419716012, - 0.43893248836120935, - 0.31059306942173587, - 0.610432205478377, - 0.3169424787501796, - 0.4147918254389591, - 0.5401336792715059, - 0.34313882741020435, - 0.4744592090436607, - 0.538797968856782, - 0.3935616264708657, - 0.3968005995847706, - 0.5562769785934503, - 0.5238164509564112, - 0.5313199554146508, - 0.4755047881842688, - 0.3688608399204176, - 0.3543648974974381, - 0.4779183844126108, - 0.32307200500410116, - 0.6139777682512135, - 0.3264768997809254, - 0.3617721941659176, - 0.39521950579807674, - 0.5059069951543934, - 0.5141987761786606, - 0.5583089813968789, - 0.4677478682062487, - 0.3058512914664646, - 0.5946906977571611, - 0.5665585116176396, - 0.3633061907066879, - 0.5332516170598633, - 0.39184173542342027, - 0.46807561798732533, - 0.6440122809508946, - 0.37222783416241023, - 0.411853151077544, - 0.62855070849502, - 0.6365416271286184, - 0.5295234503773338, - 0.5408981560610706, - 0.3242414637050544, - 0.5473131736413505, - 0.4868105539977837, - 0.4036447959985871, - 0.6657049141890035, - 0.4218671850255678, - 0.5619041886204671, - 0.31495084960526915, - 0.3448141528271809, - 0.6016313122390433, - 0.3073474864261973, - 0.31466742048542307, - 0.45143748371071557, - 0.31356502376352113, - 0.35702758250433736, - 0.6392412960560876, - 0.30505665851943037, - 0.5957200406695998, - 0.30905301448423994, - 0.6594835387138327, - 0.6235988840629333, - 0.6222225002260301, - 0.5761398414027333, - 0.518669991826773, - 0.31761863708472504, - 0.4850206231461639, - 0.6200373930516883, - 0.3594925165467324, - 0.36267621936997424, - 0.6367120174286643, - 0.5255230200410022, - 0.6254131746594495, - 0.4867465928429991, - 0.3634595631007538, - 0.4244523479598738, - 0.6054282574627764, - 0.5210520101522604, - 0.38725140941936037, - 0.3962142917471763, - 0.5984129753044982, - 0.6098592731231947, - 0.33286352475240877, - 0.5372987422930878, - 0.5875776995677813, - 0.5635271966961835, - 0.5812294793591601, - 0.5522692773365071, - 0.39470983565107587, - 0.5267191410769504, - 0.6613700603955441, - 0.48140035338791254, - 0.37822935667411706, - 0.547147042085923, - 0.587292467754251, - 0.37984016274530585, - 0.6795183863761791, - 0.4644883591869103, - 0.48351571926712256, - 0.5263002537024163, - 0.538740491434215, - 0.5404671639142193, - 0.3399677193636928, - 0.5560919773483206, - 0.557971843929459, - 0.5230403058431183, - 0.3276754516676937, - 0.376250814337562, - 0.6699531658968636, - 0.5937400288871861, - 0.6243007014772883, - 0.43194101483419833, - 0.40799088384388005, - 0.3728674436820687, - 0.6085470471951431, - 0.5998557629743042, - 0.33706476703054766, - 0.3795754209021991, - 0.5026378837692593, - 0.4524858727529403, - 0.30273425494471934, - 0.35812459479323855, - 0.4479162811822517, - 0.5868783923386003, - 0.3063148013518592, - 0.43006207638722965, - 0.340080531455262, - 0.3834680718142087, - 0.6021274770979737, - 0.5245683122929075, - 0.5661741478475603, - 0.5503573348030797, - 0.4309520045434018, - 0.43062380684372675, - 0.4694119709985139, - 0.4164414837882575, - 0.5840133873066629, - 0.43626120024279386, - 0.5407104781790707, - 0.3422281658963559, - 0.37622591501309804, - 0.5973898244666382, - 0.38692052089478635, - 0.5517201033887172, - 0.4242783978693429, - 0.32343327406496636, - 0.5670607436036796, - 0.431466967031529, - 0.6018409821842285, - 0.5695987013078456, - 0.4041407363646737, - 0.5450040085479219, - 0.45549930589262255, - 0.5492283075515088, - 0.5426744504992049, - 0.32862206411758793, - 0.408801806125585, - 0.43090974973433827, - 0.5979121779682217, - 0.5250491876472758, - 0.5883224293576064, - 0.34702657913136925, - 0.6433571680386567, - 0.6014996390277596, - 0.37605576190917556, - 0.6512213234743894, - 0.3156839475421812, - 0.4314696612221529, - 0.5721754731231644, - 0.43708046178914256, - 0.3105304795300713, - 0.3278913003939288, - 0.578918093219241, - 0.5965431258869257, - 0.517330752325307, - 0.6539409601836371, - 0.5563698948386386, - 0.37335529213959434, - 0.4582060763333682, - 0.5931683802373541, - 0.36986089401382527, - 0.5020797994253803, - 0.5340670152876423, - 0.566343365271297, - 0.4342180467871518, - 0.5144360514620366, - 0.44136719406230185, - 0.3648718479226048, - 0.41600335539067146, - 0.6246732800934989, - 0.4259934995487713, - 0.5939512221332606, - 0.6059850057871448, - 0.6371497556202979, - 0.5907760631574103, - 0.5911007949580163, - 0.5626642039755572, - 0.6733957611588421, - 0.36896435734314764, - 0.42281638089513474, - 0.3509947009281626, - 0.6353419615791529, - 0.6255473181095486, - 0.43966941561229733, - 0.4981987333729273, - 0.5326941447765197, - 0.5541143703570638, - 0.5732858219260151, - 0.6164183113185125, - 0.5017610876808458, - 0.5542921763100919, - 0.5391901816521222, - 0.6697029284133909, - 0.36101338442678077, - 0.6117720378547252, - 0.5905637270449282, - 0.35753946479746657, - 0.3281791113233787, - 0.5386564518060308, - 0.3814786845730258, - 0.39266555419295474, - 0.5874041991509358, - 0.35235176058565604, - 0.5286765167737244, - 0.6064242540446699, - 0.31731141688175124, - 0.4868760273669395, - 0.5144910216003794, - 0.5519500825953683, - 0.497508716566966, - 0.5425463459637703, - 0.4453071741230129, - 0.33360422910050613, - 0.4405118400590371, - 0.4026553787292969, - 0.46794872351077765, - 0.3674867396530454, - 0.37177442487960183, - 0.3999979069181764, - 0.4068799781238795, - 0.5599864055667014, - 0.32959294213224777, - 0.48277846383014594, - 0.41851781180138764, - 0.43882080190801903, - 0.6087317628199616, - 0.5958792612260434, - 0.303550307228961, - 0.5684148835282696, - 0.395524704061227, - 0.5298176323252166, - 0.4402012169032739, - 0.46052348053315956, - 0.32377330192299975, - 0.47069099534266523, - 0.5988800377624421, - 0.590659208163963, - 0.3718441243125499, - 0.47629531386968416, - 0.5324066135964828, - 0.5952894147334238, - 0.41358468378898505, - 0.43694960017491324, - 0.35310449479666484, - 0.33821329224459207, - 0.4785835717084883, - 0.49383815017938193, - 0.4850531978600934, - 0.4699611034542246, - 0.430185391239341, - 0.31874963307667553, - 0.5896850225390187, - 0.3096367703080784, - 0.3627333214452286, - 0.5977180976771785, - 0.5022558489864664, - 0.4180704208324118, - 0.48860349248918244, - 0.5798615907647777, - 0.4835311814122667, - 0.6074794791625362, - 0.4606753460168308, - 0.6359880312902506, - 0.6019983988318788, - 0.6061270671364348, - 0.6550020489349779, - 0.4068976206146115, - 0.4002885747076158, - 0.6576867191820637, - 0.6519083967520166, - 0.396234868223856, - 0.36986907678951897, - 0.5294320897901987, - 0.4924285301622281, - 0.3308041521549472, - 0.4351583960051403, - 0.34503393570974544, - 0.3385139686410676, - 0.4391718300381293, - 0.3872889531016976, - 0.6753833758437524, - 0.3679697897136378, - 0.5478436345110351, - 0.36401847017129774, - 0.33625671118846734, - 0.5205427857229279, - 0.5943373780342721, - 0.49012667127256176, - 0.32396965526166477, - 0.5599689118988824, - 0.4703532473544416, - 0.5011826446134435, - 0.6520155938728219, - 0.4907497853921537, - 0.5307934324107995, - 0.3100327020470444, - 0.5145386191007447, - 0.4128489041016228, - 0.42093402962644677, - 0.6596697762675119, - 0.6210203826264824, - 0.592717897408956, - 0.5393471383964457, - 0.5947272300053098, - 0.6166940548618629, - 0.4446617122246931, - 0.579503809905285, - 0.5572316361128431, - 0.527614814634662, - 0.39799864629389176, - 0.3719355471941538, - 0.4747166332960733, - 0.6102285181493892, - 0.4142456924857442, - 0.5741396028738759, - 0.38286147574488544, - 0.36892738682728726, - 0.5003705470720061, - 0.5135432707328207, - 0.49108988605047044, - 0.3943792787918869, - 0.3171740518410803, - 0.44503676603786196, - 0.5414145948080915, - 0.34119764882507947, - 0.36615043820241333, - 0.41762110568103816, - 0.45899184013545236, - 0.36353409442671897, - 0.4404772946872907, - 0.6197361758448203, - 0.586208851484078, - 0.30173319838690904, - 0.391289162064518, - 0.5177835078553384, - 0.5938343964963753, - 0.5175290641424497, - 0.41886801321230954, - 0.4266485535645723, - 0.6038069599130602, - 0.5482343466704992, - 0.5215170627986636, - 0.40399149191951833, - 0.42768738429975695, - 0.6256977392313535, - 0.5985309876081448, - 0.5062597905667576, - 0.6535405977810413, - 0.6271300776900889, - 0.6268557821097313, - 0.5523692636304998, - 0.613513633822389, - 0.47488495222111726, - 0.3850349122893144, - 0.3462423095789888, - 0.5358797140737547, - 0.38025722943340967, - 0.3723384373684099, - 0.5226020010178869, - 0.36758556157602285, - 0.311266197757469, - 0.4047403039639092, - 0.5337400601663028, - 0.3215792910416963, - 0.38150643212812047, - 0.3180545899846522, - 0.4865342036524294, - 0.3576259189936791, - 0.32768639307758224, - 0.3920934481833324, - 0.528412863416624, - 0.36230334143483994, - 0.45201902157217033, - 0.3234913811861921, - 0.4176278223678394, - 0.3922560658592217, - 0.4446176021458291, - 0.5031339767338696, - 0.5714048525385809, - 0.34529290629859744, - 0.32385607992753945, - 0.3934255317783715, - 0.6073741474879808, - 0.534346244474412, - 0.6315210901171424, - 0.5199418041011619, - 0.6780045813427937, - 0.6205416359875884, - 0.3196702714829117, - 0.634965799839177, - 0.6329217106269392, - 0.5842773548852584, - 0.4139783406814637, - 0.43852908326236056, - 0.5328803014732003, - 0.5460333727078923, - 0.5493965972202688, - 0.48219966575632645, - 0.36765902624045127, - 0.3326894606170183, - 0.40644952272457024, - 0.36324974751001005, - 0.36266934832087294, - 0.5872994222090446, - 0.3542812575376011, - 0.5010910587291015, - 0.3084105898821158, - 0.3083894042663993, - 0.5795449641134374, - 0.4391617799987002, - 0.5015303644963293, - 0.6235187468306858, - 0.5216795617943322, - 0.5083253361452161, - 0.6379462181219833, - 0.5999786584589679, - 0.6481324707081783, - 0.3040123726467539, - 0.6728948082874174, - 0.660879252025613, - 0.32504888118180275, - 0.4266717800795963, - 0.5478899644286718, - 0.5608780891280168, - 0.4910052402371814, - 0.31396353607100724, - 0.3335625572411133, - 0.43301332416505517, - 0.3233656209817055, - 0.5657282398352561, - 0.6390558634701061, - 0.3860823404630178, - 0.6500316412120783, - 0.42772778772575004, - 0.44879646620222585, - 0.5131191211944526, - 0.644168619159267, - 0.6653060517922076, - 0.42711558379530695, - 0.4464344643651791, - 0.4073418777963082, - 0.3887350074405934, - 0.3139307716487472, - 0.48902387398518277, - 0.33750877482727304, - 0.5446921094476932, - 0.6086208618765876, - 0.4704023108941088, - 0.43659407930462146, - 0.30218550486545465, - 0.3999380584257537, - 0.31547528222209253, - 0.5220937870203388, - 0.4290634947936911, - 0.46924745817885016, - 0.4876122305664879, - 0.3269676452248972, - 0.4807073915361351, - 0.30323248921752577, - 0.5314286486240833, - 0.38833530345122846, - 0.5830979197784327, - 0.49361981329139426, - 0.4015874816948413, - 0.524331500298984, - 0.5867491791571544, - 0.38954051826494734, - 0.6057741015441198, - 0.5737843473550145, - 0.6077009570860634, - 0.6211237917066175, - 0.30068747175714866, - 0.38907201299743766, - 0.5042058902111557, - 0.3790077027320272 - ], - "y": [ - -0.5950346161539204, - -0.5653565976228774, - -0.47262427994794554, - -0.49956666686825496, - -0.5005658919109182, - -0.5117269159641255, - -0.4521629655695958, - -0.5446918886543558, - -0.5393673304210611, - -0.5027579113117444, - -0.6557504736260156, - -0.40739777925493575, - -0.5342344423723342, - -0.5020433869843839, - -0.48782189221843886, - -0.5192107443908001, - -0.6050564727477882, - -0.6066853326524285, - -0.49146341771945257, - -0.6247245538887228, - -0.5735567880177477, - -0.5490442789364043, - -0.7054427089183829, - -0.6367350406274151, - -0.531108898249147, - -0.47186295618762264, - -0.6383733189564987, - -0.6296085556890587, - -0.6612415548373973, - -0.6405169857717423, - -0.5992375152553504, - -0.4768609122708666, - -0.5491217823490442, - -0.7082052070719808, - -0.5190232023138159, - -0.5751424990715793, - -0.5506709391430222, - -0.5580845408736543, - -0.42487439765774093, - -0.4885411625809657, - -0.5556436606281434, - -0.5508472826114211, - -0.47823161381819823, - -0.4698866424326037, - -0.5707656605517613, - -0.45851380792850027, - -0.6537658291799934, - -0.4789071726663011, - -0.5954627422892537, - -0.47114294492687325, - -0.6159651841142341, - -0.6759406535475808, - -0.4392717311102196, - -0.40316152813270567, - -0.6283855983001194, - -0.6903610437902068, - -0.5605904102383622, - -0.6038915386113347, - -0.4979266057204843, - -0.48367379471259087, - -0.4624652753198339, - -0.6267206436974917, - -0.6469740867656089, - -0.4103773045251917, - -0.6273536492374385, - -0.6029736901502962, - -0.6558958559759694, - -0.5742965509953045, - -0.6474899243328324, - -0.46554716516045636, - -0.6535269626394183, - -0.6095267712215406, - -0.42658521450919523, - -0.6638751597261628, - -0.469538400398521, - -0.4310345682883243, - -0.5936053540569205, - -0.4292676209159346, - -0.5959417180009322, - -0.6779465651685556, - -0.41675396703177986, - -0.6453143750105494, - -0.559613713105335, - -0.593973146736415, - -0.6585647682639691, - -0.6775956752411645, - -0.6899702670171499, - -0.5645726880539956, - -0.6653926176172715, - -0.46484563503781917, - -0.5465545280773895, - -0.42101792733791893, - -0.616681379886987, - -0.6606505019202368, - -0.4026983235181474, - -0.44150794397350973, - -0.5588383718079879, - -0.5933860902321896, - -0.6609937817122643, - -0.6920762266075893, - -0.4770306783789089, - -0.5246881204206355, - -0.48528438037609606, - -0.4450625296790044, - -0.527012148013668, - -0.4504689477710136, - -0.47751408361925035, - -0.5295087143638704, - -0.4562499995357905, - -0.6044593107324252, - -0.6757936615936819, - -0.5748369483132605, - -0.61908608295926, - -0.4338800421538465, - -0.697590030355573, - -0.698226867188112, - -0.40348298204593164, - -0.6915202327077463, - -0.7096792855735397, - -0.6249668341332887, - -0.5443660729929185, - -0.5814101045194087, - -0.7060077090105081, - -0.5811176262998837, - -0.53590714481928, - -0.5336025504941889, - -0.6166385168295887, - -0.6847464535979297, - -0.6257356299913737, - -0.6645209313826649, - -0.4724467721254916, - -0.6067537845213914, - -0.5234148304609396, - -0.694239838790041, - -0.40722084964987476, - -0.5627833599895612, - -0.46843272534173674, - -0.6222871310724086, - -0.4168985382275455, - -0.6301587589263355, - -0.6233330222997129, - -0.6516780760483432, - -0.6030415311105168, - -0.54142879606845, - -0.6902312838923164, - -0.47124890519312834, - -0.5366488796779574, - -0.6808426432386203, - -0.47527010581594276, - -0.4449226666863366, - -0.5466372680707428, - -0.4009922395120266, - -0.44299538420042406, - -0.6986222743827822, - -0.404314577738192, - -0.6330038248528523, - -0.5951543388017712, - -0.5412526611508948, - -0.5903700560841895, - -0.4942391325534695, - -0.5164407288432906, - -0.6819388156648487, - -0.42490583919256314, - -0.5191377786918887, - -0.47363189355476304, - -0.6366051178835089, - -0.6307197632247513, - -0.6004113432481331, - -0.4206039327938729, - -0.45449181861637156, - -0.6352129774337898, - -0.4067692917287215, - -0.5254297083493947, - -0.4268609918385423, - -0.4449289733751256, - -0.5033695274651362, - -0.5255064743591515, - -0.5277889641772693, - -0.5387347440709898, - -0.6083445168058749, - -0.6791892522309945, - -0.5954179256407681, - -0.5997301346092602, - -0.6690104217628385, - -0.40646997589533695, - -0.6468582373630963, - -0.5754025479390545, - -0.4402649037481503, - -0.6799407372129662, - -0.669097865692258, - -0.5692486187075343, - -0.5736886433191664, - -0.5091306549147815, - -0.5608318824891972, - -0.56071817912098, - -0.623640101938491, - -0.6399599570661749, - -0.4268653778370131, - -0.42311323052243127, - -0.4409678467743314, - -0.6414234860693977, - -0.7086723245502623, - -0.4878856784943274, - -0.5629939810701747, - -0.6305169508370959, - -0.4731550045176084, - -0.521278895204966, - -0.592802171888133, - -0.49291880204266914, - -0.44273250649859547, - -0.4199079514321877, - -0.43591038988438785, - -0.6128286027343106, - -0.6183728323941116, - -0.6828506797018217, - -0.5707853071519912, - -0.5394514131706712, - -0.5574735657306077, - -0.6668511212652295, - -0.48941650070160636, - -0.4952199917110328, - -0.6377421059743318, - -0.5009351716248533, - -0.5397095030897938, - -0.41944386597136785, - -0.4352929952601512, - -0.555453191653817, - -0.5386918078082997, - -0.6283444351186921, - -0.6441789202945956, - -0.6983941404660355, - -0.6159300428572143, - -0.5318820930577145, - -0.5995396740056265, - -0.5100394161526705, - -0.48113943082971344, - -0.6017578476466222, - -0.5461242923481278, - -0.6070680295673083, - -0.5484549808420494, - -0.6822495969393246, - -0.4051516308278244, - -0.4203759885983723, - -0.5268106085747746, - -0.4628711090714376, - -0.512510305224138, - -0.6370254150051965, - -0.46641543871463076, - -0.6353453806543001, - -0.6363790717554295, - -0.4705770282381868, - -0.6366752985528147, - -0.5818284024611556, - -0.47482490028811364, - -0.49962093348282854, - -0.43306867229103524, - -0.4146451391270478, - -0.6392324757587101, - -0.49500887608589633, - -0.4075898581565194, - -0.6083941329818345, - -0.5043523861068755, - -0.554469727359031, - -0.46150059512486985, - -0.6124146995527083, - -0.49702702072223304, - -0.7025302462052233, - -0.5023295105958783, - -0.6268613621566838, - -0.5056655012999084, - -0.692007595339788, - -0.56987469756909, - -0.5604321241172641, - -0.4663304712053405, - -0.623843233538597, - -0.6517879043988295, - -0.5753240426324733, - -0.5924743161995647, - -0.5225876734546767, - -0.5820437423789804, - -0.6537405986557541, - -0.40764858277799876, - -0.5072543301219365, - -0.5457034205293756, - -0.45806033293377324, - -0.42207160165003965, - -0.47108794560386463, - -0.5334792765485539, - -0.43112801899110065, - -0.6086735221034998, - -0.685490294162797, - -0.5339020401719043, - -0.5893829741316452, - -0.5479271710789613, - -0.6163939153057845, - -0.42431182165741216, - -0.6650609496430031, - -0.6190417616758056, - -0.5263949863247976, - -0.6716250324426222, - -0.5186593211661891, - -0.6539816525254393, - -0.6418286116671392, - -0.4907585401365593, - -0.5999875252548074, - -0.4742688426214666, - -0.6835840594696512, - -0.4142543083968623, - -0.7056381233413199, - -0.647336027520779, - -0.6439749164537603, - -0.6136350908061449, - -0.6450754308400243, - -0.5762225310741693, - -0.4703705128917665, - -0.5489552520523304, - -0.5946387756111366, - -0.47718156063170536, - -0.6089996927741572, - -0.6953774768147274, - -0.6355253006645558, - -0.7093944736998202, - -0.4647911126975207, - -0.6161478645747528, - -0.5395637546857581, - -0.4894723984200169, - -0.5009674735259712, - -0.6907237815532481, - -0.6291929848596747, - -0.5267135300807806, - -0.6034035945779791, - -0.6745220862030542, - -0.44007227111395336, - -0.5135638127766659, - -0.5491394386520584, - -0.6347071898171185, - -0.6138516317821184, - -0.4180975811404374, - -0.4950832271186921, - -0.6107598302681828, - -0.41114182340747457, - -0.6799889008868951, - -0.47797712814784454, - -0.44826994679656645, - -0.6181438322981189, - -0.6385206543649776, - -0.5024746255339978, - -0.4515421871943103, - -0.5082719403839653, - -0.6176498812782882, - -0.6213922570702971, - -0.621925569719928, - -0.6971832017372209, - -0.46100697982622707, - -0.5903088528605523, - -0.4104064768247347, - -0.7024004059242525, - -0.5006325233941604, - -0.4556278902548259, - -0.5316377343667715, - -0.4554881028059162, - -0.6248627163797602, - -0.48904665574087935, - -0.6229345219070479, - -0.4896901285829012, - -0.4468481394463325, - -0.6890837973698736, - -0.42352995730697995, - -0.5411191851858586, - -0.6227733177362307, - -0.6133994868100566, - -0.552498831439841, - -0.5512080897790658, - -0.4531946159873501, - -0.6382491670071542, - -0.6900777229622318, - -0.5791618910418681, - -0.6672741820197704, - -0.4565430380378752, - -0.6778170698055404, - -0.5923145626387024, - -0.48800660632454584, - -0.5535352489763524, - -0.5822504082741634, - -0.4388498767108097, - -0.5078871058926981, - -0.522951425705199, - -0.6198110421026299, - -0.6995142985013784, - -0.41022328091862476, - -0.5425831620807876, - -0.5345753642596538, - -0.4138190798390588, - -0.6566645620055978, - -0.4156430676539622, - -0.482507015736776, - -0.5990336799682806, - -0.5888086493869668, - -0.6300337339545402, - -0.44885402450686174, - -0.6789546973291452, - -0.6366777619475497, - -0.5241848685009481, - -0.6720820771787535, - -0.5870614699135726, - -0.41185014709400297, - -0.4373382021574038, - -0.47425934399062286, - -0.5208390942300611, - -0.6249619325380443, - -0.5664080060760766, - -0.6908464312982912, - -0.404143375459037, - -0.5545767219529589, - -0.7058212666439192, - -0.5375246256011461, - -0.5110738070461537, - -0.555749052060512, - -0.6436053112591709, - -0.4963511621925273, - -0.5477922306143188, - -0.553853615636865, - -0.6349376027995586, - -0.6681321352624056, - -0.4516028279575438, - -0.4904501516491425, - -0.5695967939507838, - -0.4825607063689932, - -0.4479218560383011, - -0.41950321442911714, - -0.40783391229243704, - -0.5210137302821423, - -0.6933589918112903, - -0.6814003036843004, - -0.6265133382119651, - -0.6268179059708853, - -0.4224952725659015, - -0.5291888360305417, - -0.6587947878691967, - -0.6037505436037542, - -0.45378134929576847, - -0.5600847665643236, - -0.615872296032874, - -0.5629186189626971, - -0.4466153700835808, - -0.4494794278000481, - -0.5453424091294063, - -0.5956697453476985, - -0.6307538911318783, - -0.6121222321636519, - -0.46061916026621696, - -0.6057055848162166, - -0.5764032625244228, - -0.45912362500325865, - -0.6568902494625074, - -0.6189943764277821, - -0.47177440714538244, - -0.4103431603241037, - -0.5646685847457364, - -0.5846317232258641, - -0.565088847531485, - -0.6617992321032713, - -0.5829269672295916, - -0.576509124990848, - -0.6360995525187122, - -0.6611930059642849, - -0.6221203558303625, - -0.5446588014433729, - -0.45492276069347765, - -0.6719142791495621, - -0.5996729965567479, - -0.41138434393573325, - -0.5338950278919896, - -0.649225188215258, - -0.4666460249890543, - -0.5696533888469141, - -0.5955167718752614, - -0.6813475375809696, - -0.45063356436204566, - -0.6666471256738351, - -0.5646487245400226, - -0.6221994763349066, - -0.4557546229889633, - -0.5455129130686898, - -0.4129634273526883, - -0.5520324342557686, - -0.6702520368257039, - -0.6313729115239236, - -0.6328186132063046, - -0.5242029506565362, - -0.5169070210919345, - -0.6404118503722933, - -0.48989355643028826, - -0.45534767853560343, - -0.5123005914977556, - -0.6226312093648769, - -0.6825321478815679, - -0.5621789523369152, - -0.5665084017360115, - -0.5639200791244998, - -0.5695363590060898, - -0.42332856455781265, - -0.4230758556718626, - -0.6380594430037136, - -0.5293181803551139, - -0.5617597655518192, - -0.4172276323227266, - -0.49382241955318573, - -0.4997942983960244, - -0.48852361096974717, - -0.6528450408158225, - -0.42126692886001066, - -0.42148654341761727, - -0.4629987386673366, - -0.5469146149829939, - -0.5120034959258244, - -0.4956133117751191, - -0.5791466397082349, - -0.6310733163776057, - -0.6331818228521832, - -0.6953467982603783, - -0.5078214881811431, - -0.5626850565926332, - -0.5439452797154091, - -0.4789225552266564, - -0.5377271165961017, - -0.6352936216577378, - -0.66632973279037, - -0.5970196690303768, - -0.5440490010970095, - -0.6788799268644871, - -0.5854181366312008, - -0.577074604070204, - -0.532940994567863, - -0.5443871536457134, - -0.6337173410934542, - -0.4134480899499558, - -0.653842832846457, - -0.4905420017593946, - -0.46151594631493564, - -0.5563363638720167, - -0.5507977201393062, - -0.5429325220802715, - -0.566088974328351, - -0.6489645741813335, - -0.4760858573442147, - -0.654630587300731, - -0.6380843318410965, - -0.6691200130979762, - -0.655723496767571, - -0.6052488391975871, - -0.551512551050845, - -0.7021676879647134, - -0.5870800482374118, - -0.5740102988374536, - -0.5423979641056765, - -0.5193582339191772, - -0.560654750961215, - -0.48040245068270904, - -0.6611029653005556, - -0.5323738476903819, - -0.46482761458486166, - -0.6140333884517787, - -0.4833308452937601, - -0.6072209890085114, - -0.5181712979227252, - -0.5441772222108281, - -0.5067982591223827, - -0.4948554184206353, - -0.6027352036388971, - -0.43644546584870025, - -0.6562877263642468, - -0.4101342846933465, - -0.5515100603085872, - -0.511225314071031, - -0.5299973881975575, - -0.5910924222611136, - -0.5426080566655948, - -0.681894974353501, - -0.6529361669667765, - -0.5190204904535712, - -0.5598363646078515, - -0.4858588009130925, - -0.40055116636159316, - -0.47335502741592617, - -0.4816534667169865, - -0.5284472741861619, - -0.6148474006699789, - -0.4875878287841202, - -0.69625423106574, - -0.4158510376000237, - -0.46705335870108283, - -0.5020291589845365, - -0.6613871371880722, - -0.4868435588223954, - -0.6451932300273482, - -0.4940098933529229, - -0.5331066524750427, - -0.6226459630069613, - -0.6450739275318763, - -0.44392772982471307, - -0.42145035446033563, - -0.4619650479648022, - -0.45542674484378565, - -0.707481118801732, - -0.643876795853931, - -0.6336610784812399, - -0.5488988459045923, - -0.7032519924544258, - -0.6654803526483256, - -0.6506815538539735, - -0.6484585342459103, - -0.5629156285257663, - -0.6974267557252242, - -0.6472764579996869, - -0.648889921079904, - -0.47211593919718786, - -0.6208700163906242, - -0.42088403830468396, - -0.4985945476594129, - -0.538796096055119, - -0.5167821717641001, - -0.4215242248005813, - -0.6264765720755122, - -0.4528031078893998, - -0.6337353675741766, - -0.6922311064442103, - -0.5814277195138107, - -0.5552771602628949, - -0.44585527760201565, - -0.6441939513820394, - -0.44466874792010297, - -0.5312723286972952, - -0.6482708053764572, - -0.48386713047819657, - -0.5971800501548483, - -0.6708383326486468, - -0.4890063144674292, - -0.6180948824414019, - -0.6930827976113246, - -0.5115806414995707, - -0.5993950347298963, - -0.6217329972645431, - -0.6693779959546784, - -0.7114497403027513, - -0.6948658543844342, - -0.6654398114088794, - -0.5077363207641876, - -0.5970694724230201, - -0.6132340877371785, - -0.5299778433139696, - -0.6800736941228234, - -0.46428107928932416, - -0.4507410919524728, - -0.5931668528231864, - -0.5157010425977173, - -0.4416561763349974, - -0.4185843043636374, - -0.43238745162689485, - -0.4390743470940032, - -0.6711683568248868, - -0.5812629741597257, - -0.6921432921753339, - -0.6251406285903551, - -0.4470250991732402, - -0.5341794571978463, - -0.7054517422965059, - -0.44494099969037515, - -0.49721900424057985, - -0.6282149798841192, - -0.5721130293120831, - -0.58736720246294, - -0.434938896253721, - -0.4160835575978905, - -0.6681416910100962, - -0.5309466096433846, - -0.47788842155760614, - -0.601219335035598, - -0.4102389403605032, - -0.6139908077649421, - -0.6255548080149851, - -0.519126648246441, - -0.5356035886874699, - -0.67385108040906, - -0.6526334349083545, - -0.5727499540999964, - -0.6322986168851564, - -0.5304630920437661, - -0.6820503630002124, - -0.6607430731422054, - -0.6762528419797658, - -0.551481232930488, - -0.5270535613039257, - -0.670219949651132, - -0.5467364441865947, - -0.5106660995994545, - -0.6116876563373135, - -0.592993099637623, - -0.6484112081498034, - -0.45359204760197364, - -0.6200893262781002, - -0.5061146711025313, - -0.5195374843625434, - -0.6878320930889315, - -0.6777069851188592, - -0.6626378934268793, - -0.553202956103102, - -0.5343124178801365, - -0.6282444134179402, - -0.4448510201921731, - -0.6740461405878633, - -0.5243256286242481, - -0.5122462565335792, - -0.4623953271751155, - -0.6678904442554463, - -0.4687408409875339, - -0.49575867653611927, - -0.5242899113376186, - -0.6388065762904834, - -0.5144955124043784, - -0.41876975305705416, - -0.45663720528192797, - -0.522964443441243, - -0.4341358000123292, - -0.5846070300453252, - -0.574313368360115, - -0.4599162080249038, - -0.43599110464795693, - -0.5646866559765713, - -0.5287211521986809, - -0.6108693031808212, - -0.6813815207810614, - -0.5089526257616668, - -0.5797210752711361, - -0.5880769414424571, - -0.5924425109636977, - -0.5863111829358968, - -0.45800915990061997, - -0.47406754361513126, - -0.45821048202278736, - -0.4432571629508686, - -0.4402251344277631, - -0.5801434175912178, - -0.48507214969510887, - -0.6568731416811187, - -0.5673308844102857, - -0.5814957792388323, - -0.6844893936772606, - -0.5401025120112342, - -0.48988545597349864, - -0.5931888061462375, - -0.5214960732259393, - -0.6507846180868752, - -0.4703439848637819, - -0.43394078274207704, - -0.6902527931190684, - -0.6246090113311329, - -0.6128823466402761, - -0.5977684090650262, - -0.6188108361631892, - -0.5140207332513618, - -0.48375386062984443, - -0.42584349609347044, - -0.44170151004589836, - -0.6932544269209271, - -0.4125646009036971, - -0.4449500701089878, - -0.6676078660091681, - -0.47640194638807387, - -0.6874980196604519, - -0.6690655869671911, - -0.539238187189378, - -0.46207138812793036, - -0.6816082617253847, - -0.47250760274787973, - -0.5852976836682025, - -0.6431126869973152, - -0.6694585378352791, - -0.5153306266374333, - -0.4750339844703184, - -0.50933661398389, - -0.4833719044358295, - -0.521052067691706, - -0.5944816360722881, - -0.6677190251458424, - -0.40821386679022115, - -0.46314617492155385, - -0.6198821971817032, - -0.6152362526224303, - -0.6842824868253917, - -0.6406141190687638, - -0.5982799569001337, - -0.6194115785045566, - -0.5714897413573339, - -0.6209771069593248, - -0.5295946231989264, - -0.6091642756983353, - -0.6591781056866298, - -0.4142486307519474, - -0.4147481536341235, - -0.5036992390462757, - -0.6050502554335543, - -0.6556945367750314, - -0.47398879568615493, - -0.6839237385804874, - -0.4824479983850688, - -0.6901632682374682, - -0.4749374471395421, - -0.6044655698159995, - -0.43028620595944966, - -0.4258777307462205, - -0.4028389498512943, - -0.6639381525705192, - -0.4695994693340742, - -0.6943302339036809, - -0.7112259538805179, - -0.4929376228333231, - -0.5243219711297671, - -0.6406194102780428, - -0.6600173802174099, - -0.5531326439471762, - -0.6758586073660043, - -0.5959203890278671, - -0.6618731703627164, - -0.4899692518777917, - -0.4798062823937752, - -0.5523462083229935, - -0.4531605400954475, - -0.6791258701599923, - -0.47761321994149514, - -0.4341332082453844, - -0.47968387479928226, - -0.6872032133198404, - -0.485405660329431, - -0.47533609984938124, - -0.4828440815644971, - -0.6761733492265949, - -0.511411508741336, - -0.6804062758865259, - -0.5854668784104032, - -0.40539795966919595, - -0.636609395221421, - -0.5138009275817071, - -0.49569259428102375, - -0.6392552395149892, - -0.5432799188214187, - -0.5374934818994854, - -0.6267564643713496, - -0.6360131818238857, - -0.4661951967040701, - -0.6515247903206303, - -0.5103614707477175, - -0.6734729055046554, - -0.645863654158806, - -0.5640015111157086, - -0.6502803160271091, - -0.6283353387047861, - -0.6429442238547713, - -0.5076227722743979, - -0.5128061721026725, - -0.6209348765163634, - -0.5598086413173432, - -0.452027367640625, - -0.5220128740003572, - -0.45603307163257867, - -0.5034249376564026, - -0.5678848943632361, - -0.6393471806709078, - -0.4762343967696803, - -0.6360734894875639, - -0.5973942143720634, - -0.6624957236242699, - -0.5483310315215949, - -0.5447478487485775, - -0.5459543808678906, - -0.42814482553571825, - -0.4116890488429294, - -0.43130370220897807, - -0.4221252293401427, - -0.5121565915410085, - -0.611186122196988, - -0.6862225954917015, - -0.5674704766612471, - -0.5472648569149374, - -0.7060902187896556, - -0.606164116325182, - -0.600241556686105, - -0.4403791341699835, - -0.46641636042972157, - -0.5672603842323853, - -0.5492248320936247, - -0.4916587789134014, - -0.5827545017205753, - -0.6186751748649695, - -0.4769428905187326, - -0.539374484458655, - -0.47604583953714086, - -0.47562256949920745, - -0.42248879608702516, - -0.41160636855733906, - -0.6932285232266057, - -0.5752532824150955, - -0.5290555591285175, - -0.4710365372432595, - -0.6205143232817887, - -0.5424517564348914, - -0.4322205511544012, - -0.6142647439916433, - -0.6085641821428065, - -0.6871973597088508, - -0.6229113688540746, - -0.6189545509216847, - -0.5013563861681412, - -0.6695273461975852, - -0.5455390137294192, - -0.49273368050739486, - -0.624274412077184, - -0.666672201308555, - -0.5096116437902258, - -0.6604872794246502, - -0.4284254603606638, - -0.4297159327592348, - -0.4349227084775545, - -0.4817627809549944, - -0.6218374189940273, - -0.45171920205204413, - -0.5759006165563253, - -0.5852887236280621, - -0.5204266181786085, - -0.7012634311933035, - -0.43632811199270827, - -0.6709690046945224, - -0.5737992084705814, - -0.5029565469720205, - -0.5697955745679427, - -0.6380982394336581, - -0.6253793996872784, - -0.4993644570061364, - -0.573482484156258, - -0.6582366571330155, - -0.523007108793048, - -0.5541421503999957, - -0.5621556422173142, - -0.5470984302751866, - -0.5063293199020574, - -0.5387928145716308, - -0.5062697802348741, - -0.40661851355358797, - -0.6905765324120471, - -0.6597703838284343, - -0.6646716508760421, - -0.43738195884334286, - -0.44329814446444493, - -0.7010331678052806, - -0.6372410535141534, - -0.6703102069577147, - -0.6324793632872069, - -0.5069741745472643, - -0.566052233492988, - -0.5642073465258317, - -0.6710772821584025, - -0.6414391999479224, - -0.6709805413765559, - -0.6921422953273964, - -0.4312572554493957, - -0.6182721953389698, - -0.5940911375598288, - -0.6679174499759559, - -0.5640982688200307, - -0.41181185084039734, - -0.6426179571065536, - -0.5664298653414089, - -0.45170167420694124, - -0.577848513249009, - -0.6157198168536858, - -0.6412878727978906, - -0.512640540793082, - -0.4158339989415734, - -0.58699812443415, - -0.6190633813743412, - -0.6726386446760126, - -0.6655221012489334, - -0.4851494114701594, - -0.5019216544660531, - -0.6591203774815515, - -0.622155885265205, - -0.4997615516929663, - -0.6248123365477576, - -0.4930733465198123, - -0.5173751086718894, - -0.5771922769289642, - -0.42508859596321596, - -0.4314768323200352, - -0.48872538574499946, - -0.692680453825402, - -0.5593720227808863, - -0.4012821038654911, - -0.6233159371721895, - -0.7039780954171423, - -0.47764767051393275 - ], - "type": "scatter" - }, - { - "marker": { - "color": "#00CC96" - }, - "mode": "markers", - "name": "Expectation", - "x": [ - 0.4768515831256438 - ], - "y": [ - -0.556314228296471 - ], - "type": "scatter" - }, - { - "fill": "toself", - "legendgroup": "140215486767840", - "line": { - "color": "#AB63FA" - }, - "mode": "lines", - "name": "Mode", - "showlegend": true, - "x": [ - 0.30000000000000004, - 0.30000000000000004, - 0.6802107054679791, - 0.6802107054679791, - 0.30000000000000004, - null - ], - "y": [ - -0.6979180764986803, - -0.44438768097407855, - -0.44438768097407855, - -0.6979180764986803, - -0.6979180764986803, - null - ], - "type": "scatter" - } - ], - "layout": { - "title": { - "text": "Marginal View of relative x and y position with respect to the milk" - }, - "xaxis": { - "title": { - "text": "relative_x" - }, - "range": [ - -1, - 1 - ] - }, - "yaxis": { - "title": { - "text": "relative_y" - }, - "range": [ - -1, - 1 - ] - }, - "template": { - "data": { - "histogram2dcontour": [ - { - "type": "histogram2dcontour", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "choropleth": [ - { - "type": "choropleth", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "histogram2d": [ - { - "type": "histogram2d", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "heatmap": [ - { - "type": "heatmap", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "heatmapgl": [ - { - "type": "heatmapgl", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "contourcarpet": [ - { - "type": "contourcarpet", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "contour": [ - { - "type": "contour", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "surface": [ - { - "type": "surface", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - }, - "colorscale": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ] - } - ], - "mesh3d": [ - { - "type": "mesh3d", - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - ], - "scatter": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scatter" - } - ], - "parcoords": [ - { - "type": "parcoords", - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterpolargl": [ - { - "type": "scatterpolargl", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "bar": [ - { - "error_x": { - "color": "#f2f5fa" - }, - "error_y": { - "color": "#f2f5fa" - }, - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "bar" - } - ], - "scattergeo": [ - { - "type": "scattergeo", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterpolar": [ - { - "type": "scatterpolar", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "histogram": [ - { - "marker": { - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "histogram" - } - ], - "scattergl": [ - { - "marker": { - "line": { - "color": "#283442" - } - }, - "type": "scattergl" - } - ], - "scatter3d": [ - { - "type": "scatter3d", - "line": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scattermapbox": [ - { - "type": "scattermapbox", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scatterternary": [ - { - "type": "scatterternary", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "scattercarpet": [ - { - "type": "scattercarpet", - "marker": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - } - } - ], - "carpet": [ - { - "aaxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "baxis": { - "endlinecolor": "#A2B1C6", - "gridcolor": "#506784", - "linecolor": "#506784", - "minorgridcolor": "#506784", - "startlinecolor": "#A2B1C6" - }, - "type": "carpet" - } - ], - "table": [ - { - "cells": { - "fill": { - "color": "#506784" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "header": { - "fill": { - "color": "#2a3f5f" - }, - "line": { - "color": "rgb(17,17,17)" - } - }, - "type": "table" - } - ], - "barpolar": [ - { - "marker": { - "line": { - "color": "rgb(17,17,17)", - "width": 0.5 - }, - "pattern": { - "fillmode": "overlay", - "size": 10, - "solidity": 0.2 - } - }, - "type": "barpolar" - } - ], - "pie": [ - { - "automargin": true, - "type": "pie" - } - ] - }, - "layout": { - "autotypenumbers": "strict", - "colorway": [ - "#636efa", - "#EF553B", - "#00cc96", - "#ab63fa", - "#FFA15A", - "#19d3f3", - "#FF6692", - "#B6E880", - "#FF97FF", - "#FECB52" - ], - "font": { - "color": "#f2f5fa" - }, - "hovermode": "closest", - "hoverlabel": { - "align": "left" - }, - "paper_bgcolor": "rgb(17,17,17)", - "plot_bgcolor": "rgb(17,17,17)", - "polar": { - "bgcolor": "rgb(17,17,17)", - "angularaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "radialaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "ternary": { - "bgcolor": "rgb(17,17,17)", - "aaxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "baxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - }, - "caxis": { - "gridcolor": "#506784", - "linecolor": "#506784", - "ticks": "" - } - }, - "coloraxis": { - "colorbar": { - "outlinewidth": 0, - "ticks": "" - } - }, - "colorscale": { - "sequential": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "sequentialminus": [ - [ - 0.0, - "#0d0887" - ], - [ - 0.1111111111111111, - "#46039f" - ], - [ - 0.2222222222222222, - "#7201a8" - ], - [ - 0.3333333333333333, - "#9c179e" - ], - [ - 0.4444444444444444, - "#bd3786" - ], - [ - 0.5555555555555556, - "#d8576b" - ], - [ - 0.6666666666666666, - "#ed7953" - ], - [ - 0.7777777777777778, - "#fb9f3a" - ], - [ - 0.8888888888888888, - "#fdca26" - ], - [ - 1.0, - "#f0f921" - ] - ], - "diverging": [ - [ - 0, - "#8e0152" - ], - [ - 0.1, - "#c51b7d" - ], - [ - 0.2, - "#de77ae" - ], - [ - 0.3, - "#f1b6da" - ], - [ - 0.4, - "#fde0ef" - ], - [ - 0.5, - "#f7f7f7" - ], - [ - 0.6, - "#e6f5d0" - ], - [ - 0.7, - "#b8e186" - ], - [ - 0.8, - "#7fbc41" - ], - [ - 0.9, - "#4d9221" - ], - [ - 1, - "#276419" - ] - ] - }, - "xaxis": { - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "automargin": true, - "zerolinewidth": 2 - }, - "yaxis": { - "gridcolor": "#283442", - "linecolor": "#506784", - "ticks": "", - "title": { - "standoff": 15 - }, - "zerolinecolor": "#283442", - "automargin": true, - "zerolinewidth": 2 - }, - "scene": { - "xaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - }, - "yaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - }, - "zaxis": { - "backgroundcolor": "rgb(17,17,17)", - "gridcolor": "#506784", - "linecolor": "#506784", - "showbackground": true, - "ticks": "", - "zerolinecolor": "#C8D4E3", - "gridwidth": 2 - } - }, - "shapedefaults": { - "line": { - "color": "#f2f5fa" - } - }, - "annotationdefaults": { - "arrowcolor": "#f2f5fa", - "arrowhead": 0, - "arrowwidth": 1 - }, - "geo": { - "bgcolor": "rgb(17,17,17)", - "landcolor": "rgb(17,17,17)", - "subunitcolor": "#506784", - "showland": true, - "showlakes": true, - "lakecolor": "rgb(17,17,17)" - }, - "title": { - "x": 0.05 - }, - "updatemenudefaults": { - "bgcolor": "#506784", - "borderwidth": 0 - }, - "sliderdefaults": { - "bgcolor": "#C8D4E3", - "borderwidth": 1, - "bordercolor": "rgb(17,17,17)", - "tickwidth": 0 - }, - "mapbox": { - "style": "dark" - } - } - } - }, - "config": { - "plotlyServerURL": "https://plot.ly" - } - }, - "text/html": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], - "execution_count": 15 - }, - { - "cell_type": "markdown", - "source": [ - "Finally, we observe our improved plan in action." - ], - "metadata": { - "collapsed": false - }, - "id": "bd4fd2fa3d469c8c" - }, - { - "cell_type": "code", - "source": [ - "from pycram.designators.action_designator import ParkArmsActionPerformable\n", - "\n", - "world.reset_world()\n", - "milk.set_pose(Pose([0.5, 3.15, 1.04]))\n", - "with simulated_robot:\n", - " \n", - " MoveTorsoActionPerformable(0.3).perform()\n", - " for sample in fpa:\n", - " try:\n", - " ParkArmsActionPerformable(Arms.RIGHT).perform()\n", - " sample.perform()\n", - " break\n", - " except PlanFailure as e:\n", - " continue" - ], - "metadata": { - "collapsed": false, - "ExecuteTime": { - "end_time": "2024-06-19T12:36:29.608976Z", - "start_time": "2024-06-19T12:36:28.003348Z" - } - }, - "id": "ea57ad24b398e28f", - "outputs": [], - "execution_count": 18 - }, - { - "cell_type": "code", - "source": [ - "# world.exit()\n", - "# viz_marker_publisher._stop_publishing()" - ], - "metadata": { - "collapsed": false - }, - "id": "62728fa8ed6e55bd", - "outputs": [], - "execution_count": null - } - ], - "metadata": { - "kernelspec": { - "name": "pycram", - "language": "python", - "display_name": "pycram" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 2 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython2", - "version": "2.7.6" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/improving_actions.md b/examples/improving_actions.md new file mode 100644 index 000000000..4cb46a28e --- /dev/null +++ b/examples/improving_actions.md @@ -0,0 +1,211 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Improving Actions using Probabilities + +In this tutorial we will look at probabilistic specifications of actions and especially at an advance plan to pick up +objects. +After this tutorial you will know: + +- Why are probabilities useful for robotics +- How to use probabilistic models to specify actions +- How to use probabilistic machine learning to improve actions + +Let's start by importing all the necessary modules. + +```python +import numpy as np +import os +import random + +import pandas as pd +import sqlalchemy.orm + +import plotly + +plotly.offline.init_notebook_mode() +import plotly.graph_objects as go +import tqdm + +from probabilistic_model.learning.jpt.jpt import JPT +from probabilistic_model.learning.jpt.variables import infer_variables_from_dataframe +from random_events.product_algebra import Event, SimpleEvent + +import pycram.orm.base +from pycram.designators.action_designator import MoveTorsoActionPerformable +from pycram.failures import PlanFailure +from pycram.designators.object_designator import ObjectDesignatorDescription +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.robot_descriptions import robot_description +from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.pose import Pose +from pycram.ros.viz_marker_publisher import VizMarkerPublisher +from pycram.process_module import ProcessModule, simulated_robot +from pycram.designators.specialized_designators.probabilistic.probabilistic_action import MoveAndPickUp, Arms, Grasp +from pycram.tasktree import task_tree, TaskTree + +ProcessModule.execution_delay = False +np.random.seed(69) +random.seed(69) +``` + +Next, we connect to a database where we can store and load robot experiences. + +```python +pycrorm_uri = os.getenv('PYCRORM_URI') +pycrorm_uri = "mysql+pymysql://" + pycrorm_uri +engine = sqlalchemy.create_engine('sqlite:///:memory:') +session = sqlalchemy.orm.sessionmaker(bind=engine)() +pycram.orm.base.Base.metadata.create_all(engine) +``` + +Now we construct an empty world with just a floating milk, where we can learn about PickUp actions. + +```python +world = BulletWorld(WorldMode.DIRECT) +print(world.prospection_world) +robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +viz_marker_publisher = VizMarkerPublisher() +milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground() +``` + +Next, we create a default, probabilistic model that describes how to pick up objects. We visualize the default policy. +The default policy tries to pick up the object by standing close to it, but not too close. + +```python +fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], + grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value]) +print(world.current_world) +p_xy = fpa.policy.marginal([fpa.variables.relative_x, fpa.variables.relative_y]) +fig = go.Figure(p_xy.root.plot(), p_xy.root.plotly_layout()) +fig.update_layout(title="Marginal View of relative x and y position of the robot with respect to the object.") +fig.show() +``` + +Next, we will perform pick up tasks using the default policy and observe the success rate. +The robot will now experiment with the behaviour specified by the default policy and observe his success rate in doing +so. +After finishing the experiments, we insert the results into the database. + +```python +pycram.orm.base.ProcessMetaData().description = "Experimenting with Pick Up Actions" +fpa.sample_amount = 100 +with simulated_robot: + fpa.batch_rollout() +task_tree.root.insert(session) +session.commit() +task_tree.reset_tree() +``` + +Let's query the data that is needed to learn a pick up action and have a look at it. + +```python +samples = pd.read_sql(fpa.query_for_database(), engine) +samples["arm"] = samples["arm"].astype(str) +samples["grasp"] = samples["grasp"].astype(str) +samples +``` + +We can now learn a probabilistic model from the data. We will use the JPT algorithm to learn a model from the data. + +```python +variables = infer_variables_from_dataframe(samples, scale_continuous_types=False) +model = JPT(variables, min_samples_leaf=25) +model.fit(samples) +model = model.probabilistic_circuit +``` + +```python +arm, grasp, relative_x, relative_y = model.variables +``` + +Let's have a look at how the model looks like. We will visualize the model density when we condition on grasping the +object from the front with the left arm. + +```python +event = SimpleEvent({arm: Arms.LEFT, grasp: Grasp.FRONT}).as_composite_set() +conditional_model, conditional_probability = model.conditional(event) +p_xy = conditional_model.marginal([relative_x, relative_y]) +fig = go.Figure(p_xy.plot(), p_xy.plotly_layout()) +fig.show() +``` + +Let's make a monte carlo estimate on the success probability of the new model. + +```python +fpa.policy = model +fpa.sample_amount = 500 +with simulated_robot: + fpa.batch_rollout() +``` + +We can see, that our new and improved model has a success probability of 60% as opposed to the 30% from the standard +policy. + +Next, we put the learned model to the test in a complex environment, where the milk is placed in a difficult to access +area. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "apartment.urdf") + +milk.set_pose(Pose([0.5, 3.15, 1.04])) +milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground() +fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], + grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP], policy=model) +fpa.sample_amount = 200 + +``` + +```python +p_xy = model.marginal([relative_x, relative_y]) +fig = go.Figure(p_xy.plot(), p_xy.plotly_layout()) +fig.show() +``` + +Let's look at the density of the relative x and y position of the robot with respect to the milk. We can see that he +would like to access the object from the right front area. + +```python +grounded_model = fpa.ground_model() +p_xy = grounded_model.marginal([relative_x, relative_y]).simplify() +fig = go.Figure(p_xy.plot(), p_xy.plotly_layout()) +fig.update_layout(title="Marginal View of relative x and y position with respect to the milk", + xaxis_range=[-1, 1], yaxis_range=[-1, 1]) +fig.show() +``` + +Finally, we observe our improved plan in action. + +```python +from pycram.designators.action_designator import ParkArmsActionPerformable + +world.reset_world() +milk.set_pose(Pose([0.5, 3.15, 1.04])) +with simulated_robot: + MoveTorsoActionPerformable(0.3).perform() + for sample in fpa: + try: + ParkArmsActionPerformable(Arms.RIGHT).perform() + sample.perform() + break + except PlanFailure as e: + continue +``` + +```python +# world.exit() +# viz_marker_publisher._stop_publishing() +``` diff --git a/examples/interface_examples/giskard.ipynb b/examples/interface_examples/giskard.ipynb deleted file mode 100644 index 54f81f4bc..000000000 --- a/examples/interface_examples/giskard.ipynb +++ /dev/null @@ -1,226 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "6577676e", - "metadata": {}, - "source": [ - "# Giskard interface in PyCRAM\n", - "This notebook should provide you with an example on how to use the Giskard interface. This includes how to use the interface, how it actually works and how to extend it. \n", - "\n", - "We start by installing and launching Giskard. For the installation please follow the instructions [here](https://github.com/SemRoCo/giskardpy).\n", - "After you finish the installation you should be able to launch Giskard by calling: \n", - "```\n", - "roslaunch giskardpy giskardpy_pr2_standalone.launch\n", - "```\n", - "\n", - "This way you can launch Giskard for any robot that is supported:\n", - "```\n", - "roslaunch giskardpy giskardpy_hsr_standalone.launch\n", - "```\n", - "\"Standalone\" means that Giskard only uses a simulated robot and does not look for a real robot. If you want to use Giskard with a real robot you have to switch out \"standalone\" with \"iai\", e.g:\n", - "```\n", - "roslaunch giskardpy giskardpy_hsr_iai.launch\n", - "```\n", - "\n", - "To see what Giskard is doing you can start RViz, there should already be a MarkerArray when starting otherwise you have to add this manually." - ] - }, - { - "cell_type": "markdown", - "id": "a86d4f3b", - "metadata": {}, - "source": [ - "## How to use the Giskard interface \n", - "Everything related to the Giskard interface is located in ```pycram.external_interfaces.giskard```. \n", - "The content of the file can be roughly divided into three parts:\n", - " 1. Methods to manage the belief states between PyCRAM and Giskard\n", - " 2. Motion goals that should be sent to Giskard for execution \n", - " 3. Helper methods to construct ROS messages\n", - " \n", - "The most useful methods are the ones for sending and executing Motion goals. These are the ones we will mostly look at.\n", - "\n", - "We will now start by setting up PyCRAM and then try to send some simple motion goals." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "50cd3e8d", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.bullet_world import BulletWorld, Object\n", - "from pycram.enums import ObjectType\n", - "\n", - "world = BulletWorld()\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" - ] - }, - { - "cell_type": "markdown", - "id": "e4166109", - "metadata": {}, - "source": [ - "When you are working on the real robot you also need to initialize the RobotStateUpdater, this module updates the robot in the BulletWorld with the pose and joint state of the real robot. \n", - "\n", - "You might need to change to topic names to fit the topic names as published by your robot. " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a3eff6ba", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.ros.robot_state_updater import RobotStateUpdater\n", - "\n", - "r = RobotStateUpdater(\"/tf\", \"/joint_states\")" - ] - }, - { - "cell_type": "markdown", - "id": "f4c4f85d", - "metadata": {}, - "source": [ - "Now we have a PyCRAM belief state set up, belief state in this case just refers to the BulletWorld since the BulletWorld represents what we believe the world to look like. \n", - "\n", - "The next step will be to send a simple motion goal. The motion goal we will be sending is moving the torso up. For this we just need to move one joint, so we can use the ```achive_joint_goal```. This method takes a dictionary with the joints that should be moved and the target value for the joints. \n", - "\n", - "Look at RViz to see the robot move, since we call Giskard for movement the robot in the BulletWorld will not move." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "79fb8a5a", - "metadata": { - "scrolled": true - }, - "outputs": [], - "source": [ - "from pycram.external_interfaces import giskard\n", - "\n", - "giskard.achieve_joint_goal({\"torso_lift_joint\": 0.28})" - ] - }, - { - "cell_type": "markdown", - "id": "77dbded9", - "metadata": {}, - "source": [ - "For Giskard everything is connected by joints (this is called a [World Tree](https://github.com/SemRoCo/giskardpy/wiki/World-Tree) by Giskard) therefore we can move the robot's base by using motion goals between the map origin and the robot base. (e.g. by sending a \"base_link\" goal in the \"map\" frame).\n", - "\n", - "In the example below we use a cartesian goal, meaning we give Giskard a goal pose, a root link and a tip link and Giskard tries to move all joints between root link and tip link such that the tip link is at the goal pose.\n", - "\n", - "This sort of movement is fine for short distances, but keep in mind that Giskard has no collision avoidance for longer journeys. So using MoveBase for longer distances is a better idea." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ec79b6b5", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.external_interfaces import giskard\n", - "from pycram.pose import Pose\n", - "\n", - "giskard.achieve_cartesian_goal(Pose([1, 0, 0]), \"base_link\", \"map\")" - ] - }, - { - "cell_type": "markdown", - "id": "98af5723", - "metadata": {}, - "source": [ - "Now for the last example: we will move the gripper using full body motion control. \n", - "\n", - "We will again use the cartesian goal, but now between \"map\" and \"r_gripper_tool_frame\" frames. This will not only move the robot (because the kinematic chain between \"map\" and \"base_link\" as used in the previous example is also part of this chain) but also move the arm of the robot such that it reaches the goal pose." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a255212e", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.external_interfaces import giskard\n", - "from pycram.pose import Pose\n", - "\n", - "giskard.achieve_cartesian_goal(Pose([1, 0.5, 0.7]), \"r_gripper_tool_frame\", \"map\")" - ] - }, - { - "cell_type": "markdown", - "id": "7dfe78ba", - "metadata": {}, - "source": [ - "That concludes this example you can now close the BulletWorld by using the \"exit\" method." - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "197aa1f0", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "2ae027ac", - "metadata": {}, - "source": [ - "## How the Giskard interface works \n", - "The PyCRAM interface to Giskard mostly relies on the Python interface that Giskard already provides ([tutorial](https://github.com/SemRoCo/giskardpy/wiki/Python-Interface) and the [source code](https://github.com/SemRoCo/giskardpy/blob/master/src/giskardpy/python_interface.py)). This interface provides methods to achieve motion goals and load things into the Giskard believe state. \n", - "\n", - "What PyCRAM does with this, is: Synchronize the belief state of Giskard with the one of PyCRAM by loading the environment URDF in Giskard, this is done before any motion goal is sent. Furthermore, the motion goals are wrapped in methods that use PyCRAM data types. \n", - "\n", - "You can also set collisions between different groups of links. By default, Giskard avoids all collisions but for things like grasping an object you want to allow collisions of the gripper. The interface also supports the following collision modes:\n", - " * avoid_all_collisions\n", - " * allow_self_collision\n", - " * allow_gripper_collision\n", - "The collision mode can be set by calling the respective method, after calling the method the collision mode is valid for the next motion goal. Afterwards, it defaults back to avoid_all_collisions.\n", - "\n", - "There is a ```init_giskard_interface``` method which can be used as a decorator. This decorator should be used on all methods that access the giskard_wrapper, since it assures that the interface is working and checks if Giskard died or the imports for the giskard_msgs failed. " - ] - }, - { - "cell_type": "markdown", - "id": "6908a9ab", - "metadata": {}, - "source": [ - "## Extend the Giskard interface\n", - "At the moment the PyCRAM Giskard interface is mostly a wrapper around the Python interface provided by Giskard. If you want to extend the interface there are two ways:\n", - " * Wrap more motion goals which are provided by the Python interface \n", - " * Design new Higher-Level motion goals by combining the motion goals already provided" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/interface_examples/giskard.md b/examples/interface_examples/giskard.md new file mode 100644 index 000000000..075230857 --- /dev/null +++ b/examples/interface_examples/giskard.md @@ -0,0 +1,157 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Giskard interface in PyCRAM + +This notebook should provide you with an example on how to use the Giskard interface. This includes how to use the +interface, how it actually works, and how to extend it. + +We start by installing and launching Giskard. For the installation please follow the +instructions [here](https://github.com/SemRoCo/giskardpy). +After you finish the installation you should be able to launch Giskard by calling: + +``` +roslaunch giskardpy giskardpy_pr2_standalone.launch +``` + +This way you can launch Giskard for any robot that is supported: + +``` +roslaunch giskardpy giskardpy_hsr_standalone.launch +``` + +"Standalone" means that Giskard only uses a simulated robot and does not look for a real robot. If you want to use +Giskard with a real robot you have to switch out "standalone" with "iai", e.g: + +``` +roslaunch giskardpy giskardpy_hsr_iai.launch +``` + +To see what Giskard is doing you can start RViz, there should already be a MarkerArray when starting otherwise you have +to add this manually. + +## How to use the Giskard interface + +Everything related to the Giskard interface is located in {class}`pycram.external_interfaces.giskard`. +The content of the file can be roughly divided into three parts: +1. Methods to manage the belief states between PyCRAM and Giskard +2. Motion goals that should be sent to Giskard for execution +3. Helper methods to construct ROS messages + +The most useful methods are the ones for sending and executing Motion goals. These are the ones we will mostly look at. + +We will now start by setting up PyCRAM and then try to send some simple motion goals. + +```python +from pycram.bullet_world import BulletWorld, Object +from pycram.enums import ObjectType + +world = BulletWorld() +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +``` + +When you are working on the real robot you also need to initialize the RobotStateUpdater, this module updates the robot +in the BulletWorld with the pose and joint state of the real robot. + +You might need to change to topic names to fit the topic names as published by your robot. + +```python +from pycram.ros_utils.robot_state_updater import RobotStateUpdater + +r = RobotStateUpdater("/tf", "/joint_states") +``` + +Now we have a PyCRAM belief state set up, belief state in this case just refers to the BulletWorld since the BulletWorld +represents what we believe the world to look like. + +The next step will be to send a simple motion goal. The motion goal we will be sending is moving the torso up. For this +we just need to move one joint, so we can use the ```achive_joint_goal```. This method takes a dictionary with the +joints that should be moved and the target value for the joints. + +Look at RViz to see the robot move, since we call Giskard for movement the robot in the BulletWorld will not move. + +```python +from pycram.external_interfaces import giskard + +giskard.achieve_joint_goal({"torso_lift_joint": 0.28}) +``` + +For Giskard everything is connected by joints (this is called +a [World Tree](https://github.com/SemRoCo/giskardpy/wiki/World-Tree) by Giskard) therefore we can move the robot's base +by using motion goals between the map origin and the robot base. (e.g. by sending a "base_link" goal in the "map" +frame). + +In the example below we use a cartesian goal, meaning we give Giskard a goal pose, a root link and a tip link and +Giskard tries to move all joints between root link and tip link such that the tip link is at the goal pose. + +This sort of movement is fine for short distances, but keep in mind that Giskard has no collision avoidance for longer +journeys. So using MoveBase for longer distances is a better idea. + +```python +from pycram.external_interfaces import giskard +from pycram.pose import Pose + +giskard.achieve_cartesian_goal(Pose([1, 0, 0]), "base_link", "map") +``` + +Now for the last example: we will move the gripper using full body motion control. + +We will again use the cartesian goal, but now between "map" and "r_gripper_tool_frame" frames. This will not only move +the robot (because the kinematic chain between "map" and "base_link" as used in the previous example is also part of +this chain) but also move the arm of the robot such that it reaches the goal pose. + +```python +from pycram.external_interfaces import giskard +from pycram.pose import Pose + +giskard.achieve_cartesian_goal(Pose([1, 0.5, 0.7]), "r_gripper_tool_frame", "map") +``` + +That concludes this example you can now close the BulletWorld by using the "exit" method. + +```python +world.exit() +``` + +## How the Giskard interface works + +The PyCRAM interface to Giskard mostly relies on the Python interface that Giskard already +provides ([tutorial](https://github.com/SemRoCo/giskardpy/wiki/Python-Interface) and +the [source code](https://github.com/SemRoCo/giskardpy/blob/master/src/giskardpy/python_interface.py)). This interface +provides methods to achieve motion goals and load things into the Giskard believe state. + +What PyCRAM does with this, is: Synchronize the belief state of Giskard with the one of PyCRAM by loading the +environment URDF in Giskard, this is done before any motion goal is sent. Furthermore, the motion goals are wrapped in +methods that use PyCRAM data types. + +You can also set collisions between different groups of links. By default, Giskard avoids all collisions but for things +like grasping an object you want to allow collisions of the gripper. The interface also supports the following collision +modes: +* avoid_all_collisions +* allow_self_collision +* allow_gripper_collision +The collision mode can be set by calling the respective method, after calling the method the collision mode is valid for +the next motion goal. Afterwards, it defaults back to avoid_all_collisions. + +There is a ```init_giskard_interface``` method which can be used as a decorator. This decorator should be used on all +methods that access the giskard_wrapper, since it assures that the interface is working and checks if Giskard died or +the imports for the giskard_msgs failed. + +## Extend the Giskard interface + +At the moment the PyCRAM Giskard interface is mostly a wrapper around the Python interface provided by Giskard. If you +want to extend the interface there are two ways: + +* Wrap more motion goals which are provided by the Python interface +* Design new Higher-Level motion goals by combining the motion goals already provided diff --git a/examples/interface_examples/robokudo.ipynb b/examples/interface_examples/robokudo.ipynb deleted file mode 100644 index 04268950b..000000000 --- a/examples/interface_examples/robokudo.ipynb +++ /dev/null @@ -1,114 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "978aa367", - "metadata": {}, - "source": [ - "# Robokudo interface in PyCRAM\n", - "This notebook should give you an example on how the RoboKudo interface in PyCRAM works. We will go over how to use the interface, how it is implemented and what can be extended. \n", - "\n", - "First, you need to install RoboKudo by following the installation instructions [here](https://robokudo.ai.uni-bremen.de/installation.html). \n", - "\n", - "RoboKudo depends on a pipline of so-called annotators to process images, depending on your use-case the used annotators will change. But for this simple example we can use the demo pipeline from the [tutorial](https://robokudo.ai.uni-bremen.de/tutorials/run_pipeline.html). You can start RoboKudo by calling \n", - "```\n", - "rosrun robokudo main.py _ae=query\n", - "```\n", - "To get a stream of images to process you need the test bag file, from [here](https://robokudo.ai.uni-bremen.de/_downloads/6cd3bff02fd0d7a3933348060faa42fc/test.bag). You can run this bag file with the following command in the directory where the bag file is. \n", - "```\n", - "rosbag play test.bag --loop\n", - "```\n", - "\n", - "There should now be two windows which show you the result of the annotators. You switch between different annotators by using the arrow keys. \n" - ] - }, - { - "cell_type": "markdown", - "id": "6c37a831", - "metadata": {}, - "source": [ - "## How to use the RoboKudo interface in PyCRAM\n", - "Everything related to the RoboKudo interface can be found in the file ```pycram.external_interfaces.robokudo```. The most important method of this file is ```query``` which takes a PyCRAM object designator and calls RoboKudo to try to find a fitting object in the camera view. The other methods are just helper for constructing messages. \n", - "\n", - "Since we are only working with the demo pipeline we will only see how the interface functions but not actually perceive objects in the images." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "74811bdf", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "[WARN] [1700146040.931888]: RoboKudo is not running, could not initialize RoboKudo interface\n" - ] - } - ], - "source": [ - "from pycram.external_interfaces import robokudo\n", - "from pycram.designators.object_designator import *\n", - "from pycram.enums import ObjectType\n", - "\n", - "object_desig_desc = ObjectDesignatorDescription(types=[ObjectType.BOWL])\n", - "robokudo.query(object_desig_desc)" - ] - }, - { - "cell_type": "markdown", - "id": "da0dbc43", - "metadata": {}, - "source": [ - "There was no object detected since the pipline we are using for this example only returns an empty message. However, this should give you an impression on how the interface works." - ] - }, - { - "cell_type": "markdown", - "id": "15a078c8", - "metadata": {}, - "source": [ - "## How the RoboKudo interface in PyCRAM works\n", - "The interface to RoboKudo is designed around the ROS service that RoboKudo provides. The interface takes an ObjectDesignatorDescription which is PyCRAMs symbolic representation of objects and converts it to a RoboKudo ObjectDesignator, the RoboKudo ObjectDesignator is then send to RoboKudo. \n", - "\n", - "The result from this is a list of RoboKudo ObjectDesignators which are possible matches that were found in the camera FOV. Each of these ObjectDesignators has a list of possible poses that are the result of different pose estimators (currently PyCRAM picks the pose from 'ClusterPoseBBAnnotator' from the list of possible poses).\n", - "PyCRAM then transforms all possible poses for the found Objects to 'map' frame and returns them as a dictionary.\n", - "\n", - "When using the interface the decorator ```init_robokudo_interface``` should be added to all methods that want to send queries to RoboKudo. This decorator makes sure that RoboKudo is running and creates an action client which can be used via the global variable ```robokudo_action_client```." - ] - }, - { - "cell_type": "markdown", - "id": "74f8c0d2", - "metadata": {}, - "source": [ - "## How to extend the RoboKudo interface in PyCRAM\n", - "At the moment the RoboKudo interface is tailored towards a specific scenario, in which only two types of objects need to be detected. The distinction is mainly made by the difference in color, which is written in the RoboKudo ObjectDesignator depending on the ObjectType of the PyCRAM ObjectDesignator. \n", - "\n", - "The main point for extension would be to make the interface more universal and extend it to work with other pipelines for example for human detection." - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/interface_examples/robokudo.md b/examples/interface_examples/robokudo.md new file mode 100644 index 000000000..db6815980 --- /dev/null +++ b/examples/interface_examples/robokudo.md @@ -0,0 +1,85 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Robokudo interface in PyCRAM + +This notebook should give you an example on how the RoboKudo interface in PyCRAM works. We will go over how to use the +interface, how it is implemented and what can be extended. + +First, you need to install RoboKudo by following the installation +instructions [here](https://robokudo.ai.uni-bremen.de/installation.html). + +RoboKudo depends on a pipline of so-called annotators to process images, depending on your use-case the used annotators +will change. But for this simple example we can use the demo pipeline from +the [tutorial](https://robokudo.ai.uni-bremen.de/tutorials/run_pipeline.html). You can start RoboKudo by calling + +``` +rosrun robokudo main.py _ae=query +``` + +To get a stream of images to process you need the test bag file, +from [here](https://robokudo.ai.uni-bremen.de/_downloads/6cd3bff02fd0d7a3933348060faa42fc/test.bag). You can run this +bag file with the following command in the directory where the bag file is. + +``` +rosbag play test.bag --loop +``` + +There should now be two windows which show you the result of the annotators. You switch between different annotators by +using the arrow keys. + +## How to use the RoboKudo interface in PyCRAM + +Everything related to the RoboKudo interface can be found in the file {class}`pycram.external_interfaces.robokudo`. The +most important method of this file is {meth}`~pycram.external_interfaces.robokudo.query` which takes a PyCRAM object designator and calls RoboKudo to try to +find a fitting object in the camera view. The other methods are just helper for constructing messages. + +Since we are only working with the demo pipeline we will only see how the interface functions but not actually perceive +objects in the images. + +```python +from pycram.external_interfaces import robokudo +from pycram.designators.object_designator import * +from pycram.enums import ObjectType + +object_desig_desc = ObjectDesignatorDescription(types=[ObjectType.BOWL]) +robokudo.query(object_desig_desc) +``` + +There was no object detected since the pipline we are using for this example only returns an empty message. However, +this should give you an impression on how the interface works. + +## How the RoboKudo interface in PyCRAM works + +The interface to RoboKudo is designed around the ROS service that RoboKudo provides. The interface takes an +ObjectDesignatorDescription which is PyCRAMs symbolic representation of objects and converts it to a RoboKudo +ObjectDesignator, the RoboKudo ObjectDesignator is then send to RoboKudo. + +The result from this is a list of RoboKudo ObjectDesignators which are possible matches that were found in the camera +FOV. Each of these ObjectDesignators has a list of possible poses that are the result of different pose estimators ( +currently PyCRAM picks the pose from 'ClusterPoseBBAnnotator' from the list of possible poses). +PyCRAM then transforms all possible poses for the found Objects to 'map' frame and returns them as a dictionary. + +When using the interface the decorator {meth}`~pycram.external_interfaces.robokudo.init_robokudo_interface` should be added to all methods that want to send +queries to RoboKudo. This decorator makes sure that RoboKudo is running and creates an action client which can be used +via the global variable {attr}`~pycram.external_interfaces.robokudo.robokudo_action_client`. + +## How to extend the RoboKudo interface in PyCRAM + +At the moment the RoboKudo interface is tailored towards a specific scenario, in which only two types of objects need to +be detected. The distinction is mainly made by the difference in color, which is written in the RoboKudo +ObjectDesignator depending on the ObjectType of the PyCRAM ObjectDesignator. + +The main point for extension would be to make the interface more universal and extend it to work with other pipelines +for example for human detection. diff --git a/examples/intro.ipynb b/examples/intro.ipynb deleted file mode 100644 index fe1dd321b..000000000 --- a/examples/intro.ipynb +++ /dev/null @@ -1,9178 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# PyCRAM Presentation" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:50.953932170Z", - "start_time": "2024-01-29T16:14:50.284068023Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "pybullet build time: May 20 2022 19:44:17\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1706544890.889466]: Failed to import Giskard messages\n", - "[WARN] [1706544890.894063]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" - ] - } - ], - "source": [ - "import pycram" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Bullet World\n", - "\n", - "The BulletWorld is the internal simulation of PyCRAM. You can simulate different actions and reason about the outcome of different actions. \n", - "\n", - "It is possible to spawn objects and robots into the BulletWorld, these objects can come from URDF, OBJ or STL files. \n", - "\n", - "A BulletWorld can be created by simply creating an object of the BulletWorld class. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:51.215354072Z", - "start_time": "2024-01-29T16:14:50.980844838Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" - ] - } - ], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "world = BulletWorld()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The BulletWorld allows to render images from arbitrary positions. In the following example we render images with the camera at the position [0.3, 0, 1] and pointing towards [1, 0, 1], so we are looking upwards along the x-axis. \n", - "\n", - "The renderer returns 3 different kinds of images which are also shown on the left side of the BulletWorld window. (If these winodws are missing, click the BulletWorld window to focus it, and press \"g\") These images are:\n", - "* An RGB image which shows everything like it is rendered in the BulletWorld window, just from another perspective. \n", - "* A depth image which consists of distance values from the camera towards the objects in the field of view. \n", - "* A segmentation mask image which segments the image into the different objects displayed. The segmentation is done by assigning every pixel the unique id of the object that is displayed there. " - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:51.251936758Z", - "start_time": "2024-01-29T16:14:51.234948638Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" - ] - }, - { - "data": { - "text/plain": [ - "[array([[[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " [[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " [[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " ...,\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]],\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]],\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]]], dtype=uint8),\n", - " array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " ...,\n", - " [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n", - " 0.80473447],\n", - " [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n", - " 0.8031688 ],\n", - " [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n", - " 0.80160314]], dtype=float32),\n", - " array([[-1, -1, -1, ..., -1, -1, -1],\n", - " [-1, -1, -1, ..., -1, -1, -1],\n", - " [-1, -1, -1, ..., -1, -1, -1],\n", - " ...,\n", - " [ 1, 1, 1, ..., 1, 1, 1],\n", - " [ 1, 1, 1, ..., 1, 1, 1],\n", - " [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "world.get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1]))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Objects\n", - "Everything that is located inside the BulletWorld is an Object. \n", - "Objects can be created from URDF, OBJ or STL files. Since everything is of type Object a robot might share the same methods as a milk (with some limitations).\n", - "\n", - "Signature:\n", - "Object:\n", - "* Name \n", - "* Type\n", - "* Filename or Filepath\n", - "\n", - " Optional:\n", - " * Position\n", - " * Orientation\n", - " * World \n", - " * Color \n", - " * Ignore Cached Files\n", - "\n", - "If there is only a filename and no path, PyCRAM will check in the resource directory if there is a matching file. \n" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:51.319150211Z", - "start_time": "2024-01-29T16:14:51.238519616Z" - } - }, - "outputs": [], - "source": [ - "milk = Object(\"Milk\", ObjectType.MILK, \"milk.stl\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "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. \n", - "\n", - "These methods are the same for every Object, however since some Objects may not have joints or more than one link methods related to these will not work. " - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:51.361155485Z", - "start_time": "2024-01-29T16:14:51.360054431Z" - } - }, - "outputs": [], - "source": [ - "milk.set_position(Pose([1, 0, 0]))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "To remove an Object from the BulletWorld just call the 'remove' method on the Object." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:51.404210573Z", - "start_time": "2024-01-29T16:14:51.360232825Z" - } - }, - "outputs": [], - "source": [ - "milk.remove()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "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." - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:54.380977290Z", - "start_time": "2024-01-29T16:14:51.404113811Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Scalar element defined multiple times: limit\n" - ] - } - ], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Costmaps\n", - "\n", - "Costmaps are a way to get positions with respect to certain criterias. \n", - "The currently available costmaps are:\n", - "* Occupancy Costmap\n", - "* Visibility Costmap\n", - "* Semantic Costmap \n", - "* Gaussian Costmap\n", - "\n", - "It is also possible to merge multiple costmaps to combine different criteria." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Visibility Costmaps\n", - "Visibility costmaps determine every position, around a target position, from which the target is visible. Visibility Costmaps are able to work with cameras that are movable in height for example, if the robot has a movable torso. " - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:14:54.951307507Z", - "start_time": "2024-01-29T16:14:54.380864026Z" - } - }, - "outputs": [], - "source": [ - "import pycram.costmaps as cm\n", - "v = cm.VisibilityCostmap(1.27, 1.60, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:01.756580723Z", - "start_time": "2024-01-29T16:14:54.961615686Z" - } - }, - "outputs": [], - "source": [ - "v.visualize()" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:05.092885577Z", - "start_time": "2024-01-29T16:15:05.091325419Z" - } - }, - "outputs": [], - "source": [ - "v.close_visualization()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Occupancy Costmap\n", - "Is valid for every position where the robot can be placed without colliding with an object." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:05.847569295Z", - "start_time": "2024-01-29T16:15:05.093669437Z" - } - }, - "outputs": [], - "source": [ - "o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:05.944506368Z", - "start_time": "2024-01-29T16:15:05.849514274Z" - } - }, - "outputs": [], - "source": [ - "s = cm.SemanticCostmap(kitchen, \"kitchen_island_surface\", size=100, resolution=0.02)\n", - "\n", - "g = cm.GaussianCostmap(200, 15, resolution=0.02)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "You can visualize the costmap in the BulletWorld to get an impression what information is actually contained in the costmap. With this you could also check if the costmap was created correctly. \n", - "Visualization can be done via the 'visualize' method of each costmap." - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:06.810575822Z", - "start_time": "2024-01-29T16:15:05.955432369Z" - } - }, - "outputs": [], - "source": [ - "o.visualize()" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:07.161867271Z", - "start_time": "2024-01-29T16:15:07.159806241Z" - } - }, - "outputs": [], - "source": [ - "o.close_visualization()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "It is also possible to combine two costmap, this will result in a new costmap with the same size which contains the information of both previous costmaps. Combination is done by checking for each position in the two costmaps if they are zero, in this case to same position in the new costmap will also be zero in any other case the new position will be the normalized product of the two combined costmaps." - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:07.193879719Z", - "start_time": "2024-01-29T16:15:07.161787791Z" - } - }, - "outputs": [], - "source": [ - "ov = o + v" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:09.283523209Z", - "start_time": "2024-01-29T16:15:07.188128850Z" - } - }, - "outputs": [], - "source": [ - "ov.visualize()" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:10.347228575Z", - "start_time": "2024-01-29T16:15:09.280179794Z" - } - }, - "outputs": [], - "source": [ - "ov.close_visualization()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Bullet World Reasoning \n", - "Allows for geometric reasoning in the BulletWorld. At the moment the following types of reasoning are supported:\n", - "* Stable\n", - "* Contact\n", - "* Visible \n", - "* Occluding \n", - "* Reachable \n", - "* Blocking\n", - "* Supporting\n", - "\n", - "To show the geometric reasoning we first spawn a robot as well as the milk Object again." - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:12.961385558Z", - "start_time": "2024-01-29T16:15:10.347682457Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - } - ], - "source": [ - "import pycram.world_reasoning as btr\n", - "milk = Object(\"Milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1, 0, 1]))\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We start with testing for visibility " - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:13.246942739Z", - "start_time": "2024-01-29T16:15:12.963076402Z" - }, - "scrolled": true - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Milk visible: True\n" - ] - } - ], - "source": [ - "milk.set_position(Pose([1,0,1]))\n", - "visible = btr.visible(milk, pr2.get_link_pose(\"wide_stereo_optical_frame\"))\n", - "print(f\"Milk visible: {visible}\")" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:13.334740244Z", - "start_time": "2024-01-29T16:15:13.249042978Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Milk is in contact with the floor: True\n" - ] - } - ], - "source": [ - "milk.set_position(Pose([1, 0, 0.05]))\n", - "\n", - "plane = BulletWorld.current_bullet_world.objects[0]\n", - "contact = btr.contact(milk, plane)\n", - "print(f\"Milk is in contact with the floor: {contact}\")" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:13.430296220Z", - "start_time": "2024-01-29T16:15:13.334507309Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Milk is reachable for the PR2: True\n" - ] - } - ], - "source": [ - "milk.set_position(Pose([0.6, -0.5, 0.7]))\n", - "\n", - "reachable = btr.reachable(milk, pr2, \"r_gripper_tool_frame\")\n", - "print(f\"Milk is reachable for the PR2: {reachable}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Designators\n", - "Designators are symbolic descriptions of Actions, Motions, Objects or Locations. In PyCRAM the different types of designators are represented by a class which takes a description, the description then tells the designator what to do. \n", - "\n", - "For example, let's look at a Motion Designator to move the robot to a specific location. \n", - "\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Motion Designators\n", - "\n", - "When using a Motion Designator you need to specify which Process Module needs to be used, either the Process Module for the real or the simulated robot. A Process Module is the interface between a real or simulated robot, and PyCRAM designators. By exchanging the Process Module, one can quickly change the robot the plan is executed on, allowing PyCRAM plans to be re-used across multiple robot platforms. This can be done either with a decorator which can be added to a function and then every designator executed within this function, will be executed on the specified robot. The other possibility is a \"with\" scope which wraps a code piece.\n", - "\n", - "These two ways can also be combined, you could write a function which should be executed on the real robot and the function contains a \"with\" scope which executes something on the simulated robot for reasoning purposes. " - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:13.939910370Z", - "start_time": "2024-01-29T16:15:13.437727375Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import *\n", - "from pycram.process_module import simulated_robot, with_simulated_robot\n", - "\n", - "description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", - "\n", - "with simulated_robot:\n", - " description.perform()\n", - " \n" - ] - }, - { - "cell_type": "code", - "execution_count": 23, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:14.446281217Z", - "start_time": "2024-01-29T16:15:13.948689644Z" - }, - "scrolled": true - }, - "outputs": [], - "source": [ - "from pycram.process_module import with_simulated_robot\n", - "\n", - "@with_simulated_robot\n", - "def move():\n", - " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).perform()\n", - "\n", - "move()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Other implemented Motion Designator descriptions are:\n", - "* Accessing\n", - "* Move TCP\n", - "* Looking\n", - "* Move Gripper\n", - "* Detecting\n", - "* Move Arm Joint \n", - "* World State Detecting " - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Object Designators\n", - "\n", - "An Object Designator represents objects. These objects could either be from the BulletWorld or the real world. Object Designators are used, for example, by the PickUpAction to know which object should be picked up." - ] - }, - { - "cell_type": "code", - "execution_count": 24, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:14.453344058Z", - "start_time": "2024-01-29T16:15:14.448819583Z" - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544910\n", - " nsecs: 372853994\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544913\n", - " nsecs: 335583925\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses: ..., \n", - "_current_link_transforms: ..., \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry: ...), _pose=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544910\n", - " nsecs: 372853994\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544913\n", - " nsecs: 335583925\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses: ..., \n", - "_current_link_transforms: ..., \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry: ...)>)" - ] - }, - "execution_count": 24, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "from pycram.designators.object_designator import *\n", - "\n", - "milk_desig = BelieveObject(names=[\"Milk\"])\n", - "milk_desig.resolve()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Location Designator\n", - "Location Designator can create a position in cartisian space from a symbolic desctiption" - ] - }, - { - "cell_type": "code", - "execution_count": 25, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:14.493010865Z", - "start_time": "2024-01-29T16:15:14.451251059Z" - }, - "scrolled": false - }, - "outputs": [ - { - "data": { - "text/plain": [ - "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544910\n", - " nsecs: 372853994\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544913\n", - " nsecs: 335583925\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses: ..., \n", - "_current_link_transforms: ..., \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry: ...), _pose=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544910\n", - " nsecs: 372853994\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544913\n", - " nsecs: 335583925\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses: ..., \n", - "_current_link_transforms: ..., \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry: ...)>)" - ] - }, - "execution_count": 25, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "from pycram.designators.object_designator import *\n", - "\n", - "milk_desig = BelieveObject(names=[\"Milk\"])\n", - "milk_desig.resolve()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Location Designators\n", - "Location Designators can create a position in cartesian space from a symbolic description." - ] - }, - { - "cell_type": "code", - "execution_count": 26, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:22.177685226Z", - "start_time": "2024-01-29T16:15:14.482072615Z" - }, - "scrolled": true - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Resolved: CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 201824188\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -1.26\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.443467406158451\n", - " w: 0.8962904996010476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 927674055\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -1.26\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.443467406158451\n", - " w: 0.8962904996010476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 938544273\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.08868143268496492\n", - " w: -0.9960600401064899, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 949182987\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.1341778331057823\n", - " w: -0.9909572690600926, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 959582567\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.1018321670517163\n", - " w: -0.9948015931599383, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 969888687\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.1181477706660459\n", - " w: -0.9929960243055576, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 979798555\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7414525335518785\n", - " w: -0.6710053207609464, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544915\n", - " nsecs: 994071245\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7421299374143975\n", - " w: -0.6702560376403205, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 9548187\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.07625058147116927\n", - " w: -0.997088686539622, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 24910449\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.0637115975084186\n", - " w: -0.9979683523754275, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 40581226\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.08526395561915938\n", - " w: -0.9963583983046332, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 71025371\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.025615781497071395\n", - " w: -0.9996718620318842, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 103567123\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.44000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7435677691352427\n", - " w: -0.6686605810897175, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 120177507\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.42000000000000004\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7443316400531483\n", - " w: -0.6678101598626592, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 135888099\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7451280178851306\n", - " w: -0.6669214623646299, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 147926568\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.0684794522339555\n", - " w: -0.9976525269961167, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 158867120\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.44\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.051517987991811\n", - " w: -0.9986720667532839, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 171792984\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.03442144373073932\n", - " w: -0.9994074065222308, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 183446645\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.051081118766858044\n", - " w: -0.9986945074974259, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 195348024\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 225474596\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 235300779\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.012817353286048666\n", - " w: 0.9999178543534167, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 246283054\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.54\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.025615781497071274\n", - " w: 0.9996718620318842, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 255914211\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.56\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.03837651950358723\n", - " w: 0.9992633500488202, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 265790700\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.58\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.051081118766857926\n", - " w: 0.9986945074974259, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 276698589\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.6\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.06371159750841848\n", - " w: 0.9979683523754275, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 289893150\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.18000000000000005\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.07625058147116914\n", - " w: 0.997088686539622, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 303444147\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6400579972438699\n", - " w: -0.7683265973296552, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 314954519\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.45999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6386358371363977\n", - " w: -0.7695091081495349, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 332621335\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.44000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.637152936590636\n", - " w: -0.7707373971684058, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 348136901\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.42000000000000004\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6356053782081866\n", - " w: -0.7720141211097294, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 359711885\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6339889056055383\n", - " w: -0.7733421413379021, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 370139122\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6322988865446836\n", - " w: -0.7747245433535415, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 395307302\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.21999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6154122094026357\n", - " w: -0.7882054380161091, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 405689239\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.20000000000000007\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6127638044913316\n", - " w: -0.7902661070204827, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 416645288\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.017233697316775633\n", - " w: -0.9998514888106103, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 437982320\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.01723369731677551\n", - " w: 0.9998514888106103, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 448891639\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3745654411331824\n", - " w: 0.9272004801059501, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 469527959\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.21999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7541778209662121\n", - " w: -0.6566702478129005, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 480023622\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.20000000000000007\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7554539549957066\n", - " w: -0.6552017413601289, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 490493774\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.18000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7568004989740795\n", - " w: -0.6536459322542933, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 501381158\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.16000000000000003\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7582233771877503\n", - " w: -0.6519948698310459, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 511650800\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.14\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7597291886829179\n", - " w: -0.6502396172667391, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 521751880\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.12\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7613253045921371\n", - " w: -0.6483700953835624, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 532479286\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.09999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7630199824727258\n", - " w: -0.6463748961301956, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 561986207\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.040000000000000036\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7687942703292956\n", - " w: -0.6394961844365031, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 571810960\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7785979849482799\n", - " w: -0.6275230496439776, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 581784725\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.54\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.034421443730739416\n", - " w: 0.9994074065222308, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 601855516\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.58\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.06847945223395538\n", - " w: 0.9976525269961167, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 611686706\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.6\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.08526395561915948\n", - " w: 0.9963583983046332, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 622118949\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.10183216705171617\n", - " w: 0.9948015931599383, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 632574319\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.118147770666046\n", - " w: 0.9929960243055576, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 643788337\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.13417783310578218\n", - " w: 0.9909572690600926, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 654880285\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.6799999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1498930671991917\n", - " w: 0.9887022142210559, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 675857067\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.18028099412059023\n", - " w: 0.98361514992343, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 696542024\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.76\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.20915386140954798\n", - " w: 0.9778827446363269, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 728210210\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.8400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2620133869693895\n", - " w: 0.9650642388197943, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 769653797\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.30826764220152825\n", - " w: 0.9512996692796181, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 779698371\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.318832815984807\n", - " w: 0.947810970315916, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 790320158\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.32901544105897695\n", - " w: 0.9443245414288284, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 800973892\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3388256770636711\n", - " w: 0.9408491699323249, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 811413764\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.34827434473373386\n", - " w: 0.9373926502807073, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 832077503\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.36613250713959444\n", - " w: 0.9305627261048418, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 853376865\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: 0.09999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8022929282893952\n", - " w: -0.5969305296404493, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 865315675\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.636848728643575\n", - " w: 0.7709887786635173, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 875989675\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6394961844365034\n", - " w: 0.7687942703292954, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 896036386\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6275230496439778\n", - " w: 0.7785979849482799, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 906243562\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6238505014869475\n", - " w: 0.7815436979430415, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 916708469\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6198304093435398\n", - " w: 0.7847357922594203, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 927308559\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6419537995511603\n", - " w: 0.7667433203111904, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 937535762\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.26\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4586899104028806\n", - " w: 0.8885964022516619, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 947962045\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4068385849311433\n", - " w: 0.9135000633887361, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 958276748\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.08\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.39911243533693425\n", - " w: 0.9169019925594128, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 969069242\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.39106548646982875\n", - " w: 0.9203628552327153, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 980567216\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3826834323650898\n", - " w: 0.9238795325112867, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544916\n", - " nsecs: 992730140\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3739517258340434\n", - " w: 0.9274481693042154, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 3598690\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: 0.07999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5435734062236173\n", - " w: -0.8393616336517022, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 14332056\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.36485567419311876\n", - " w: 0.9310640885616225, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 24833202\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7398679249122764\n", - " w: 0.6727521487784355, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 44901132\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6908662457673519\n", - " w: -0.7229825934691131, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 55425643\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7370989134318549\n", - " w: 0.6757848709593749, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 66057682\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5039557239143624\n", - " w: -0.8637294879381802, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 78454971\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.49561612558176465\n", - " w: -0.8685416835496846, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 98734617\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6928318560989847\n", - " w: -0.7210991742988171, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 108744382\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6933854908702646\n", - " w: -0.7205668331602574, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 119309425\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3553805575128866\n", - " w: 0.9347217015464174, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 129229784\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.10682611122099109\n", - " w: -0.9942777187292293, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 139137268\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4866443184625216\n", - " w: 0.8736001987798239, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 149181365\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.0859890841337593\n", - " w: -0.9962960791902362, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 159062862\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5039557239143623\n", - " w: 0.8637294879381803, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 197809934\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6908662457673519\n", - " w: 0.7229825934691132, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 207918167\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6915789093529722\n", - " w: 0.7223009152272711, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 227355718\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.44\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.06480582077938048\n", - " w: -0.9978978933704143, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 237487077\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.043355575214250826\n", - " w: -0.9990597049715505, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 247480630\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6938978092954753\n", - " w: 0.7200734894821085, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 257900953\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.58\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6943732675901296\n", - " w: 0.7196150118335541, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 278165102\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 288398027\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.02172373868536963\n", - " w: 0.9997640117435364, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 298751831\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.33999999999999997\n", - " y: -1.12\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5537478323882296\n", - " w: 0.8326844168863359, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 308977603\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7967527613758144\n", - " w: -0.6043054171857262, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 319071054\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8000000000000002\n", - " w: -0.5999999999999999, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 329062938\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.54\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.04335557521425048\n", - " w: 0.9990597049715505, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 339415788\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.56\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.06480582077938013\n", - " w: 0.9978978933704143, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 349754333\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8112421851755609\n", - " w: 0.584710284663765, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 359853982\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8072185755038553\n", - " w: 0.5902526335066423, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 371557712\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8034804340438839\n", - " w: 0.5953311617147653, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 381916522\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.58\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.08598908413375941\n", - " w: 0.9962960791902362, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 391955852\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.6\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.10682611122099096\n", - " w: 0.9942777187292293, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 401677608\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6043054171857263\n", - " w: -0.7967527613758143, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 411783218\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6\n", - " w: -0.8, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 421791553\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7384225572267273\n", - " w: -0.6743382882342812, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 432043552\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7937170381968226\n", - " w: 0.6082871552778866, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 442192077\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.74\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.790873617837808\n", - " w: 0.6119795099577574, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 452643394\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7347602405829027\n", - " w: -0.6783269041240771, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 474348306\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.12724528989938316\n", - " w: 0.9918712800552408, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 484361648\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1471837619629651\n", - " w: 0.9891091649633165, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 504082441\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7327590618734483\n", - " w: 0.680488175681506, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 514279603\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.64\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7318630507095947\n", - " w: 0.6814517407755631, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 524206399\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.33523500300737585\n", - " w: 0.9421345406886665, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 534056901\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.74\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.23813352749512762\n", - " w: 0.9712324248513985, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 544548749\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.76\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2543984682342504\n", - " w: 0.9670994878294927, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 554365396\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.78\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2700013373907261\n", - " w: 0.9628599471404028, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 564265251\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.8\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.28494683992052433\n", - " w: 0.9585433210968125, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 574109077\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.13999999999999996\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2992447144182441\n", - " w: 0.9541763992536934, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 593857049\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.9\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.31340294084074444\n", - " w: 0.9496202381333145, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 604115009\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.88\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3018224483858295\n", - " w: 0.9533641537473408, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 626145839\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.8400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.27727888479854484\n", - " w: 0.9607894774844672, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 636278867\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.26429951415565045\n", - " w: 0.9644406497120946, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 657084703\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.8\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2508413082792208\n", - " w: 0.9680282217274292, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 667837142\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6308905395898716\n", - " w: -0.7758718496349771, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 689748287\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6368487286435749\n", - " w: -0.7709887786635174, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 709959506\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.45999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6308905395898716\n", - " w: -0.7758718496349771, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 720077991\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.44000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6292425428779993\n", - " w: -0.7772089952081288, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 729877233\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.8400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.323044113557137\n", - " w: 0.9463839076696536, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 740610361\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.30924417189076625\n", - " w: 0.9509826718461247, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 760720729\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.78\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.27958765375807687\n", - " w: 0.9601201715754407, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 780767917\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6257273935913882\n", - " w: -0.7800418122827314, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 791338205\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6238505014869477\n", - " w: -0.7815436979430415, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 803500652\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.7\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2116996959579716\n", - " w: 0.9773347628787704, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 816298246\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.6799999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.19294175778498965\n", - " w: 0.9812102109654376, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 826271057\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.17350299206578976\n", - " w: 0.9848333421164307, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 836186170\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.15341808887275654\n", - " w: 0.988161368404286, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 846061468\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.78\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.23690237287865074\n", - " w: 0.9715334609391818, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 866801500\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.58\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.08980559531591699\n", - " w: 0.9959593139531121, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 879305124\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.56\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.06771200763417459\n", - " w: 0.9977049082880918, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 889095067\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.54\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0453144211719414\n", - " w: 0.9989727740203193, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 899603128\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.022709687246860417\n", - " w: 0.9997421017968333, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 909643411\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 919872760\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.022709687246860538\n", - " w: -0.9997421017968333, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 930837392\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.045314421171941524\n", - " w: -0.9989727740203193, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 940669298\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.44\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.06771200763417472\n", - " w: -0.9977049082880918, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 950444221\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.08980559531591711\n", - " w: -0.9959593139531121, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 960928440\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.11150592856108661\n", - " w: -0.9937637686571844, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 971531629\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.1327331510254085\n", - " w: -0.9911518100769762, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 981504678\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.15341808887275665\n", - " w: -0.9881613684042859, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544917\n", - " nsecs: 991544485\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.17350299206579006\n", - " w: -0.9848333421164306, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 1651048\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.76\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.22248407835269934\n", - " w: 0.9749363234999248, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 11669158\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.74\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.20759148751784465\n", - " w: 0.978215607271796, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 21538734\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.19223376424350164\n", - " w: 0.9813491630835448, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 41085720\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.6799999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1601822430069672\n", - " w: 0.9870874576374967, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 50888299\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.24\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6079025311911547\n", - " w: -0.7940116577049655, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 60572385\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.21999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6051264893449574\n", - " w: -0.7961293436955124, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 79907894\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.18000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5990966850218961\n", - " w: -0.8006766900539662, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 92547893\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.16000000000000003\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.595815661390807\n", - " w: -0.8031212222581565, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 102537870\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.14\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5923364782216495\n", - " w: -0.805690695346529, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 112884759\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.12\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5886413254594184\n", - " w: -0.8083943282590367, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 123359680\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.09999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5847102846637651\n", - " w: -0.8112421851755608, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 144013881\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.06000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5760484367663208\n", - " w: -0.8174155604703632, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 153965234\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.040000000000000036\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5712642314097769\n", - " w: -0.8207662139195283, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 164059638\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5661364326251805\n", - " w: -0.8243115549684078, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 194094181\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5483036971357891\n", - " w: -0.8362792928843958, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 203748703\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.541385747733913\n", - " w: -0.840774328907937, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 213503837\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.14353028825236291\n", - " w: 0.9896459247398505, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 224017381\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.12649723679630143\n", - " w: 0.9919669596730026, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 233902931\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.10911677189150902\n", - " w: 0.9940289382568178, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 263562440\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.31999999999999995\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.4812091581666131\n", - " w: -0.8766058099833582, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 273334980\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.56\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.05530038574178813\n", - " w: 0.9984697628555457, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 294827461\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.01850900096213863\n", - " w: 0.9998286937687794, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 304959297\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 314709424\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.36\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.532573437384568\n", - " w: 0.8463837981627399, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 325293064\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.36\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5257311121191336\n", - " w: 0.85065080835204, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 335991859\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.018509000962138755\n", - " w: -0.9998286937687794, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 346140861\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.036961098084952626\n", - " w: -0.9993167051682638, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 356065750\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.36\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5019268181932334\n", - " w: 0.8649100931185952, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 377432346\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.31999999999999995\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.49806072645607874\n", - " w: 0.8671421525690256, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 397984027\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -0.020000000000000018\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.16966474960750313\n", - " w: 0.9855018380199112, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 408214569\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.07347291217220556\n", - " w: -0.9972972130598458, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 418574810\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.36\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5104644303570166\n", - " w: -0.8598988692516618, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 428881645\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.36\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5183792016009997\n", - " w: -0.8551508658403557, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 438925266\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.09142755332923423\n", - " w: -0.9958117304451831, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 449546337\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.10911677189150912\n", - " w: -0.9940289382568177, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 459606885\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.12649723679630132\n", - " w: -0.9919669596730026, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 469818115\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.14353028825236303\n", - " w: -0.9896459247398504, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 480182886\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: -0.32\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.16018224300696732\n", - " w: -0.9870874576374967, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 490321397\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.05999999999999994\n", - " y: 0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.47918806782228074\n", - " w: -0.8777122510576854, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 501243829\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.46\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7630199824727257\n", - " w: 0.6463748961301957, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 511368751\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.44\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.764133282892573\n", - " w: 0.6450583895864149, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 531534433\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.3\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7733421413379024\n", - " w: 0.6339889056055382, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 542391061\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.28\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7749013304032105\n", - " w: 0.6320822162815011, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 552552223\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.26\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7765341206443674\n", - " w: 0.6300752014443031, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 562627077\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.24\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7782457171509326\n", - " w: 0.6279598743042668, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 572496891\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.22\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7800418122827314\n", - " w: 0.6257273935913882, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 582713603\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.2000000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7819286417257463\n", - " w: 0.6233679485255313, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 593607664\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.1800000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.783913048024316\n", - " w: 0.6208706251202633, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 604231595\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.1600000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7860025526744504\n", - " w: 0.6182232502820061, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 615185499\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.1400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7882054380161092\n", - " w: 0.6154122094026356, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 626099348\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.12\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7905308403287533\n", - " w: 0.6124222321969667, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 635951280\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7929888557099438\n", - " w: 0.6092361403592484, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 656157970\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7983486481160277\n", - " w: 0.602195513144453, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 666541576\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8012765844860855\n", - " w: 0.5982941042282742, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 696901559\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7882054380161092\n", - " w: -0.6154122094026356, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 706899642\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8043897838851272\n", - " w: 0.5941019067308558, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 717074871\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7947066772690753\n", - " w: -0.6069936549124264, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 727422475\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8077053073689017\n", - " w: 0.5895864113496071, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 737593412\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.584710284663765\n", - " w: 0.8112421851755609, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 748543024\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4310817166804798\n", - " w: 0.9023128911546209, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 759326457\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6794495938313833\n", - " w: -0.7337221881900318, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 780078649\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5800385487693184\n", - " w: 0.8145890264063119, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 790158510\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5750132745107971\n", - " w: 0.818144079081656, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 812283277\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8112421851755609\n", - " w: 0.584710284663765, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 822262287\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5637378164623551\n", - " w: 0.8259537967043049, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 832731246\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5847102846637648\n", - " w: 0.8112421851755609, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 842995643\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5902526335066423\n", - " w: 0.8072185755038553, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 853036403\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.88\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.35948867915153393\n", - " w: 0.9331494465314146, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 863647460\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.86\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3469462477349362\n", - " w: 0.9378850149046248, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 873960256\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.8400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.333732774306416\n", - " w: 0.9426677226646422, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 884471654\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3198189174888669\n", - " w: 0.9474786857846721, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 895127058\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5953311617147652\n", - " w: 0.8034804340438839, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 906480073\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5573899686393252\n", - " w: 0.8302508192469624, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 917049169\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5504910087462066\n", - " w: 0.8348410922382677, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 927591085\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.74\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2566679351570243\n", - " w: 0.9664996487646695, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 947772502\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.7\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2203854364245398\n", - " w: 0.9754128661300122, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 957824230\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.6799999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.20106587226819733\n", - " w: 0.9795777228015289, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 968458414\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1809865534660407\n", - " w: 0.9834855705420817, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 979157924\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1601822430069672\n", - " w: 0.9870874576374967, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544918\n", - " nsecs: 989204168\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.13870121188940068\n", - " w: 0.9903342737785114, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 20420312\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.56\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.07088902009067935\n", - " w: 0.9974842088126424, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 31285047\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.54\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.04745802042883704\n", - " w: 0.9988732333469429, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 41488409\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.023789307215243066\n", - " w: 0.9997169943850204, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 51862955\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.5\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 1.2246467991473532e-16\n", - " w: -1.0, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 62264919\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.02378930721524297\n", - " w: -0.9997169943850204, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 82359075\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.44\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.07088902009067946\n", - " w: -0.9974842088126424, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 103036642\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.11660571451398305\n", - " w: -0.9931782857788845, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 113466262\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.13870121188940082\n", - " w: -0.9903342737785114, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 133843421\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.1809865534660408\n", - " w: -0.9834855705420817, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 144020557\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.18\n", - " y: -0.32\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.20106587226819744\n", - " w: -0.9795777228015289, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 163890361\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6082871552778866\n", - " w: 0.7937170381968226, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 173745393\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.45999999999999996\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6119795099577574\n", - " w: 0.790873617837808, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 183910846\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8150216638044884\n", - " w: 0.5794304855022417, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 194156885\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.12\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.16018224300696732\n", - " w: -0.9870874576374967, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 204169988\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8190674767950149\n", - " w: 0.5736971923032635, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 214705228\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8234061353888457\n", - " w: 0.5674524968700076, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 235476016\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5413857477339129\n", - " w: 0.8407743289079371, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 245808124\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6727521487784355\n", - " w: 0.7398679249122764, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 255650997\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6743382882342814\n", - " w: 0.7384225572267273, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 265812635\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6757848709593749\n", - " w: 0.7370989134318549, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 275789737\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6771094889847062\n", - " w: 0.7358822867326472, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 286756038\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5483036971357889\n", - " w: 0.8362792928843958, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 298326969\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.554700196225229\n", - " w: 0.8320502943378437, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 308969020\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6804881756815059\n", - " w: 0.7327590618734483, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 319038152\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.5599999999999999\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6814517407755631\n", - " w: 0.7318630507095948, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 329315423\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5606288093051838\n", - " w: 0.8280672304692729, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 339347362\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5661364326251803\n", - " w: 0.8243115549684079, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 349482059\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5712642314097768\n", - " w: 0.8207662139195284, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 359558582\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5760484367663208\n", - " w: 0.8174155604703632, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 369482517\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.08\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5805210231682381\n", - " w: 0.8142452589114058, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 379415988\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.584710284663765\n", - " w: 0.8112421851755609, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 389329671\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.12\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5886413254594183\n", - " w: 0.8083943282590367, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 399363040\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.1400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5923364782216495\n", - " w: 0.805690695346529, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 409301280\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.1600000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5958156613908069\n", - " w: 0.8031212222581566, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 419255733\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.1800000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5990966850218961\n", - " w: 0.8006766900539662, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 429352521\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.3\n", - " y: 0.06000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5137015609692951\n", - " w: -0.8579689424785198, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 439219951\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.2000000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.602195513144453\n", - " w: 0.7983486481160277, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 449213981\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.12\n", - " y: -0.7\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.19611613513818393\n", - " w: 0.9805806756909202, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 459560871\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.12\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2132313029385177\n", - " w: 0.9770017458772231, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 469874858\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.12\n", - " y: -0.74\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2297529205473612\n", - " w: 0.9732489894677302, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 479789495\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7983486481160277\n", - " w: 0.602195513144453, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 489479780\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.3\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.4672596576628944\n", - " w: -0.8841201345522873, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 499516248\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7947066772690754\n", - " w: 0.6069936549124263, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 509546995\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7913349155531438\n", - " w: 0.6113829008293401, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 519337415\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7882054380161092\n", - " w: 0.6154122094026356, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 539964437\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7825789118992277\n", - " w: 0.6225514008101024, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 550527572\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7800418122827314\n", - " w: 0.6257273935913882, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 560472249\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.72\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.77766608796156\n", - " w: 0.6286775450376476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 580207109\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.22\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6051264893449573\n", - " w: 0.7961293436955124, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 590407609\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.24\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6079025311911546\n", - " w: 0.7940116577049655, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 621572971\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.3\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.47630463962314995\n", - " w: 0.8792803251941108, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 631641149\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.3\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4847685323929452\n", - " w: 0.8746424812468178, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 641553401\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.28\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6130353373714668\n", - " w: 0.790055488642318, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 653319358\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.42\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6275230496439778\n", - " w: 0.7785979849482799, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 663541078\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.44\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6292425428779992\n", - " w: 0.7772089952081289, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 683467149\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.48\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6324713384826219\n", - " w: 0.7745837630611687, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 693924903\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.3\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5257311121191336\n", - " w: 0.85065080835204, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 704140663\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.5\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6339889056055381\n", - " w: 0.7733421413379024, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 713812112\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7438189039121905\n", - " w: 0.6683812072334675, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 723740100\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7451280178851304\n", - " w: 0.6669214623646302, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 733844518\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.39999999999999997\n", - " y: -1.52\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6354469054448959\n", - " w: 0.7721445657132514, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 744749307\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.819067476795015\n", - " w: -0.5736971923032633, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 774046421\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7533635260394922\n", - " w: 0.6576042865077321, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 783934116\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7554539549957063\n", - " w: 0.655201741360129, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 794060468\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8150216638044883\n", - " w: -0.5794304855022419, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 804785490\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8112421851755609\n", - " w: -0.5847102846637648, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 825284957\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5637378164623552\n", - " w: -0.8259537967043047, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 845585823\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.42\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5750132745107973\n", - " w: -0.8181440790816558, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 856135606\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8043897838851272\n", - " w: -0.5941019067308557, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 876502752\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.06000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7983486481160278\n", - " w: -0.6021955131444529, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 886613845\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.07999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7955906604925091\n", - " w: -0.6058345491444782, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 896639585\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.09999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7929888557099438\n", - " w: -0.6092361403592484, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 907559156\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7071067811865475\n", - " w: 0.7071067811865476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 927424430\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.12\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7905308403287534\n", - " w: -0.6124222321969663, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 937479496\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.14\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7882054380161092\n", - " w: -0.6154122094026356, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 957934379\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7071067811865475\n", - " w: 0.7071067811865476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 977858066\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7071067811865475\n", - " w: 0.7071067811865476, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 987823247\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.16000000000000003\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7860025526744504\n", - " w: -0.618223250282006, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544919\n", - " nsecs: 997573375\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.18000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.783913048024316\n", - " w: -0.6208706251202633, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 7592201\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.20000000000000007\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7819286417257465\n", - " w: -0.623367948525531, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 27238607\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.24\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7782457171509329\n", - " w: -0.6279598743042666, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 37187337\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.26\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7765341206443676\n", - " w: -0.6300752014443028, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 57217121\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7664963583466964\n", - " w: -0.6422486532809958, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 67085981\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.42000000000000004\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7652911781639873\n", - " w: -0.6436842491659838, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 77337503\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6552017413601289\n", - " w: 0.7554539549957063, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 87342262\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6532424277825721\n", - " w: 0.7571488166435519, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 97246408\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.44000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.764133282892573\n", - " w: -0.6450583895864149, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 107070446\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.45999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7630199824727258\n", - " w: -0.6463748961301956, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 117566823\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6463748961301958\n", - " w: 0.7630199824727257, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 127794265\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6436842491659837\n", - " w: 0.7652911781639874, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 137711286\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6407474392457675\n", - " w: 0.767751730118527, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 150225162\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6375295648477821\n", - " w: 0.7704258912737796, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 160764455\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.76\n", - " y: 0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7619487840078729\n", - " w: -0.647637283167765, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 170743227\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.4\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4998611817921905\n", - " w: 0.8661055356810247, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 180782556\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.38\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4956161255817646\n", - " w: 0.8685416835496848, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 201177597\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.3399999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4866443184625216\n", - " w: 0.8736001987798239, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 213806390\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.32\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.48190142905041683\n", - " w: 0.8762254348506247, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 223469972\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: 0.09999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8112421851755609\n", - " w: -0.5847102846637648, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 243908166\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.28\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4718579255320242\n", - " w: 0.8816745987679439, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 263970375\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4153730020007066\n", - " w: 0.9096511799634632, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 273994445\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.08\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4077100588947825\n", - " w: 0.9131114432948549, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 284109830\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7533635260394923\n", - " w: -0.657604286507732, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 294022321\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8320502943378438\n", - " w: -0.5547001962252289, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 326006412\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6599999999999999\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7465333575886381\n", - " w: -0.6653479886551357, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 337008714\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.39971796346469235\n", - " w: 0.9166381781726305, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 347310543\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.39138108854039505\n", - " w: 0.9202286908877246, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 358036518\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3826834323650898\n", - " w: 0.9238795325112867, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 370610475\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3736087091451055\n", - " w: 0.9275864016095363, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 381522893\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3641404642253537\n", - " w: 0.9313440407893014, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 391707181\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.3542622175095866\n", - " w: 0.9351461282843395, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 402300357\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.34395763883862607\n", - " w: 0.9389851663815341, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 412396192\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.33321075897716623\n", - " w: 0.9428523691977768, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 422230243\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.9\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.32200621957921877\n", - " w: 0.9467375531541463, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 432636737\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7205668331602575\n", - " w: -0.6933854908702645, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 442850828\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7210991742988173\n", - " w: -0.6928318560989846, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 473550319\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.15423335048016681\n", - " w: -0.9880344496016634, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 483459949\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.13608082209815303\n", - " w: -0.9906977388977382, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 493614435\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.11750042131729284\n", - " w: -0.993072832671531, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 504096269\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.27999999999999997\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.44382247843994094\n", - " w: 0.8961147290561785, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 513952732\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.27999999999999997\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.453777645066917\n", - " w: 0.8911149470396753, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 523840188\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.27999999999999997\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.46310663556107695\n", - " w: 0.8863025691598214, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 533734798\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.09853761796664212\n", - " w: -0.9951333266680702, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 544054985\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7071067811865476\n", - " w: -0.7071067811865475, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 553883552\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7229825934691132\n", - " w: -0.6908662457673518, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 563676357\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8455570886676865\n", - " w: 0.5338850155265888, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 573505640\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7071067811865476\n", - " w: -0.7071067811865475, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 583408832\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7071067811865476\n", - " w: -0.7071067811865475, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 593615531\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7709887786635176\n", - " w: -0.6368487286435748, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 612910509\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7758718496349772\n", - " w: -0.6308905395898715, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 622579574\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8407743289079371\n", - " w: 0.5413857477339129, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 632315635\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7815436979430416\n", - " w: -0.6238505014869474, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 642050027\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.8400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2855086566344833\n", - " w: 0.9583761302259008, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 651688337\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2723432423390162\n", - " w: 0.9622001654293517, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 661351203\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8280672304692729\n", - " w: 0.5606288093051838, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 671063661\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8243115549684079\n", - " w: 0.5661364326251804, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 680626392\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.8\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2586642746412806\n", - " w: 0.9659672836200511, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 690305233\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.78\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.24446768710590863\n", - " w: 0.9696574394914359, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 699806213\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.76\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2297529205473612\n", - " w: 0.9732489894677302, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 719159364\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.198787259314375\n", - " w: 0.9800426651601856, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 728794574\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.8\n", - " y: -1.1400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.805690695346529\n", - " w: 0.5923364782216496, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 739019632\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.7\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.18255737974256264\n", - " w: 0.9831952009146149, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 758497953\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.14869598393710892\n", - " w: 0.9888829578676007, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 768382310\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.13111872764660712\n", - " w: 0.9913666724579432, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 778136730\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.020000000000000018\n", - " y: -0.8200000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.24942164698879468\n", - " w: 0.968394982439189, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 797852516\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6653479886551358\n", - " w: -0.746533357588638, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 807753801\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6636470369788332\n", - " w: -0.7480458611002506, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 817374706\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.6\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.09485133899780393\n", - " w: 0.9954914482256106, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 827094316\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.58\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.07625058147116914\n", - " w: 0.997088686539622, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 837395668\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6576042865077321\n", - " w: -0.7533635260394922, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 848101377\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.07924445748219482\n", - " w: -0.9968552131369695, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 897163629\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.01922011145500681\n", - " w: -0.9998152765964606, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 907011747\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.03837651950358736\n", - " w: -0.9992633500488202, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 916730880\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.44\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.057406724906011355\n", - " w: -0.9983508741597643, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 926568746\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.42\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.07625058147116927\n", - " w: -0.997088686539622, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 937285184\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.09485133899780428\n", - " w: -0.9954914482256106, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 947034358\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.11315653826752582\n", - " w: -0.9935771725675414, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 957026481\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.13111872764660726\n", - " w: -0.9913666724579432, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 967617511\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.21999999999999997\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4056394187149922\n", - " w: 0.9140331842906817, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 977944612\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: -0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.14869598393710906\n", - " w: -0.9888829578676007, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 988276481\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.46\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.769509108149535\n", - " w: 0.6386358371363977, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544920\n", - " nsecs: 998299598\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.44\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7707373971684058\n", - " w: 0.6371529365906361, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 8562088\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.42\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7720141211097296\n", - " w: 0.6356053782081865, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 18221139\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7847357922594201\n", - " w: 0.6198304093435398, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 28006315\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7815436979430415\n", - " w: 0.6238505014869475, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 37898063\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7785979849482798\n", - " w: 0.6275230496439778, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 57746171\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: 0.30000000000000004\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.476975706616483\n", - " w: -0.8789164779987384, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 67290544\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7709887786635173\n", - " w: 0.636848728643575, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 77006340\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7687942703292954\n", - " w: 0.6394961844365034, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 86667060\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7667433203111904\n", - " w: 0.6419537995511603, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 96265316\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: 0.32000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.48190142905041666\n", - " w: -0.8762254348506247, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 105965852\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.28\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7825789118992277\n", - " w: 0.6225514008101024, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 115576744\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.26\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7843680207272645\n", - " w: 0.6202957424167875, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 125284671\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.24\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.78624155930207\n", - " w: 0.6179192588245245, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 135140895\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5524309953122208\n", - " w: 0.8335586334615874, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 144938230\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5464711272034338\n", - " w: 0.8374779442665988, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 154725313\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5400672594718117\n", - " w: 0.8416218600099494, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 183697938\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5257311121191336\n", - " w: 0.85065080835204, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 194594383\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5176837863645347\n", - " w: 0.8555720293086252, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 204714298\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6488487878132037\n", - " w: -0.7609173743274208, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 214483737\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6511308684688737\n", - " w: -0.7589654749242356, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 244819402\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6225514008101024\n", - " w: 0.7825789118992277, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 255036592\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.2000000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7902661070204828\n", - " w: 0.6127638044913315, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 265106439\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6552017413601289\n", - " w: 0.7554539549957063, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 275486230\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.657604286507732\n", - " w: 0.7533635260394923, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 285556316\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6597957320005307\n", - " w: 0.7514450026674501, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 295557022\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6618025632357402\n", - " w: 0.7496781758158658, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 305361986\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.11750042131729249\n", - " w: 0.9930728326715311, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 314964056\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.64\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1360808220981529\n", - " w: 0.9906977388977382, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 324617624\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6669214623646302\n", - " w: 0.7451280178851305, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 334374666\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.54\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6683812072334675\n", - " w: 0.7438189039121905, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 344164848\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.66\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.15423335048016673\n", - " w: 0.9880344496016635, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 353968620\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.6799999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.1719194406273956\n", - " w: 0.9851110119851283, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 363894939\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.7\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.18910752115495127\n", - " w: 0.9819563867314218, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 373680114\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.72\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.205772893716877\n", - " w: 0.9785997732532861, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 383259534\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: -0.74\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.22189743411284238\n", - " w: 0.9750700122217567, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 393896102\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6154122094026356\n", - " w: 0.7882054380161092, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 403790950\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.61138290082934\n", - " w: 0.791334915553144, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 413312673\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6069936549124263\n", - " w: 0.7947066772690754, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 422859430\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7237282464487099\n", - " w: 0.6900850855454531, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 432488441\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7229825934691132\n", - " w: 0.6908662457673519, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 442560195\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7223009152272711\n", - " w: 0.6915789093529722, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 452620267\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7216753233664641\n", - " w: 0.6922317008371615, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 462723493\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.1800000000000002\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7924306153589356\n", - " w: 0.6099620642645399, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 472694635\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.1600000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7947066772690754\n", - " w: 0.6069936549124263, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 482583999\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.43999999999999995\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5982941042282742\n", - " w: 0.8012765844860855, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 492875099\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.25999999999999995\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4305820312855818\n", - " w: 0.9025514469181146, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 503608465\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.25999999999999995\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.44076779540038685\n", - " w: 0.8976211620376843, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 545543432\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.43999999999999995\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5794304855022417\n", - " w: 0.8150216638044885, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 556905508\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.43999999999999995\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.5736971923032635\n", - " w: 0.8190674767950149, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 579474687\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.62\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7196150118335541\n", - " w: 0.6943732675901296, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 590476512\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.1400000000000001\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7971027464882692\n", - " w: 0.6038436979391754, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 600560188\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.12\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7996280994512247\n", - " w: 0.6004955475005809, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 611992359\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.1\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8022929282893952\n", - " w: 0.5969305296404492, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 623099565\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.08\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8051084445192626\n", - " w: 0.5931276359804638, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 633123397\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8080869942070349\n", - " w: 0.5890631628216448, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 655219793\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.02\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8145890264063119\n", - " w: 0.5800385487693183, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 665919780\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -1.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.818144079081656\n", - " w: 0.575013274510797, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 676297664\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8219256175556252\n", - " w: 0.5695948377626013, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 686551332\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6225514008101027\n", - " w: -0.7825789118992276, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 697294473\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6191231894884393\n", - " w: -0.7852938788999072, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 707584142\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6154122094026357\n", - " w: -0.7882054380161091, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 717669010\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -0.96\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8259537967043049\n", - " w: 0.563737816462355, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 729904651\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6069936549124266\n", - " w: -0.7947066772690753, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 755266666\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.8348410922382677\n", - " w: 0.5504910087462066, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 765379905\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5257311121191337\n", - " w: -0.8506508083520399, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 775438785\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: -0.78\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.2165835404696473\n", - " w: 0.9762640882454054, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 785464763\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7589654749242356\n", - " w: -0.6511308684688735, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 797777175\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.7609173743274209\n", - " w: -0.6488487878132035, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 810024023\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.03999999999999998\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5331718778667884\n", - " w: -0.8460069436192604, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 821498632\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5400672594718116\n", - " w: -0.8416218600099494, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 832209587\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: -0.06\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.767751730118527\n", - " w: -0.6407474392457674, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 842942476\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: 0.33999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.4866443184625217\n", - " w: -0.8736001987798239, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 854440927\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.07999999999999996\n", - " y: 0.36\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.49121312130825956\n", - " w: -0.8710394190015727, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 864692211\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.48\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6248846524332634\n", - " w: -0.780717087781073, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 874951601\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.45999999999999996\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6231467899582747\n", - " w: -0.7821049022763493, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 885563135\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.44000000000000006\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6213354700248949\n", - " w: -0.7835446596646187, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 897155761\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.42000000000000004\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6194460375118611\n", - " w: -0.7850392388988298, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 907792329\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.4\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6174734445800121\n", - " w: -0.7865917271612352, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 917936801\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.38\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.6154122094026357\n", - " w: -0.7882054380161091, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 928446054\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.48\n", - " y: -0.9199999999999999\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6021955131444529\n", - " w: 0.7983486481160278, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 961369752\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.24\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5979254685648697\n", - " w: -0.8015517039102849, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 973284721\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.21999999999999997\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5948871592163426\n", - " w: -0.8038092235098512, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 987850427\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.20000000000000007\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5916812500238643\n", - " w: -0.8061720029684716, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544921\n", - " nsecs: 998610258\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.18000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5882940228258898\n", - " w: -0.8086471064112771, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 8282661\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.38\n", - " y: 0.16000000000000003\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5847102846637651\n", - " w: -0.8112421851755608, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 27345895\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: -1.06\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7554539549957063\n", - " w: 0.655201741360129, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 37099599\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: -1.04\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7571488166435519\n", - " w: 0.6532424277825721, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 67073106\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: -0.98\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7630199824727256\n", - " w: 0.6463748961301958, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 86748838\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6799999999999999\n", - " y: -0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.7677517301185269\n", - " w: 0.6407474392457675, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 105825424\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.43999999999999995\n", - " y: -0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5847102846637651\n", - " w: -0.8112421851755608, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 115547418\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.43999999999999995\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5895864113496071\n", - " w: -0.8077053073689017, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 134811162\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: 0.020000000000000018\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.814589026406312\n", - " w: -0.5800385487693182, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 144512414\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: 0.040000000000000036\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8112421851755609\n", - " w: -0.5847102846637648, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 154634714\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.78\n", - " y: 0.06000000000000005\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8080869942070349\n", - " w: -0.5890631628216447, reachable_arms=None)\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1706544922\n", - " nsecs: 164654016\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.15999999999999998\n", - " y: -0.62\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.13273315102540856\n", - " w: 0.9911518100769761, reachable_arms=None)\n" - ] - } - ], - "source": [ - "from pycram.designators.location_designator import *\n", - "from pycram.designators.object_designator import *\n", - "\n", - "robot_desig = BelieveObject(types=[ObjectType.ROBOT]).resolve()\n", - "milk_desig = BelieveObject(names=[\"Milk\"]).resolve()\n", - "location_desig = CostmapLocation(target=milk_desig, visible_for=robot_desig)\n", - "\n", - "print(f\"Resolved: {location_desig.resolve()}\")\n", - "print()\n", - "\n", - "for pose in location_desig:\n", - " print(pose)\n", - " " - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Action Designator\n", - "Action Designators are used to describe high-level actions. Action Designators are usually composed of other Designators to describe the high-level action in detail. " - ] - }, - { - "cell_type": "code", - "execution_count": 27, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:22.690059907Z", - "start_time": "2024-01-29T16:15:22.178420266Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.enums import Arms\n", - "\n", - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Making a simple plan\n", - "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." - ] - }, - { - "cell_type": "code", - "execution_count": 28, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:22.784167007Z", - "start_time": "2024-01-29T16:15:22.739698077Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.object_designator import *\n", - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.4, 1, 0.95]))\n" - ] - }, - { - "cell_type": "code", - "execution_count": 29, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.008203353Z", - "start_time": "2024-01-29T16:15:22.784090487Z" - } - }, - "outputs": [], - "source": [ - "cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", - "kitchen_desig = ObjectDesignatorDescription(names=[\"kitchen\"])\n", - "robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()\n", - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " MoveTorsoAction([0.3]).resolve().perform()\n", - "\n", - " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", - " pickup_arm = pickup_pose.reachable_arms[0]\n", - "\n", - " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", - "\n", - " PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[\"front\"]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", kitchen_desig.resolve(), cereal_desig.resolve()).resolve()\n", - "\n", - " place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve()\n", - "\n", - " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", - "\n", - " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " \n", - " \n", - " " - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Task Trees\n", - "Task trees are a hierarchical representation of all Actions involved in a plan. The Task tree can later be used to inspect and restructure the execution order of Actions in the plan." - ] - }, - { - "cell_type": "code", - "execution_count": 30, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.015721112Z", - "start_time": "2024-01-29T16:15:37.010004750Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n", - "├── perform(MoveMotion)\n", - "├── perform(MoveMotion)\n", - "├── perform(ParkArmsActionPerformable)\n", - "├── perform(ParkArmsActionPerformable)\n", - "├── perform(MoveTorsoActionPerformable)\n", - "├── perform(NavigateActionPerformable)\n", - "│ └── perform(MoveMotion)\n", - "├── perform(PickUpActionPerformable)\n", - "│ ├── perform(MoveTCPMotion)\n", - "│ ├── perform(MoveGripperMotion)\n", - "│ ├── perform(MoveTCPMotion)\n", - "│ ├── perform(MoveGripperMotion)\n", - "│ └── perform(MoveTCPMotion)\n", - "├── perform(ParkArmsActionPerformable)\n", - "├── perform(NavigateActionPerformable)\n", - "│ └── perform(MoveMotion)\n", - "├── perform(PlaceActionPerformable)\n", - "│ ├── perform(MoveTCPMotion)\n", - "│ ├── perform(MoveGripperMotion)\n", - "│ └── perform(MoveTCPMotion)\n", - "└── perform(ParkArmsActionPerformable)\n" - ] - } - ], - "source": [ - "import pycram.task\n", - "import anytree\n", - "tt = pycram.task.task_tree\n", - "print(anytree.RenderTree(tt))" - ] - }, - { - "cell_type": "code", - "execution_count": 31, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.200246997Z", - "start_time": "2024-01-29T16:15:37.012295886Z" - } - }, - "outputs": [], - "source": [ - "from anytree.dotexport import RenderTreeGraph, DotExporter\n", - "RenderTreeGraph(tt).to_picture(\"tree.png\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# ORM\n" - ] - }, - { - "cell_type": "code", - "execution_count": 32, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.447175714Z", - "start_time": "2024-01-29T16:15:37.236464380Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 22/22 [00:00<00:00, 119.33it/s]\n" - ] - }, - { - "data": { - "text/plain": [ - "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 14, 50, 893086), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" - ] - }, - "execution_count": 32, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "import sqlalchemy.orm\n", - "import pycram.orm.base\n", - "import pycram.orm.action_designator\n", - "\n", - "# set description of what we are doing\n", - "pycram.orm.base.ProcessMetaData().description = \"Tutorial for getting familiar with the ORM.\"\n", - "\n", - "engine = sqlalchemy.create_engine(\"sqlite+pysqlite:///:memory:\", echo=False)\n", - "session = sqlalchemy.orm.Session(bind=engine)\n", - "pycram.orm.base.Base.metadata.create_all(engine)\n", - "session.commit()\n", - "\n", - "\n", - "tt.insert(session)" - ] - }, - { - "cell_type": "code", - "execution_count": 33, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.488726709Z", - "start_time": "2024-01-29T16:15:37.444290355Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7)\n", - "NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16)\n" - ] - } - ], - "source": [ - "from sqlalchemy import select\n", - "\n", - "navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all()\n", - "print(*navigations, sep=\"\\n\")" - ] - }, - { - "cell_type": "code", - "execution_count": 34, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.489720308Z", - "start_time": "2024-01-29T16:15:37.488078620Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7)\n", - "NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16)\n" - ] - } - ], - "source": [ - "navigations = (session.scalars(select(pycram.orm.action_designator.NavigateAction, pycram.orm.base.Position, pycram.orm.base.Quaternion).\n", - " join(pycram.orm.action_designator.NavigateAction.pose).\n", - " join(pycram.orm.base.Pose.position).\n", - " join(pycram.orm.base.Pose.orientation)).all())\n", - "print(*navigations, sep=\"\\n\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The world can also be closed with the 'exit' method" - ] - }, - { - "cell_type": "code", - "execution_count": 35, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:15:37.732405944Z", - "start_time": "2024-01-29T16:15:37.488193658Z" - } - }, - "outputs": [], - "source": [ - "world.exit()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/examples/intro.md b/examples/intro.md new file mode 100644 index 000000000..c9d3cb5cc --- /dev/null +++ b/examples/intro.md @@ -0,0 +1,436 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# PyCRAM Introduction + +```python +import pycram +``` + +# Bullet World + +The BulletWorld is the internal simulation of PyCRAM. You can simulate different actions and reason about the outcome of +different actions. + +It is possible to spawn objects and robots into the BulletWorld, these objects can come from URDF, OBJ or STL files. + +A BulletWorld can be created by simply creating an object of the BulletWorld class. + +```python +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(mode=WorldMode.GUI) +``` + +The BulletWorld allows to render images from arbitrary positions. In the following example we render images with the +camera at the position [0.3, 0, 1] and pointing towards [1, 0, 1], so we are looking upwards along the x-axis. + +The renderer returns 3 different kinds of images which are also shown on the left side of the BulletWorld window. (If +these winodws are missing, click the BulletWorld window to focus it, and press "g") These images are: + +* An RGB image which shows everything like it is rendered in the BulletWorld window, just from another perspective. +* A depth image which consists of distance values from the camera towards the objects in the field of view. +* A segmentation mask image which segments the image into the different objects displayed. The segmentation is done by + assigning every pixel the unique id of the object that is displayed there. + +```python +world.get_images_for_target(Pose([1, 0, 1], [0, 0, 0, 1]), Pose([0.3, 0, 1], [0, 0, 0, 1])) +``` + +## Objects + +Everything that is located inside the BulletWorld is an Object. +Objects can be created from URDF, OBJ or STL files. Since everything is of type Object a robot might share the same +methods as a milk (with some limitations). + +Signature: +Object: + +* Name +* Type +* Filename or Filepath + +Optional: + +* Position +* Orientation +* World +* 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. + +These methods are the same for every Object, however since some Objects may not have joints or more than one link +methods related to these will not work. + +```python +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. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +``` + +## Costmaps + +Costmaps are a way to get positions with respect to certain criterias. +The currently available costmaps are: + +* {class}`~pycram.costmaps.OccupancyCostmap` +* {class}`~pycram.costmaps.VisibilityCostmap` +* {class}`~pycram.costmaps.SemanticCostmap` +* {class}`~pycram.costmaps.GaussianCostmap` + +It is also possible to merge multiple costmaps to combine different criteria. + +### Visibility Costmaps + +Visibility costmaps determine every position, around a target position, from which the target is visible. Visibility +Costmaps are able to work with cameras that are movable in height for example, if the robot has a movable torso. + +```python +import pycram.costmaps as cm + +v = cm.VisibilityCostmap(1.27, 1.60, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1])) +``` + +```python +v.visualize() +``` + +```python +v.close_visualization() +``` + +### Occupancy Costmap + +Is valid for every position where the robot can be placed without colliding with an object. + +```python +o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1])) +``` + +```python +s = cm.SemanticCostmap(kitchen, "kitchen_island_surface", size=100, resolution=0.02) + +g = cm.GaussianCostmap(200, 15, resolution=0.02) +``` + +You can visualize the costmap in the BulletWorld to get an impression what information is actually contained in the +costmap. With this you could also check if the costmap was created correctly. +Visualization can be done via the 'visualize' method of each costmap. + +```python +o.visualize() +``` + +```python +o.close_visualization() +``` + +It is also possible to combine two costmap, this will result in a new costmap with the same size which contains the +information of both previous costmaps. Combination is done by checking for each position in the two costmaps if they are +zero, in this case to same position in the new costmap will also be zero in any other case the new position will be the +normalized product of the two combined costmaps. + +```python +ov = o + v +``` + +```python +ov.visualize() +``` + +```python +ov.close_visualization() +``` + +## Bullet World Reasoning + +Allows for geometric reasoning in the BulletWorld. At the moment the following types of reasoning are supported: + +* {meth}`~pycram.world_reasoning.stable` +* {meth}`~pycram.world_reasoning.contact` +* {meth}`~pycram.world_reasoning.visible` +* {meth}`~pycram.world_reasoning.occluding` +* {meth}`~pycram.world_reasoning.reachable` +* {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 +milk.set_position(Pose([1, 0, 1])) +visible = btr.visible(milk, pr2.get_link_pose("wide_stereo_optical_frame")) +print(f"Milk visible: {visible}") +``` + +```python +milk.set_position(Pose([1, 0, 0.05])) + +plane = BulletWorld.current_bullet_world.objects[0] +contact = btr.contact(milk, plane) +print(f"Milk is in contact with the floor: {contact}") +``` + +```python +milk.set_position(Pose([0.6, -0.5, 0.7])) + +reachable = btr.reachable(milk, pr2, "r_gripper_tool_frame") +print(f"Milk is reachable for the PR2: {reachable}") +``` + +# Designators + +Designators are symbolic descriptions of Actions, Motions, Objects or Locations. In PyCRAM the different types of +designators are represented by a class which takes a description, the description then tells the designator what to do. + +For example, let's look at a Motion Designator to move the robot to a specific location. + +## Motion Designators + +When using a Motion Designator you need to specify which Process Module needs to be used, either the Process Module for +the real or the simulated robot. A Process Module is the interface between a real or simulated robot, and PyCRAM +designators. By exchanging the Process Module, one can quickly change the robot the plan is executed on, allowing PyCRAM +plans to be re-used across multiple robot platforms. This can be done either with a decorator which can be added to a +function and then every designator executed within this function, will be executed on the specified robot. The other +possibility is a "with" scope which wraps a code piece. + +These two ways can also be combined, you could write a function which should be executed on the real robot and the +function contains a "with" scope which executes something on the simulated robot for reasoning purposes. + +```python +from pycram.designators.motion_designator import * +from pycram.process_module import simulated_robot, with_simulated_robot + +description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1])) + +with simulated_robot: + description.perform() + + +``` + +```python +from pycram.process_module import with_simulated_robot + + +@with_simulated_robot +def move(): + MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).perform() + + +move() +``` + +Other implemented Motion Designator descriptions are: + +* Accessing +* Move TCP +* Looking +* Move Gripper +* Detecting +* Move Arm Joint +* World State Detecting + +## Object Designators + +An Object Designator represents objects. These objects could either be from the BulletWorld or the real world. Object +Designators are used, for example, by the PickUpAction to know which object should be picked up. + +```python +from pycram.designators.object_designator import * + +milk_desig = BelieveObject(names=["Milk"]) +milk_desig.resolve() +``` + +## Location Designator + +Location Designator can create a position in cartisian space from a symbolic desctiption + +```python +from pycram.designators.object_designator import * + +milk_desig = BelieveObject(names=["Milk"]) +milk_desig.resolve() +``` + +## Location Designators + +Location Designators can create a position in cartesian space from a symbolic description. + +```python +from pycram.designators.location_designator import * +from pycram.designators.object_designator import * + +robot_desig = BelieveObject(types=[ObjectType.ROBOT]).resolve() +milk_desig = BelieveObject(names=["Milk"]).resolve() +location_desig = CostmapLocation(target=milk_desig, visible_for=robot_desig) + +print(f"Resolved: {location_desig.resolve()}") +print() + +for pose in location_desig: + print(pose) + +``` + +# Action Designator + +Action Designators are used to describe high-level actions. Action Designators are usually composed of other Designators +to describe the high-level action in detail. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.enums import Arms + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + +``` + +# Making a simple plan + +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 * + +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) + +``` + +```python +from pycram.datastructures.enums import Grasp + +cereal_desig = ObjectDesignatorDescription(names=["cereal"]) +kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) +robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.3]).resolve().perform() + + pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] + + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() + + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_desig.resolve(), + cereal_desig.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + + +``` + +# Task Trees + +Task trees are a hierarchical representation of all Actions involved in a plan. The Task tree can later be used to +inspect and restructure the execution order of Actions in the plan. + +```python +import pycram.tasktree +import anytree + +tt = pycram.tasktree.task_tree +print(anytree.RenderTree(tt)) +``` + +```python +from anytree.dotexport import RenderTreeGraph, DotExporter + +RenderTreeGraph(tt).to_picture("tree.png") +``` + +# ORM + +```python +import sqlalchemy.orm +import pycram.orm.base +import pycram.orm.action_designator + +# set description of what we are doing +pycram.orm.base.ProcessMetaData().description = "Tutorial for getting familiar with the ORM." + +engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) +session = sqlalchemy.orm.Session(bind=engine) +pycram.orm.base.Base.metadata.create_all(engine) +session.commit() + +tt.insert(session) +``` + +```python +from sqlalchemy import select + +navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all() +print(*navigations, sep="\n") +``` + +```python +navigations = (session.scalars( + select(pycram.orm.action_designator.NavigateAction, pycram.orm.base.Position, pycram.orm.base.Quaternion). + join(pycram.orm.action_designator.NavigateAction.pose). + join(pycram.orm.base.Pose.position). + join(pycram.orm.base.Pose.orientation)).all()) +print(*navigations, sep="\n") +``` + +The world can also be closed with the 'exit' method + +```python +world.exit() +``` diff --git a/examples/language.ipynb b/examples/language.ipynb deleted file mode 100644 index 5ab5156e2..000000000 --- a/examples/language.ipynb +++ /dev/null @@ -1,479 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "d6690980", - "metadata": {}, - "source": [ - "# Plan Language\n", - "The PyCRAM plan language is a way to structure the execuition of your plan. In generall the plan language allows to execute designators either sequential or in parallel. Furthermore, excpetions that occur during execution of a plan with the plan language do not interupt the execution instead they are catched and saved to a dictionary for later analysis. All language expressions return a State, this can either be SUCCEDED or FAILED.\n", - "\n", - "There are 4 language expressions: \n", - "\n", - "| Expression | Name | Description| \n", - "| ---------- | ---- | ---------- |\n", - "| + | **Sequential** | Executes the designators one after another, if one of the designators raises an exception the execution is aborted and the state FAILED will be returned. |\n", - "| - | **Try In Order** | Executes the designators one after another, if one designator raises an exception the exception is catched and saved but the execution is not interrupted and the other designators are executed. Returns the state SUCCEDED if at least one designator can be executed without exception. |\n", - "| * | **Repeat** | Repeat the previous language expression a number of time. Has to be used with a language expression and an integer. | \n", - "| \\| | **Parallel** | Executes all designators in parallel. For each designator there will be a new thread created and the designator is executed in this thread. If one of the designators raises an exception the returned state will be FAILED. |\n", - "| ^ | **Try All** | Executes all designators in parallel with a designated thread for each designator. Returns the state SUCCEDED if at least one designator can be executed without an exception|\n", - "| >> | **Monitor** | Monitors the execution of the attached langauge expression, will interrupt the execution as soon as a given condition is fulfilled. | \n", - "\n", - "The Sequential expression is the only one which aborts the execution once an error is raised. \n", - "\n", - "When using the plan language a tree structure of the plan is created where the language expressions are nodes and designators are leafs. This tree uses AnyTree (like the task tree) and can be rendered with the anytree Renderer. " - ] - }, - { - "cell_type": "markdown", - "id": "fb240bbc", - "metadata": {}, - "source": [ - "## Sequential\n", - "This language expression allows to execute designators one after another, if one of the designators raises an exception the execution will be aborted and the state FAILED will be returned. \n", - "\n", - "We will start with a simple example that uses an action designator for moving the robot and parking its arms. " - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "30b01905", - "metadata": {}, - "outputs": [], - "source": [ - "import time\n", - "\n", - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "\n", - "plan = navigate + park" - ] - }, - { - "cell_type": "markdown", - "id": "aa1a94a2", - "metadata": {}, - "source": [ - "With this simple plan created we can inspect it and render the created tree structure. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "33eb9292", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "├── \n", - "└── \n" - ] - } - ], - "source": [ - "from anytree import RenderTree\n", - "\n", - "print(RenderTree(plan))" - ] - }, - { - "cell_type": "markdown", - "id": "b1cd7fd2", - "metadata": {}, - "source": [ - "As you can see there is the root node which is the language expression and then there are the leafs which are the designators. When executing this plan the Sequential node will try to execute the NavigateAction and if that is finished without any error the ParkArmsAction will be executed. \n", - "\n", - "The plan can be executed by wrapping it inside a ```with simulated_robot``` envirionment and calling perform on the plan. \n", - "\n", - "If you are performing a plan with a simulated robot, you need a BulletWorld. " - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "e86caf8d", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-10T13:40:14.972773461Z", - "start_time": "2024-01-10T13:40:14.922450288Z" - } - }, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "world = BulletWorld()\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" - ] - }, - { - "cell_type": "markdown", - "id": "67eb61d2", - "metadata": {}, - "source": [ - "If you are finished with this example you can close the world with the cell below." - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "fab4c0a8", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "4bdda375", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.process_module import simulated_robot\n", - "world.reset_bullet_world()\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "93eaa9bb", - "metadata": {}, - "source": [ - "## Try In Order\n", - "Try in order is similar to Sequential, it also executes all designators one after another but the key difference is that an exception in one of the designators does not terminate the whole execution. Furthermore, the state FAILED will only be returned if all designator executions raise an error. \n", - "\n", - "Besides the described differnce in behaviour this language expression can be used in the same way as Sequential." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "86d8887e", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "world.reset_bullet_world()\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "\n", - "plan = navigate - park\n", - "\n", - "with simulated_robot:\n", - " \n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "8edad65b", - "metadata": {}, - "source": [ - "## Parallel \n", - "Parallel executes all designator at once in dedicated threads. The execution of other designators is not aborted when a exception is raised, this is the case since threads can not be killed from the outside and this would also cause unforseen problems. The state returned will be SUCCEDED if all designators could be executed without an exception raised in any other case FAILED will be returned.\n", - "\n", - "Since executing designators in parallel can get chaotic especially with complex actions like PickUp or Transport. For this reason not all action designators can be used in parallel and try all expressions. The list of action designator that cannot be used in language expressions can be seen in ```Language.parallel_blocklist```.\n", - "\n", - "Designators that cannot be used in parallel and try all:\n", - " * PickUpAction\n", - " * PlaceAction\n", - " * OpenAction \n", - " * CloseAction \n", - " * TransportAction\n", - " \n", - "Using the parallel expressions works like Sequential and TryInOrder. " - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "2180c7eb", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "world.reset_world()\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "\n", - "plan = navigate | park\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "ff711f9a", - "metadata": {}, - "source": [ - "## Try All \n", - "TryAll is to Parallel what TryInOrder is to Sequential, meaning TryAll will also execute all designators in parallel but will return SUCCEEDED if at least one designator is executed without raising an exception. \n", - "\n", - "TryAll can be used like any other language expression. " - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "c0b093cb", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "world.reset_bullet_world()\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "\n", - "plan = navigate ^ park\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "bfbe1df4", - "metadata": {}, - "source": [ - "## Combination of Expressions \n", - "You can also combine different language expressions to further structure your plans. If you combine sequential and paralle expression please keep in mind that sequential expressions bind stringer than parallel ones. For example: \n", - "```\n", - "navigate | park + move_torso\n", - "```\n", - "In this case 'park' and 'move_torso' would form a Sequential expression and 'naviagte' would form a Parallel expression with Sequential. You can try this yourself in the following cell." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "4f1ea709", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "world.reset_world()\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "move_torso = MoveTorsoAction([0.3])\n", - "\n", - "plan = navigate | park + move_torso\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "0daef354", - "metadata": {}, - "source": [ - "## Code Objects\n", - "You can not only use designators in the plan language but also python code. For this there is the ```Code``` object which takes a callable and the arguments for this callable. This allows you to execute arbitrary code in a plan. \n", - "\n", - "The callable that is used in the ```Code``` object can either be a lambda expression or, for more complex code, a function. If you use a function you can provide parameters as keyword-arguments." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "f3f7784b", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "This is from the code object--------------------\n", - "Code funtion\n", - "\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.datastructures.enums import Arms\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.language import Code\n", - "\n", - "def code_test(param):\n", - " print(\"-\" * 20)\n", - " print(param)\n", - "\n", - "park = ParkArmsAction([Arms.BOTH])\n", - "code = Code(lambda: print(\"This is from the code object\"))\n", - "code_func = Code(code_test, {\"param\": \"Code funtion\"})\n", - "\n", - "plan = navigate | code | code_func\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "0879b81e", - "metadata": {}, - "source": [ - "## Exception Handling\n", - "If an exception is raised during the execution of a designator when it is used in a language expression the exception will be catched and saved to a dictionary. In genereal all designators in a language expression are executed regardless of exceptions raised, the only exception from this is the Sequential expression which stops after it encountered an exception. \n", - "\n", - "The language will only catch exceptions that are of type ```PlanFailure``` meaning errors that are defined in plan_failures.py in PyCRAM. This also means normal Python errors, such as KeyError, will interrupt the execution of your designators. \n", - "\n", - "We will see how exceptions are handled at a simple example." - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "f6d8ec84", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "{: [PlanFailure()]}\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.language import Code\n", - "from pycram.plan_failures import PlanFailure\n", - "\n", - "def code_test():\n", - " raise PlanFailure\n", - "\n", - "navigate = NavigateAction([Pose([1, 1, 0])])\n", - "code_func = Code(code_test)\n", - "\n", - "plan = navigate | code_func\n", - "\n", - "with simulated_robot:\n", - " plan.perform()\n", - " \n", - "print(plan.exceptions)" - ] - }, - { - "cell_type": "markdown", - "id": "1e9d9e9c", - "metadata": {}, - "source": [ - "## Repeat \n", - "Repeat simply repeats a language expression a number of times. As all other language expressions Repeat capturtes exceptions that occur during execution and saves them to the dictionary in the root of the plan.\n", - "\n", - "Since Repeat uses the \\* operator you should keep in mind that it will be evaluated before any other operator, so use parentheses to ensure the correct structure of your plan. \n", - "\n", - "You can see an example of how to use Repeat below." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "8781fdd5", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "move_torso_up = MoveTorsoAction([0.3])\n", - "move_torso_down = MoveTorsoAction([0.])\n", - "\n", - "plan = (move_torso_up + move_torso_down) * 5\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "ed931ef4", - "metadata": {}, - "source": [ - "## Monitor\n", - "Monitor allows to monitor the execution of a language expression and interrupt it as soon as a given condition is fulfilled. The condition can either be a Callable which returns a boolean or a Fluent.\n", - "When executed the Monitor will create a separate thread which will check if the condition is satisfied with a frequency of 10 Hz. If the condition is satisfied the execution of the language expression will be interrupted.\n", - "\n", - "For the example on how Monitors work we will use the previous example with the robot moving up and down. We will use a Monitor to interrupt the execution after 2 seconds instead of executing the whole plan 5 times." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "5695d740", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.language import Monitor\n", - "\n", - "move_torso_up = MoveTorsoAction([0.3])\n", - "move_torso_down = MoveTorsoAction([0.])\n", - "\n", - "def monitor_func():\n", - " time.sleep(2)\n", - " return True\n", - "\n", - "plan = (move_torso_up + move_torso_down) * 5 >> Monitor(monitor_func)\n", - "\n", - "with simulated_robot:\n", - " plan.perform()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/language.md b/examples/language.md new file mode 100644 index 000000000..197c879bb --- /dev/null +++ b/examples/language.md @@ -0,0 +1,333 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Plan Language + +The PyCRAM plan language is a way to structure the execution of your plan. In generally the plan language allows to +execute designators either sequential or in parallel. Furthermore, exceptions that occur during execution of a plan with +the plan language do not interrupt the execution instead they are caught and saved to a dictionary for later analysis. +All language expressions return a State, this can either be SUCCEDED or FAILED. + +There are 4 language expressions: + +| Expression | Name | Description | +|------------|------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| + | **Sequential** | Executes the designators one after another, if one of the designators raises an exception the execution is aborted and the state FAILED will be returned. | +| - | **Try In Order** | Executes the designators one after another, if one designator raises an exception the exception is caught and saved but the execution is not interrupted and the other designators are executed. Returns the state SUCCEDED if at least one designator can be executed without exception. | +| * | **Repeat** | Repeat the previous language expression a number of time. Has to be used with a language expression and an integer. | +| \| | **Parallel** | Executes all designators in parallel. For each designator there will be a new thread created and the designator is executed in this thread. If one of the designators raises an exception the returned state will be FAILED. | +| ^ | **Try All** | Executes all designators in parallel with a designated thread for each designator. Returns the state SUCCEDED if at least one designator can be executed without an exception | +| >> | **Monitor** | Monitors the execution of the attached langauge expression, will interrupt the execution as soon as a given condition is fulfilled. | + +The Sequential expression is the only one which aborts the execution once an error is raised. + +When using the plan language a tree structure of the plan is created where the language expressions are nodes and +designators are leafs. This tree uses AnyTree (like the task tree) and can be rendered with the anytree Renderer. + +## Sequential + +This language expression allows to execute designators one after another, if one of the designators raises an exception +the execution will be aborted and the state FAILED will be returned. + +We will start with a simple example that uses an action designator for moving the robot and parking its arms. + +```python +import time + +from pycram.designators.action_designator import * +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms + +navigate = NavigateAction([Pose([1, 1, 0])]) +park = ParkArmsAction([Arms.BOTH]) + +plan = navigate + park +``` + +With this simple plan created we can inspect it and render the created tree structure. + +```python +from anytree import RenderTree + +print(RenderTree(plan)) +``` + +As you can see there is the root node which is the language expression and then there are the leafs which are the +designators. When executing this plan the Sequential node will try to execute the NavigateAction and if that is finished +without any error the ParkArmsAction will be executed. + +The plan can be executed by wrapping it inside a ```with simulated_robot``` environment and calling perform on the +plan. + +If you are performing a plan with a simulated robot, you need a BulletWorld. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import ObjectType + +world = BulletWorld() +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +``` + +```python +from pycram.process_module import simulated_robot + +world.reset_bullet_world() + +with simulated_robot: + plan.perform() +``` + +## Try In Order + +Try in order is similar to Sequential, it also executes all designators one after another but the key difference is that +an exception in one of the designators does not terminate the whole execution. Furthermore, the state FAILED will only +be returned if all designator executions raise an error. + +Besides the described difference in behaviour this language expression can be used in the same way as Sequential. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot + +world.reset_bullet_world() + +navigate = NavigateAction([Pose([1, 1, 0])]) +park = ParkArmsAction([Arms.BOTH]) + +plan = navigate - park + +with simulated_robot: + plan.perform() +``` + +## Parallel + +Parallel executes all designator at once in dedicated threads. The execution of other designators is not aborted when a +exception is raised, this is the case since threads can not be killed from the outside and this would also cause +unforeseen problems. The state returned will be SUCCEDED if all designators could be executed without an exception raised +in any other case FAILED will be returned. + +Since executing designators in parallel can get chaotic especially with complex actions like PickUp or Transport. For +this reason not all action designators can be used in parallel and try all expressions. The list of action designator +that cannot be used in language expressions can be seen in {attr}`~pycram.language.Language.parallel_blocklist`. + +Designators that cannot be used in parallel and try all: + +* PickUpAction +* PlaceAction +* OpenAction +* CloseAction +* TransportAction + +Using the parallel expressions works like Sequential and TryInOrder. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot + +world.reset_world() + +navigate = NavigateAction([Pose([1, 1, 0])]) +park = ParkArmsAction([Arms.BOTH]) + +plan = navigate | park + +with simulated_robot: + plan.perform() +``` + +## Try All + +TryAll is to Parallel what TryInOrder is to Sequential, meaning TryAll will also execute all designators in parallel but +will return SUCCEEDED if at least one designator is executed without raising an exception. + +TryAll can be used like any other language expression. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot + +world.reset_bullet_world() + +navigate = NavigateAction([Pose([1, 1, 0])]) +park = ParkArmsAction([Arms.BOTH]) + +plan = navigate ^ park + +with simulated_robot: + plan.perform() +``` + +## Combination of Expressions + +You can also combine different language expressions to further structure your plans. If you combine sequential and +parallel expression please keep in mind that sequential expressions bind stronger than parallel ones. For example: + +``` +navigate | park + move_torso +``` + +In this case 'park' and 'move_torso' would form a Sequential expression and 'naviagte' would form a Parallel expression +with Sequential. You can try this yourself in the following cell. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.pose import Pose +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot + +world.reset_world() + +navigate = NavigateAction([Pose([1, 1, 0])]) +park = ParkArmsAction([Arms.BOTH]) +move_torso = MoveTorsoAction([0.3]) + +plan = navigate | park + move_torso + +with simulated_robot: + plan.perform() +``` + +## Code Objects + +You can not only use designators in the plan language but also python code. For this there is the {class}`~pycram.language.Code` object +which takes a callable and the arguments for this callable. This allows you to execute arbitrary code in a plan. + +The callable that is used in the {class}`~pycram.language.Code` object can either be a lambda expression or, for more complex code, a +function. If you use a function you can provide parameters as keyword-arguments. + +```python +from pycram.designators.action_designator import * +from pycram.datastructures.enums import Arms +from pycram.process_module import simulated_robot +from pycram.language import Code + + +def code_test(param): + print("-" * 20) + print(param) + + +park = ParkArmsAction([Arms.BOTH]) +code = Code(lambda: print("This is from the code object")) +code_func = Code(code_test, {"param": "Code function"}) + +plan = park | code | code_func + +with simulated_robot: + plan.perform() +``` + +## Exception Handling + +If an exception is raised during the execution of a designator when it is used in a language expression the exception +will be caught and saved to a dictionary. In general all designators in a language expression are executed regardless +of exceptions raised, the only exception from this is the Sequential expression which stops after it encountered an +exception. + +The language will only catch exceptions that are of type {class}`~pycram.plan_failures.PlanFailure` meaning errors that are defined in +plan_failures.py in PyCRAM. This also means normal Python errors, such as KeyError, will interrupt the execution of your +designators. + +We will see how exceptions are handled at a simple example. + +```python +from pycram.designators.action_designator import * +from pycram.process_module import simulated_robot +from pycram.language import Code +from pycram.failures import PlanFailure + + +def code_test(): + raise PlanFailure + + +navigate = NavigateAction([Pose([1, 1, 0])]) +code_func = Code(code_test) + +plan = navigate | code_func + +with simulated_robot: + plan.perform() + +print(plan.exceptions) +``` + +## Repeat + +Repeat simply repeats a language expression a number of times. As all other language expressions Repeat captures +exceptions that occur during execution and saves them to the dictionary in the root of the plan. + +Since Repeat uses the \* operator you should keep in mind that it will be evaluated before any other operator, so use +parentheses to ensure the correct structure of your plan. + +You can see an example of how to use Repeat below. + +```python +from pycram.designators.action_designator import * +from pycram.process_module import simulated_robot + +move_torso_up = MoveTorsoAction([0.3]) +move_torso_down = MoveTorsoAction([0.]) + +plan = (move_torso_up + move_torso_down) * 5 + +with simulated_robot: + plan.perform() +``` + +## Monitor + +Monitor allows to monitor the execution of a language expression and interrupt it as soon as a given condition is +fulfilled. The condition can either be a Callable which returns a boolean or a Fluent. +When executed the Monitor will create a separate thread which will check if the condition is satisfied with a frequency +of 10 Hz. If the condition is satisfied the execution of the language expression will be interrupted. + +For the example on how Monitors work we will use the previous example with the robot moving up and down. We will use a +Monitor to interrupt the execution after 2 seconds instead of executing the whole plan 5 times. + +```python +from pycram.designators.action_designator import * +from pycram.process_module import simulated_robot +from pycram.language import Monitor +import time + +move_torso_up = MoveTorsoAction([0.3]) +move_torso_down = MoveTorsoAction([0.]) + + +def monitor_func(): + time.sleep(2) + return True + + +plan = (move_torso_up + move_torso_down) * 5 >> Monitor(monitor_func) + +with simulated_robot: + plan.perform() +``` + +If you are finished with this example you can close the world with the cell below. + +```python +world.exit() +``` \ No newline at end of file diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb deleted file mode 100644 index cdc1eed6f..000000000 --- a/examples/local_transformer.ipynb +++ /dev/null @@ -1,325 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "8ac66f75", - "metadata": {}, - "source": [ - "# Local Transformer\n", - "The local transformer is used to handle transforms between different frames in PyCRAM. This is useful when you want to transform a pose from one frame to another, for example, from the map frame to the frame of an object. This example will introduce the Local Transformer and how to use it to transform poses between frames.\n", - "\n", - "## Setting up the Environment\n", - "\n", - "This step involves importing the required modules and initializing key components for our tasks.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "982e0c0e", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:18:56.397119146Z", - "start_time": "2024-02-07T09:18:56.384091718Z" - } - }, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.datastructures.pose import Transform, Pose\n", - "from pycram.local_transformer import LocalTransformer" - ] - }, - { - "cell_type": "markdown", - "id": "fcb189ce", - "metadata": {}, - "source": [ - "\n", - "## Initializing the World\n", - "\n", - "Every robot simulation requires a world where it can interact. This world serves as the playground where the robot performs tasks. \n", - "Let's start by creating this world.\n", - "\n", - "Since the local transformer can only transform between frames of objects which are in the world, we need to create a world first.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "e1761e68", - "metadata": {}, - "outputs": [], - "source": [ - "# Create an instance of the BulletWorld\n", - "world = BulletWorld(WorldMode.GUI)\n" - ] - }, - { - "cell_type": "markdown", - "id": "6d11e975", - "metadata": {}, - "source": [ - "\n", - "## Adding Objects to the World\n", - "\n", - "For our robot to perform meaningful tasks, we need to populate its world with objects. \n", - "In this section, we'll add a variety of objects, from a simple floor plane to kitchen setups and items like milk and bowls. \n", - "These objects will be used in subsequent tasks, to provide the frames to which we will transform poses.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "7b5f237b", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:19:12.883651520Z", - "start_time": "2024-02-07T09:19:09.027873229Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='bowl_object']/link[@name='bowl_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='bowl_object']/link[@name='bowl_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "from pycram.worlds.bullet_world import Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([0.9, 1, 0.95]))\n", - "bowl = Object(\"bowl\", ObjectType.BOWL, \"bowl.stl\", pose=Pose([1.6, 1, 0.90]))" - ] - }, - { - "cell_type": "markdown", - "id": "bebad92e", - "metadata": {}, - "source": [ - "## Creating a Local Transfomer\n", - "The local transformer is implemented as a singelton, meaing regardless of how much and where an instance is created it will always be the same instance. This is done since the local transfomer collects all transformations between frames and would there always be a new instance, all transformations woulb need to be re-collected. " - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "c53232dd", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:19:17.445812011Z", - "start_time": "2024-02-07T09:19:17.433695422Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "\n", - "\n" - ] - } - ], - "source": [ - "from pycram.local_transformer import LocalTransformer\n", - "\n", - "local_transformer = LocalTransformer()\n", - "print(local_transformer)\n", - "\n", - "new_local_transformer = LocalTransformer()\n", - "print(new_local_transformer)" - ] - }, - { - "cell_type": "markdown", - "id": "9f48045d", - "metadata": {}, - "source": [ - "## Transformations with LocalTransformer\n", - "Now that we have our world set up, let's perform some transformations. We'll use the LocalTransformer to transform poses relative to our objects." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "3e4ddcbb", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:19:19.412829961Z", - "start_time": "2024-02-07T09:19:19.398278560Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719580039\n", - " nsecs: 535519838\n", - " frame_id: \"milk\"\n", - "pose: \n", - " position: \n", - " x: 0.09999999999999998\n", - " y: 0.0\n", - " z: 0.050000000000000044\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n", - "-------------------\n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719580039\n", - " nsecs: 538168430\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 1.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.local_transformer import LocalTransformer\n", - "\n", - "l = LocalTransformer()\n", - "test_pose = Pose([1, 1, 1], [0, 0, 0, 1], \"map\")\n", - "\n", - "transformed_pose = l.transform_to_object_frame(test_pose, milk)\n", - "print(transformed_pose)\n", - "\n", - "print(\"-------------------\")\n", - "new_pose = l.transform_pose(transformed_pose, \"map\")\n", - "print(new_pose)" - ] - }, - { - "cell_type": "markdown", - "id": "10dde4b6", - "metadata": {}, - "source": [ - "In the above code, we first transformed a pose to the object frame of the milk object, and then we transformed it back to the map frame. This demonstrates how we can easily manipulate poses relative to objects in our environment.\n", - "You can also transform poses relative to other poses. by using the transform_pose method. Further you can set a Transform." - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "7caa6ec2", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:19:24.941756539Z", - "start_time": "2024-02-07T09:19:24.929728567Z" - } - }, - "outputs": [], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"test_frame\"))\n", - "p = Pose()\n", - "\n", - "transformed_pose = l.transform_pose(p, \"test_frame\")" - ] - }, - { - "cell_type": "markdown", - "id": "53a9e75e", - "metadata": {}, - "source": [ - "## Transformation frames\n", - "As you can see in the example above the frame_id of the object is not 'milk' but 'milk_4'. This is done since frame_ids need to be unique, however, the name of an Object does not. To solve this problem the name of an Object is concatenated with a unique id therefore making it unique. \n", - "\n", - "Furthermore, links of an Object are represented by the Object frame_id + '/' + link name. Since link names need to be unique for an URDF this is no problem.\n", - "\n", - "These frames need to be used in whenever you are transforming something with the local transformer. To get the base frame of an Object, meaning the frame name without any link there is the attribute tf_frame and for the frame of a link there is the collection links from which you can access all link objects by name, link objects also have the attribute tf_frame which gives the tf_frame of the link. " - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "ef49ffc8", - "metadata": { - "ExecuteTime": { - "end_time": "2024-02-07T09:20:26.251771677Z", - "start_time": "2024-02-07T09:20:26.240730107Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "milk\n", - "kitchen/kitchen_island_surface\n" - ] - } - ], - "source": [ - "print(milk.tf_frame)\n", - "\n", - "print(kitchen.get_link_tf_frame(\"kitchen_island_surface\"))" - ] - }, - { - "cell_type": "markdown", - "id": "9dd72437", - "metadata": {}, - "source": [ - "You can use the cell below to exit the simulation." - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "5d241b6d", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/local_transformer.md b/examples/local_transformer.md new file mode 100644 index 000000000..268af2024 --- /dev/null +++ b/examples/local_transformer.md @@ -0,0 +1,131 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Local Transformer + +The local transformer is used to handle transforms between different frames in PyCRAM. This is useful when you want to +transform a pose from one frame to another, for example, from the map frame to the frame of an object. This example will +introduce the Local Transformer and how to use it to transform poses between frames. + +## Setting up the Environment + +This step involves importing the required modules and initializing key components for our tasks. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.pose import Transform, Pose +from pycram.local_transformer import LocalTransformer +from pycram.datastructures.enums import WorldMode +``` + +## Initializing the World + +Every robot simulation requires a world where it can interact. This world serves as the playground where the robot +performs tasks. +Let's start by creating this world. + +Since the local transformer can only transform between frames of objects which are in the world, we need to create a +world first. + +```python +# Create an instance of the BulletWorld +world = BulletWorld(WorldMode.GUI) +``` + +## Adding Objects to the World + +For our robot to perform meaningful tasks, we need to populate its world with objects. +In this section, we'll add a variety of objects, from a simple floor plane to kitchen setups and items like milk and +bowls. +These objects will be used in subsequent tasks, to provide the frames to which we will transform poses. + +```python +from pycram.worlds.bullet_world import Object +from pycram.datastructures.enums import ObjectType + +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([0.9, 1, 0.95])) +bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([1.6, 1, 0.90])) +``` + +## Creating a Local Transfomer + +The local transformer is implemented as a singelton, meaing regardless of how much and where an instance is created it +will always be the same instance. This is done since the local transfomer collects all transformations between frames +and would there always be a new instance, all transformations woulb need to be re-collected. + +```python +from pycram.local_transformer import LocalTransformer + +local_transformer = LocalTransformer() +print(local_transformer) + +new_local_transformer = LocalTransformer() +print(new_local_transformer) +``` + +## Transformations with LocalTransformer + +Now that we have our world set up, let's perform some transformations. We'll use the LocalTransformer to transform poses +relative to our objects. + +```python +from pycram.local_transformer import LocalTransformer + +l = LocalTransformer() +test_pose = Pose([1, 1, 1], [0, 0, 0, 1], "map") + +transformed_pose = l.transform_to_object_frame(test_pose, milk) +print(transformed_pose) + +print("-------------------") +new_pose = l.transform_pose(transformed_pose, "map") +print(new_pose) +``` + +In the above code, we first transformed a pose to the object frame of the milk object, and then we transformed it back +to the map frame. This demonstrates how we can easily manipulate poses relative to objects in our environment. +You can also transform poses relative to other poses. by using the transform_pose method. Further you can set a +Transform. + +```python +from pycram.datastructures.pose import Transform + +l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame")) +p = Pose() + +transformed_pose = l.transform_pose(p, "test_frame") +``` + +## Transformation frames + +Links of an Object are represented by the Object frame_id + '/' + link name. Since link names need to be +unique for an URDF this is no problem. + +These frames need to be used in whenever you are transforming something with the local transformer. To get the base +frame of an Object, meaning the frame name without any link, there is the attribute tf_frame and for the frame of a link +there is a method which returns the frame name of a link given the link name. + +```python +print(milk.tf_frame) + +print(kitchen.get_link_tf_frame("kitchen_island_surface")) +``` + +You can use the cell below to exit the simulation. + +```python +world.exit() +``` diff --git a/examples/location_designator.ipynb b/examples/location_designator.ipynb deleted file mode 100644 index 164b9a511..000000000 --- a/examples/location_designator.ipynb +++ /dev/null @@ -1,509 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "c77a50af", - "metadata": {}, - "source": [ - "# Location Designator\n", - "This example will show you what location designators are, how to use them and what they are capable of. \n", - "\n", - "Location Designators are used to semantically describe locations in the world. You could, for example, create a location designator that describes every position where a robot can be placed without colliding with the environment. Location designator can describe locations for:\n", - "\n", - " * Visibility \n", - " * Reachability\n", - " * Occupancy \n", - " * URDF Links (for example a table)\n", - "\n", - "To find locations that fit the given constrains, location designator create Costmaps. Costmaps are a 2D distribution that have a value greater than 0 for every position that fits the costmap criteria.\n", - "\n", - "Location designators work similar to other designators, meaning you have to create a location designator description which describes the location. This description can then be resolved to the actual 6D pose on runtime.\n", - "\n", - "## Occupancy\n", - "\n", - "We will start with a simple location designator that describes a location where the robot can be placed without colliding with the environment. To do this we need a BulletWorld since the costmaps are mostly created from the current state of the BulletWorld. " - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "f49247b7", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Scalar element defined multiple times: limit\n" - ] - } - ], - "source": [ - "from pycram.worlds.bullet_worldbullet_worldbullet_worldt_worldt_world import BulletWorld, Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "world = BulletWorld()\n", - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "bfe54eb8", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "11a4ce95", - "metadata": {}, - "source": [ - "Next up we will create the location designator description, the ```CostmapLocation``` that we will be using needs a target as a parameter. This target describes what the location designator is for, this could either be a pose or object that the robot should be able to see or reach.\n", - "\n", - "In this case we only want poses where the robot can be placed, this is the default behaviour of the location designator which we will be extending later. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "dc560c16", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699625777\n", - " nsecs: 318477869\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.32\n", - " y: 0.46\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.8863025691598214\n", - " w: -0.46310663556107684, reachable_arms=None)\n" - ] - } - ], - "source": [ - "from pycram.designators.location_designator import CostmapLocation\n", - "\n", - "target = kitchen.get_pose()\n", - "\n", - "location_description = CostmapLocation(target)\n", - "\n", - "pose = location_description.resolve()\n", - "\n", - "print(pose)" - ] - }, - { - "cell_type": "markdown", - "id": "acceb76b", - "metadata": {}, - "source": [ - "## Reachable\n", - "Next we want to locations from where the robot can reach a specific point, like an object the robot should pick up. This can also be done with the ```CostmapLocation``` description, but this time we need to provide an additional argument. The additional argument is the robo which should be able to reach the pose. \n", - "\n", - "Since a robot is needed we will use the PR2 and use a milk as a target point for the robot to reach. The torso of the PR2 will be set to 0.2 since otherwise the arms of the robot will be too low to reach on the countertop.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "5db13f3e", - "metadata": {}, - "outputs": [], - "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "pr2.set_joint_state(\"torso_lift_joint\", 0.2)\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "9de582fd", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448386\n", - " nsecs: 194609165\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.52\n", - " y: 0.94\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.03837651950358723\n", - " w: 0.9992633500488202, reachable_arms=['left', 'right'])\n" - ] - } - ], - "source": [ - "from pycram.designators.location_designator import CostmapLocation\n", - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "target = BelieveObject(names=[\"milk\"]).resolve()\n", - "robot_desig = BelieveObject(names=[\"pr2\"]).resolve()\n", - "\n", - "location_description = CostmapLocation(target=target, reachable_for=robot_desig)\n", - "\n", - "print(location_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "f91ef6f1", - "metadata": {}, - "source": [ - "As you can see we get a pose near the countertop where the robot can be placed without colliding with it. Furthermore, we get a list of arms with which the robot can reach the given object." - ] - }, - { - "cell_type": "markdown", - "id": "6c890279", - "metadata": {}, - "source": [ - "## Visibile\n", - "The ```CostmapLocation``` can also find position from which the robot can see a given object or location. This is very similar to how reachable locations are described, meaning we provide a object designator or a pose and a robot designator but this time we use the ```visible_for``` parameter. \n", - "\n", - "For this example we need the milk as well as the PR2, so if you did not spawn them during the previous location designator you can spawn them with the following cell. " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "ccf6f27e", - "metadata": {}, - "outputs": [], - "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "62397202", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448390\n", - " nsecs: 664081811\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.7000000000000001\n", - " y: 0.040000000000000036\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.4847685323929452\n", - " w: 0.8746424812468178, reachable_arms=None)\n" - ] - } - ], - "source": [ - "from pycram.designators.location_designator import CostmapLocation\n", - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "target = BelieveObject(names=[\"milk\"]).resolve()\n", - "robot_desig = BelieveObject(names=[\"pr2\"]).resolve()\n", - "\n", - "location_description = CostmapLocation(target=target, visible_for=robot_desig)\n", - "\n", - "print(location_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "fac411a9", - "metadata": {}, - "source": [ - "## Semantic \n", - "Semantic location designator are used to create location descriptions for semantic entities, like a table. An example of this is: You have a robot that picked up an object and should place it on a table. Semantic location designator then allows to find poses that are on this table.\n", - "\n", - "Semantic location designator need an object from which the target entity is a part and the URDF link representing the entity. In this case we want a position on the kitchen island, so we have to provide the kitchen object designator since the island is a part of the kitchen and the link name of the island surface. \n", - "\n", - "For this example we need the kitchen as well as the milk. If you spawned them in one of the previous examples you don't need to execute the following cell." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "0cf46208", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\")" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "c16dcb0f", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "SemanticCostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448395\n", - " nsecs: 710832595\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: -1.2074999952316285\n", - " y: 1.019200015068054\n", - " z: 0.9398907270729542\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.6339889056055381\n", - " w: 0.7733421413379024)\n" - ] - } - ], - "source": [ - "from pycram.designators.location_designator import SemanticCostmapLocation \n", - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "kitchen_desig = BelieveObject(names=[\"kitchen\"]).resolve()\n", - "milk_desig = BelieveObject(names=[\"milk\"]).resolve()\n", - "\n", - "location_description = SemanticCostmapLocation(urdf_link_name=\"kitchen_island_surface\", \n", - " part_of=kitchen_desig,\n", - " for_object=milk_desig)\n", - "\n", - "print(location_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "539752b0", - "metadata": {}, - "source": [ - "## Location Designator as Generator\n", - "Location designator descriptions implement an iter method, so they can be used as generators which generate valid poses for the location described in the description. This can be useful if the first pose does not work for some reason. \n", - "\n", - "We will see this at the example of a location designator for visibility. For this example we need the milk, if you already have a milk spawned in you world you can ignore the following cell." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "7c5f021b", - "metadata": {}, - "outputs": [], - "source": [ - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "238a573e", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.designators.location_designator import CostmapLocation\n", - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "target = BelieveObject(names=[\"milk\"]).resolve()\n", - "robot_desig = BelieveObject(names=[\"pr2\"]).resolve()\n", - "\n", - "location_description = CostmapLocation(target=target, visible_for=robot_desig)\n", - "\n", - "for pose in location_description:\n", - " print(pose.pose)" - ] - }, - { - "cell_type": "markdown", - "id": "60507a07", - "metadata": {}, - "source": [ - "## Accessing Locations\n", - "Accessing describes a location from which the robot can open a drawer. The drawer is specified by a ObjetcPart designator which describes the handle of the drawer.\n", - "\n", - "At the moment this location designator only works in the apartment environment, so please remove the kitchen if you spawned it in a previous example. Furthermore, we need a robot, so we also spawn the PR2 if it isn't spawned already." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "872ad635", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen.remove()" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "9fe5c6b1", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n", - "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n" - ] - } - ], - "source": [ - "apartment = Object(\"apartment\", ObjectType.ENVIRONMENT, \"apartment.urdf\")" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "a324476f", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - } - ], - "source": [ - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "pr2.set_joint_state(\"torso_lift_joint\", 0.25)" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "3da86bec", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448422\n", - " nsecs: 547766208\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.8074915790557862\n", - " y: 2.7473597526550293\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.5893608715092853\n", - " w: -0.8078698924541103\n" - ] - } - ], - "source": [ - "from pycram.designators.object_designator import *\n", - "from pycram.designators.location_designator import *\n", - "\n", - "apartment_desig = BelieveObject(names=[\"apartment\"])\n", - "handle_desig = ObjectPart(names=[\"handle_cab10_t\"], part_of=apartment_desig.resolve())\n", - "robot_desig = BelieveObject(names=[\"pr2\"])\n", - "\n", - "access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve()\n", - "print(access_location.pose)" - ] - }, - { - "cell_type": "markdown", - "id": "5bed17c4", - "metadata": {}, - "source": [ - "## Giskard Location\n", - "Some robots like the HSR or the Stretch2 need a full-body ik solver to utilize the whole body. For this case robots the ```GiskardLocation``` can be used. This location designator uses giskard as an ik solver to find a pose for the robot to reach a target pose. \n", - "\n", - "**Note:** The GiskardLocation relies on Giskard, therefore Giskard needs to run in order for this Location Designator to work." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "a8f382e9", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.resolver.location.giskard_location import GiskardLocation\n", - "\n", - "robot_desig = BelieveObject(names=[\"pr2\"]).resolve()\n", - "\n", - "loc = GiskardLocation(target=Pose([1, 1, 1]), reachable_for=robot_desig).resolve()\n", - "print(loc.pose)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/location_designator.md b/examples/location_designator.md new file mode 100644 index 000000000..1fa54ef03 --- /dev/null +++ b/examples/location_designator.md @@ -0,0 +1,238 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Location Designator + +This example will show you what location designators are, how to use them and what they are capable of. + +Location Designators are used to semantically describe locations in the world. You could, for example, create a location +designator that describes every position where a robot can be placed without colliding with the environment. Location +designator can describe locations for: + +* Visibility +* Reachability +* Occupancy +* URDF Links (for example a table) + +To find locations that fit the given constrains, location designator create Costmaps. Costmaps are a 2D distribution +that have a value greater than 0 for every position that fits the costmap criteria. + +Location designators work similar to other designators, meaning you have to create a location designator description +which describes the location. This description can then be resolved to the actual 6D pose on runtime. + +## Occupancy + +We will start with a simple location designator that describes a location where the robot can be placed without +colliding with the environment. To do this we need a BulletWorld since the costmaps are mostly created from the current +state of the BulletWorld. + +```python +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() +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +``` + +Next up we will create the location designator description, the {meth}`~pycram.designators.location_designator.CostmapLocation` that we will be using needs a +target as a parameter. This target describes what the location designator is for, this could either be a pose or object +that the robot should be able to see or reach. + +In this case we only want poses where the robot can be placed, this is the default behaviour of the location designator +which we will be extending later. + +```python +from pycram.designators.location_designator import CostmapLocation + +target = kitchen.get_pose() + +location_description = CostmapLocation(target) + +pose = location_description.resolve() + +print(pose) +``` + +## Reachable + +Next we want to have locations from where the robot can reach a specific point, like an object the robot should pick up. This +can also be done with the {meth}`~pycram.designators.location_designator.CostmapLocation` description, but this time we need to provide an additional argument. +The additional argument is the robot which should be able to reach the pose. + +Since a robot is needed we will use the PR2 and use a milk as a target point for the robot to reach. The torso of the +PR2 will be set to 0.2 since otherwise the arms of the robot will be too low to reach on the countertop. + +```python +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +pr2.set_joint_state("torso_lift_joint", 0.2) +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + +``` + +```python +from pycram.designators.location_designator import CostmapLocation +from pycram.designators.object_designator import BelieveObject + +target = BelieveObject(names=["milk"]).resolve() +robot_desig = BelieveObject(names=["pr2"]).resolve() + +location_description = CostmapLocation(target=target, reachable_for=robot_desig) + +print(location_description.resolve()) +``` + +As you can see we get a pose near the countertop where the robot can be placed without colliding with it. Furthermore, +we get a list of arms with which the robot can reach the given object. + +## Visible + +The {meth}`~pycram.designators.location_designator.CostmapLocation` can also find position from which the robot can see a given object or location. This is very +similar to how reachable locations are described, meaning we provide a object designator or a pose and a robot +designator but this time we use the ```visible_for``` parameter. + +For this example we need the milk as well as the PR2, so if you did not spawn them during the previous location +designator you can spawn them with the following cell. + +```python +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +``` + +```python +from pycram.designators.location_designator import CostmapLocation +from pycram.designators.object_designator import BelieveObject + +target = BelieveObject(names=["milk"]).resolve() +robot_desig = BelieveObject(names=["pr2"]).resolve() + +location_description = CostmapLocation(target=target, visible_for=robot_desig) + +print(location_description.resolve()) +``` + +## Semantic + +Semantic location designator are used to create location descriptions for semantic entities, like a table. An example of +this is: You have a robot that picked up an object and should place it on a table. Semantic location designator then +allows to find poses that are on this table. + +Semantic location designator need an object from which the target entity is a part and the URDF link representing the +entity. In this case we want a position on the kitchen island, so we have to provide the kitchen object designator since +the island is a part of the kitchen and the link name of the island surface. + +For this example we need the kitchen as well as the milk. If you spawned them in one of the previous examples you don't +need to execute the following cell. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl") +``` + +```python +from pycram.designators.location_designator import SemanticCostmapLocation +from pycram.designators.object_designator import BelieveObject + +kitchen_desig = BelieveObject(names=["kitchen"]).resolve() +milk_desig = BelieveObject(names=["milk"]).resolve() + +location_description = SemanticCostmapLocation(urdf_link_name="kitchen_island_surface", + part_of=kitchen_desig, + for_object=milk_desig) + +print(location_description.resolve()) +``` + +## Location Designator as Generator + +Location designator descriptions implement an iter method, so they can be used as generators which generate valid poses +for the location described in the description. This can be useful if the first pose does not work for some reason. + +We will see this at the example of a location designator for visibility. For this example we need the milk, if you +already have a milk spawned in you world you can ignore the following cell. + +```python +milk = Object("milk", ObjectType.MILK, "milk.stl") +``` + +```python +from pycram.designators.location_designator import CostmapLocation +from pycram.designators.object_designator import BelieveObject + +target = BelieveObject(names=["milk"]).resolve() +robot_desig = BelieveObject(names=["pr2"]).resolve() + +location_description = CostmapLocation(target=target, visible_for=robot_desig) + +for pose in location_description: + print(pose.pose) +``` + +## Accessing Locations + +Accessing describes a location from which the robot can open a drawer. The drawer is specified by a ObjetcPart +designator which describes the handle of the drawer. + +At the moment this location designator only works in the apartment environment, so please remove the kitchen if you +spawned it in a previous example. Furthermore, we need a robot, so we also spawn the PR2 if it isn't spawned already. + +```python +kitchen.remove() +``` + +```python +apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") +``` + +```python +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +pr2.set_joint_state("torso_lift_joint", 0.25) +``` + +```python +from pycram.designators.object_designator import * +from pycram.designators.location_designator import * + +apartment_desig = BelieveObject(names=["apartment"]) +handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) +robot_desig = BelieveObject(names=["pr2"]) + +access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve() +print(access_location.pose) +``` + +## Giskard Location + +Some robots like the HSR or the Stretch2 need a full-body ik solver to utilize the whole body. For this case +the {meth}`~pycram.designators.specialized_designators.location.giskard_location.GiskardLocation` can be used. This location designator uses giskard as an ik solver to find a pose for the +robot to reach a target pose. + +**Note:** The GiskardLocation relies on Giskard, therefore Giskard needs to run in order for this Location Designator to +work. + +```python +from pycram.designators.specialized_designators.location.giskard_location import GiskardLocation + +robot_desig = BelieveObject(names=["pr2"]).resolve() + +loc = GiskardLocation(target=Pose([1, 1, 1]), reachable_for=robot_desig).resolve() +print(loc.pose) +``` + +If you are finished with this example you can close the world with the following cell: + +```python +world.exit() +``` diff --git a/examples/migrate_neems.ipynb b/examples/migrate_neems.ipynb deleted file mode 100644 index 18ce227c4..000000000 --- a/examples/migrate_neems.ipynb +++ /dev/null @@ -1,1204 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "81be4599", - "metadata": {}, - "source": [ - "# Migrate NEEMs\n", - "\n", - "In this tutorial we will go through the process of migrating locally stored PyCRORM NEEMs to an already existing \n", - "PyCRORM NEEM-Hub.\n", - "\n", - "In some cases it my occur that you want to record data from a pycram controlled robot locally and perform some local \n", - "actions before migrating your data to a big database server. In such cases, you can easily make a local database and\n", - "connect your pycram process to it. \n", - "\n", - "After you recorded your data locally you can migrate the data using the `migrate_neems` function.\n", - "\n", - "First, lets create an in memory database engine called `source_engine` where we record our current process." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "5d0ee112", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:51:09.603703147Z", - "start_time": "2023-12-19T10:51:08.852604926Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "pybullet build time: Nov 28 2023 23:51:11\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "/home/nleusmann/catkin_ws/src/pycram/src/pycram/orm/action_designator.py:10: SAWarning: Implicitly combining column Designator.dtype with column Action.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", - " class Action(MapperArgsMixin, Designator):\n", - "/home/nleusmann/catkin_ws/src/pycram/src/pycram/orm/motion_designator.py:16: SAWarning: Implicitly combining column Designator.dtype with column Motion.dtype under attribute 'dtype'. Please configure one or more attributes for these same-named columns explicitly.\n", - " class Motion(MapperArgsMixin, Designator):\n", - "[WARN] [1702983069.575976]: Failed to import Giskard messages\n" - ] - } - ], - "source": [ - "import sqlalchemy.orm\n", - "import pycram\n", - "\n", - "source_engine: sqlalchemy.engine.Engine\n", - "source_engine = sqlalchemy.create_engine(\"sqlite+pysqlite:///:memory:\", echo=False)\n", - "source_session_maker = sqlalchemy.orm.sessionmaker(bind=source_engine)\n", - "pycram.orm.base.Base.metadata.create_all(source_engine) #create all Tables" - ] - }, - { - "cell_type": "markdown", - "id": "15caed73", - "metadata": {}, - "source": [ - "Next, create an engine called `destination_engine` for the destination database where you want to migrate your NEEMs to.\n", - "`Note:` This is just an example configuration." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "b0b9e110", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:51:22.457680892Z", - "start_time": "2023-12-19T10:51:22.427575091Z" - } - }, - "outputs": [], - "source": [ - "destination_engine: sqlalchemy.engine.Engine\n", - "destination_engine = sqlalchemy.create_engine(\"postgresql+psycopg2://alice:alice123@localhost:5433/pycram\", echo=False) # example values\n", - "destination_session_maker = sqlalchemy.orm.sessionmaker(bind=destination_engine)" - ] - }, - { - "cell_type": "markdown", - "id": "96a1892e", - "metadata": {}, - "source": [ - "If you already have some data in your local database you can skip the next block, otherwise we will quickly create \n", - "some example data" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "bdda80e8", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:51:24.988885709Z", - "start_time": "2023-12-19T10:51:24.745896959Z" - } - }, - "outputs": [], - "source": [ - "from pycram.datastructures.enums import Arms, ObjectType\n", - "from pycram.designators.action_designator import *\n", - "from pycram.designators.location_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.task import with_tree\n", - "from pycram.worlds.bullet_world import Object\n", - "from pycram.designators.object_designator import *\n", - "\n", - "\n", - "class ExamplePlans:\n", - " def __init__(self):\n", - " self.world = BulletWorld(\"DIRECT\")\n", - " self.pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - " self.kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - " self.milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - " self.cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.3, 0.7, 0.95]))\n", - " self.milk_desig = ObjectDesignatorDescription(names=[\"milk\"])\n", - " self.cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", - " self.robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()\n", - " self.kitchen_desig = ObjectDesignatorDescription(names=[\"kitchen\"])\n", - "\n", - " @with_tree\n", - " def pick_and_place_plan(self):\n", - " with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - " MoveTorsoAction([0.3]).resolve().perform()\n", - " pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve()\n", - " pickup_arm = pickup_pose.reachable_arms[0]\n", - " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", - " PickUpAction(object_designator_description=self.cereal_desig, arms=[pickup_arm],\n", - " grasps=[\"front\"]).resolve().perform()\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", self.kitchen_desig.resolve(),\n", - " self.cereal_desig.resolve()).resolve()\n", - "\n", - " place_stand = CostmapLocation(place_island.pose, reachable_for=self.robot_desig,\n", - " reachable_arm=pickup_arm).resolve()\n", - "\n", - " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", - "\n", - " PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "378408c7", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:52:11.153019806Z", - "start_time": "2023-12-19T10:51:35.523989623Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Scalar element defined multiple times: limit\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Scalar element defined multiple times: limit\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ExamplePlans run 0\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 99.07it/s]\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ExamplePlans run 1\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 39/39 [00:00<00:00, 125.94it/s]\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ExamplePlans run 2\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 58/58 [00:00<00:00, 127.64it/s]\n" - ] - } - ], - "source": [ - "import pycram.orm.utils \n", - "import pycram.tasktree\n", - " \n", - "with source_session_maker() as session:\n", - " example_plans = ExamplePlans()\n", - " for i in range(3):\n", - " try:\n", - " print(\"ExamplePlans run {}\".format(i))\n", - " example_plans.pick_and_place_plan()\n", - " example_plans.world.reset_bullet_world()\n", - " process_meta_data = pycram.orm.base.ProcessMetaData()\n", - " process_meta_data.description = \"Example Plan {}\".format(i)\n", - " process_meta_data.insert(session)\n", - " pycram.tasktree.task_tree.root.insert(session)\n", - " process_meta_data.reset()\n", - " except Exception as e:\n", - " print(\"Error: {}\\n{}\".format(type(e).__name__, e))\n", - " session.commit()\n", - " example_plans.world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "6304196f", - "metadata": {}, - "source": [ - "Now that we have some example data or already had some example data all we need to do it migrate it over to\n", - "the already existing PyCRORM NEEM-Hub." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "a479af24", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:52:22.393762307Z", - "start_time": "2023-12-19T10:52:20.364700821Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1702983140.375293]: ~~~~~~~~~~~~~~~~~~~~~~~~~ProcessMetaData~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.377518]: ~~~~~~~~~~~~~~~~~~~~~~~~~Color~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.379543]: ~~~~~~~~~~~~~~~~~~~~~~~~~Designator~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.381153]: ~~~~~~~~~~~~~~~~~~~~~~~~~Position~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.382709]: ~~~~~~~~~~~~~~~~~~~~~~~~~Quaternion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.384590]: ~~~~~~~~~~~~~~~~~~~~~~~~~Code~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.386105]: ~~~~~~~~~~~~~~~~~~~~~~~~~Motion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.387769]: ~~~~~~~~~~~~~~~~~~~~~~~~~Pose~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.389364]: ~~~~~~~~~~~~~~~~~~~~~~~~~ClosingMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.390867]: ~~~~~~~~~~~~~~~~~~~~~~~~~DetectingMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.392384]: ~~~~~~~~~~~~~~~~~~~~~~~~~LookingMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.393864]: ~~~~~~~~~~~~~~~~~~~~~~~~~MoveGripperMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.395363]: ~~~~~~~~~~~~~~~~~~~~~~~~~MoveMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.396842]: ~~~~~~~~~~~~~~~~~~~~~~~~~MoveTCPMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.398589]: ~~~~~~~~~~~~~~~~~~~~~~~~~Object~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.400058]: ~~~~~~~~~~~~~~~~~~~~~~~~~OpeningMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.401549]: ~~~~~~~~~~~~~~~~~~~~~~~~~RobotState~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.403036]: ~~~~~~~~~~~~~~~~~~~~~~~~~TaskTreeNode~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.404592]: ~~~~~~~~~~~~~~~~~~~~~~~~~WorldStateDetectingMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.406397]: ~~~~~~~~~~~~~~~~~~~~~~~~~AccessingMotion~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.407969]: ~~~~~~~~~~~~~~~~~~~~~~~~~Action~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.409532]: ~~~~~~~~~~~~~~~~~~~~~~~~~BelieveObject~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.411078]: ~~~~~~~~~~~~~~~~~~~~~~~~~ObjectPart~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.412633]: ~~~~~~~~~~~~~~~~~~~~~~~~~CloseAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.414165]: ~~~~~~~~~~~~~~~~~~~~~~~~~DetectAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.415699]: ~~~~~~~~~~~~~~~~~~~~~~~~~GraspingAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.417279]: ~~~~~~~~~~~~~~~~~~~~~~~~~GripAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.418822]: ~~~~~~~~~~~~~~~~~~~~~~~~~LookAtAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.420371]: ~~~~~~~~~~~~~~~~~~~~~~~~~MoveTorsoAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.422016]: ~~~~~~~~~~~~~~~~~~~~~~~~~NavigateAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.424709]: ~~~~~~~~~~~~~~~~~~~~~~~~~OpenAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.427379]: ~~~~~~~~~~~~~~~~~~~~~~~~~ParkArmsAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.429815]: ~~~~~~~~~~~~~~~~~~~~~~~~~PickUpAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.432317]: ~~~~~~~~~~~~~~~~~~~~~~~~~PlaceAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.434606]: ~~~~~~~~~~~~~~~~~~~~~~~~~Release~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.436415]: ~~~~~~~~~~~~~~~~~~~~~~~~~SetGripperAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.438479]: ~~~~~~~~~~~~~~~~~~~~~~~~~TransportAction~~~~~~~~~~~~~~~~~~~~~~~~~\n", - "[INFO] [1702983140.443188]: Found primary_key collision in table ProcessMetaData value: 1 max value in memory 56\n", - "[INFO] [1702983140.451132]: Found primary_key collision in table ProcessMetaData value: 2 max value in memory 57\n", - "[INFO] [1702983140.458050]: Found primary_key collision in table ProcessMetaData value: 3 max value in memory 58\n", - "[INFO] [1702983140.487381]: Found primary_key collision in table Designator value: 1 max value in memory 1999\n", - "[INFO] [1702983140.491166]: Found primary_key collision in table Designator value: 2 max value in memory 2000\n", - "[INFO] [1702983140.493167]: Found primary_key collision in table Designator value: 3 max value in memory 2001\n", - "[INFO] [1702983140.495515]: Found primary_key collision in table Designator value: 4 max value in memory 2002\n", - "[INFO] [1702983140.498704]: Found primary_key collision in table Designator value: 5 max value in memory 2003\n", - "[INFO] [1702983140.500769]: Found primary_key collision in table Designator value: 6 max value in memory 2004\n", - "[INFO] [1702983140.502732]: Found primary_key collision in table Designator value: 7 max value in memory 2005\n", - "[INFO] [1702983140.504756]: Found primary_key collision in table Designator value: 8 max value in memory 2006\n", - "[INFO] [1702983140.506561]: Found primary_key collision in table Designator value: 9 max value in memory 2007\n", - "[INFO] [1702983140.508294]: Found primary_key collision in table Designator value: 10 max value in memory 2008\n", - "[INFO] [1702983140.510082]: Found primary_key collision in table Designator value: 11 max value in memory 2009\n", - "[INFO] [1702983140.512037]: Found primary_key collision in table Designator value: 12 max value in memory 2010\n", - "[INFO] [1702983140.513960]: Found primary_key collision in table Designator value: 13 max value in memory 2011\n", - "[INFO] [1702983140.515506]: Found primary_key collision in table Designator value: 14 max value in memory 2012\n", - "[INFO] [1702983140.517187]: Found primary_key collision in table Designator value: 15 max value in memory 2013\n", - "[INFO] [1702983140.518769]: Found primary_key collision in table Designator value: 16 max value in memory 2014\n", - "[INFO] [1702983140.520280]: Found primary_key collision in table Designator value: 17 max value in memory 2015\n", - "[INFO] [1702983140.521794]: Found primary_key collision in table Designator value: 18 max value in memory 2016\n", - "[INFO] [1702983140.523352]: Found primary_key collision in table Designator value: 19 max value in memory 2017\n", - "[INFO] [1702983140.525299]: Found primary_key collision in table Designator value: 21 max value in memory 2018\n", - "[INFO] [1702983140.526868]: Found primary_key collision in table Designator value: 22 max value in memory 2019\n", - "[INFO] [1702983140.528404]: Found primary_key collision in table Designator value: 23 max value in memory 2020\n", - "[INFO] [1702983140.529980]: Found primary_key collision in table Designator value: 24 max value in memory 2021\n", - "[INFO] [1702983140.531643]: Found primary_key collision in table Designator value: 25 max value in memory 2022\n", - "[INFO] [1702983140.533286]: Found primary_key collision in table Designator value: 26 max value in memory 2023\n", - "[INFO] [1702983140.534886]: Found primary_key collision in table Designator value: 27 max value in memory 2024\n", - "[INFO] [1702983140.536453]: Found primary_key collision in table Designator value: 28 max value in memory 2025\n", - "[INFO] [1702983140.537953]: Found primary_key collision in table Designator value: 29 max value in memory 2026\n", - "[INFO] [1702983140.539587]: Found primary_key collision in table Designator value: 30 max value in memory 2027\n", - "[INFO] [1702983140.541199]: Found primary_key collision in table Designator value: 31 max value in memory 2028\n", - "[INFO] [1702983140.542732]: Found primary_key collision in table Designator value: 32 max value in memory 2029\n", - "[INFO] [1702983140.544326]: Found primary_key collision in table Designator value: 33 max value in memory 2030\n", - "[INFO] [1702983140.545831]: Found primary_key collision in table Designator value: 34 max value in memory 2031\n", - "[INFO] [1702983140.547328]: Found primary_key collision in table Designator value: 35 max value in memory 2032\n", - "[INFO] [1702983140.548820]: Found primary_key collision in table Designator value: 36 max value in memory 2033\n", - "[INFO] [1702983140.550406]: Found primary_key collision in table Designator value: 37 max value in memory 2034\n", - "[INFO] [1702983140.551958]: Found primary_key collision in table Designator value: 38 max value in memory 2035\n", - "[INFO] [1702983140.553548]: Found primary_key collision in table Designator value: 39 max value in memory 2036\n", - "[INFO] [1702983140.555182]: Found primary_key collision in table Designator value: 40 max value in memory 2037\n", - "[INFO] [1702983140.556719]: Found primary_key collision in table Designator value: 41 max value in memory 2038\n", - "[INFO] [1702983140.558255]: Found primary_key collision in table Designator value: 42 max value in memory 2039\n", - "[INFO] [1702983140.559757]: Found primary_key collision in table Designator value: 43 max value in memory 2040\n", - "[INFO] [1702983140.561242]: Found primary_key collision in table Designator value: 44 max value in memory 2041\n", - "[INFO] [1702983140.562705]: Found primary_key collision in table Designator value: 45 max value in memory 2042\n", - "[INFO] [1702983140.564208]: Found primary_key collision in table Designator value: 46 max value in memory 2043\n", - "[INFO] [1702983140.565747]: Found primary_key collision in table Designator value: 47 max value in memory 2044\n", - "[INFO] [1702983140.567669]: Found primary_key collision in table Designator value: 48 max value in memory 2045\n", - "[INFO] [1702983140.569421]: Found primary_key collision in table Designator value: 49 max value in memory 2046\n", - "[INFO] [1702983140.570911]: Found primary_key collision in table Designator value: 50 max value in memory 2047\n", - "[INFO] [1702983140.572484]: Found primary_key collision in table Designator value: 51 max value in memory 2048\n", - "[INFO] [1702983140.574052]: Found primary_key collision in table Designator value: 52 max value in memory 2049\n", - "[INFO] [1702983140.575636]: Found primary_key collision in table Designator value: 53 max value in memory 2050\n", - "[INFO] [1702983140.577566]: Found primary_key collision in table Designator value: 54 max value in memory 2051\n", - "[INFO] [1702983140.579314]: Found primary_key collision in table Designator value: 55 max value in memory 2052\n", - "[INFO] [1702983140.581164]: Found primary_key collision in table Designator value: 56 max value in memory 2053\n", - "[INFO] [1702983140.583703]: Found primary_key collision in table Designator value: 57 max value in memory 2054\n", - "[INFO] [1702983140.585811]: Found primary_key collision in table Designator value: 58 max value in memory 2055\n", - "[INFO] [1702983140.587881]: Found primary_key collision in table Designator value: 59 max value in memory 2056\n", - "[INFO] [1702983140.589876]: Found primary_key collision in table Designator value: 60 max value in memory 2057\n", - "[INFO] [1702983140.591695]: Found primary_key collision in table Designator value: 61 max value in memory 2058\n", - "[INFO] [1702983140.593204]: Found primary_key collision in table Designator value: 62 max value in memory 2059\n", - "[INFO] [1702983140.594645]: Found primary_key collision in table Designator value: 63 max value in memory 2060\n", - "[INFO] [1702983140.596159]: Found primary_key collision in table Designator value: 64 max value in memory 2061\n", - "[INFO] [1702983140.597612]: Found primary_key collision in table Designator value: 65 max value in memory 2062\n", - "[INFO] [1702983140.599078]: Found primary_key collision in table Designator value: 66 max value in memory 2063\n", - "[INFO] [1702983140.600559]: Found primary_key collision in table Designator value: 67 max value in memory 2064\n", - "[INFO] [1702983140.602111]: Found primary_key collision in table Designator value: 68 max value in memory 2065\n", - "[INFO] [1702983140.603884]: Found primary_key collision in table Designator value: 69 max value in memory 2066\n", - "[INFO] [1702983140.605649]: Found primary_key collision in table Designator value: 70 max value in memory 2067\n", - "[INFO] [1702983140.607356]: Found primary_key collision in table Designator value: 71 max value in memory 2068\n", - "[INFO] [1702983140.609249]: Found primary_key collision in table Designator value: 72 max value in memory 2069\n", - "[INFO] [1702983140.611541]: Found primary_key collision in table Designator value: 73 max value in memory 2070\n", - "[INFO] [1702983140.613838]: Found primary_key collision in table Designator value: 74 max value in memory 2071\n", - "[INFO] [1702983140.616140]: Found primary_key collision in table Designator value: 75 max value in memory 2072\n", - "[INFO] [1702983140.618710]: Found primary_key collision in table Designator value: 76 max value in memory 2073\n", - "[INFO] [1702983140.620785]: Found primary_key collision in table Designator value: 20 max value in memory 2074\n", - "[INFO] [1702983140.622509]: Found primary_key collision in table Designator value: 77 max value in memory 2075\n", - "[INFO] [1702983140.624118]: Found primary_key collision in table Designator value: 78 max value in memory 2076\n", - "[INFO] [1702983140.625955]: Found primary_key collision in table Designator value: 79 max value in memory 2077\n", - "[INFO] [1702983140.627949]: Found primary_key collision in table Designator value: 80 max value in memory 2078\n", - "[INFO] [1702983140.629957]: Found primary_key collision in table Designator value: 81 max value in memory 2079\n", - "[INFO] [1702983140.631733]: Found primary_key collision in table Designator value: 82 max value in memory 2080\n", - "[INFO] [1702983140.633257]: Found primary_key collision in table Designator value: 83 max value in memory 2081\n", - "[INFO] [1702983140.634989]: Found primary_key collision in table Designator value: 84 max value in memory 2082\n", - "[INFO] [1702983140.637035]: Found primary_key collision in table Designator value: 85 max value in memory 2083\n", - "[INFO] [1702983140.638781]: Found primary_key collision in table Designator value: 86 max value in memory 2084\n", - "[INFO] [1702983140.640450]: Found primary_key collision in table Designator value: 87 max value in memory 2085\n", - "[INFO] [1702983140.641970]: Found primary_key collision in table Designator value: 88 max value in memory 2086\n", - "[INFO] [1702983140.643768]: Found primary_key collision in table Designator value: 89 max value in memory 2087\n", - "[INFO] [1702983140.645530]: Found primary_key collision in table Designator value: 90 max value in memory 2088\n", - "[INFO] [1702983140.647256]: Found primary_key collision in table Designator value: 91 max value in memory 2089\n", - "[INFO] [1702983140.648766]: Found primary_key collision in table Designator value: 92 max value in memory 2090\n", - "[INFO] [1702983140.650302]: Found primary_key collision in table Designator value: 93 max value in memory 2091\n", - "[INFO] [1702983140.651830]: Found primary_key collision in table Designator value: 94 max value in memory 2092\n", - "[INFO] [1702983140.653315]: Found primary_key collision in table Designator value: 95 max value in memory 2093\n", - "[INFO] [1702983140.654847]: Found primary_key collision in table Designator value: 96 max value in memory 2094\n", - "[INFO] [1702983140.656360]: Found primary_key collision in table Designator value: 97 max value in memory 2095\n", - "[INFO] [1702983140.657838]: Found primary_key collision in table Designator value: 98 max value in memory 2096\n", - "[INFO] [1702983140.659347]: Found primary_key collision in table Designator value: 99 max value in memory 2097\n", - "[INFO] [1702983140.660909]: Found primary_key collision in table Designator value: 100 max value in memory 2098\n", - "[INFO] [1702983140.662428]: Found primary_key collision in table Designator value: 101 max value in memory 2099\n", - "[INFO] [1702983140.663996]: Found primary_key collision in table Designator value: 102 max value in memory 2100\n", - "[INFO] [1702983140.665594]: Found primary_key collision in table Designator value: 103 max value in memory 2101\n", - "[INFO] [1702983140.667286]: Found primary_key collision in table Designator value: 104 max value in memory 2102\n", - "[INFO] [1702983140.668983]: Found primary_key collision in table Designator value: 105 max value in memory 2103\n", - "[INFO] [1702983140.670539]: Found primary_key collision in table Designator value: 106 max value in memory 2104\n", - "[INFO] [1702983140.672079]: Found primary_key collision in table Designator value: 107 max value in memory 2105\n", - "[INFO] [1702983140.674216]: Found primary_key collision in table Designator value: 108 max value in memory 2106\n", - "[INFO] [1702983140.690823]: Found primary_key collision in table Position value: 104 max value in memory 2221\n", - "[INFO] [1702983140.693785]: Found primary_key collision in table Position value: 105 max value in memory 2222\n", - "[INFO] [1702983140.695448]: Found primary_key collision in table Position value: 106 max value in memory 2223\n", - "[INFO] [1702983140.696633]: Found primary_key collision in table Position value: 107 max value in memory 2224\n", - "[INFO] [1702983140.697783]: Found primary_key collision in table Position value: 108 max value in memory 2225\n", - "[INFO] [1702983140.698926]: Found primary_key collision in table Position value: 109 max value in memory 2226\n", - "[INFO] [1702983140.700045]: Found primary_key collision in table Position value: 110 max value in memory 2227\n", - "[INFO] [1702983140.701158]: Found primary_key collision in table Position value: 111 max value in memory 2228\n", - "[INFO] [1702983140.702438]: Found primary_key collision in table Position value: 112 max value in memory 2229\n", - "[INFO] [1702983140.703901]: Found primary_key collision in table Position value: 113 max value in memory 2230\n", - "[INFO] [1702983140.705375]: Found primary_key collision in table Position value: 114 max value in memory 2231\n", - "[INFO] [1702983140.706686]: Found primary_key collision in table Position value: 115 max value in memory 2232\n", - "[INFO] [1702983140.708006]: Found primary_key collision in table Position value: 116 max value in memory 2233\n", - "[INFO] [1702983140.709398]: Found primary_key collision in table Position value: 117 max value in memory 2234\n", - "[INFO] [1702983140.710747]: Found primary_key collision in table Position value: 118 max value in memory 2235\n", - "[INFO] [1702983140.712269]: Found primary_key collision in table Position value: 119 max value in memory 2236\n", - "[INFO] [1702983140.714001]: Found primary_key collision in table Position value: 120 max value in memory 2237\n", - "[INFO] [1702983140.715966]: Found primary_key collision in table Position value: 1 max value in memory 2238\n", - "[INFO] [1702983140.718587]: Found primary_key collision in table Position value: 2 max value in memory 2239\n", - "[INFO] [1702983140.721472]: Found primary_key collision in table Position value: 3 max value in memory 2240\n", - "[INFO] [1702983140.723544]: Found primary_key collision in table Position value: 4 max value in memory 2241\n", - "[INFO] [1702983140.726239]: Found primary_key collision in table Position value: 5 max value in memory 2242\n", - "[INFO] [1702983140.729005]: Found primary_key collision in table Position value: 6 max value in memory 2243\n", - "[INFO] [1702983140.731281]: Found primary_key collision in table Position value: 7 max value in memory 2244\n", - "[INFO] [1702983140.733507]: Found primary_key collision in table Position value: 8 max value in memory 2245\n", - "[INFO] [1702983140.735553]: Found primary_key collision in table Position value: 9 max value in memory 2246\n", - "[INFO] [1702983140.737260]: Found primary_key collision in table Position value: 10 max value in memory 2247\n", - "[INFO] [1702983140.738605]: Found primary_key collision in table Position value: 11 max value in memory 2248\n", - "[INFO] [1702983140.739798]: Found primary_key collision in table Position value: 12 max value in memory 2249\n", - "[INFO] [1702983140.740976]: Found primary_key collision in table Position value: 13 max value in memory 2250\n", - "[INFO] [1702983140.743260]: Found primary_key collision in table Position value: 14 max value in memory 2251\n", - "[INFO] [1702983140.744622]: Found primary_key collision in table Position value: 15 max value in memory 2252\n", - "[INFO] [1702983140.745999]: Found primary_key collision in table Position value: 16 max value in memory 2253\n", - "[INFO] [1702983140.747611]: Found primary_key collision in table Position value: 17 max value in memory 2254\n", - "[INFO] [1702983140.749185]: Found primary_key collision in table Position value: 18 max value in memory 2255\n", - "[INFO] [1702983140.750929]: Found primary_key collision in table Position value: 19 max value in memory 2256\n", - "[INFO] [1702983140.752607]: Found primary_key collision in table Position value: 20 max value in memory 2257\n", - "[INFO] [1702983140.754260]: Found primary_key collision in table Position value: 21 max value in memory 2258\n", - "[INFO] [1702983140.756540]: Found primary_key collision in table Position value: 22 max value in memory 2259\n", - "[INFO] [1702983140.758622]: Found primary_key collision in table Position value: 23 max value in memory 2260\n", - "[INFO] [1702983140.761086]: Found primary_key collision in table Position value: 24 max value in memory 2261\n", - "[INFO] [1702983140.763119]: Found primary_key collision in table Position value: 25 max value in memory 2262\n", - "[INFO] [1702983140.765159]: Found primary_key collision in table Position value: 26 max value in memory 2263\n", - "[INFO] [1702983140.767105]: Found primary_key collision in table Position value: 27 max value in memory 2264\n", - "[INFO] [1702983140.768697]: Found primary_key collision in table Position value: 28 max value in memory 2265\n", - "[INFO] [1702983140.770432]: Found primary_key collision in table Position value: 29 max value in memory 2266\n", - "[INFO] [1702983140.772270]: Found primary_key collision in table Position value: 30 max value in memory 2267\n", - "[INFO] [1702983140.773938]: Found primary_key collision in table Position value: 31 max value in memory 2268\n", - "[INFO] [1702983140.775504]: Found primary_key collision in table Position value: 32 max value in memory 2269\n", - "[INFO] [1702983140.777225]: Found primary_key collision in table Position value: 33 max value in memory 2270\n", - "[INFO] [1702983140.778982]: Found primary_key collision in table Position value: 34 max value in memory 2271\n", - "[INFO] [1702983140.780921]: Found primary_key collision in table Position value: 35 max value in memory 2272\n", - "[INFO] [1702983140.782705]: Found primary_key collision in table Position value: 36 max value in memory 2273\n", - "[INFO] [1702983140.784870]: Found primary_key collision in table Position value: 37 max value in memory 2274\n", - "[INFO] [1702983140.786869]: Found primary_key collision in table Position value: 38 max value in memory 2275\n", - "[INFO] [1702983140.788549]: Found primary_key collision in table Position value: 39 max value in memory 2276\n", - "[INFO] [1702983140.790158]: Found primary_key collision in table Position value: 40 max value in memory 2277\n", - "[INFO] [1702983140.791539]: Found primary_key collision in table Position value: 41 max value in memory 2278\n", - "[INFO] [1702983140.792974]: Found primary_key collision in table Position value: 42 max value in memory 2279\n", - "[INFO] [1702983140.794354]: Found primary_key collision in table Position value: 43 max value in memory 2280\n", - "[INFO] [1702983140.795963]: Found primary_key collision in table Position value: 44 max value in memory 2281\n", - "[INFO] [1702983140.797665]: Found primary_key collision in table Position value: 45 max value in memory 2282\n", - "[INFO] [1702983140.799089]: Found primary_key collision in table Position value: 46 max value in memory 2283\n", - "[INFO] [1702983140.800694]: Found primary_key collision in table Position value: 47 max value in memory 2284\n", - "[INFO] [1702983140.802086]: Found primary_key collision in table Position value: 48 max value in memory 2285\n", - "[INFO] [1702983140.803602]: Found primary_key collision in table Position value: 49 max value in memory 2286\n", - "[INFO] [1702983140.805382]: Found primary_key collision in table Position value: 50 max value in memory 2287\n", - "[INFO] [1702983140.806960]: Found primary_key collision in table Position value: 51 max value in memory 2288\n", - "[INFO] [1702983140.808692]: Found primary_key collision in table Position value: 52 max value in memory 2289\n", - "[INFO] [1702983140.810307]: Found primary_key collision in table Position value: 53 max value in memory 2290\n", - "[INFO] [1702983140.811870]: Found primary_key collision in table Position value: 54 max value in memory 2291\n", - "[INFO] [1702983140.813625]: Found primary_key collision in table Position value: 55 max value in memory 2292\n", - "[INFO] [1702983140.815728]: Found primary_key collision in table Position value: 56 max value in memory 2293\n", - "[INFO] [1702983140.817941]: Found primary_key collision in table Position value: 57 max value in memory 2294\n", - "[INFO] [1702983140.819819]: Found primary_key collision in table Position value: 58 max value in memory 2295\n", - "[INFO] [1702983140.821540]: Found primary_key collision in table Position value: 59 max value in memory 2296\n", - "[INFO] [1702983140.823102]: Found primary_key collision in table Position value: 60 max value in memory 2297\n", - "[INFO] [1702983140.824502]: Found primary_key collision in table Position value: 61 max value in memory 2298\n", - "[INFO] [1702983140.825991]: Found primary_key collision in table Position value: 62 max value in memory 2299\n", - "[INFO] [1702983140.827542]: Found primary_key collision in table Position value: 63 max value in memory 2300\n", - "[INFO] [1702983140.829317]: Found primary_key collision in table Position value: 64 max value in memory 2301\n", - "[INFO] [1702983140.830838]: Found primary_key collision in table Position value: 65 max value in memory 2302\n", - "[INFO] [1702983140.832246]: Found primary_key collision in table Position value: 66 max value in memory 2303\n", - "[INFO] [1702983140.833657]: Found primary_key collision in table Position value: 67 max value in memory 2304\n", - "[INFO] [1702983140.834992]: Found primary_key collision in table Position value: 68 max value in memory 2305\n", - "[INFO] [1702983140.836339]: Found primary_key collision in table Position value: 69 max value in memory 2306\n", - "[INFO] [1702983140.837836]: Found primary_key collision in table Position value: 70 max value in memory 2307\n", - "[INFO] [1702983140.839260]: Found primary_key collision in table Position value: 71 max value in memory 2308\n", - "[INFO] [1702983140.840621]: Found primary_key collision in table Position value: 72 max value in memory 2309\n", - "[INFO] [1702983140.842064]: Found primary_key collision in table Position value: 73 max value in memory 2310\n", - "[INFO] [1702983140.843440]: Found primary_key collision in table Position value: 74 max value in memory 2311\n", - "[INFO] [1702983140.844786]: Found primary_key collision in table Position value: 75 max value in memory 2312\n", - "[INFO] [1702983140.846137]: Found primary_key collision in table Position value: 76 max value in memory 2313\n", - "[INFO] [1702983140.847443]: Found primary_key collision in table Position value: 77 max value in memory 2314\n", - "[INFO] [1702983140.848782]: Found primary_key collision in table Position value: 78 max value in memory 2315\n", - "[INFO] [1702983140.850315]: Found primary_key collision in table Position value: 79 max value in memory 2316\n", - "[INFO] [1702983140.852046]: Found primary_key collision in table Position value: 80 max value in memory 2317\n", - "[INFO] [1702983140.853552]: Found primary_key collision in table Position value: 81 max value in memory 2318\n", - "[INFO] [1702983140.855037]: Found primary_key collision in table Position value: 82 max value in memory 2319\n", - "[INFO] [1702983140.856592]: Found primary_key collision in table Position value: 83 max value in memory 2320\n", - "[INFO] [1702983140.857977]: Found primary_key collision in table Position value: 84 max value in memory 2321\n", - "[INFO] [1702983140.859483]: Found primary_key collision in table Position value: 85 max value in memory 2322\n", - "[INFO] [1702983140.860808]: Found primary_key collision in table Position value: 86 max value in memory 2323\n", - "[INFO] [1702983140.862532]: Found primary_key collision in table Position value: 87 max value in memory 2324\n", - "[INFO] [1702983140.864031]: Found primary_key collision in table Position value: 88 max value in memory 2325\n", - "[INFO] [1702983140.865524]: Found primary_key collision in table Position value: 89 max value in memory 2326\n", - "[INFO] [1702983140.867004]: Found primary_key collision in table Position value: 90 max value in memory 2327\n", - "[INFO] [1702983140.868497]: Found primary_key collision in table Position value: 91 max value in memory 2328\n", - "[INFO] [1702983140.869885]: Found primary_key collision in table Position value: 92 max value in memory 2329\n", - "[INFO] [1702983140.871481]: Found primary_key collision in table Position value: 93 max value in memory 2330\n", - "[INFO] [1702983140.873601]: Found primary_key collision in table Position value: 94 max value in memory 2331\n", - "[INFO] [1702983140.875400]: Found primary_key collision in table Position value: 95 max value in memory 2332\n", - "[INFO] [1702983140.877516]: Found primary_key collision in table Position value: 96 max value in memory 2333\n", - "[INFO] [1702983140.878923]: Found primary_key collision in table Position value: 97 max value in memory 2334\n", - "[INFO] [1702983140.880291]: Found primary_key collision in table Position value: 98 max value in memory 2335\n", - "[INFO] [1702983140.881685]: Found primary_key collision in table Position value: 99 max value in memory 2336\n", - "[INFO] [1702983140.883095]: Found primary_key collision in table Position value: 100 max value in memory 2337\n", - "[INFO] [1702983140.884486]: Found primary_key collision in table Position value: 101 max value in memory 2338\n", - "[INFO] [1702983140.885833]: Found primary_key collision in table Position value: 102 max value in memory 2339\n", - "[INFO] [1702983140.887165]: Found primary_key collision in table Position value: 103 max value in memory 2340\n", - "[INFO] [1702983140.898126]: Found primary_key collision in table Quaternion value: 1 max value in memory 2221\n", - "[INFO] [1702983140.900321]: Found primary_key collision in table Quaternion value: 2 max value in memory 2222\n", - "[INFO] [1702983140.901849]: Found primary_key collision in table Quaternion value: 3 max value in memory 2223\n", - "[INFO] [1702983140.903312]: Found primary_key collision in table Quaternion value: 4 max value in memory 2224\n", - "[INFO] [1702983140.904759]: Found primary_key collision in table Quaternion value: 5 max value in memory 2225\n", - "[INFO] [1702983140.906202]: Found primary_key collision in table Quaternion value: 6 max value in memory 2226\n", - "[INFO] [1702983140.907699]: Found primary_key collision in table Quaternion value: 7 max value in memory 2227\n", - "[INFO] [1702983140.909689]: Found primary_key collision in table Quaternion value: 8 max value in memory 2228\n", - "[INFO] [1702983140.911434]: Found primary_key collision in table Quaternion value: 9 max value in memory 2229\n", - "[INFO] [1702983140.913111]: Found primary_key collision in table Quaternion value: 10 max value in memory 2230\n", - "[INFO] [1702983140.914713]: Found primary_key collision in table Quaternion value: 11 max value in memory 2231\n", - "[INFO] [1702983140.916343]: Found primary_key collision in table Quaternion value: 12 max value in memory 2232\n", - "[INFO] [1702983140.917862]: Found primary_key collision in table Quaternion value: 13 max value in memory 2233\n", - "[INFO] [1702983140.919335]: Found primary_key collision in table Quaternion value: 14 max value in memory 2234\n", - "[INFO] [1702983140.920754]: Found primary_key collision in table Quaternion value: 15 max value in memory 2235\n", - "[INFO] [1702983140.922106]: Found primary_key collision in table Quaternion value: 16 max value in memory 2236\n", - "[INFO] [1702983140.923407]: Found primary_key collision in table Quaternion value: 17 max value in memory 2237\n", - "[INFO] [1702983140.924741]: Found primary_key collision in table Quaternion value: 18 max value in memory 2238\n", - "[INFO] [1702983140.926223]: Found primary_key collision in table Quaternion value: 19 max value in memory 2239\n", - "[INFO] [1702983140.927543]: Found primary_key collision in table Quaternion value: 20 max value in memory 2240\n", - "[INFO] [1702983140.928918]: Found primary_key collision in table Quaternion value: 21 max value in memory 2241\n", - "[INFO] [1702983140.930319]: Found primary_key collision in table Quaternion value: 22 max value in memory 2242\n", - "[INFO] [1702983140.931756]: Found primary_key collision in table Quaternion value: 23 max value in memory 2243\n", - "[INFO] [1702983140.933092]: Found primary_key collision in table Quaternion value: 24 max value in memory 2244\n", - "[INFO] [1702983140.934408]: Found primary_key collision in table Quaternion value: 25 max value in memory 2245\n", - "[INFO] [1702983140.935831]: Found primary_key collision in table Quaternion value: 26 max value in memory 2246\n", - "[INFO] [1702983140.937280]: Found primary_key collision in table Quaternion value: 27 max value in memory 2247\n", - "[INFO] [1702983140.938918]: Found primary_key collision in table Quaternion value: 28 max value in memory 2248\n", - "[INFO] [1702983140.940654]: Found primary_key collision in table Quaternion value: 29 max value in memory 2249\n", - "[INFO] [1702983140.942223]: Found primary_key collision in table Quaternion value: 30 max value in memory 2250\n", - "[INFO] [1702983140.943880]: Found primary_key collision in table Quaternion value: 31 max value in memory 2251\n", - "[INFO] [1702983140.945344]: Found primary_key collision in table Quaternion value: 32 max value in memory 2252\n", - "[INFO] [1702983140.946694]: Found primary_key collision in table Quaternion value: 33 max value in memory 2253\n", - "[INFO] [1702983140.948061]: Found primary_key collision in table Quaternion value: 34 max value in memory 2254\n", - "[INFO] [1702983140.949365]: Found primary_key collision in table Quaternion value: 35 max value in memory 2255\n", - "[INFO] [1702983140.950659]: Found primary_key collision in table Quaternion value: 36 max value in memory 2256\n", - "[INFO] [1702983140.951962]: Found primary_key collision in table Quaternion value: 37 max value in memory 2257\n", - "[INFO] [1702983140.953274]: Found primary_key collision in table Quaternion value: 38 max value in memory 2258\n", - "[INFO] [1702983140.954577]: Found primary_key collision in table Quaternion value: 39 max value in memory 2259\n", - "[INFO] [1702983140.955968]: Found primary_key collision in table Quaternion value: 40 max value in memory 2260\n", - "[INFO] [1702983140.957290]: Found primary_key collision in table Quaternion value: 41 max value in memory 2261\n", - "[INFO] [1702983140.958584]: Found primary_key collision in table Quaternion value: 42 max value in memory 2262\n", - "[INFO] [1702983140.959878]: Found primary_key collision in table Quaternion value: 43 max value in memory 2263\n", - "[INFO] [1702983140.961226]: Found primary_key collision in table Quaternion value: 44 max value in memory 2264\n", - "[INFO] [1702983140.962569]: Found primary_key collision in table Quaternion value: 45 max value in memory 2265\n", - "[INFO] [1702983140.963907]: Found primary_key collision in table Quaternion value: 46 max value in memory 2266\n", - "[INFO] [1702983140.965240]: Found primary_key collision in table Quaternion value: 47 max value in memory 2267\n", - "[INFO] [1702983140.966537]: Found primary_key collision in table Quaternion value: 48 max value in memory 2268\n", - "[INFO] [1702983140.967880]: Found primary_key collision in table Quaternion value: 49 max value in memory 2269\n", - "[INFO] [1702983140.969196]: Found primary_key collision in table Quaternion value: 50 max value in memory 2270\n", - "[INFO] [1702983140.970513]: Found primary_key collision in table Quaternion value: 51 max value in memory 2271\n", - "[INFO] [1702983140.971958]: Found primary_key collision in table Quaternion value: 52 max value in memory 2272\n", - "[INFO] [1702983140.973289]: Found primary_key collision in table Quaternion value: 53 max value in memory 2273\n", - "[INFO] [1702983140.974588]: Found primary_key collision in table Quaternion value: 54 max value in memory 2274\n", - "[INFO] [1702983140.975921]: Found primary_key collision in table Quaternion value: 55 max value in memory 2275\n", - "[INFO] [1702983140.977266]: Found primary_key collision in table Quaternion value: 56 max value in memory 2276\n", - "[INFO] [1702983140.978573]: Found primary_key collision in table Quaternion value: 57 max value in memory 2277\n", - "[INFO] [1702983140.979904]: Found primary_key collision in table Quaternion value: 58 max value in memory 2278\n", - "[INFO] [1702983140.981456]: Found primary_key collision in table Quaternion value: 59 max value in memory 2279\n", - "[INFO] [1702983140.982997]: Found primary_key collision in table Quaternion value: 60 max value in memory 2280\n", - "[INFO] [1702983140.986715]: Found primary_key collision in table Quaternion value: 61 max value in memory 2281\n", - "[INFO] [1702983140.988128]: Found primary_key collision in table Quaternion value: 62 max value in memory 2282\n", - "[INFO] [1702983140.989493]: Found primary_key collision in table Quaternion value: 63 max value in memory 2283\n", - "[INFO] [1702983140.991034]: Found primary_key collision in table Quaternion value: 64 max value in memory 2284\n", - "[INFO] [1702983140.992369]: Found primary_key collision in table Quaternion value: 65 max value in memory 2285\n", - "[INFO] [1702983140.993685]: Found primary_key collision in table Quaternion value: 66 max value in memory 2286\n", - "[INFO] [1702983140.995034]: Found primary_key collision in table Quaternion value: 67 max value in memory 2287\n", - "[INFO] [1702983140.996588]: Found primary_key collision in table Quaternion value: 68 max value in memory 2288\n", - "[INFO] [1702983140.998092]: Found primary_key collision in table Quaternion value: 69 max value in memory 2289\n", - "[INFO] [1702983140.999534]: Found primary_key collision in table Quaternion value: 70 max value in memory 2290\n", - "[INFO] [1702983141.000989]: Found primary_key collision in table Quaternion value: 71 max value in memory 2291\n", - "[INFO] [1702983141.002463]: Found primary_key collision in table Quaternion value: 72 max value in memory 2292\n", - "[INFO] [1702983141.003873]: Found primary_key collision in table Quaternion value: 73 max value in memory 2293\n", - "[INFO] [1702983141.005302]: Found primary_key collision in table Quaternion value: 74 max value in memory 2294\n", - "[INFO] [1702983141.007018]: Found primary_key collision in table Quaternion value: 75 max value in memory 2295\n", - "[INFO] [1702983141.008644]: Found primary_key collision in table Quaternion value: 76 max value in memory 2296\n", - "[INFO] [1702983141.010314]: Found primary_key collision in table Quaternion value: 77 max value in memory 2297\n", - "[INFO] [1702983141.011965]: Found primary_key collision in table Quaternion value: 78 max value in memory 2298\n", - "[INFO] [1702983141.013427]: Found primary_key collision in table Quaternion value: 79 max value in memory 2299\n", - "[INFO] [1702983141.014794]: Found primary_key collision in table Quaternion value: 80 max value in memory 2300\n", - "[INFO] [1702983141.016180]: Found primary_key collision in table Quaternion value: 81 max value in memory 2301\n", - "[INFO] [1702983141.017545]: Found primary_key collision in table Quaternion value: 82 max value in memory 2302\n", - "[INFO] [1702983141.019102]: Found primary_key collision in table Quaternion value: 83 max value in memory 2303\n", - "[INFO] [1702983141.020585]: Found primary_key collision in table Quaternion value: 84 max value in memory 2304\n", - "[INFO] [1702983141.022021]: Found primary_key collision in table Quaternion value: 85 max value in memory 2305\n", - "[INFO] [1702983141.023510]: Found primary_key collision in table Quaternion value: 86 max value in memory 2306\n", - "[INFO] [1702983141.025079]: Found primary_key collision in table Quaternion value: 87 max value in memory 2307\n", - "[INFO] [1702983141.026657]: Found primary_key collision in table Quaternion value: 88 max value in memory 2308\n", - "[INFO] [1702983141.028398]: Found primary_key collision in table Quaternion value: 89 max value in memory 2309\n", - "[INFO] [1702983141.029981]: Found primary_key collision in table Quaternion value: 90 max value in memory 2310\n", - "[INFO] [1702983141.031583]: Found primary_key collision in table Quaternion value: 91 max value in memory 2311\n", - "[INFO] [1702983141.033281]: Found primary_key collision in table Quaternion value: 92 max value in memory 2312\n", - "[INFO] [1702983141.034969]: Found primary_key collision in table Quaternion value: 93 max value in memory 2313\n", - "[INFO] [1702983141.036636]: Found primary_key collision in table Quaternion value: 94 max value in memory 2314\n", - "[INFO] [1702983141.038156]: Found primary_key collision in table Quaternion value: 95 max value in memory 2315\n", - "[INFO] [1702983141.039649]: Found primary_key collision in table Quaternion value: 96 max value in memory 2316\n", - "[INFO] [1702983141.041187]: Found primary_key collision in table Quaternion value: 97 max value in memory 2317\n", - "[INFO] [1702983141.042617]: Found primary_key collision in table Quaternion value: 98 max value in memory 2318\n", - "[INFO] [1702983141.044012]: Found primary_key collision in table Quaternion value: 99 max value in memory 2319\n", - "[INFO] [1702983141.045381]: Found primary_key collision in table Quaternion value: 100 max value in memory 2320\n", - "[INFO] [1702983141.048997]: Found primary_key collision in table Quaternion value: 101 max value in memory 2321\n", - "[INFO] [1702983141.050425]: Found primary_key collision in table Quaternion value: 102 max value in memory 2322\n", - "[INFO] [1702983141.051759]: Found primary_key collision in table Quaternion value: 103 max value in memory 2323\n", - "[INFO] [1702983141.053110]: Found primary_key collision in table Quaternion value: 104 max value in memory 2324\n", - "[INFO] [1702983141.054426]: Found primary_key collision in table Quaternion value: 105 max value in memory 2325\n", - "[INFO] [1702983141.055745]: Found primary_key collision in table Quaternion value: 106 max value in memory 2326\n", - "[INFO] [1702983141.057128]: Found primary_key collision in table Quaternion value: 107 max value in memory 2327\n", - "[INFO] [1702983141.058709]: Found primary_key collision in table Quaternion value: 108 max value in memory 2328\n", - "[INFO] [1702983141.060469]: Found primary_key collision in table Quaternion value: 109 max value in memory 2329\n", - "[INFO] [1702983141.062376]: Found primary_key collision in table Quaternion value: 110 max value in memory 2330\n", - "[INFO] [1702983141.063997]: Found primary_key collision in table Quaternion value: 111 max value in memory 2331\n", - "[INFO] [1702983141.065480]: Found primary_key collision in table Quaternion value: 112 max value in memory 2332\n", - "[INFO] [1702983141.066829]: Found primary_key collision in table Quaternion value: 113 max value in memory 2333\n", - "[INFO] [1702983141.068192]: Found primary_key collision in table Quaternion value: 114 max value in memory 2334\n", - "[INFO] [1702983141.069578]: Found primary_key collision in table Quaternion value: 115 max value in memory 2335\n", - "[INFO] [1702983141.071041]: Found primary_key collision in table Quaternion value: 116 max value in memory 2336\n", - "[INFO] [1702983141.072470]: Found primary_key collision in table Quaternion value: 117 max value in memory 2337\n", - "[INFO] [1702983141.073859]: Found primary_key collision in table Quaternion value: 118 max value in memory 2338\n", - "[INFO] [1702983141.075313]: Found primary_key collision in table Quaternion value: 119 max value in memory 2339\n", - "[INFO] [1702983141.076733]: Found primary_key collision in table Quaternion value: 120 max value in memory 2340\n", - "[INFO] [1702983141.088747]: Found primary_key collision in table Code value: 1 max value in memory 2165\n", - "[INFO] [1702983141.091197]: Found primary_key collision in table Code value: 2 max value in memory 2166\n", - "[INFO] [1702983141.092789]: Found primary_key collision in table Code value: 21 max value in memory 2167\n", - "[INFO] [1702983141.093907]: Found primary_key collision in table Code value: 22 max value in memory 2168\n", - "[INFO] [1702983141.095402]: Found primary_key collision in table Code value: 41 max value in memory 2169\n", - "[INFO] [1702983141.096872]: Found primary_key collision in table Code value: 60 max value in memory 2170\n", - "[INFO] [1702983141.098152]: Found primary_key collision in table Code value: 61 max value in memory 2171\n", - "[INFO] [1702983141.099483]: Found primary_key collision in table Code value: 80 max value in memory 2172\n", - "[INFO] [1702983141.100978]: Found primary_key collision in table Code value: 99 max value in memory 2173\n", - "[INFO] [1702983141.102331]: Found primary_key collision in table Code value: 3 max value in memory 2174\n", - "[INFO] [1702983141.103963]: Found primary_key collision in table Code value: 4 max value in memory 2175\n", - "[INFO] [1702983141.105609]: Found primary_key collision in table Code value: 5 max value in memory 2176\n", - "[INFO] [1702983141.107313]: Found primary_key collision in table Code value: 6 max value in memory 2177\n", - "[INFO] [1702983141.109206]: Found primary_key collision in table Code value: 7 max value in memory 2178\n", - "[INFO] [1702983141.110592]: Found primary_key collision in table Code value: 8 max value in memory 2179\n", - "[INFO] [1702983141.111916]: Found primary_key collision in table Code value: 9 max value in memory 2180\n", - "[INFO] [1702983141.113242]: Found primary_key collision in table Code value: 10 max value in memory 2181\n", - "[INFO] [1702983141.114548]: Found primary_key collision in table Code value: 11 max value in memory 2182\n", - "[INFO] [1702983141.115838]: Found primary_key collision in table Code value: 12 max value in memory 2183\n", - "[INFO] [1702983141.117163]: Found primary_key collision in table Code value: 13 max value in memory 2184\n", - "[INFO] [1702983141.118581]: Found primary_key collision in table Code value: 14 max value in memory 2185\n", - "[INFO] [1702983141.119880]: Found primary_key collision in table Code value: 15 max value in memory 2186\n", - "[INFO] [1702983141.121192]: Found primary_key collision in table Code value: 16 max value in memory 2187\n", - "[INFO] [1702983141.122492]: Found primary_key collision in table Code value: 17 max value in memory 2188\n", - "[INFO] [1702983141.123806]: Found primary_key collision in table Code value: 18 max value in memory 2189\n", - "[INFO] [1702983141.125157]: Found primary_key collision in table Code value: 19 max value in memory 2190\n", - "[INFO] [1702983141.126626]: Found primary_key collision in table Code value: 20 max value in memory 2191\n", - "[INFO] [1702983141.128310]: Found primary_key collision in table Code value: 23 max value in memory 2192\n", - "[INFO] [1702983141.129658]: Found primary_key collision in table Code value: 25 max value in memory 2193\n", - "[INFO] [1702983141.130972]: Found primary_key collision in table Code value: 26 max value in memory 2194\n", - "[INFO] [1702983141.132664]: Found primary_key collision in table Code value: 27 max value in memory 2195\n", - "[INFO] [1702983141.134043]: Found primary_key collision in table Code value: 28 max value in memory 2196\n", - "[INFO] [1702983141.135368]: Found primary_key collision in table Code value: 29 max value in memory 2197\n", - "[INFO] [1702983141.136713]: Found primary_key collision in table Code value: 30 max value in memory 2198\n", - "[INFO] [1702983141.138046]: Found primary_key collision in table Code value: 31 max value in memory 2199\n", - "[INFO] [1702983141.139432]: Found primary_key collision in table Code value: 32 max value in memory 2200\n", - "[INFO] [1702983141.141065]: Found primary_key collision in table Code value: 33 max value in memory 2201\n", - "[INFO] [1702983141.142772]: Found primary_key collision in table Code value: 34 max value in memory 2202\n", - "[INFO] [1702983141.144274]: Found primary_key collision in table Code value: 35 max value in memory 2203\n", - "[INFO] [1702983141.145721]: Found primary_key collision in table Code value: 36 max value in memory 2204\n", - "[INFO] [1702983141.147188]: Found primary_key collision in table Code value: 37 max value in memory 2205\n", - "[INFO] [1702983141.148605]: Found primary_key collision in table Code value: 38 max value in memory 2206\n", - "[INFO] [1702983141.150080]: Found primary_key collision in table Code value: 39 max value in memory 2207\n", - "[INFO] [1702983141.151910]: Found primary_key collision in table Code value: 40 max value in memory 2208\n", - "[INFO] [1702983141.153322]: Found primary_key collision in table Code value: 42 max value in memory 2209\n", - "[INFO] [1702983141.154582]: Found primary_key collision in table Code value: 43 max value in memory 2210\n", - "[INFO] [1702983141.155949]: Found primary_key collision in table Code value: 44 max value in memory 2211\n", - "[INFO] [1702983141.157273]: Found primary_key collision in table Code value: 45 max value in memory 2212\n", - "[INFO] [1702983141.158810]: Found primary_key collision in table Code value: 46 max value in memory 2213\n", - "[INFO] [1702983141.160491]: Found primary_key collision in table Code value: 47 max value in memory 2214\n", - "[INFO] [1702983141.161900]: Found primary_key collision in table Code value: 48 max value in memory 2215\n", - "[INFO] [1702983141.163523]: Found primary_key collision in table Code value: 49 max value in memory 2216\n", - "[INFO] [1702983141.165102]: Found primary_key collision in table Code value: 50 max value in memory 2217\n", - "[INFO] [1702983141.166693]: Found primary_key collision in table Code value: 51 max value in memory 2218\n", - "[INFO] [1702983141.167995]: Found primary_key collision in table Code value: 52 max value in memory 2219\n", - "[INFO] [1702983141.169437]: Found primary_key collision in table Code value: 53 max value in memory 2220\n", - "[INFO] [1702983141.170921]: Found primary_key collision in table Code value: 54 max value in memory 2221\n", - "[INFO] [1702983141.172504]: Found primary_key collision in table Code value: 55 max value in memory 2222\n", - "[INFO] [1702983141.174295]: Found primary_key collision in table Code value: 56 max value in memory 2223\n", - "[INFO] [1702983141.175949]: Found primary_key collision in table Code value: 57 max value in memory 2224\n", - "[INFO] [1702983141.177617]: Found primary_key collision in table Code value: 58 max value in memory 2225\n", - "[INFO] [1702983141.179288]: Found primary_key collision in table Code value: 59 max value in memory 2226\n", - "[INFO] [1702983141.180998]: Found primary_key collision in table Code value: 62 max value in memory 2227\n", - "[INFO] [1702983141.183250]: Found primary_key collision in table Code value: 63 max value in memory 2228\n", - "[INFO] [1702983141.184985]: Found primary_key collision in table Code value: 64 max value in memory 2229\n", - "[INFO] [1702983141.186500]: Found primary_key collision in table Code value: 65 max value in memory 2230\n", - "[INFO] [1702983141.188090]: Found primary_key collision in table Code value: 66 max value in memory 2231\n", - "[INFO] [1702983141.189617]: Found primary_key collision in table Code value: 67 max value in memory 2232\n", - "[INFO] [1702983141.191160]: Found primary_key collision in table Code value: 68 max value in memory 2233\n", - "[INFO] [1702983141.192691]: Found primary_key collision in table Code value: 69 max value in memory 2234\n", - "[INFO] [1702983141.194211]: Found primary_key collision in table Code value: 70 max value in memory 2235\n", - "[INFO] [1702983141.195727]: Found primary_key collision in table Code value: 71 max value in memory 2236\n", - "[INFO] [1702983141.197345]: Found primary_key collision in table Code value: 72 max value in memory 2237\n", - "[INFO] [1702983141.198761]: Found primary_key collision in table Code value: 73 max value in memory 2238\n", - "[INFO] [1702983141.200398]: Found primary_key collision in table Code value: 74 max value in memory 2239\n", - "[INFO] [1702983141.201935]: Found primary_key collision in table Code value: 75 max value in memory 2240\n", - "[INFO] [1702983141.203305]: Found primary_key collision in table Code value: 76 max value in memory 2241\n", - "[INFO] [1702983141.204779]: Found primary_key collision in table Code value: 77 max value in memory 2242\n", - "[INFO] [1702983141.206375]: Found primary_key collision in table Code value: 78 max value in memory 2243\n", - "[INFO] [1702983141.208308]: Found primary_key collision in table Code value: 79 max value in memory 2244\n", - "[INFO] [1702983141.209922]: Found primary_key collision in table Code value: 81 max value in memory 2245\n", - "[INFO] [1702983141.211468]: Found primary_key collision in table Code value: 82 max value in memory 2246\n", - "[INFO] [1702983141.213253]: Found primary_key collision in table Code value: 83 max value in memory 2247\n", - "[INFO] [1702983141.214950]: Found primary_key collision in table Code value: 84 max value in memory 2248\n", - "[INFO] [1702983141.216438]: Found primary_key collision in table Code value: 24 max value in memory 2249\n", - "[INFO] [1702983141.217956]: Found primary_key collision in table Code value: 85 max value in memory 2250\n", - "[INFO] [1702983141.219383]: Found primary_key collision in table Code value: 86 max value in memory 2251\n", - "[INFO] [1702983141.220852]: Found primary_key collision in table Code value: 87 max value in memory 2252\n", - "[INFO] [1702983141.222367]: Found primary_key collision in table Code value: 88 max value in memory 2253\n", - "[INFO] [1702983141.223833]: Found primary_key collision in table Code value: 89 max value in memory 2254\n", - "[INFO] [1702983141.225266]: Found primary_key collision in table Code value: 90 max value in memory 2255\n", - "[INFO] [1702983141.226738]: Found primary_key collision in table Code value: 91 max value in memory 2256\n", - "[INFO] [1702983141.228542]: Found primary_key collision in table Code value: 92 max value in memory 2257\n", - "[INFO] [1702983141.230120]: Found primary_key collision in table Code value: 93 max value in memory 2258\n", - "[INFO] [1702983141.231697]: Found primary_key collision in table Code value: 94 max value in memory 2259\n", - "[INFO] [1702983141.233503]: Found primary_key collision in table Code value: 95 max value in memory 2260\n", - "[INFO] [1702983141.234985]: Found primary_key collision in table Code value: 96 max value in memory 2261\n", - "[INFO] [1702983141.236391]: Found primary_key collision in table Code value: 97 max value in memory 2262\n", - "[INFO] [1702983141.237755]: Found primary_key collision in table Code value: 98 max value in memory 2263\n", - "[INFO] [1702983141.239153]: Found primary_key collision in table Code value: 100 max value in memory 2264\n", - "[INFO] [1702983141.240549]: Found primary_key collision in table Code value: 101 max value in memory 2265\n", - "[INFO] [1702983141.242136]: Found primary_key collision in table Code value: 102 max value in memory 2266\n", - "[INFO] [1702983141.243600]: Found primary_key collision in table Code value: 103 max value in memory 2267\n", - "[INFO] [1702983141.245151]: Found primary_key collision in table Code value: 104 max value in memory 2268\n", - "[INFO] [1702983141.246552]: Found primary_key collision in table Code value: 105 max value in memory 2269\n", - "[INFO] [1702983141.247849]: Found primary_key collision in table Code value: 106 max value in memory 2270\n", - "[INFO] [1702983141.249274]: Found primary_key collision in table Code value: 107 max value in memory 2271\n", - "[INFO] [1702983141.250571]: Found primary_key collision in table Code value: 108 max value in memory 2272\n", - "[INFO] [1702983141.251873]: Found primary_key collision in table Code value: 109 max value in memory 2273\n", - "[INFO] [1702983141.253914]: Found primary_key collision in table Code value: 110 max value in memory 2274\n", - "[INFO] [1702983141.255218]: Found primary_key collision in table Code value: 111 max value in memory 2275\n", - "[INFO] [1702983141.256518]: Found primary_key collision in table Code value: 112 max value in memory 2276\n", - "[INFO] [1702983141.257833]: Found primary_key collision in table Code value: 113 max value in memory 2277\n", - "[INFO] [1702983141.259129]: Found primary_key collision in table Code value: 114 max value in memory 2278\n", - "[INFO] [1702983141.260462]: Found primary_key collision in table Code value: 115 max value in memory 2279\n", - "[INFO] [1702983141.261757]: Found primary_key collision in table Code value: 116 max value in memory 2280\n", - "[INFO] [1702983141.263041]: Found primary_key collision in table Code value: 117 max value in memory 2281\n", - "[INFO] [1702983141.282719]: Found primary_key collision in table Pose value: 104 max value in memory 2221\n", - "[INFO] [1702983141.284815]: Found primary_key collision in table Pose value: 105 max value in memory 2222\n", - "[INFO] [1702983141.286346]: Found primary_key collision in table Pose value: 106 max value in memory 2223\n", - "[INFO] [1702983141.287699]: Found primary_key collision in table Pose value: 107 max value in memory 2224\n", - "[INFO] [1702983141.289181]: Found primary_key collision in table Pose value: 108 max value in memory 2225\n", - "[INFO] [1702983141.290734]: Found primary_key collision in table Pose value: 109 max value in memory 2226\n", - "[INFO] [1702983141.294156]: Found primary_key collision in table Pose value: 1 max value in memory 2227\n", - "[INFO] [1702983141.295552]: Found primary_key collision in table Pose value: 2 max value in memory 2228\n", - "[INFO] [1702983141.296952]: Found primary_key collision in table Pose value: 3 max value in memory 2229\n", - "[INFO] [1702983141.298329]: Found primary_key collision in table Pose value: 4 max value in memory 2230\n", - "[INFO] [1702983141.299704]: Found primary_key collision in table Pose value: 5 max value in memory 2231\n", - "[INFO] [1702983141.301085]: Found primary_key collision in table Pose value: 6 max value in memory 2232\n", - "[INFO] [1702983141.302458]: Found primary_key collision in table Pose value: 7 max value in memory 2233\n", - "[INFO] [1702983141.303839]: Found primary_key collision in table Pose value: 8 max value in memory 2234\n", - "[INFO] [1702983141.305592]: Found primary_key collision in table Pose value: 9 max value in memory 2235\n", - "[INFO] [1702983141.307268]: Found primary_key collision in table Pose value: 10 max value in memory 2236\n", - "[INFO] [1702983141.308744]: Found primary_key collision in table Pose value: 11 max value in memory 2237\n", - "[INFO] [1702983141.310133]: Found primary_key collision in table Pose value: 12 max value in memory 2238\n", - "[INFO] [1702983141.311624]: Found primary_key collision in table Pose value: 13 max value in memory 2239\n", - "[INFO] [1702983141.313061]: Found primary_key collision in table Pose value: 14 max value in memory 2240\n", - "[INFO] [1702983141.314437]: Found primary_key collision in table Pose value: 15 max value in memory 2241\n", - "[INFO] [1702983141.315780]: Found primary_key collision in table Pose value: 16 max value in memory 2242\n", - "[INFO] [1702983141.317169]: Found primary_key collision in table Pose value: 17 max value in memory 2243\n", - "[INFO] [1702983141.318581]: Found primary_key collision in table Pose value: 18 max value in memory 2244\n", - "[INFO] [1702983141.319979]: Found primary_key collision in table Pose value: 19 max value in memory 2245\n", - "[INFO] [1702983141.321419]: Found primary_key collision in table Pose value: 20 max value in memory 2246\n", - "[INFO] [1702983141.322928]: Found primary_key collision in table Pose value: 21 max value in memory 2247\n", - "[INFO] [1702983141.324472]: Found primary_key collision in table Pose value: 22 max value in memory 2248\n", - "[INFO] [1702983141.325832]: Found primary_key collision in table Pose value: 23 max value in memory 2249\n", - "[INFO] [1702983141.327389]: Found primary_key collision in table Pose value: 24 max value in memory 2250\n", - "[INFO] [1702983141.329190]: Found primary_key collision in table Pose value: 25 max value in memory 2251\n", - "[INFO] [1702983141.331033]: Found primary_key collision in table Pose value: 26 max value in memory 2252\n", - "[INFO] [1702983141.332608]: Found primary_key collision in table Pose value: 27 max value in memory 2253\n", - "[INFO] [1702983141.334348]: Found primary_key collision in table Pose value: 28 max value in memory 2254\n", - "[INFO] [1702983141.335992]: Found primary_key collision in table Pose value: 29 max value in memory 2255\n", - "[INFO] [1702983141.337676]: Found primary_key collision in table Pose value: 30 max value in memory 2256\n", - "[INFO] [1702983141.339061]: Found primary_key collision in table Pose value: 31 max value in memory 2257\n", - "[INFO] [1702983141.340194]: Found primary_key collision in table Pose value: 32 max value in memory 2258\n", - "[INFO] [1702983141.341306]: Found primary_key collision in table Pose value: 33 max value in memory 2259\n", - "[INFO] [1702983141.342493]: Found primary_key collision in table Pose value: 34 max value in memory 2260\n", - "[INFO] [1702983141.343671]: Found primary_key collision in table Pose value: 35 max value in memory 2261\n", - "[INFO] [1702983141.344848]: Found primary_key collision in table Pose value: 36 max value in memory 2262\n", - "[INFO] [1702983141.346068]: Found primary_key collision in table Pose value: 37 max value in memory 2263\n", - "[INFO] [1702983141.347195]: Found primary_key collision in table Pose value: 38 max value in memory 2264\n", - "[INFO] [1702983141.348558]: Found primary_key collision in table Pose value: 39 max value in memory 2265\n", - "[INFO] [1702983141.349966]: Found primary_key collision in table Pose value: 40 max value in memory 2266\n", - "[INFO] [1702983141.351308]: Found primary_key collision in table Pose value: 41 max value in memory 2267\n", - "[INFO] [1702983141.352673]: Found primary_key collision in table Pose value: 42 max value in memory 2268\n", - "[INFO] [1702983141.354024]: Found primary_key collision in table Pose value: 43 max value in memory 2269\n", - "[INFO] [1702983141.355370]: Found primary_key collision in table Pose value: 44 max value in memory 2270\n", - "[INFO] [1702983141.356941]: Found primary_key collision in table Pose value: 45 max value in memory 2271\n", - "[INFO] [1702983141.358344]: Found primary_key collision in table Pose value: 46 max value in memory 2272\n", - "[INFO] [1702983141.359674]: Found primary_key collision in table Pose value: 47 max value in memory 2273\n", - "[INFO] [1702983141.361128]: Found primary_key collision in table Pose value: 48 max value in memory 2274\n", - "[INFO] [1702983141.362496]: Found primary_key collision in table Pose value: 49 max value in memory 2275\n", - "[INFO] [1702983141.363866]: Found primary_key collision in table Pose value: 50 max value in memory 2276\n", - "[INFO] [1702983141.365257]: Found primary_key collision in table Pose value: 51 max value in memory 2277\n", - "[INFO] [1702983141.366621]: Found primary_key collision in table Pose value: 52 max value in memory 2278\n", - "[INFO] [1702983141.367993]: Found primary_key collision in table Pose value: 53 max value in memory 2279\n", - "[INFO] [1702983141.369585]: Found primary_key collision in table Pose value: 54 max value in memory 2280\n", - "[INFO] [1702983141.371029]: Found primary_key collision in table Pose value: 55 max value in memory 2281\n", - "[INFO] [1702983141.372468]: Found primary_key collision in table Pose value: 56 max value in memory 2282\n", - "[INFO] [1702983141.373868]: Found primary_key collision in table Pose value: 57 max value in memory 2283\n", - "[INFO] [1702983141.375313]: Found primary_key collision in table Pose value: 58 max value in memory 2284\n", - "[INFO] [1702983141.376772]: Found primary_key collision in table Pose value: 59 max value in memory 2285\n", - "[INFO] [1702983141.378164]: Found primary_key collision in table Pose value: 60 max value in memory 2286\n", - "[INFO] [1702983141.379555]: Found primary_key collision in table Pose value: 61 max value in memory 2287\n", - "[INFO] [1702983141.380974]: Found primary_key collision in table Pose value: 62 max value in memory 2288\n", - "[INFO] [1702983141.382690]: Found primary_key collision in table Pose value: 63 max value in memory 2289\n", - "[INFO] [1702983141.384348]: Found primary_key collision in table Pose value: 64 max value in memory 2290\n", - "[INFO] [1702983141.385754]: Found primary_key collision in table Pose value: 65 max value in memory 2291\n", - "[INFO] [1702983141.387141]: Found primary_key collision in table Pose value: 66 max value in memory 2292\n", - "[INFO] [1702983141.388516]: Found primary_key collision in table Pose value: 67 max value in memory 2293\n", - "[INFO] [1702983141.389944]: Found primary_key collision in table Pose value: 68 max value in memory 2294\n", - "[INFO] [1702983141.391337]: Found primary_key collision in table Pose value: 69 max value in memory 2295\n", - "[INFO] [1702983141.392718]: Found primary_key collision in table Pose value: 70 max value in memory 2296\n", - "[INFO] [1702983141.394100]: Found primary_key collision in table Pose value: 71 max value in memory 2297\n", - "[INFO] [1702983141.395546]: Found primary_key collision in table Pose value: 72 max value in memory 2298\n", - "[INFO] [1702983141.396934]: Found primary_key collision in table Pose value: 73 max value in memory 2299\n", - "[INFO] [1702983141.398365]: Found primary_key collision in table Pose value: 74 max value in memory 2300\n", - "[INFO] [1702983141.399726]: Found primary_key collision in table Pose value: 75 max value in memory 2301\n", - "[INFO] [1702983141.401153]: Found primary_key collision in table Pose value: 76 max value in memory 2302\n", - "[INFO] [1702983141.402524]: Found primary_key collision in table Pose value: 77 max value in memory 2303\n", - "[INFO] [1702983141.403937]: Found primary_key collision in table Pose value: 78 max value in memory 2304\n", - "[INFO] [1702983141.405444]: Found primary_key collision in table Pose value: 79 max value in memory 2305\n", - "[INFO] [1702983141.406863]: Found primary_key collision in table Pose value: 80 max value in memory 2306\n", - "[INFO] [1702983141.408277]: Found primary_key collision in table Pose value: 81 max value in memory 2307\n", - "[INFO] [1702983141.409698]: Found primary_key collision in table Pose value: 82 max value in memory 2308\n", - "[INFO] [1702983141.411084]: Found primary_key collision in table Pose value: 83 max value in memory 2309\n", - "[INFO] [1702983141.412926]: Found primary_key collision in table Pose value: 84 max value in memory 2310\n", - "[INFO] [1702983141.414299]: Found primary_key collision in table Pose value: 85 max value in memory 2311\n", - "[INFO] [1702983141.415661]: Found primary_key collision in table Pose value: 86 max value in memory 2312\n", - "[INFO] [1702983141.417256]: Found primary_key collision in table Pose value: 87 max value in memory 2313\n", - "[INFO] [1702983141.418627]: Found primary_key collision in table Pose value: 88 max value in memory 2314\n", - "[INFO] [1702983141.419992]: Found primary_key collision in table Pose value: 89 max value in memory 2315\n", - "[INFO] [1702983141.421392]: Found primary_key collision in table Pose value: 90 max value in memory 2316\n", - "[INFO] [1702983141.422773]: Found primary_key collision in table Pose value: 91 max value in memory 2317\n", - "[INFO] [1702983141.424221]: Found primary_key collision in table Pose value: 92 max value in memory 2318\n", - "[INFO] [1702983141.425601]: Found primary_key collision in table Pose value: 93 max value in memory 2319\n", - "[INFO] [1702983141.426990]: Found primary_key collision in table Pose value: 94 max value in memory 2320\n", - "[INFO] [1702983141.428366]: Found primary_key collision in table Pose value: 95 max value in memory 2321\n", - "[INFO] [1702983141.429733]: Found primary_key collision in table Pose value: 96 max value in memory 2322\n", - "[INFO] [1702983141.431120]: Found primary_key collision in table Pose value: 97 max value in memory 2323\n", - "[INFO] [1702983141.432565]: Found primary_key collision in table Pose value: 98 max value in memory 2324\n", - "[INFO] [1702983141.433939]: Found primary_key collision in table Pose value: 99 max value in memory 2325\n", - "[INFO] [1702983141.435302]: Found primary_key collision in table Pose value: 100 max value in memory 2326\n", - "[INFO] [1702983141.436674]: Found primary_key collision in table Pose value: 101 max value in memory 2327\n", - "[INFO] [1702983141.438158]: Found primary_key collision in table Pose value: 102 max value in memory 2328\n", - "[INFO] [1702983141.439639]: Found primary_key collision in table Pose value: 103 max value in memory 2329\n", - "[INFO] [1702983141.441098]: Found primary_key collision in table Pose value: 110 max value in memory 2330\n", - "[INFO] [1702983141.442490]: Found primary_key collision in table Pose value: 111 max value in memory 2331\n", - "[INFO] [1702983141.443902]: Found primary_key collision in table Pose value: 112 max value in memory 2332\n", - "[INFO] [1702983141.445372]: Found primary_key collision in table Pose value: 113 max value in memory 2333\n", - "[INFO] [1702983141.446768]: Found primary_key collision in table Pose value: 114 max value in memory 2334\n", - "[INFO] [1702983141.448152]: Found primary_key collision in table Pose value: 115 max value in memory 2335\n", - "[INFO] [1702983141.449548]: Found primary_key collision in table Pose value: 116 max value in memory 2336\n", - "[INFO] [1702983141.450931]: Found primary_key collision in table Pose value: 117 max value in memory 2337\n", - "[INFO] [1702983141.452311]: Found primary_key collision in table Pose value: 118 max value in memory 2338\n", - "[INFO] [1702983141.453662]: Found primary_key collision in table Pose value: 119 max value in memory 2339\n", - "[INFO] [1702983141.454998]: Found primary_key collision in table Pose value: 120 max value in memory 2340\n", - "[INFO] [1702983141.476626]: Found primary_key collision in table Object value: 11 max value in memory 223\n", - "[INFO] [1702983141.479099]: Found primary_key collision in table Object value: 1 max value in memory 224\n", - "[INFO] [1702983141.480560]: Found primary_key collision in table Object value: 2 max value in memory 225\n", - "[INFO] [1702983141.482409]: Found primary_key collision in table Object value: 3 max value in memory 226\n", - "[INFO] [1702983141.484268]: Found primary_key collision in table Object value: 4 max value in memory 227\n", - "[INFO] [1702983141.485847]: Found primary_key collision in table Object value: 5 max value in memory 228\n", - "[INFO] [1702983141.487321]: Found primary_key collision in table Object value: 6 max value in memory 229\n", - "[INFO] [1702983141.488611]: Found primary_key collision in table Object value: 7 max value in memory 230\n", - "[INFO] [1702983141.489894]: Found primary_key collision in table Object value: 8 max value in memory 231\n", - "[INFO] [1702983141.491164]: Found primary_key collision in table Object value: 9 max value in memory 232\n", - "[INFO] [1702983141.492443]: Found primary_key collision in table Object value: 10 max value in memory 233\n", - "[INFO] [1702983141.493731]: Found primary_key collision in table Object value: 12 max value in memory 234\n", - "[INFO] [1702983141.504940]: Found primary_key collision in table RobotState value: 44 max value in memory 889\n", - "[INFO] [1702983141.507209]: Found primary_key collision in table RobotState value: 1 max value in memory 890\n", - "[INFO] [1702983141.508798]: Found primary_key collision in table RobotState value: 2 max value in memory 891\n", - "[INFO] [1702983141.510223]: Found primary_key collision in table RobotState value: 3 max value in memory 892\n", - "[INFO] [1702983141.511511]: Found primary_key collision in table RobotState value: 4 max value in memory 893\n", - "[INFO] [1702983141.512776]: Found primary_key collision in table RobotState value: 5 max value in memory 894\n", - "[INFO] [1702983141.514047]: Found primary_key collision in table RobotState value: 6 max value in memory 895\n", - "[INFO] [1702983141.515297]: Found primary_key collision in table RobotState value: 7 max value in memory 896\n", - "[INFO] [1702983141.516497]: Found primary_key collision in table RobotState value: 8 max value in memory 897\n", - "[INFO] [1702983141.517718]: Found primary_key collision in table RobotState value: 9 max value in memory 898\n", - "[INFO] [1702983141.519063]: Found primary_key collision in table RobotState value: 10 max value in memory 899\n", - "[INFO] [1702983141.520516]: Found primary_key collision in table RobotState value: 11 max value in memory 900\n", - "[INFO] [1702983141.521817]: Found primary_key collision in table RobotState value: 12 max value in memory 901\n", - "[INFO] [1702983141.523295]: Found primary_key collision in table RobotState value: 13 max value in memory 902\n", - "[INFO] [1702983141.524951]: Found primary_key collision in table RobotState value: 14 max value in memory 903\n", - "[INFO] [1702983141.526586]: Found primary_key collision in table RobotState value: 15 max value in memory 904\n", - "[INFO] [1702983141.528196]: Found primary_key collision in table RobotState value: 16 max value in memory 905\n", - "[INFO] [1702983141.529798]: Found primary_key collision in table RobotState value: 17 max value in memory 906\n", - "[INFO] [1702983141.531359]: Found primary_key collision in table RobotState value: 18 max value in memory 907\n", - "[INFO] [1702983141.532957]: Found primary_key collision in table RobotState value: 19 max value in memory 908\n", - "[INFO] [1702983141.534208]: Found primary_key collision in table RobotState value: 20 max value in memory 909\n", - "[INFO] [1702983141.535596]: Found primary_key collision in table RobotState value: 21 max value in memory 910\n", - "[INFO] [1702983141.536886]: Found primary_key collision in table RobotState value: 22 max value in memory 911\n", - "[INFO] [1702983141.538518]: Found primary_key collision in table RobotState value: 23 max value in memory 912\n", - "[INFO] [1702983141.540249]: Found primary_key collision in table RobotState value: 24 max value in memory 913\n", - "[INFO] [1702983141.542006]: Found primary_key collision in table RobotState value: 25 max value in memory 914\n", - "[INFO] [1702983141.543688]: Found primary_key collision in table RobotState value: 26 max value in memory 915\n", - "[INFO] [1702983141.545352]: Found primary_key collision in table RobotState value: 27 max value in memory 916\n", - "[INFO] [1702983141.546910]: Found primary_key collision in table RobotState value: 28 max value in memory 917\n", - "[INFO] [1702983141.548205]: Found primary_key collision in table RobotState value: 29 max value in memory 918\n", - "[INFO] [1702983141.549478]: Found primary_key collision in table RobotState value: 30 max value in memory 919\n", - "[INFO] [1702983141.550742]: Found primary_key collision in table RobotState value: 31 max value in memory 920\n", - "[INFO] [1702983141.552010]: Found primary_key collision in table RobotState value: 32 max value in memory 921\n", - "[INFO] [1702983141.553291]: Found primary_key collision in table RobotState value: 33 max value in memory 922\n", - "[INFO] [1702983141.554597]: Found primary_key collision in table RobotState value: 34 max value in memory 923\n", - "[INFO] [1702983141.555874]: Found primary_key collision in table RobotState value: 35 max value in memory 924\n", - "[INFO] [1702983141.557105]: Found primary_key collision in table RobotState value: 36 max value in memory 925\n", - "[INFO] [1702983141.558276]: Found primary_key collision in table RobotState value: 37 max value in memory 926\n", - "[INFO] [1702983141.559433]: Found primary_key collision in table RobotState value: 38 max value in memory 927\n", - "[INFO] [1702983141.560648]: Found primary_key collision in table RobotState value: 39 max value in memory 928\n", - "[INFO] [1702983141.561815]: Found primary_key collision in table RobotState value: 40 max value in memory 929\n", - "[INFO] [1702983141.562998]: Found primary_key collision in table RobotState value: 41 max value in memory 930\n", - "[INFO] [1702983141.564190]: Found primary_key collision in table RobotState value: 42 max value in memory 931\n", - "[INFO] [1702983141.565479]: Found primary_key collision in table RobotState value: 43 max value in memory 932\n", - "[INFO] [1702983141.566730]: Found primary_key collision in table RobotState value: 45 max value in memory 933\n", - "[INFO] [1702983141.567975]: Found primary_key collision in table RobotState value: 46 max value in memory 934\n", - "[INFO] [1702983141.569290]: Found primary_key collision in table RobotState value: 47 max value in memory 935\n", - "[INFO] [1702983141.570597]: Found primary_key collision in table RobotState value: 48 max value in memory 936\n", - "[INFO] [1702983141.585400]: Found primary_key collision in table TaskTreeNode value: 1 max value in memory 2165\n", - "[INFO] [1702983141.587664]: Found primary_key collision in table TaskTreeNode value: 2 max value in memory 2166\n", - "[INFO] [1702983141.589488]: Found primary_key collision in table TaskTreeNode value: 21 max value in memory 2167\n", - "[INFO] [1702983141.591018]: Found primary_key collision in table TaskTreeNode value: 22 max value in memory 2168\n", - "[INFO] [1702983141.592587]: Found primary_key collision in table TaskTreeNode value: 41 max value in memory 2169\n", - "[INFO] [1702983141.594145]: Found primary_key collision in table TaskTreeNode value: 60 max value in memory 2170\n", - "[INFO] [1702983141.595514]: Found primary_key collision in table TaskTreeNode value: 61 max value in memory 2171\n", - "[INFO] [1702983141.597373]: Found primary_key collision in table TaskTreeNode value: 3 max value in memory 2172\n", - "[INFO] [1702983141.598804]: Found primary_key collision in table TaskTreeNode value: 4 max value in memory 2173\n", - "[INFO] [1702983141.600162]: Found primary_key collision in table TaskTreeNode value: 5 max value in memory 2174\n", - "[INFO] [1702983141.601508]: Found primary_key collision in table TaskTreeNode value: 6 max value in memory 2175\n", - "[INFO] [1702983141.602909]: Found primary_key collision in table TaskTreeNode value: 7 max value in memory 2176\n", - "[INFO] [1702983141.604420]: Found primary_key collision in table TaskTreeNode value: 8 max value in memory 2177\n", - "[INFO] [1702983141.605931]: Found primary_key collision in table TaskTreeNode value: 9 max value in memory 2178\n", - "[INFO] [1702983141.607299]: Found primary_key collision in table TaskTreeNode value: 10 max value in memory 2179\n", - "[INFO] [1702983141.608654]: Found primary_key collision in table TaskTreeNode value: 11 max value in memory 2180\n", - "[INFO] [1702983141.610231]: Found primary_key collision in table TaskTreeNode value: 12 max value in memory 2181\n", - "[INFO] [1702983141.611811]: Found primary_key collision in table TaskTreeNode value: 13 max value in memory 2182\n", - "[INFO] [1702983141.613328]: Found primary_key collision in table TaskTreeNode value: 14 max value in memory 2183\n", - "[INFO] [1702983141.614797]: Found primary_key collision in table TaskTreeNode value: 15 max value in memory 2184\n", - "[INFO] [1702983141.616238]: Found primary_key collision in table TaskTreeNode value: 16 max value in memory 2185\n", - "[INFO] [1702983141.617742]: Found primary_key collision in table TaskTreeNode value: 17 max value in memory 2186\n", - "[INFO] [1702983141.619210]: Found primary_key collision in table TaskTreeNode value: 18 max value in memory 2187\n", - "[INFO] [1702983141.620724]: Found primary_key collision in table TaskTreeNode value: 19 max value in memory 2188\n", - "[INFO] [1702983141.622019]: Found primary_key collision in table TaskTreeNode value: 20 max value in memory 2189\n", - "[INFO] [1702983141.623241]: Found primary_key collision in table TaskTreeNode value: 23 max value in memory 2190\n", - "[INFO] [1702983141.624627]: Found primary_key collision in table TaskTreeNode value: 25 max value in memory 2191\n", - "[INFO] [1702983141.626490]: Found primary_key collision in table TaskTreeNode value: 26 max value in memory 2192\n", - "[INFO] [1702983141.627821]: Found primary_key collision in table TaskTreeNode value: 27 max value in memory 2193\n", - "[INFO] [1702983141.629475]: Found primary_key collision in table TaskTreeNode value: 28 max value in memory 2194\n", - "[INFO] [1702983141.631025]: Found primary_key collision in table TaskTreeNode value: 29 max value in memory 2195\n", - "[INFO] [1702983141.632535]: Found primary_key collision in table TaskTreeNode value: 30 max value in memory 2196\n", - "[INFO] [1702983141.634051]: Found primary_key collision in table TaskTreeNode value: 31 max value in memory 2197\n", - "[INFO] [1702983141.635555]: Found primary_key collision in table TaskTreeNode value: 32 max value in memory 2198\n", - "[INFO] [1702983141.636987]: Found primary_key collision in table TaskTreeNode value: 33 max value in memory 2199\n", - "[INFO] [1702983141.638620]: Found primary_key collision in table TaskTreeNode value: 34 max value in memory 2200\n", - "[INFO] [1702983141.640110]: Found primary_key collision in table TaskTreeNode value: 35 max value in memory 2201\n", - "[INFO] [1702983141.641553]: Found primary_key collision in table TaskTreeNode value: 36 max value in memory 2202\n", - "[INFO] [1702983141.643102]: Found primary_key collision in table TaskTreeNode value: 37 max value in memory 2203\n", - "[INFO] [1702983141.644543]: Found primary_key collision in table TaskTreeNode value: 38 max value in memory 2204\n", - "[INFO] [1702983141.645966]: Found primary_key collision in table TaskTreeNode value: 39 max value in memory 2205\n", - "[INFO] [1702983141.647556]: Found primary_key collision in table TaskTreeNode value: 40 max value in memory 2206\n", - "[INFO] [1702983141.649023]: Found primary_key collision in table TaskTreeNode value: 42 max value in memory 2207\n", - "[INFO] [1702983141.650366]: Found primary_key collision in table TaskTreeNode value: 43 max value in memory 2208\n", - "[INFO] [1702983141.651679]: Found primary_key collision in table TaskTreeNode value: 44 max value in memory 2209\n", - "[INFO] [1702983141.653207]: Found primary_key collision in table TaskTreeNode value: 45 max value in memory 2210\n", - "[INFO] [1702983141.654582]: Found primary_key collision in table TaskTreeNode value: 46 max value in memory 2211\n", - "[INFO] [1702983141.656107]: Found primary_key collision in table TaskTreeNode value: 47 max value in memory 2212\n", - "[INFO] [1702983141.657613]: Found primary_key collision in table TaskTreeNode value: 48 max value in memory 2213\n", - "[INFO] [1702983141.659037]: Found primary_key collision in table TaskTreeNode value: 49 max value in memory 2214\n", - "[INFO] [1702983141.660518]: Found primary_key collision in table TaskTreeNode value: 50 max value in memory 2215\n", - "[INFO] [1702983141.662003]: Found primary_key collision in table TaskTreeNode value: 51 max value in memory 2216\n", - "[INFO] [1702983141.663416]: Found primary_key collision in table TaskTreeNode value: 52 max value in memory 2217\n", - "[INFO] [1702983141.664814]: Found primary_key collision in table TaskTreeNode value: 53 max value in memory 2218\n", - "[INFO] [1702983141.666370]: Found primary_key collision in table TaskTreeNode value: 54 max value in memory 2219\n", - "[INFO] [1702983141.667813]: Found primary_key collision in table TaskTreeNode value: 55 max value in memory 2220\n", - "[INFO] [1702983141.669267]: Found primary_key collision in table TaskTreeNode value: 56 max value in memory 2221\n", - "[INFO] [1702983141.670597]: Found primary_key collision in table TaskTreeNode value: 57 max value in memory 2222\n", - "[INFO] [1702983141.672418]: Found primary_key collision in table TaskTreeNode value: 58 max value in memory 2223\n", - "[INFO] [1702983141.673928]: Found primary_key collision in table TaskTreeNode value: 59 max value in memory 2224\n", - "[INFO] [1702983141.675638]: Found primary_key collision in table TaskTreeNode value: 62 max value in memory 2225\n", - "[INFO] [1702983141.677115]: Found primary_key collision in table TaskTreeNode value: 63 max value in memory 2226\n", - "[INFO] [1702983141.678431]: Found primary_key collision in table TaskTreeNode value: 64 max value in memory 2227\n", - "[INFO] [1702983141.679759]: Found primary_key collision in table TaskTreeNode value: 65 max value in memory 2228\n", - "[INFO] [1702983141.681187]: Found primary_key collision in table TaskTreeNode value: 24 max value in memory 2229\n", - "[INFO] [1702983141.686399]: Found primary_key collision in table TaskTreeNode value: 80 max value in memory 2230\n", - "[INFO] [1702983141.688126]: Found primary_key collision in table TaskTreeNode value: 99 max value in memory 2231\n", - "[INFO] [1702983141.690083]: Found primary_key collision in table TaskTreeNode value: 66 max value in memory 2232\n", - "[INFO] [1702983141.691794]: Found primary_key collision in table TaskTreeNode value: 67 max value in memory 2233\n", - "[INFO] [1702983141.693514]: Found primary_key collision in table TaskTreeNode value: 68 max value in memory 2234\n", - "[INFO] [1702983141.694839]: Found primary_key collision in table TaskTreeNode value: 69 max value in memory 2235\n", - "[INFO] [1702983141.696450]: Found primary_key collision in table TaskTreeNode value: 70 max value in memory 2236\n", - "[INFO] [1702983141.698000]: Found primary_key collision in table TaskTreeNode value: 71 max value in memory 2237\n", - "[INFO] [1702983141.699522]: Found primary_key collision in table TaskTreeNode value: 72 max value in memory 2238\n", - "[INFO] [1702983141.701004]: Found primary_key collision in table TaskTreeNode value: 73 max value in memory 2239\n", - "[INFO] [1702983141.702406]: Found primary_key collision in table TaskTreeNode value: 74 max value in memory 2240\n", - "[INFO] [1702983141.703714]: Found primary_key collision in table TaskTreeNode value: 75 max value in memory 2241\n", - "[INFO] [1702983141.705336]: Found primary_key collision in table TaskTreeNode value: 76 max value in memory 2242\n", - "[INFO] [1702983141.706756]: Found primary_key collision in table TaskTreeNode value: 77 max value in memory 2243\n", - "[INFO] [1702983141.708212]: Found primary_key collision in table TaskTreeNode value: 78 max value in memory 2244\n", - "[INFO] [1702983141.709671]: Found primary_key collision in table TaskTreeNode value: 79 max value in memory 2245\n", - "[INFO] [1702983141.711140]: Found primary_key collision in table TaskTreeNode value: 81 max value in memory 2246\n", - "[INFO] [1702983141.712490]: Found primary_key collision in table TaskTreeNode value: 82 max value in memory 2247\n", - "[INFO] [1702983141.713899]: Found primary_key collision in table TaskTreeNode value: 83 max value in memory 2248\n", - "[INFO] [1702983141.715286]: Found primary_key collision in table TaskTreeNode value: 84 max value in memory 2249\n", - "[INFO] [1702983141.716646]: Found primary_key collision in table TaskTreeNode value: 85 max value in memory 2250\n", - "[INFO] [1702983141.718064]: Found primary_key collision in table TaskTreeNode value: 86 max value in memory 2251\n", - "[INFO] [1702983141.719503]: Found primary_key collision in table TaskTreeNode value: 87 max value in memory 2252\n", - "[INFO] [1702983141.720819]: Found primary_key collision in table TaskTreeNode value: 88 max value in memory 2253\n", - "[INFO] [1702983141.722142]: Found primary_key collision in table TaskTreeNode value: 89 max value in memory 2254\n", - "[INFO] [1702983141.723453]: Found primary_key collision in table TaskTreeNode value: 90 max value in memory 2255\n", - "[INFO] [1702983141.724818]: Found primary_key collision in table TaskTreeNode value: 91 max value in memory 2256\n", - "[INFO] [1702983141.726517]: Found primary_key collision in table TaskTreeNode value: 92 max value in memory 2257\n", - "[INFO] [1702983141.727923]: Found primary_key collision in table TaskTreeNode value: 93 max value in memory 2258\n", - "[INFO] [1702983141.729253]: Found primary_key collision in table TaskTreeNode value: 94 max value in memory 2259\n", - "[INFO] [1702983141.730654]: Found primary_key collision in table TaskTreeNode value: 95 max value in memory 2260\n", - "[INFO] [1702983141.732088]: Found primary_key collision in table TaskTreeNode value: 96 max value in memory 2261\n", - "[INFO] [1702983141.733676]: Found primary_key collision in table TaskTreeNode value: 97 max value in memory 2262\n", - "[INFO] [1702983141.735387]: Found primary_key collision in table TaskTreeNode value: 98 max value in memory 2263\n", - "[INFO] [1702983141.737046]: Found primary_key collision in table TaskTreeNode value: 100 max value in memory 2264\n", - "[INFO] [1702983141.738800]: Found primary_key collision in table TaskTreeNode value: 101 max value in memory 2265\n", - "[INFO] [1702983141.740422]: Found primary_key collision in table TaskTreeNode value: 102 max value in memory 2266\n", - "[INFO] [1702983141.742202]: Found primary_key collision in table TaskTreeNode value: 103 max value in memory 2267\n", - "[INFO] [1702983141.743893]: Found primary_key collision in table TaskTreeNode value: 104 max value in memory 2268\n", - "[INFO] [1702983141.745732]: Found primary_key collision in table TaskTreeNode value: 105 max value in memory 2269\n", - "[INFO] [1702983141.747544]: Found primary_key collision in table TaskTreeNode value: 106 max value in memory 2270\n", - "[INFO] [1702983141.748964]: Found primary_key collision in table TaskTreeNode value: 107 max value in memory 2271\n", - "[INFO] [1702983141.750298]: Found primary_key collision in table TaskTreeNode value: 108 max value in memory 2272\n", - "[INFO] [1702983141.751606]: Found primary_key collision in table TaskTreeNode value: 109 max value in memory 2273\n", - "[INFO] [1702983141.752909]: Found primary_key collision in table TaskTreeNode value: 110 max value in memory 2274\n", - "[INFO] [1702983141.754252]: Found primary_key collision in table TaskTreeNode value: 111 max value in memory 2275\n", - "[INFO] [1702983141.755623]: Found primary_key collision in table TaskTreeNode value: 112 max value in memory 2276\n", - "[INFO] [1702983141.757048]: Found primary_key collision in table TaskTreeNode value: 113 max value in memory 2277\n", - "[INFO] [1702983141.758515]: Found primary_key collision in table TaskTreeNode value: 114 max value in memory 2278\n", - "[INFO] [1702983141.759985]: Found primary_key collision in table TaskTreeNode value: 115 max value in memory 2279\n", - "[INFO] [1702983141.761436]: Found primary_key collision in table TaskTreeNode value: 116 max value in memory 2280\n", - "[INFO] [1702983141.762921]: Found primary_key collision in table TaskTreeNode value: 117 max value in memory 2281\n" - ] - } - ], - "source": [ - "pycram.orm.utils.migrate_neems(source_session_maker,destination_session_maker)" - ] - }, - { - "cell_type": "markdown", - "id": "ce5819f4", - "metadata": {}, - "source": [ - "If the command ran successful the content of the source database should now be copied within the destination database. For example if we query for all the different meta_data, the previously defined instance come up." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "f05ead5e", - "metadata": { - "ExecuteTime": { - "end_time": "2023-12-19T10:52:27.864687278Z", - "start_time": "2023-12-19T10:52:27.860301559Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "(datetime.datetime(2023, 11, 29, 12, 28, 19, 421998), 'nleusmann', 'Unittest: Example pick and place 2', '1062bfbfa94148ed688780f8dd4abcffe469002a', 4)\n", - "(datetime.datetime(2023, 11, 29, 12, 30, 38, 751063), 'nleusmann', 'Unittest: Example pick and place 0', '1062bfbfa94148ed688780f8dd4abcffe469002a', 5)\n", - "(datetime.datetime(2023, 11, 29, 12, 30, 50, 255843), 'nleusmann', 'Unittest: Example pick and place 1', '1062bfbfa94148ed688780f8dd4abcffe469002a', 6)\n", - "(datetime.datetime(2023, 11, 29, 12, 31, 2, 61501), 'nleusmann', 'Unittest: Example pick and place 2', '1062bfbfa94148ed688780f8dd4abcffe469002a', 7)\n", - "(datetime.datetime(2023, 11, 29, 12, 21, 0, 107802), 'nleusmann', 'Not all who wander are lost', '1062bfbfa94148ed688780f8dd4abcffe469002a', 8)\n", - "(datetime.datetime(2023, 11, 29, 12, 27, 55, 788287), 'nleusmann', 'Unittest: Example pick and place 0', '1062bfbfa94148ed688780f8dd4abcffe469002a', 9)\n", - "(datetime.datetime(2023, 11, 29, 12, 28, 7, 431814), 'nleusmann', 'Unittest: Example pick and place 1', '1062bfbfa94148ed688780f8dd4abcffe469002a', 10)\n", - "(datetime.datetime(2023, 11, 29, 13, 1, 49), 'nleusmann', 'Unittest: Example pick and place 0', '1062bfbfa94148ed688780f8dd4abcffe469002a', 11)\n", - "(datetime.datetime(2023, 11, 29, 13, 2), 'nleusmann', 'Unittest: Example pick and place 1', '1062bfbfa94148ed688780f8dd4abcffe469002a', 12)\n", - "(datetime.datetime(2023, 11, 29, 13, 2, 12), 'nleusmann', 'Unittest: Example pick and place 2', '1062bfbfa94148ed688780f8dd4abcffe469002a', 13)\n", - "(datetime.datetime(2023, 11, 29, 13, 5, 29), 'nleusmann', 'Hedgehog Unittest: Example pick and place 0', '1062bfbfa94148ed688780f8dd4abcffe469002a', 14)\n", - "(datetime.datetime(2023, 11, 29, 13, 5, 40), 'nleusmann', 'Hedgehog Unittest: Example pick and place 1', '1062bfbfa94148ed688780f8dd4abcffe469002a', 15)\n", - "(datetime.datetime(2023, 11, 29, 13, 5, 52), 'nleusmann', 'Hedgehog Unittest: Example pick and place 2', '1062bfbfa94148ed688780f8dd4abcffe469002a', 16)\n", - "(datetime.datetime(2023, 11, 29, 14, 9, 42), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '1062bfbfa94148ed688780f8dd4abcffe469002a', 17)\n", - "(datetime.datetime(2023, 11, 29, 14, 9, 54), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '1062bfbfa94148ed688780f8dd4abcffe469002a', 18)\n", - "(datetime.datetime(2023, 11, 29, 14, 10, 5), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '1062bfbfa94148ed688780f8dd4abcffe469002a', 19)\n", - "(datetime.datetime(2023, 11, 30, 9, 50), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '0e658d295505ea8081ed18334dce5f9331aa0796', 20)\n", - "(datetime.datetime(2023, 11, 30, 9, 50, 11), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '0e658d295505ea8081ed18334dce5f9331aa0796', 21)\n", - "(datetime.datetime(2023, 11, 30, 9, 50, 23), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '0e658d295505ea8081ed18334dce5f9331aa0796', 22)\n", - "(datetime.datetime(2023, 11, 30, 12, 58, 57), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '0e658d295505ea8081ed18334dce5f9331aa0796', 23)\n", - "(datetime.datetime(2023, 11, 30, 12, 59, 9), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '0e658d295505ea8081ed18334dce5f9331aa0796', 24)\n", - "(datetime.datetime(2023, 11, 30, 12, 59, 21), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '0e658d295505ea8081ed18334dce5f9331aa0796', 25)\n", - "(datetime.datetime(2023, 11, 30, 14, 38, 8), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '0e658d295505ea8081ed18334dce5f9331aa0796', 26)\n", - "(datetime.datetime(2023, 11, 30, 14, 38, 19), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '0e658d295505ea8081ed18334dce5f9331aa0796', 27)\n", - "(datetime.datetime(2023, 11, 30, 14, 38, 31), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '0e658d295505ea8081ed18334dce5f9331aa0796', 28)\n", - "(datetime.datetime(2023, 12, 1, 13, 39, 10), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '04f2df4808909991521385b95d69908c943dc359', 29)\n", - "(datetime.datetime(2023, 12, 1, 13, 39, 22), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '04f2df4808909991521385b95d69908c943dc359', 30)\n", - "(datetime.datetime(2023, 12, 1, 13, 39, 34), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '04f2df4808909991521385b95d69908c943dc359', 31)\n", - "(datetime.datetime(2023, 12, 8, 13, 17, 28), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 32)\n", - "(datetime.datetime(2023, 12, 8, 13, 17, 40), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 33)\n", - "(datetime.datetime(2023, 12, 8, 13, 17, 52), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 34)\n", - "(datetime.datetime(2023, 12, 8, 13, 35, 39), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 35)\n", - "(datetime.datetime(2023, 12, 8, 13, 35, 51), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 36)\n", - "(datetime.datetime(2023, 12, 8, 13, 36, 3), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 37)\n", - "(datetime.datetime(2023, 12, 8, 13, 47, 5), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 38)\n", - "(datetime.datetime(2023, 12, 8, 13, 47, 17), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 39)\n", - "(datetime.datetime(2023, 12, 8, 13, 47, 29), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '3cde3e28da8d3f3aacaae7a5687fb15138c920c9', 40)\n", - "(datetime.datetime(2023, 12, 8, 13, 58, 44), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '18b25fd2fcc327adad7cb736fc236a973253e74d', 41)\n", - "(datetime.datetime(2023, 12, 8, 13, 58, 55), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '18b25fd2fcc327adad7cb736fc236a973253e74d', 42)\n", - "(datetime.datetime(2023, 12, 8, 13, 59, 7), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '18b25fd2fcc327adad7cb736fc236a973253e74d', 43)\n", - "(datetime.datetime(2023, 12, 8, 15, 3, 5), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '18b25fd2fcc327adad7cb736fc236a973253e74d', 44)\n", - "(datetime.datetime(2023, 12, 8, 15, 3, 17), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '18b25fd2fcc327adad7cb736fc236a973253e74d', 45)\n", - "(datetime.datetime(2023, 12, 8, 15, 3, 29), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '18b25fd2fcc327adad7cb736fc236a973253e74d', 46)\n", - "(datetime.datetime(2023, 12, 11, 11, 0, 18), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '96c94164939638dd2eda3d2bbf750dd552715d5f', 47)\n", - "(datetime.datetime(2023, 12, 11, 11, 0, 30), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '96c94164939638dd2eda3d2bbf750dd552715d5f', 48)\n", - "(datetime.datetime(2023, 12, 11, 11, 0, 42), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '96c94164939638dd2eda3d2bbf750dd552715d5f', 49)\n", - "(datetime.datetime(2023, 12, 11, 11, 4, 4), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '96c94164939638dd2eda3d2bbf750dd552715d5f', 50)\n", - "(datetime.datetime(2023, 12, 11, 11, 4, 15), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '96c94164939638dd2eda3d2bbf750dd552715d5f', 51)\n", - "(datetime.datetime(2023, 12, 11, 11, 4, 26), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '96c94164939638dd2eda3d2bbf750dd552715d5f', 52)\n", - "(datetime.datetime(2023, 12, 18, 9, 39, 36), 'nleusmann', 'Example Plan 0', 'e2e4e75b71e621a0ad569b9c828bd0b6d11abfe7', 53)\n", - "(datetime.datetime(2023, 12, 18, 9, 39, 48), 'nleusmann', 'Example Plan 1', 'e2e4e75b71e621a0ad569b9c828bd0b6d11abfe7', 54)\n", - "(datetime.datetime(2023, 12, 18, 9, 39, 59), 'nleusmann', 'Example Plan 2', 'e2e4e75b71e621a0ad569b9c828bd0b6d11abfe7', 55)\n", - "(datetime.datetime(2023, 12, 19, 10, 2, 17), 'nleusmann', 'Database merger Unittest: Example pick and place 0', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 56)\n", - "(datetime.datetime(2023, 12, 19, 10, 2, 29), 'nleusmann', 'Database merger Unittest: Example pick and place 1', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 57)\n", - "(datetime.datetime(2023, 12, 19, 10, 2, 40), 'nleusmann', 'Database merger Unittest: Example pick and place 2', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 58)\n", - "(datetime.datetime(2023, 12, 19, 10, 51, 47), 'nleusmann', 'Example Plan 0', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 1)\n", - "(datetime.datetime(2023, 12, 19, 10, 51, 58), 'nleusmann', 'Example Plan 1', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 2)\n", - "(datetime.datetime(2023, 12, 19, 10, 52, 10), 'nleusmann', 'Example Plan 2', '54c6b1241a5c305e5ccd55bbcb742e3b986dbce5', 3)\n" - ] - } - ], - "source": [ - "with destination_session_maker() as session:\n", - " statement = sqlalchemy.select('*').select_from(pycram.orm.base.ProcessMetaData)\n", - " result = session.execute(statement).all()\n", - " for item in result:\n", - " print(item)" - ] - }, - { - "cell_type": "markdown", - "id": "b0dc1a48", - "metadata": {}, - "source": [ - "Looking at all the output, we can clearly see that the PyCRORM NEEM-Hub now contains our Example Plans 0 - 2. " - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/migrate_neems.md b/examples/migrate_neems.md new file mode 100644 index 000000000..9fc8e63c5 --- /dev/null +++ b/examples/migrate_neems.md @@ -0,0 +1,138 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Migrate NEEMs + +In this tutorial we will go through the process of migrating locally stored PyCRORM NEEMs to an already existing +PyCRORM NEEM-Hub. + +In some cases it my occur that you want to record data from a pycram controlled robot locally and perform some local +actions before migrating your data to a big database server. In such cases, you can easily make a local database and +connect your pycram process to it. + +After you recorded your data locally you can migrate the data using the `migrate_neems` function. + +First, lets create an in-memory database engine called `source_engine` where we record our current process. + +```python +import sqlalchemy.orm +import pycram + +source_engine: sqlalchemy.engine.Engine +source_engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) +source_session_maker = sqlalchemy.orm.sessionmaker(bind=source_engine) +pycram.orm.base.Base.metadata.create_all(source_engine) #create all Tables +``` + +Next, create an engine called `destination_engine` for the destination database where you want to migrate your NEEMs to. +`Note:` This is just an example configuration. + +```python +destination_engine: sqlalchemy.engine.Engine +destination_engine = sqlalchemy.create_engine("postgresql+psycopg2://alice:alice123@localhost:5433/pycram", echo=False) # example values +destination_session_maker = sqlalchemy.orm.sessionmaker(bind=destination_engine) +``` + +If you already have some data in your local database you can skip the next block, otherwise we will quickly create +some example data + +```python +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.tasktree import with_tree +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.designators.object_designator import * + + +class ExamplePlans: + def __init__(self): + self.world = BulletWorld("DIRECT") + self.pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") + self.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") + self.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + self.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) + self.milk_desig = ObjectDesignatorDescription(names=["milk"]) + self.cereal_desig = ObjectDesignatorDescription(names=["cereal"]) + self.robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() + self.kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) + + @with_tree + def pick_and_place_plan(self): + with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + MoveTorsoAction([0.3]).resolve().perform() + pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() + PickUpAction(object_designator_description=self.cereal_desig, arms=[pickup_arm], + grasps=["front"]).resolve().perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + + place_island = SemanticCostmapLocation("kitchen_island_surface", self.kitchen_desig.resolve(), + self.cereal_desig.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=self.robot_desig, + reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + +``` + +```python +import pycram.orm.utils +import pycram.tasktree + +with source_session_maker() as session: + example_plans = ExamplePlans() + for i in range(3): + try: + print("ExamplePlans run {}".format(i)) + example_plans.pick_and_place_plan() + example_plans.world.reset_bullet_world() + process_meta_data = pycram.orm.base.ProcessMetaData() + process_meta_data.description = "Example Plan {}".format(i) + process_meta_data.insert(session) + pycram.tasktree.task_tree.root.insert(session) + process_meta_data.reset() + except Exception as e: + print("Error: {}\n{}".format(type(e).__name__, e)) + session.commit() + example_plans.world.exit() +``` + +Now that we have some example data or already had some example data all we need to do it migrate it over to +the already existing PyCRORM NEEM-Hub. + +```python +pycram.orm.utils.migrate_neems(source_session_maker,destination_session_maker) +``` + +If the command ran successful the content of the source database should now be copied within the destination database. For example if we query for all the different meta_data, the previously defined instance come up. + +```python +with destination_session_maker() as session: + statement = sqlalchemy.select('*').select_from(pycram.orm.base.ProcessMetaData) + result = session.execute(statement).all() + for item in result: + print(item) +``` + +Looking at all the output, we can clearly see that the PyCRORM NEEM-Hub now contains our Example Plans 0 - 2. diff --git a/examples/minimal_task_tree.ipynb b/examples/minimal_task_tree.ipynb deleted file mode 100644 index f7192595d..000000000 --- a/examples/minimal_task_tree.ipynb +++ /dev/null @@ -1,404 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# TaskTree Tutorial\n", - "\n", - "In this tutorial we will walk through the capabilities of task trees in pycram.\n", - "\n", - "First we have to import the necessary functionality from pycram." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:15.273850937Z", - "start_time": "2024-01-05T11:47:14.439086041Z" - } - }, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld\n", - "from pycram.robot_descriptions import robot_description\n", - "import pycram.tasktree\n", - "from pycram.datastructures.enums import Arms, ObjectType\n", - "from pycram.designators.action_designator import *\n", - "from pycram.designators.location_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.designators.object_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "from pycram.datastructures.enums import ObjectType\n", - "import anytree\n", - "import pycram.plan_failures" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:21.361713709Z", - "start_time": "2024-01-05T11:47:15.370855034Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "world = BulletWorld(WorldMode.GUI)\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.3, 0.7, 0.95]))\n", - "milk_desig = ObjectDesignatorDescription(names=[\"milk\"])\n", - "cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", - "robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()\n", - "kitchen_desig = ObjectDesignatorDescription(names=[\"kitchen\"])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Finally, we create a plan where the robot parks his arms, walks to the kitchen counter and picks the cereal and places it on the table. Then we execute the plan." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:35.217688853Z", - "start_time": "2024-01-05T11:47:21.361234790Z" - } - }, - "outputs": [ - { - "ename": "ROSInterruptException", - "evalue": "rospy shutdown", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mROSInterruptException\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[8], line 25\u001b[0m\n\u001b[1;32m 21\u001b[0m ParkArmsAction([Arms\u001b[38;5;241m.\u001b[39mBOTH])\u001b[38;5;241m.\u001b[39mresolve()\u001b[38;5;241m.\u001b[39mperform()\n\u001b[1;32m 23\u001b[0m ParkArmsActionPerformable(Arms\u001b[38;5;241m.\u001b[39mBOTH)\u001b[38;5;241m.\u001b[39mperform()\n\u001b[0;32m---> 25\u001b[0m \u001b[43mplan\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/tasktree.py:247\u001b[0m, in \u001b[0;36mwith_tree..handle_tree\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m 245\u001b[0m task_tree\u001b[38;5;241m.\u001b[39mstatus \u001b[38;5;241m=\u001b[39m TaskStatus\u001b[38;5;241m.\u001b[39mCREATED\n\u001b[1;32m 246\u001b[0m task_tree\u001b[38;5;241m.\u001b[39mstart_time \u001b[38;5;241m=\u001b[39m datetime\u001b[38;5;241m.\u001b[39mdatetime\u001b[38;5;241m.\u001b[39mnow()\n\u001b[0;32m--> 247\u001b[0m result \u001b[38;5;241m=\u001b[39m \u001b[43mfun\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43margs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[38;5;241;43m*\u001b[39;49m\u001b[43mkwargs\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 249\u001b[0m \u001b[38;5;66;03m# if it succeeded set the flag\u001b[39;00m\n\u001b[1;32m 250\u001b[0m task_tree\u001b[38;5;241m.\u001b[39mstatus \u001b[38;5;241m=\u001b[39m TaskStatus\u001b[38;5;241m.\u001b[39mSUCCEEDED\n", - "Cell \u001b[0;32mIn[8], line 6\u001b[0m, in \u001b[0;36mplan\u001b[0;34m()\u001b[0m\n\u001b[1;32m 4\u001b[0m ParkArmsActionPerformable(Arms\u001b[38;5;241m.\u001b[39mBOTH)\u001b[38;5;241m.\u001b[39mperform()\n\u001b[1;32m 5\u001b[0m MoveTorsoAction([\u001b[38;5;241m0.22\u001b[39m])\u001b[38;5;241m.\u001b[39mresolve()\u001b[38;5;241m.\u001b[39mperform()\n\u001b[0;32m----> 6\u001b[0m pickup_pose \u001b[38;5;241m=\u001b[39m \u001b[43mCostmapLocation\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtarget\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcereal_desig\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mresolve\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mreachable_for\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mrobot_desig\u001b[49m\u001b[43m)\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mresolve\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 7\u001b[0m pickup_arm \u001b[38;5;241m=\u001b[39m pickup_pose\u001b[38;5;241m.\u001b[39mreachable_arms[\u001b[38;5;241m0\u001b[39m]\n\u001b[1;32m 8\u001b[0m NavigateAction(target_locations\u001b[38;5;241m=\u001b[39m[pickup_pose\u001b[38;5;241m.\u001b[39mpose])\u001b[38;5;241m.\u001b[39mresolve()\u001b[38;5;241m.\u001b[39mperform()\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/designators/location_designator.py:139\u001b[0m, in \u001b[0;36mCostmapLocation.ground\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 133\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mground\u001b[39m(\u001b[38;5;28mself\u001b[39m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m Location:\n\u001b[1;32m 134\u001b[0m \u001b[38;5;250m \u001b[39m\u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 135\u001b[0m \u001b[38;5;124;03m Default specialized_designators which returns the first result from the iterator of this instance.\u001b[39;00m\n\u001b[1;32m 136\u001b[0m \n\u001b[1;32m 137\u001b[0m \u001b[38;5;124;03m :return: A resolved location\u001b[39;00m\n\u001b[1;32m 138\u001b[0m \u001b[38;5;124;03m \"\"\"\u001b[39;00m\n\u001b[0;32m--> 139\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;43mnext\u001b[39;49m\u001b[43m(\u001b[49m\u001b[38;5;28;43miter\u001b[39;49m\u001b[43m(\u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m)\u001b[49m\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/designators/location_designator.py:191\u001b[0m, in \u001b[0;36mCostmapLocation.__iter__\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 189\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m description \u001b[38;5;129;01min\u001b[39;00m RobotDescription\u001b[38;5;241m.\u001b[39mcurrent_robot_description\u001b[38;5;241m.\u001b[39mget_manipulator_chains():\n\u001b[1;32m 190\u001b[0m hand_links \u001b[38;5;241m+\u001b[39m\u001b[38;5;241m=\u001b[39m description\u001b[38;5;241m.\u001b[39mlinks\n\u001b[0;32m--> 191\u001b[0m valid, arms \u001b[38;5;241m=\u001b[39m \u001b[43mreachability_validator\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmaybe_pose\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtest_robot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtarget_pose\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 192\u001b[0m \u001b[43m \u001b[49m\u001b[43mallowed_collision\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m{\u001b[49m\u001b[43mtest_robot\u001b[49m\u001b[43m:\u001b[49m\u001b[43m \u001b[49m\u001b[43mhand_links\u001b[49m\u001b[43m}\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 193\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mreachable_arm:\n\u001b[1;32m 194\u001b[0m res \u001b[38;5;241m=\u001b[39m res \u001b[38;5;129;01mand\u001b[39;00m valid \u001b[38;5;129;01mand\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mreachable_arm \u001b[38;5;129;01min\u001b[39;00m arms\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/pose_generator_and_validator.py:204\u001b[0m, in \u001b[0;36mreachability_validator\u001b[0;34m(pose, robot, target, allowed_collision)\u001b[0m\n\u001b[1;32m 201\u001b[0m joint_state_before_ik \u001b[38;5;241m=\u001b[39m robot\u001b[38;5;241m.\u001b[39mget_positions_of_all_joints()\n\u001b[1;32m 202\u001b[0m \u001b[38;5;28;01mtry\u001b[39;00m:\n\u001b[1;32m 203\u001b[0m \u001b[38;5;66;03m# test the possible solution and apply it to the robot\u001b[39;00m\n\u001b[0;32m--> 204\u001b[0m pose, joint_states \u001b[38;5;241m=\u001b[39m \u001b[43mrequest_ik\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtarget\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mjoints\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtool_frame\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 205\u001b[0m robot\u001b[38;5;241m.\u001b[39mset_pose(pose)\n\u001b[1;32m 206\u001b[0m robot\u001b[38;5;241m.\u001b[39mset_joint_positions(joint_states)\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/external_interfaces/ik.py:172\u001b[0m, in \u001b[0;36mrequest_ik\u001b[0;34m(target_pose, robot, joints, gripper)\u001b[0m\n\u001b[1;32m 161\u001b[0m \u001b[38;5;250m\u001b[39m\u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 162\u001b[0m \u001b[38;5;124;03mTop-level method to request ik solution for a given pose. This method will check if the giskard node is running\u001b[39;00m\n\u001b[1;32m 163\u001b[0m \u001b[38;5;124;03mand if so will call the giskard service. If the giskard node is not running the kdl_ik_service will be called.\u001b[39;00m\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 169\u001b[0m \u001b[38;5;124;03m:return: A Pose at which the robt should stand as well as a dictionary of joint values\u001b[39;00m\n\u001b[1;32m 170\u001b[0m \u001b[38;5;124;03m\"\"\"\u001b[39;00m\n\u001b[1;32m 171\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m/giskard\u001b[39m\u001b[38;5;124m\"\u001b[39m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;129;01min\u001b[39;00m rosnode\u001b[38;5;241m.\u001b[39mget_node_names():\n\u001b[0;32m--> 172\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m robot\u001b[38;5;241m.\u001b[39mpose, \u001b[43mrequest_kdl_ik\u001b[49m\u001b[43m(\u001b[49m\u001b[43mtarget_pose\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mjoints\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mgripper\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 173\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m request_giskard_ik(target_pose, robot, gripper)\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/external_interfaces/ik.py:199\u001b[0m, in \u001b[0;36mrequest_kdl_ik\u001b[0;34m(target_pose, robot, joints, gripper)\u001b[0m\n\u001b[1;32m 196\u001b[0m wrist_tool_frame_offset \u001b[38;5;241m=\u001b[39m robot\u001b[38;5;241m.\u001b[39mget_transform_between_links(end_effector, gripper)\n\u001b[1;32m 197\u001b[0m target_diff \u001b[38;5;241m=\u001b[39m target_torso\u001b[38;5;241m.\u001b[39mto_transform(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mtarget\u001b[39m\u001b[38;5;124m\"\u001b[39m)\u001b[38;5;241m.\u001b[39minverse_times(wrist_tool_frame_offset)\u001b[38;5;241m.\u001b[39mto_pose()\n\u001b[0;32m--> 199\u001b[0m inv \u001b[38;5;241m=\u001b[39m \u001b[43mcall_ik\u001b[49m\u001b[43m(\u001b[49m\u001b[43mbase_link\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mend_effector\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtarget_diff\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrobot\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mjoints\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 201\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mdict\u001b[39m(\u001b[38;5;28mzip\u001b[39m(joints, inv))\n", - "File \u001b[0;32m~/workspace/ros/src/pycram-1/src/pycram/external_interfaces/ik.py:78\u001b[0m, in \u001b[0;36mcall_ik\u001b[0;34m(root_link, tip_link, target_pose, robot_object, joints)\u001b[0m\n\u001b[1;32m 75\u001b[0m ik_service \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m/kdl_ik_service/get_ik\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 77\u001b[0m rospy\u001b[38;5;241m.\u001b[39mloginfo_once(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mWaiting for IK service: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mik_service\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m---> 78\u001b[0m \u001b[43mrospy\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mwait_for_service\u001b[49m\u001b[43m(\u001b[49m\u001b[43mik_service\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 80\u001b[0m req \u001b[38;5;241m=\u001b[39m _make_request_msg(root_link, tip_link, target_pose, robot_object, joints)\n\u001b[1;32m 81\u001b[0m req\u001b[38;5;241m.\u001b[39mpose_stamped\u001b[38;5;241m.\u001b[39mheader\u001b[38;5;241m.\u001b[39mframe_id \u001b[38;5;241m=\u001b[39m root_link\n", - "File \u001b[0;32m/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py:166\u001b[0m, in \u001b[0;36mwait_for_service\u001b[0;34m(service, timeout)\u001b[0m\n\u001b[1;32m 164\u001b[0m time\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m0.3\u001b[39m)\n\u001b[1;32m 165\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m rospy\u001b[38;5;241m.\u001b[39mcore\u001b[38;5;241m.\u001b[39mis_shutdown():\n\u001b[0;32m--> 166\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m ROSInterruptException(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mrospy shutdown\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", - "\u001b[0;31mROSInterruptException\u001b[0m: rospy shutdown" - ] - } - ], - "source": [ - "@pycram.tasktree.with_tree\n", - "def plan():\n", - " with simulated_robot:\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - " MoveTorsoAction([0.22]).resolve().perform()\n", - " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", - " pickup_arm = pickup_pose.reachable_arms[0]\n", - " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", - " PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform()\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", kitchen_desig.resolve(),\n", - " cereal_desig.resolve()).resolve()\n", - "\n", - " place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve()\n", - "\n", - " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", - "\n", - " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - "\n", - "plan()\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Now we get the task tree from its module and render it. Rendering can be done with any render method described in the anytree package. We will use ascii rendering here for ease of displaying." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:35.222415842Z", - "start_time": "2024-01-05T11:47:35.217889291Z" - } - }, - "outputs": [], - "source": [ - "tt = pycram.task.task_tree\n", - "print(anytree.RenderTree(tt))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node. This is done because several, not connected, plans that get executed after each other should still appear in the task tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same tree even though they are not connected." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:49.422118950Z", - "start_time": "2024-01-05T11:47:35.221784522Z" - } - }, - "outputs": [], - "source": [ - "world.reset_bullet_world()\n", - "plan()\n", - "print(anytree.RenderTree(tt))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly reset objects are available. At the end of a with block the old state is restored. The root for such things is then called ``simulation()``." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:49.434500008Z", - "start_time": "2024-01-05T11:47:49.422026208Z" - } - }, - "outputs": [], - "source": [ - "with pycram.tasktree.SimulatedTaskTree() as stt:\n", - " print(anytree.RenderTree(pycram.task.task_tree))\n", - "print(anytree.RenderTree(pycram.task.task_tree))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we would write" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:49.438410783Z", - "start_time": "2024-01-05T11:47:49.433036544Z" - } - }, - "outputs": [], - "source": [ - "tt.root.children = (tt.root.children[0],)\n", - "print(anytree.RenderTree(tt, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree functionality. This will not append the re-execution to the task tree." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.076211143Z", - "start_time": "2024-01-05T11:47:49.437672197Z" - } - }, - "outputs": [], - "source": [ - "world.reset_world()\n", - "with simulated_robot:\n", - " [node.code.execute() for node in tt.root.leaves]\n", - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Nodes in the task tree contain additional information about the status and time of a task." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.081178616Z", - "start_time": "2024-01-05T11:47:57.077818445Z" - } - }, - "outputs": [], - "source": [ - "print(pycram.task.task_tree.children[0])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The task tree can also be reset to an empty one by invoking" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.090325704Z", - "start_time": "2024-01-05T11:47:57.081062518Z" - } - }, - "outputs": [], - "source": [ - "pycram.tasktree.reset_tree()\n", - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "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." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.130889801Z", - "start_time": "2024-01-05T11:47:57.129989892Z" - } - }, - "outputs": [], - "source": [ - "@pycram.task.with_tree\n", - "def failing_plan():\n", - " raise pycram.plan_failures.PlanFailure(\"Oopsie!\")\n", - "\n", - "try:\n", - " failing_plan()\n", - "except pycram.plan_failures.PlanFailure as e:\n", - " print(e)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can now investigate the nodes of the tree, and we will see that the tree indeed contains a failed task." - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.131271385Z", - "start_time": "2024-01-05T11:47:57.130193323Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "no_operation()\n", - "+-- failing_plan()\n", - "Code: failing_plan() \n", - " start_time: 2024-01-05 12:47:57.088480 \n", - " Status: TaskStatus.FAILED \n", - " end_time: 2024-01-05 12:47:57.089024 \n", - " \n" - ] - } - ], - "source": [ - "print(anytree.RenderTree(pycram.task.task_tree, style=anytree.render.AsciiStyle()))\n", - "print(pycram.tasktree.task_tree.children[0])" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-05T11:47:57.328473210Z", - "start_time": "2024-01-05T11:47:57.130261104Z" - } - }, - "outputs": [], - "source": [ - "world.exit()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 1 -} diff --git a/examples/minimal_task_tree.md b/examples/minimal_task_tree.md new file mode 100644 index 000000000..ff93d7680 --- /dev/null +++ b/examples/minimal_task_tree.md @@ -0,0 +1,155 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# TaskTree Tutorial + +In this tutorial we will walk through the capabilities of task trees in pycram. + +First we have to import the necessary functionality from pycram. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +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.datastructures.enums import ObjectType, WorldMode +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(WorldMode.DIRECT) +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) +milk_desig = ObjectDesignatorDescription(names=["milk"]) +cereal_desig = ObjectDesignatorDescription(names=["cereal"]) +robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() +kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) +``` + +Finally, we create a plan where the robot parks his arms, walks to the kitchen counter and picks the cereal and places it on the table. Then we execute the plan. + +```python +@pycram.tasktree.with_tree +def plan(): + with simulated_robot: + ParkArmsActionPerformable(Arms.BOTH).perform() + MoveTorsoAction([0.22]).resolve().perform() + pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + + place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_desig.resolve(), + cereal_desig.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + ParkArmsActionPerformable(Arms.BOTH).perform() + +plan() + +``` + +Now we get the task tree from its module and render it. Rendering can be done with any render method described in the anytree package. We will use ascii rendering here for ease of displaying. + +```python +tt = pycram.tasktree.task_tree +print(anytree.RenderTree(tt.root)) +``` + +As we see every task in the plan got recorded correctly. It is noticeable that the tree begins with a NoOperation node. This is done because several, not connected, plans that get executed after each other should still appear in the task tree. Hence, a NoOperation node is the root of any tree. If we re-execute the plan we would see them appear in the same tree even though they are not connected. + +```python +world.reset_current_world() +plan() +print(anytree.RenderTree(tt.root)) +``` + +Projecting a plan in a new environment with its own task tree that only exists while the projected plan is running can be done with the ``with`` keyword. When this is done, both the bullet world and task tree are saved and new, freshly reset objects are available. At the end of a with block the old state is restored. The root for such things is then called ``simulation()``. + +```python +with pycram.tasktree.SimulatedTaskTree() as stt: + print(anytree.RenderTree(pycram.tasktree.task_tree.root)) +print(anytree.RenderTree(pycram.tasktree.task_tree.root)) +``` + +Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we would write + +```python +tt.root.children = (tt.root.children[0],) +print(anytree.RenderTree(tt.root, style=anytree.render.AsciiStyle())) +``` +We can now re-execute this (modified) plan by executing the leaf in pre-ordering iteration using the anytree functionality. This will not append the re-execution to the task tree. + +```python +world.reset_world() +with simulated_robot: + [node.code.execute() for node in tt.root.leaves] +print(anytree.RenderTree(tt.root, style=anytree.render.AsciiStyle())) +``` + +Nodes in the task tree contain additional information about the status and time of a task. + +```python +print(pycram.tasktree.task_tree.root.children[0]) +``` + +The task tree can also be reset to an empty one by invoking + +```python +pycram.tasktree.task_tree.reset_tree() +print(anytree.RenderTree(pycram.tasktree.task_tree.root, 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.tasktree.with_tree +def failing_plan(): + raise pycram.plan_failures.PlanFailure("Oopsie!") + +try: + failing_plan() +except pycram.plan_failures.PlanFailure as e: + print(e) +``` + +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.tasktree.task_tree.root, style=anytree.render.AsciiStyle())) +print(pycram.tasktree.task_tree.root.children[0]) +``` + +```python +world.exit() +``` diff --git a/examples/motion_designator.ipynb b/examples/motion_designator.ipynb deleted file mode 100644 index 08735c58b..000000000 --- a/examples/motion_designator.ipynb +++ /dev/null @@ -1,563 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "f31d623d", - "metadata": {}, - "source": [ - "# Motion Designator\n", - "Motion designators are similar to action designators, but unlike action designators, motion designators represent atomic low-level motions. Motion designators only take the parameter that they should execute and not a list of possible parameters, like the other designators. Like action designators, motion designators can be performed, performing motion designator verifies the parameter and passes the designator to the respective process module. \n", - "\n", - "Since motion designators perform a motion on the robot, we need a robot which we can use. Therefore, we will create a BulletWorld as well as a PR2 robot." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "f398e610", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:00.051126267Z", - "start_time": "2024-01-29T16:46:56.312023994Z" - } - }, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "world = BulletWorld(WorldMode.GUI)\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")" - ] - }, - { - "cell_type": "markdown", - "id": "ee905321", - "metadata": {}, - "source": [ - "The following cell can be used after testing the examples, to close the BulletWorld." - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "30930ddd", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:00.051774984Z", - "start_time": "2024-01-29T16:47:00.048239557Z" - } - }, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "891ecbe3", - "metadata": {}, - "source": [ - "## Move\n", - "Move is used to let the robot drive to the given target pose. Motion designator are used in the same way as the other designator, first create a description then resolve it to the actual designator and lastly, perform the resolved designator. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "30f6ca00", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:00.576314628Z", - "start_time": "2024-01-29T16:47:00.051456576Z" - } - }, - "outputs": [], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "from pycram.designators.motion_designator import MoveMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " motion_description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", - " \n", - " motion_description.perform()" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "e5c8fb91", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:00.594527954Z", - "start_time": "2024-01-29T16:47:00.591545638Z" - } - }, - "outputs": [], - "source": [ - "world.reset_world()" - ] - }, - { - "cell_type": "markdown", - "id": "6db2d1dc", - "metadata": {}, - "source": [ - "## MoveTCP\n", - "MoveTCP is used to move the tool center point (TCP) of the given arm to the target position specified by the parameter. Like any designator we start by creating a description and then resolving and performing it." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "fd55216d", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:01.122650746Z", - "start_time": "2024-01-29T16:47:00.593841452Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import MoveTCPMotion\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import Arms\n", - "\n", - "with simulated_robot:\n", - " motion_description = MoveTCPMotion(target=Pose([0.5, 0.6, 0.6], [0, 0, 0, 1]), arm=Arms.LEFT)\n", - " \n", - " motion_description.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "51b1ea44", - "metadata": {}, - "source": [ - "## Looking\n", - "Looking motion designator adjusts the robot state such that the cameras point towards the target pose. Although this motion designator takes the target as position and orientation, in reality only the position is used. " - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "146237f1", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:01.633288773Z", - "start_time": "2024-01-29T16:47:01.135133088Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import LookingMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " motion_description = LookingMotion(target=Pose([1, 1, 1], [0, 0, 0, 1]))\n", - " \n", - " motion_description.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "0c188f24", - "metadata": {}, - "source": [ - "## Move Gripper\n", - "Move gripper moves the gripper of an arm to one of two states. The states can be ```open``` and ```close```, which open and close the gripper respectively." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "8491975c", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:02.149528442Z", - "start_time": "2024-01-29T16:47:01.647168572Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import MoveGripperMotion\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import Arms, GripperState\n", - "\n", - "with simulated_robot:\n", - " motion_description = MoveGripperMotion(motion=GripperState.OPEN, gripper=Arms.LEFT)\n", - " \n", - " motion_description.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "e230abc3", - "metadata": {}, - "source": [ - "## Detecting \n", - "This is the motion designator implementation of detecting, if an object with the given object type is in the field of view (FOV) this motion designator will return an object designator describing the object.\n", - "\n", - "Since we need an object that we can detect, we will spawn a milk for this." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "9e0bc8c9", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:02.199992775Z", - "start_time": "2024-01-29T16:47:02.150827945Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.5, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "2a3ae28d", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:03.473751479Z", - "start_time": "2024-01-29T16:47:02.201339779Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ObjectDesignatorDescription.Object(name=milk, obj_type=ObjectType.MILK, world_object=Object(_saved_states={}, \n", - "id=3, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>, pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0)\n" - ] - } - ], - "source": [ - "from pycram.designators.motion_designator import DetectingMotion, LookingMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform()\n", - " \n", - " motion_description = DetectingMotion(object_type=ObjectType.MILK)\n", - " \n", - " obj = motion_description.perform()\n", - " \n", - " print(obj)" - ] - }, - { - "cell_type": "markdown", - "id": "04010416", - "metadata": {}, - "source": [ - "## Move Arm Joints\n", - "This motion designator moves one or both arms. Movement targets are a dictionary with joint name as key and target pose as value. " - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "e3547377", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:03.979196754Z", - "start_time": "2024-01-29T16:47:03.482859314Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import MoveArmJointsMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " motion_description = MoveArmJointsMotion(right_arm_poses={\"r_shoulder_pan_joint\": -0.7})\n", - " \n", - " motion_description.perform()" - ] - }, - { - "cell_type": "markdown", - "id": "dc761408", - "metadata": {}, - "source": [ - "## World State Detecting\n", - "World state detecting is also used to detect objects, however, the object is not required to be in the FOV of the robot. As long as the object is somewhere in the belief state (BulletWorld) a resolved object designator will be returned.\n", - "\n", - "Sine we want to detect something we will spawn an object that we can detect. If you already spawned the milk from the previous example, you can skip this step." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "b408d5ee", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:04.035948673Z", - "start_time": "2024-01-29T16:47:03.980202995Z" - } - }, - "outputs": [], - "source": [ - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([-1, 0, 1]))" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "238d9bfc", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:04.540918323Z", - "start_time": "2024-01-29T16:47:04.035986657Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Object(_saved_states={}, \n", - "id=3, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719582873\n", - " nsecs: 134599447\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)\n" - ] - } - ], - "source": [ - "from pycram.designators.motion_designator import WorldStateDetectingMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK)\n", - " \n", - " obj = motion_description.perform()\n", - " \n", - " print(obj)" - ] - }, - { - "cell_type": "markdown", - "id": "f12d1a35", - "metadata": {}, - "source": [ - "## Move Joints\n", - "Move joints can move any number of joints of the robot, the designator takes two lists as parameter. The first list are the names of all joints that should be moved and the second list are the positions to which the joints should be moved." - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "c3cbeac7", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:47:05.046180891Z", - "start_time": "2024-01-29T16:47:04.548113629Z" - } - }, - "outputs": [], - "source": [ - "from pycram.designators.motion_designator import MoveJointsMotion\n", - "from pycram.process_module import simulated_robot\n", - "\n", - "with simulated_robot:\n", - " motion_description = MoveJointsMotion(names=[\"torso_lift_joint\", \"r_shoulder_pan_joint\"], positions=[0.2, -1.2])\n", - " \n", - " motion_description.perform()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/motion_designator.md b/examples/motion_designator.md new file mode 100644 index 000000000..a1eb4d8a1 --- /dev/null +++ b/examples/motion_designator.md @@ -0,0 +1,185 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Motion Designator + +Motion designators are similar to action designators, but unlike action designators, motion designators represent atomic +low-level motions. Motion designators only take the parameter that they should execute and not a list of possible +parameters, like the other designators. Like action designators, motion designators can be performed. Performing a motion +designator verifies the parameter and passes the designator to the respective process module. + +Since motion designators perform a motion on the robot, we need a robot which we can use. Therefore, we will create a +BulletWorld as well as a PR2 robot. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import ObjectType, WorldMode + +world = BulletWorld(WorldMode.GUI) +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +``` + +## Move + +Move is used to let the robot drive to the given target pose. Motion designator are used in the same way as the other +designator, first create a description then resolve it to the actual designator and lastly, perform the resolved +designator. + +```python +from pycram.datastructures.pose import Pose +from pycram.designators.motion_designator import MoveMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + motion_description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1])) + + motion_description.perform() +``` + +```python +world.reset_world() +``` + +## MoveTCP + +MoveTCP is used to move the tool center point (TCP) of the given arm to the target position specified by the parameter. +Like any designator we start by creating a description and then resolving and performing it. + +```python +from pycram.designators.motion_designator import MoveTCPMotion +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import Arms + +with simulated_robot: + motion_description = MoveTCPMotion(target=Pose([0.5, 0.6, 0.6], [0, 0, 0, 1]), arm=Arms.LEFT) + + motion_description.perform() +``` + +## Looking + +Looking motion designator adjusts the robot state such that the cameras point towards the target pose. Although this +motion designator takes the target as position and orientation, in reality only the position is used. + +```python +from pycram.designators.motion_designator import LookingMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + motion_description = LookingMotion(target=Pose([1, 1, 1], [0, 0, 0, 1])) + + motion_description.perform() +``` + +## Move Gripper + +Move gripper moves the gripper of an arm to one of two states. The states can be {attr}`~pycram.datastructures.enums.GripperState.OPEN` and {attr}`~pycram.datastructures.enums.GripperState.CLOSE`, which open +and close the gripper respectively. + +```python +from pycram.designators.motion_designator import MoveGripperMotion +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import Arms, GripperState + +with simulated_robot: + motion_description = MoveGripperMotion(motion=GripperState.OPEN, gripper=Arms.LEFT) + + motion_description.perform() +``` + +## Detecting + +This is the motion designator implementation of detecting, if an object with the given object type is in the field of +view (FOV) this motion designator will return an object designator describing the object. + +Since we need an object that we can detect, we will spawn a milk for this. + +```python +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.5, 0, 1])) +``` + +```python +from pycram.designators.motion_designator import DetectingMotion, LookingMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform() + + motion_description = DetectingMotion(object_type=ObjectType.MILK) + + obj = motion_description.perform() + + print(obj) +``` + +## Move Arm Joints + +This motion designator moves one or both arms. Movement targets are a dictionary with joint name as key and target pose +as value. + +```python +from pycram.designators.motion_designator import MoveArmJointsMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + motion_description = MoveArmJointsMotion(right_arm_poses={"r_shoulder_pan_joint": -0.7}) + + motion_description.perform() +``` + +## World State Detecting + +World state detecting is also used to detect objects, however, the object is not required to be in the FOV of the robot. +As long as the object is somewhere in the belief state (BulletWorld) a resolved object designator will be returned. + +Sine we want to detect something we will spawn an object that we can detect. If you already spawned the milk from the +previous example, you can skip this step. + +```python +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([-1, 0, 1])) +``` + +```python +from pycram.designators.motion_designator import WorldStateDetectingMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK) + + obj = motion_description.perform() + + print(obj) +``` + +## Move Joints + +Move joints can move any number of joints of the robot, the designator takes two lists as parameter. The first list are +the names of all joints that should be moved and the second list are the positions to which the joints should be moved. + +```python +from pycram.designators.motion_designator import MoveJointsMotion +from pycram.process_module import simulated_robot + +with simulated_robot: + motion_description = MoveJointsMotion(names=["torso_lift_joint", "r_shoulder_pan_joint"], positions=[0.2, -1.2]) + + motion_description.perform() +``` + +The following cell can be used after testing the examples, to close the BulletWorld. + +```python +world.exit() +``` \ No newline at end of file diff --git a/examples/object_designator.ipynb b/examples/object_designator.ipynb deleted file mode 100644 index 67ceb2d76..000000000 --- a/examples/object_designator.ipynb +++ /dev/null @@ -1,769 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "bc366b3d", - "metadata": {}, - "source": [ - "# Object Designator\n", - "Object designators are used to describe objects located in the BulletWorld or the real environment and then resolve them during runtime to concrete objects.\n", - "\n", - "Object designators are different from the Object class in bullet_world.py in the way that they just describe an object and do not create objects or provide methods to manipulate them. Nevertheless, object designators contain a reference to the BulletWorld object.\n", - "\n", - "An Object designator takes two parameters, of which at least one has to be provided. These parameters are:\n", - "\n", - " * A list of names \n", - " * A list of types \n", - " \n", - "Object Designators work similar to Location designators, they get constrains describing a set of objects and when resolved return a specific instance. \n", - "\n", - "For all following examples we need a BulletWorld, so let's create one." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "ad1de24e", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld\n", - "from pycram.world_concepts.world_object import Object\n", - "from pycram.datastructures.enums import ObjectType\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "world = BulletWorld(WorldMode.GUI)" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "8b849b07", - "metadata": {}, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "id": "4bd2dc4c", - "metadata": {}, - "source": [ - "## Believe Object\n", - "This object designator is used to describe objects that are located in the BulletWorld. So objects that are in the belief state, hence the name. In the future when there is a perception interface, there will be a ```RealObject``` description which will be used to describe objects in the real world. \n", - "\n", - "Since ```BelieveObject``` describes Objects in the BulletWorld we create a few." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "315e1ae7", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='spoon_object']/link[@name='spoon_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='spoon_object']/link[@name='spoon_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "cereal = Object(\"froot_loops\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.3, 0.9, 0.95]))\n", - "spoon = Object(\"spoon\", ObjectType.SPOON, \"spoon.stl\", pose=Pose([1.3, 1.1, 0.87]))" - ] - }, - { - "cell_type": "markdown", - "id": "88c0f513", - "metadata": {}, - "source": [ - "Now that we have objects we can create an object designator to describe them. For the start we want an object designator only describing the milk. Since all objects have unique names we can create an object designator using a list with only the name of the object. " - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "97360ab9", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BelieveObject.Object(name='milk', obj_type=, world_object=Object(_saved_states={}, \n", - "id=3, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>)\n" - ] - } - ], - "source": [ - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "object_description = BelieveObject(names=[\"milk\"])\n", - "\n", - "print(object_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "a2e7c704", - "metadata": {}, - "source": [ - "You can also use the type to describe objects, so now we want to have an object designator that describes every food in the world. " - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "f7413076", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BelieveObject.Object(name='milk', obj_type=, world_object=Object(_saved_states={}, \n", - "id=3, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>)\n" - ] - } - ], - "source": [ - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL])\n", - "\n", - "print(object_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "4f698d12", - "metadata": {}, - "source": [ - "## Object Part \n", - "Part of object designators can be used to describe something as part of another object. For example, you could describe a specific drawer as part of the kitchen. This is necessary since the drawer is no single BulletWorld Object but rather a link of the kitchen which is a BulletWorld Object.\n", - "\n", - "For this example we need just need the kitchen, if you didn't spawn it in the previous example you can spawn it with the following cell." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "93715852", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "dd812bb6", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ObjectPart.Object(name='sink_area_left_upper_drawer_main', obj_type=None, world_object=Object(_saved_states={}, \n", - "id=2, \n", - "world=, \n", - "name=kitchen, \n", - "obj_type=ObjectType.ENVIRONMENT, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583008\n", - " nsecs: 859450817\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583008\n", - " nsecs: 859450817\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/kitchen.urdf, \n", - "tf_frame=kitchen, \n", - "joint_name_to_id={'world_room_joint': 0, 'sink_area_footprint_joint': 1, 'sink_area_main_joint': 2, 'sink_area_surface_joint': 3, 'sink_area_sink_joint': 4, 'sink_area_right_panel_joint': 5, 'sink_area_trash_drawer_main_joint': 6, 'sink_area_trash_drawer_handle_joint': 7, 'sink_area_left_upper_drawer_main_joint': 8, 'sink_area_left_upper_drawer_handle_joint': 9, 'sink_area_left_middle_drawer_main_joint': 10, 'sink_area_left_middle_drawer_handle_joint': 11, 'sink_area_left_bottom_drawer_main_joint': 12, 'sink_area_left_bottom_drawer_handle_joint': 13, 'sink_area_dish_washer_main_joint': 14, 'sink_area_dish_washer_door_joint': 15, 'sink_area_dish_washer_door_handle_joint': 16, 'oven_area_footprint_joint': 17, 'oven_area_main_joint': 18, 'oven_area_oven_main_joint': 19, 'oven_area_oven_door_joint': 20, 'oven_area_oven_door_handle_joint': 21, 'oven_area_oven_panel_joint': 22, 'oven_area_oven_knob_stove_1_joint': 23, 'oven_area_oven_knob_stove_2_joint': 24, 'oven_area_oven_knob_stove_3_joint': 25, 'oven_area_oven_knob_stove_4_joint': 26, 'oven_area_oven_knob_oven_joint': 27, 'oven_area_area_middle_upper_drawer_main_joint': 28, 'oven_area_area_middle_upper_drawer_handle_joint': 29, 'oven_area_area_middle_lower_drawer_main_joint': 30, 'oven_area_area_middle_lower_drawer_handle_joint': 31, 'oven_area_area_left_drawer_main_joint': 32, 'oven_area_area_left_drawer_handle_joint': 33, 'oven_area_area_right_drawer_joint': 34, 'drawer_oven_right_front_joint': 35, 'oven_area_area_right_drawer_handle_joint': 36, 'drawer_oven_right_board0_joint': 37, 'drawer_oven_right_board0_barrier_right_joint': 38, 'drawer_oven_right_board0_barrier_left_joint': 39, 'drawer_oven_right_board1_joint': 40, 'drawer_oven_right_board1_barrier_right_joint': 41, 'drawer_oven_right_board1_barrier_left_joint': 42, 'drawer_oven_right_board2_joint': 43, 'drawer_oven_right_board2_barrier_right_joint': 44, 'drawer_oven_right_board2_barrier_left_joint': 45, 'drawer_oven_right_board3_joint': 46, 'drawer_oven_right_board3_barrier_right_joint': 47, 'drawer_oven_right_board3_barrier_left_joint': 48, 'kitchen_island_footprint_joint': 49, 'kitchen_island_joint': 50, 'kitchen_island_surface_joint': 51, 'kitchen_island_stove_joint': 52, 'kitchen_island_left_panel_joint': 53, 'kitchen_island_left_upper_drawer_main_joint': 54, 'kitchen_island_left_upper_drawer_handle_joint': 55, 'kitchen_island_left_lower_drawer_main_joint': 56, 'kitchen_island_left_lower_drawer_handle_joint': 57, 'kitchen_island_middle_panel_joint': 58, 'kitchen_island_middle_upper_drawer_main_joint': 59, 'kitchen_island_middle_upper_drawer_handle_joint': 60, 'kitchen_island_middle_lower_drawer_main_joint': 61, 'kitchen_island_middle_lower_drawer_handle_joint': 62, 'kitchen_island_right_panel_joint': 63, 'kitchen_island_right_upper_drawer_main_joint': 64, 'kitchen_island_right_upper_drawer_handle_joint': 65, 'kitchen_island_right_lower_drawer_main_joint': 66, 'kitchen_island_right_lower_drawer_handle_joint': 67, 'fridge_area_footprint_joint': 68, 'fridge_area_main_joint': 69, 'fridge_area_lower_drawer_main_joint': 70, 'fridge_area_lower_drawer_handle_joint': 71, 'iai_fridge_main_joint': 72, 'iai_fridge_door_joint': 73, 'iai_fridge_door_handle_joint': 74, 'table_area_main_joint': 75, 'walls_main_joint': 76, 'wall_1_joint': 77, 'wall_2_joint': 78, 'wall_3_joint': 79, 'wall_4_joint': 80, 'wall_5_joint': 81, 'wall_6_joint': 82}, \n", - "joint_id_to_name={0: 'world_room_joint', 1: 'sink_area_footprint_joint', 2: 'sink_area_main_joint', 3: 'sink_area_surface_joint', 4: 'sink_area_sink_joint', 5: 'sink_area_right_panel_joint', 6: 'sink_area_trash_drawer_main_joint', 7: 'sink_area_trash_drawer_handle_joint', 8: 'sink_area_left_upper_drawer_main_joint', 9: 'sink_area_left_upper_drawer_handle_joint', 10: 'sink_area_left_middle_drawer_main_joint', 11: 'sink_area_left_middle_drawer_handle_joint', 12: 'sink_area_left_bottom_drawer_main_joint', 13: 'sink_area_left_bottom_drawer_handle_joint', 14: 'sink_area_dish_washer_main_joint', 15: 'sink_area_dish_washer_door_joint', 16: 'sink_area_dish_washer_door_handle_joint', 17: 'oven_area_footprint_joint', 18: 'oven_area_main_joint', 19: 'oven_area_oven_main_joint', 20: 'oven_area_oven_door_joint', 21: 'oven_area_oven_door_handle_joint', 22: 'oven_area_oven_panel_joint', 23: 'oven_area_oven_knob_stove_1_joint', 24: 'oven_area_oven_knob_stove_2_joint', 25: 'oven_area_oven_knob_stove_3_joint', 26: 'oven_area_oven_knob_stove_4_joint', 27: 'oven_area_oven_knob_oven_joint', 28: 'oven_area_area_middle_upper_drawer_main_joint', 29: 'oven_area_area_middle_upper_drawer_handle_joint', 30: 'oven_area_area_middle_lower_drawer_main_joint', 31: 'oven_area_area_middle_lower_drawer_handle_joint', 32: 'oven_area_area_left_drawer_main_joint', 33: 'oven_area_area_left_drawer_handle_joint', 34: 'oven_area_area_right_drawer_joint', 35: 'drawer_oven_right_front_joint', 36: 'oven_area_area_right_drawer_handle_joint', 37: 'drawer_oven_right_board0_joint', 38: 'drawer_oven_right_board0_barrier_right_joint', 39: 'drawer_oven_right_board0_barrier_left_joint', 40: 'drawer_oven_right_board1_joint', 41: 'drawer_oven_right_board1_barrier_right_joint', 42: 'drawer_oven_right_board1_barrier_left_joint', 43: 'drawer_oven_right_board2_joint', 44: 'drawer_oven_right_board2_barrier_right_joint', 45: 'drawer_oven_right_board2_barrier_left_joint', 46: 'drawer_oven_right_board3_joint', 47: 'drawer_oven_right_board3_barrier_right_joint', 48: 'drawer_oven_right_board3_barrier_left_joint', 49: 'kitchen_island_footprint_joint', 50: 'kitchen_island_joint', 51: 'kitchen_island_surface_joint', 52: 'kitchen_island_stove_joint', 53: 'kitchen_island_left_panel_joint', 54: 'kitchen_island_left_upper_drawer_main_joint', 55: 'kitchen_island_left_upper_drawer_handle_joint', 56: 'kitchen_island_left_lower_drawer_main_joint', 57: 'kitchen_island_left_lower_drawer_handle_joint', 58: 'kitchen_island_middle_panel_joint', 59: 'kitchen_island_middle_upper_drawer_main_joint', 60: 'kitchen_island_middle_upper_drawer_handle_joint', 61: 'kitchen_island_middle_lower_drawer_main_joint', 62: 'kitchen_island_middle_lower_drawer_handle_joint', 63: 'kitchen_island_right_panel_joint', 64: 'kitchen_island_right_upper_drawer_main_joint', 65: 'kitchen_island_right_upper_drawer_handle_joint', 66: 'kitchen_island_right_lower_drawer_main_joint', 67: 'kitchen_island_right_lower_drawer_handle_joint', 68: 'fridge_area_footprint_joint', 69: 'fridge_area_main_joint', 70: 'fridge_area_lower_drawer_main_joint', 71: 'fridge_area_lower_drawer_handle_joint', 72: 'iai_fridge_main_joint', 73: 'iai_fridge_door_joint', 74: 'iai_fridge_door_handle_joint', 75: 'table_area_main_joint', 76: 'walls_main_joint', 77: 'wall_1_joint', 78: 'wall_2_joint', 79: 'wall_3_joint', 80: 'wall_4_joint', 81: 'wall_5_joint', 82: 'wall_6_joint'}, \n", - "link_name_to_id={'room_link': 0, 'sink_area_footprint': 1, 'sink_area': 2, 'sink_area_surface': 3, 'sink_area_sink': 4, 'sink_area_right_panel': 5, 'sink_area_trash_drawer_main': 6, 'sink_area_trash_drawer_handle': 7, 'sink_area_left_upper_drawer_main': 8, 'sink_area_left_upper_drawer_handle': 9, 'sink_area_left_middle_drawer_main': 10, 'sink_area_left_middle_drawer_handle': 11, 'sink_area_left_bottom_drawer_main': 12, 'sink_area_left_bottom_drawer_handle': 13, 'sink_area_dish_washer_main': 14, 'sink_area_dish_washer_door': 15, 'sink_area_dish_washer_door_handle': 16, 'oven_area_area_footprint': 17, 'oven_area_area': 18, 'oven_area_oven_main': 19, 'oven_area_oven_door': 20, 'oven_area_oven_door_handle': 21, 'oven_area_oven_panel': 22, 'oven_area_oven_knob_stove_1': 23, 'oven_area_oven_knob_stove_2': 24, 'oven_area_oven_knob_stove_3': 25, 'oven_area_oven_knob_stove_4': 26, 'oven_area_oven_knob_oven': 27, 'oven_area_area_middle_upper_drawer_main': 28, 'oven_area_area_middle_upper_drawer_handle': 29, 'oven_area_area_middle_lower_drawer_main': 30, 'oven_area_area_middle_lower_drawer_handle': 31, 'oven_area_area_left_drawer_main': 32, 'oven_area_area_left_drawer_handle': 33, 'oven_area_area_right_drawer_main': 34, 'drawer_oven_right_front_link': 35, 'oven_area_area_right_drawer_handle': 36, 'drawer_oven_right_board0_link': 37, 'drawer_oven_right_board0_barrier_right_link': 38, 'drawer_oven_right_board0_barrier_left_link': 39, 'drawer_oven_right_board1_link': 40, 'drawer_oven_right_board1_barrier_right_link': 41, 'drawer_oven_right_board1_barrier_left_link': 42, 'drawer_oven_right_board2_link': 43, 'drawer_oven_right_board2_barrier_right_link': 44, 'drawer_oven_right_board2_barrier_left_link': 45, 'drawer_oven_right_board3_link': 46, 'drawer_oven_right_board3_barrier_right_link': 47, 'drawer_oven_right_board3_barrier_left_link': 48, 'kitchen_island_footprint': 49, 'kitchen_island': 50, 'kitchen_island_surface': 51, 'kitchen_island_stove': 52, 'kitchen_island_left_panel': 53, 'kitchen_island_left_upper_drawer_main': 54, 'kitchen_island_left_upper_drawer_handle': 55, 'kitchen_island_left_lower_drawer_main': 56, 'kitchen_island_left_lower_drawer_handle': 57, 'kitchen_island_middle_panel': 58, 'kitchen_island_middle_upper_drawer_main': 59, 'kitchen_island_middle_upper_drawer_handle': 60, 'kitchen_island_middle_lower_drawer_main': 61, 'kitchen_island_middle_lower_drawer_handle': 62, 'kitchen_island_right_panel': 63, 'kitchen_island_right_upper_drawer_main': 64, 'kitchen_island_right_upper_drawer_handle': 65, 'kitchen_island_right_lower_drawer_main': 66, 'kitchen_island_right_lower_drawer_handle': 67, 'fridge_area_footprint': 68, 'fridge_area': 69, 'fridge_area_lower_drawer_main': 70, 'fridge_area_lower_drawer_handle': 71, 'iai_fridge_main': 72, 'iai_fridge_door': 73, 'iai_fridge_door_handle': 74, 'table_area_main': 75, 'kitchen_walls': 76, 'kitchen_wall_1': 77, 'kitchen_wall_2': 78, 'kitchen_wall_3': 79, 'kitchen_wall_4': 80, 'kitchen_wall_5': 81, 'kitchen_wall_6': 82, 'world': -1}, \n", - "link_id_to_name={0: 'room_link', 1: 'sink_area_footprint', 2: 'sink_area', 3: 'sink_area_surface', 4: 'sink_area_sink', 5: 'sink_area_right_panel', 6: 'sink_area_trash_drawer_main', 7: 'sink_area_trash_drawer_handle', 8: 'sink_area_left_upper_drawer_main', 9: 'sink_area_left_upper_drawer_handle', 10: 'sink_area_left_middle_drawer_main', 11: 'sink_area_left_middle_drawer_handle', 12: 'sink_area_left_bottom_drawer_main', 13: 'sink_area_left_bottom_drawer_handle', 14: 'sink_area_dish_washer_main', 15: 'sink_area_dish_washer_door', 16: 'sink_area_dish_washer_door_handle', 17: 'oven_area_area_footprint', 18: 'oven_area_area', 19: 'oven_area_oven_main', 20: 'oven_area_oven_door', 21: 'oven_area_oven_door_handle', 22: 'oven_area_oven_panel', 23: 'oven_area_oven_knob_stove_1', 24: 'oven_area_oven_knob_stove_2', 25: 'oven_area_oven_knob_stove_3', 26: 'oven_area_oven_knob_stove_4', 27: 'oven_area_oven_knob_oven', 28: 'oven_area_area_middle_upper_drawer_main', 29: 'oven_area_area_middle_upper_drawer_handle', 30: 'oven_area_area_middle_lower_drawer_main', 31: 'oven_area_area_middle_lower_drawer_handle', 32: 'oven_area_area_left_drawer_main', 33: 'oven_area_area_left_drawer_handle', 34: 'oven_area_area_right_drawer_main', 35: 'drawer_oven_right_front_link', 36: 'oven_area_area_right_drawer_handle', 37: 'drawer_oven_right_board0_link', 38: 'drawer_oven_right_board0_barrier_right_link', 39: 'drawer_oven_right_board0_barrier_left_link', 40: 'drawer_oven_right_board1_link', 41: 'drawer_oven_right_board1_barrier_right_link', 42: 'drawer_oven_right_board1_barrier_left_link', 43: 'drawer_oven_right_board2_link', 44: 'drawer_oven_right_board2_barrier_right_link', 45: 'drawer_oven_right_board2_barrier_left_link', 46: 'drawer_oven_right_board3_link', 47: 'drawer_oven_right_board3_barrier_right_link', 48: 'drawer_oven_right_board3_barrier_left_link', 49: 'kitchen_island_footprint', 50: 'kitchen_island', 51: 'kitchen_island_surface', 52: 'kitchen_island_stove', 53: 'kitchen_island_left_panel', 54: 'kitchen_island_left_upper_drawer_main', 55: 'kitchen_island_left_upper_drawer_handle', 56: 'kitchen_island_left_lower_drawer_main', 57: 'kitchen_island_left_lower_drawer_handle', 58: 'kitchen_island_middle_panel', 59: 'kitchen_island_middle_upper_drawer_main', 60: 'kitchen_island_middle_upper_drawer_handle', 61: 'kitchen_island_middle_lower_drawer_main', 62: 'kitchen_island_middle_lower_drawer_handle', 63: 'kitchen_island_right_panel', 64: 'kitchen_island_right_upper_drawer_main', 65: 'kitchen_island_right_upper_drawer_handle', 66: 'kitchen_island_right_lower_drawer_main', 67: 'kitchen_island_right_lower_drawer_handle', 68: 'fridge_area_footprint', 69: 'fridge_area', 70: 'fridge_area_lower_drawer_main', 71: 'fridge_area_lower_drawer_handle', 72: 'iai_fridge_main', 73: 'iai_fridge_door', 74: 'iai_fridge_door_handle', 75: 'table_area_main', 76: 'kitchen_walls', 77: 'kitchen_wall_1', 78: 'kitchen_wall_2', 79: 'kitchen_wall_3', 80: 'kitchen_wall_4', 81: 'kitchen_wall_5', 82: 'kitchen_wall_6', -1: 'world'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=kitchen, \n", - "obj_type=ObjectType.ENVIRONMENT, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583008\n", - " nsecs: 859450817\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583008\n", - " nsecs: 859450817\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/kitchen.urdf, \n", - "tf_frame=kitchen, \n", - "joint_name_to_id={'world_room_joint': 0, 'sink_area_footprint_joint': 1, 'sink_area_main_joint': 2, 'sink_area_surface_joint': 3, 'sink_area_sink_joint': 4, 'sink_area_right_panel_joint': 5, 'sink_area_trash_drawer_main_joint': 6, 'sink_area_trash_drawer_handle_joint': 7, 'sink_area_left_upper_drawer_main_joint': 8, 'sink_area_left_upper_drawer_handle_joint': 9, 'sink_area_left_middle_drawer_main_joint': 10, 'sink_area_left_middle_drawer_handle_joint': 11, 'sink_area_left_bottom_drawer_main_joint': 12, 'sink_area_left_bottom_drawer_handle_joint': 13, 'sink_area_dish_washer_main_joint': 14, 'sink_area_dish_washer_door_joint': 15, 'sink_area_dish_washer_door_handle_joint': 16, 'oven_area_footprint_joint': 17, 'oven_area_main_joint': 18, 'oven_area_oven_main_joint': 19, 'oven_area_oven_door_joint': 20, 'oven_area_oven_door_handle_joint': 21, 'oven_area_oven_panel_joint': 22, 'oven_area_oven_knob_stove_1_joint': 23, 'oven_area_oven_knob_stove_2_joint': 24, 'oven_area_oven_knob_stove_3_joint': 25, 'oven_area_oven_knob_stove_4_joint': 26, 'oven_area_oven_knob_oven_joint': 27, 'oven_area_area_middle_upper_drawer_main_joint': 28, 'oven_area_area_middle_upper_drawer_handle_joint': 29, 'oven_area_area_middle_lower_drawer_main_joint': 30, 'oven_area_area_middle_lower_drawer_handle_joint': 31, 'oven_area_area_left_drawer_main_joint': 32, 'oven_area_area_left_drawer_handle_joint': 33, 'oven_area_area_right_drawer_joint': 34, 'drawer_oven_right_front_joint': 35, 'oven_area_area_right_drawer_handle_joint': 36, 'drawer_oven_right_board0_joint': 37, 'drawer_oven_right_board0_barrier_right_joint': 38, 'drawer_oven_right_board0_barrier_left_joint': 39, 'drawer_oven_right_board1_joint': 40, 'drawer_oven_right_board1_barrier_right_joint': 41, 'drawer_oven_right_board1_barrier_left_joint': 42, 'drawer_oven_right_board2_joint': 43, 'drawer_oven_right_board2_barrier_right_joint': 44, 'drawer_oven_right_board2_barrier_left_joint': 45, 'drawer_oven_right_board3_joint': 46, 'drawer_oven_right_board3_barrier_right_joint': 47, 'drawer_oven_right_board3_barrier_left_joint': 48, 'kitchen_island_footprint_joint': 49, 'kitchen_island_joint': 50, 'kitchen_island_surface_joint': 51, 'kitchen_island_stove_joint': 52, 'kitchen_island_left_panel_joint': 53, 'kitchen_island_left_upper_drawer_main_joint': 54, 'kitchen_island_left_upper_drawer_handle_joint': 55, 'kitchen_island_left_lower_drawer_main_joint': 56, 'kitchen_island_left_lower_drawer_handle_joint': 57, 'kitchen_island_middle_panel_joint': 58, 'kitchen_island_middle_upper_drawer_main_joint': 59, 'kitchen_island_middle_upper_drawer_handle_joint': 60, 'kitchen_island_middle_lower_drawer_main_joint': 61, 'kitchen_island_middle_lower_drawer_handle_joint': 62, 'kitchen_island_right_panel_joint': 63, 'kitchen_island_right_upper_drawer_main_joint': 64, 'kitchen_island_right_upper_drawer_handle_joint': 65, 'kitchen_island_right_lower_drawer_main_joint': 66, 'kitchen_island_right_lower_drawer_handle_joint': 67, 'fridge_area_footprint_joint': 68, 'fridge_area_main_joint': 69, 'fridge_area_lower_drawer_main_joint': 70, 'fridge_area_lower_drawer_handle_joint': 71, 'iai_fridge_main_joint': 72, 'iai_fridge_door_joint': 73, 'iai_fridge_door_handle_joint': 74, 'table_area_main_joint': 75, 'walls_main_joint': 76, 'wall_1_joint': 77, 'wall_2_joint': 78, 'wall_3_joint': 79, 'wall_4_joint': 80, 'wall_5_joint': 81, 'wall_6_joint': 82}, \n", - "joint_id_to_name={0: 'world_room_joint', 1: 'sink_area_footprint_joint', 2: 'sink_area_main_joint', 3: 'sink_area_surface_joint', 4: 'sink_area_sink_joint', 5: 'sink_area_right_panel_joint', 6: 'sink_area_trash_drawer_main_joint', 7: 'sink_area_trash_drawer_handle_joint', 8: 'sink_area_left_upper_drawer_main_joint', 9: 'sink_area_left_upper_drawer_handle_joint', 10: 'sink_area_left_middle_drawer_main_joint', 11: 'sink_area_left_middle_drawer_handle_joint', 12: 'sink_area_left_bottom_drawer_main_joint', 13: 'sink_area_left_bottom_drawer_handle_joint', 14: 'sink_area_dish_washer_main_joint', 15: 'sink_area_dish_washer_door_joint', 16: 'sink_area_dish_washer_door_handle_joint', 17: 'oven_area_footprint_joint', 18: 'oven_area_main_joint', 19: 'oven_area_oven_main_joint', 20: 'oven_area_oven_door_joint', 21: 'oven_area_oven_door_handle_joint', 22: 'oven_area_oven_panel_joint', 23: 'oven_area_oven_knob_stove_1_joint', 24: 'oven_area_oven_knob_stove_2_joint', 25: 'oven_area_oven_knob_stove_3_joint', 26: 'oven_area_oven_knob_stove_4_joint', 27: 'oven_area_oven_knob_oven_joint', 28: 'oven_area_area_middle_upper_drawer_main_joint', 29: 'oven_area_area_middle_upper_drawer_handle_joint', 30: 'oven_area_area_middle_lower_drawer_main_joint', 31: 'oven_area_area_middle_lower_drawer_handle_joint', 32: 'oven_area_area_left_drawer_main_joint', 33: 'oven_area_area_left_drawer_handle_joint', 34: 'oven_area_area_right_drawer_joint', 35: 'drawer_oven_right_front_joint', 36: 'oven_area_area_right_drawer_handle_joint', 37: 'drawer_oven_right_board0_joint', 38: 'drawer_oven_right_board0_barrier_right_joint', 39: 'drawer_oven_right_board0_barrier_left_joint', 40: 'drawer_oven_right_board1_joint', 41: 'drawer_oven_right_board1_barrier_right_joint', 42: 'drawer_oven_right_board1_barrier_left_joint', 43: 'drawer_oven_right_board2_joint', 44: 'drawer_oven_right_board2_barrier_right_joint', 45: 'drawer_oven_right_board2_barrier_left_joint', 46: 'drawer_oven_right_board3_joint', 47: 'drawer_oven_right_board3_barrier_right_joint', 48: 'drawer_oven_right_board3_barrier_left_joint', 49: 'kitchen_island_footprint_joint', 50: 'kitchen_island_joint', 51: 'kitchen_island_surface_joint', 52: 'kitchen_island_stove_joint', 53: 'kitchen_island_left_panel_joint', 54: 'kitchen_island_left_upper_drawer_main_joint', 55: 'kitchen_island_left_upper_drawer_handle_joint', 56: 'kitchen_island_left_lower_drawer_main_joint', 57: 'kitchen_island_left_lower_drawer_handle_joint', 58: 'kitchen_island_middle_panel_joint', 59: 'kitchen_island_middle_upper_drawer_main_joint', 60: 'kitchen_island_middle_upper_drawer_handle_joint', 61: 'kitchen_island_middle_lower_drawer_main_joint', 62: 'kitchen_island_middle_lower_drawer_handle_joint', 63: 'kitchen_island_right_panel_joint', 64: 'kitchen_island_right_upper_drawer_main_joint', 65: 'kitchen_island_right_upper_drawer_handle_joint', 66: 'kitchen_island_right_lower_drawer_main_joint', 67: 'kitchen_island_right_lower_drawer_handle_joint', 68: 'fridge_area_footprint_joint', 69: 'fridge_area_main_joint', 70: 'fridge_area_lower_drawer_main_joint', 71: 'fridge_area_lower_drawer_handle_joint', 72: 'iai_fridge_main_joint', 73: 'iai_fridge_door_joint', 74: 'iai_fridge_door_handle_joint', 75: 'table_area_main_joint', 76: 'walls_main_joint', 77: 'wall_1_joint', 78: 'wall_2_joint', 79: 'wall_3_joint', 80: 'wall_4_joint', 81: 'wall_5_joint', 82: 'wall_6_joint'}, \n", - "link_name_to_id={'room_link': 0, 'sink_area_footprint': 1, 'sink_area': 2, 'sink_area_surface': 3, 'sink_area_sink': 4, 'sink_area_right_panel': 5, 'sink_area_trash_drawer_main': 6, 'sink_area_trash_drawer_handle': 7, 'sink_area_left_upper_drawer_main': 8, 'sink_area_left_upper_drawer_handle': 9, 'sink_area_left_middle_drawer_main': 10, 'sink_area_left_middle_drawer_handle': 11, 'sink_area_left_bottom_drawer_main': 12, 'sink_area_left_bottom_drawer_handle': 13, 'sink_area_dish_washer_main': 14, 'sink_area_dish_washer_door': 15, 'sink_area_dish_washer_door_handle': 16, 'oven_area_area_footprint': 17, 'oven_area_area': 18, 'oven_area_oven_main': 19, 'oven_area_oven_door': 20, 'oven_area_oven_door_handle': 21, 'oven_area_oven_panel': 22, 'oven_area_oven_knob_stove_1': 23, 'oven_area_oven_knob_stove_2': 24, 'oven_area_oven_knob_stove_3': 25, 'oven_area_oven_knob_stove_4': 26, 'oven_area_oven_knob_oven': 27, 'oven_area_area_middle_upper_drawer_main': 28, 'oven_area_area_middle_upper_drawer_handle': 29, 'oven_area_area_middle_lower_drawer_main': 30, 'oven_area_area_middle_lower_drawer_handle': 31, 'oven_area_area_left_drawer_main': 32, 'oven_area_area_left_drawer_handle': 33, 'oven_area_area_right_drawer_main': 34, 'drawer_oven_right_front_link': 35, 'oven_area_area_right_drawer_handle': 36, 'drawer_oven_right_board0_link': 37, 'drawer_oven_right_board0_barrier_right_link': 38, 'drawer_oven_right_board0_barrier_left_link': 39, 'drawer_oven_right_board1_link': 40, 'drawer_oven_right_board1_barrier_right_link': 41, 'drawer_oven_right_board1_barrier_left_link': 42, 'drawer_oven_right_board2_link': 43, 'drawer_oven_right_board2_barrier_right_link': 44, 'drawer_oven_right_board2_barrier_left_link': 45, 'drawer_oven_right_board3_link': 46, 'drawer_oven_right_board3_barrier_right_link': 47, 'drawer_oven_right_board3_barrier_left_link': 48, 'kitchen_island_footprint': 49, 'kitchen_island': 50, 'kitchen_island_surface': 51, 'kitchen_island_stove': 52, 'kitchen_island_left_panel': 53, 'kitchen_island_left_upper_drawer_main': 54, 'kitchen_island_left_upper_drawer_handle': 55, 'kitchen_island_left_lower_drawer_main': 56, 'kitchen_island_left_lower_drawer_handle': 57, 'kitchen_island_middle_panel': 58, 'kitchen_island_middle_upper_drawer_main': 59, 'kitchen_island_middle_upper_drawer_handle': 60, 'kitchen_island_middle_lower_drawer_main': 61, 'kitchen_island_middle_lower_drawer_handle': 62, 'kitchen_island_right_panel': 63, 'kitchen_island_right_upper_drawer_main': 64, 'kitchen_island_right_upper_drawer_handle': 65, 'kitchen_island_right_lower_drawer_main': 66, 'kitchen_island_right_lower_drawer_handle': 67, 'fridge_area_footprint': 68, 'fridge_area': 69, 'fridge_area_lower_drawer_main': 70, 'fridge_area_lower_drawer_handle': 71, 'iai_fridge_main': 72, 'iai_fridge_door': 73, 'iai_fridge_door_handle': 74, 'table_area_main': 75, 'kitchen_walls': 76, 'kitchen_wall_1': 77, 'kitchen_wall_2': 78, 'kitchen_wall_3': 79, 'kitchen_wall_4': 80, 'kitchen_wall_5': 81, 'kitchen_wall_6': 82, 'world': -1}, \n", - "link_id_to_name={0: 'room_link', 1: 'sink_area_footprint', 2: 'sink_area', 3: 'sink_area_surface', 4: 'sink_area_sink', 5: 'sink_area_right_panel', 6: 'sink_area_trash_drawer_main', 7: 'sink_area_trash_drawer_handle', 8: 'sink_area_left_upper_drawer_main', 9: 'sink_area_left_upper_drawer_handle', 10: 'sink_area_left_middle_drawer_main', 11: 'sink_area_left_middle_drawer_handle', 12: 'sink_area_left_bottom_drawer_main', 13: 'sink_area_left_bottom_drawer_handle', 14: 'sink_area_dish_washer_main', 15: 'sink_area_dish_washer_door', 16: 'sink_area_dish_washer_door_handle', 17: 'oven_area_area_footprint', 18: 'oven_area_area', 19: 'oven_area_oven_main', 20: 'oven_area_oven_door', 21: 'oven_area_oven_door_handle', 22: 'oven_area_oven_panel', 23: 'oven_area_oven_knob_stove_1', 24: 'oven_area_oven_knob_stove_2', 25: 'oven_area_oven_knob_stove_3', 26: 'oven_area_oven_knob_stove_4', 27: 'oven_area_oven_knob_oven', 28: 'oven_area_area_middle_upper_drawer_main', 29: 'oven_area_area_middle_upper_drawer_handle', 30: 'oven_area_area_middle_lower_drawer_main', 31: 'oven_area_area_middle_lower_drawer_handle', 32: 'oven_area_area_left_drawer_main', 33: 'oven_area_area_left_drawer_handle', 34: 'oven_area_area_right_drawer_main', 35: 'drawer_oven_right_front_link', 36: 'oven_area_area_right_drawer_handle', 37: 'drawer_oven_right_board0_link', 38: 'drawer_oven_right_board0_barrier_right_link', 39: 'drawer_oven_right_board0_barrier_left_link', 40: 'drawer_oven_right_board1_link', 41: 'drawer_oven_right_board1_barrier_right_link', 42: 'drawer_oven_right_board1_barrier_left_link', 43: 'drawer_oven_right_board2_link', 44: 'drawer_oven_right_board2_barrier_right_link', 45: 'drawer_oven_right_board2_barrier_left_link', 46: 'drawer_oven_right_board3_link', 47: 'drawer_oven_right_board3_barrier_right_link', 48: 'drawer_oven_right_board3_barrier_left_link', 49: 'kitchen_island_footprint', 50: 'kitchen_island', 51: 'kitchen_island_surface', 52: 'kitchen_island_stove', 53: 'kitchen_island_left_panel', 54: 'kitchen_island_left_upper_drawer_main', 55: 'kitchen_island_left_upper_drawer_handle', 56: 'kitchen_island_left_lower_drawer_main', 57: 'kitchen_island_left_lower_drawer_handle', 58: 'kitchen_island_middle_panel', 59: 'kitchen_island_middle_upper_drawer_main', 60: 'kitchen_island_middle_upper_drawer_handle', 61: 'kitchen_island_middle_lower_drawer_main', 62: 'kitchen_island_middle_lower_drawer_handle', 63: 'kitchen_island_right_panel', 64: 'kitchen_island_right_upper_drawer_main', 65: 'kitchen_island_right_upper_drawer_handle', 66: 'kitchen_island_right_lower_drawer_main', 67: 'kitchen_island_right_lower_drawer_handle', 68: 'fridge_area_footprint', 69: 'fridge_area', 70: 'fridge_area_lower_drawer_main', 71: 'fridge_area_lower_drawer_handle', 72: 'iai_fridge_main', 73: 'iai_fridge_door', 74: 'iai_fridge_door_handle', 75: 'table_area_main', 76: 'kitchen_walls', 77: 'kitchen_wall_1', 78: 'kitchen_wall_2', 79: 'kitchen_wall_3', 80: 'kitchen_wall_4', 81: 'kitchen_wall_5', 82: 'kitchen_wall_6', -1: 'world'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>, part_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 340429782\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.524999976158142\n", - " y: 0.8899999856948853\n", - " z: 0.7549999952316284\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 1.0\n", - " w: -1.0341155557737347e-13)\n" - ] - } - ], - "source": [ - "from pycram.designators.object_designator import ObjectPart, BelieveObject\n", - "\n", - "kitchen_desig = BelieveObject(names=[\"kitchen\"]).resolve()\n", - "\n", - "object_description = ObjectPart(names=[\"sink_area_left_upper_drawer_main\"], part_of=kitchen_desig)\n", - "\n", - "print(object_description.resolve())" - ] - }, - { - "cell_type": "markdown", - "id": "873ab754", - "metadata": {}, - "source": [ - "## Object Designators as Generators \n", - "Similar to location designators object designators can be used as generators to iterate through every object that they are describing. We will see this at the example of an object designator describing every type of food. \n", - "\n", - "For this we need some objects, so if you didn't already spawn them you can use the next cell for this." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "efe61fc1", - "metadata": {}, - "outputs": [], - "source": [ - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "cereal = Object(\"froot_loops\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.3, 0.9, 0.95]))\n", - "spoon = Object(\"spoon\", ObjectType.SPOON, \"spoon.stl\", pose=Pose([1.3, 1.1, 0.87]))" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "1004c440", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "BelieveObject.Object(name='milk', obj_type=, world_object=Object(_saved_states={}, \n", - "id=3, \n", - "world=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=milk, \n", - "obj_type=ObjectType.MILK, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 350784540\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 1.0\n", - " z: 0.9\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/milk.urdf, \n", - "tf_frame=milk, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'milk_main': -1}, \n", - "link_id_to_name={-1: 'milk_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>) \n", - "\n", - "BelieveObject.Object(name='froot_loops', obj_type=, world_object=Object(_saved_states={}, \n", - "id=4, \n", - "world=, \n", - "name=froot_loops, \n", - "obj_type=ObjectType.BREAKFAST_CEREAL, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 431292772\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 0.9\n", - " z: 0.95\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 431292772\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 0.9\n", - " z: 0.95\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/breakfast_cereal.urdf, \n", - "tf_frame=froot_loops, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'cereal_main': -1}, \n", - "link_id_to_name={-1: 'cereal_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...), _pose=, \n", - "name=froot_loops, \n", - "obj_type=ObjectType.BREAKFAST_CEREAL, \n", - "color=Color(R=1, G=1, B=1, A=1), \n", - "description: ..., \n", - "cache_manager=, \n", - "local_transformer=, \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 431292772\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 0.9\n", - " z: 0.95\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719583011\n", - " nsecs: 431292772\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.3\n", - " y: 0.9\n", - " z: 0.95\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/datastructures/../../../resources/cached/breakfast_cereal.urdf, \n", - "tf_frame=froot_loops, \n", - "joint_name_to_id={}, \n", - "joint_id_to_name={}, \n", - "link_name_to_id={'cereal_main': -1}, \n", - "link_id_to_name={-1: 'cereal_main'}, \n", - "links: ..., \n", - "joints: ..., \n", - "attachments: ...)>) \n", - "\n" - ] - } - ], - "source": [ - "from pycram.designators.object_designator import BelieveObject\n", - "\n", - "object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL])\n", - "\n", - "for obj in object_description:\n", - " print(obj, \"\\n\")" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/object_designator.md b/examples/object_designator.md new file mode 100644 index 000000000..db4d8645b --- /dev/null +++ b/examples/object_designator.md @@ -0,0 +1,131 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Object Designator + +Object designators are used to describe objects located in the BulletWorld or the real environment and then resolve them +during runtime to concrete objects. + +Object designators are different from the Object class in bullet_world.py in the way that they just describe an object +and do not create objects or provide methods to manipulate them. Nevertheless, object designators contain a reference to +the BulletWorld object. + +An Object designator takes two parameters, of which at least one has to be provided. These parameters are: + +* A list of names +* A list of types + +Object Designators work similar to Location designators, they get constrains describing a set of objects and when +resolved return a specific instance. + +For all following examples we need a BulletWorld, so let's create one. + +```python +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) +``` + +## Believe Object + +This object designator is used to describe objects that are located in the BulletWorld. So objects that are in the +belief state, hence the name. In the future when there is a perception interface, there will be a ```RealObject``` +description which will be used to describe objects in the real world. + +Since {meth}`~pycram.designators.object_designator.BelieveObject` describes Objects in the BulletWorld we create a few. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("froot_loops", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.9, 0.95])) +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([1.3, 1.1, 0.87])) +``` + +Now that we have objects we can create an object designator to describe them. For the start we want an object designator +only describing the milk. Since all objects have unique names we can create an object designator using a list with only +the name of the object. + +```python +from pycram.designators.object_designator import BelieveObject + +object_description = BelieveObject(names=["milk"]) + +print(object_description.resolve()) +``` + +You can also use the type to describe objects, so now we want to have an object designator that describes every food in +the world. + +```python +from pycram.designators.object_designator import BelieveObject + +object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL]) + +print(object_description.resolve()) +``` + +## Object Part + +Part of object designators can be used to describe something as part of another object. For example, you could describe +a specific drawer as part of the kitchen. This is necessary since the drawer is no single BulletWorld Object but rather +a link of the kitchen which is a BulletWorld Object. + +For this example we need just need the kitchen, if you didn't spawn it in the previous example you can spawn it with the +following cell. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +``` + +```python +from pycram.designators.object_designator import ObjectPart, BelieveObject + +kitchen_desig = BelieveObject(names=["kitchen"]).resolve() + +object_description = ObjectPart(names=["sink_area_left_upper_drawer_main"], part_of=kitchen_desig) + +print(object_description.resolve()) +``` + +## Object Designators as Generators + +Similar to location designators object designators can be used as generators to iterate through every object that they +are describing. We will see this at the example of an object designator describing every type of food. + +For this we need some objects, so if you didn't already spawn them you can use the next cell for this. + +```python +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("froot_loops", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.9, 0.95])) +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([1.3, 1.1, 0.87])) +``` + +```python +from pycram.designators.object_designator import BelieveObject + +object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL]) + +for obj in object_description: + print(obj, "\n") +``` + +To close the world use the following exit function. + +```python +world.exit() +``` diff --git a/examples/ontology.ipynb b/examples/ontology.ipynb deleted file mode 100644 index a98d4bb79..000000000 --- a/examples/ontology.ipynb +++ /dev/null @@ -1,1075 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "# Ontology interface\n", - "\n", - "This tutorial demonstrates basic usages of __owlready2__ API for ontology manipulation. Notably, new ontology concept triple classes (subject, predicate, object) will be dynamically created, with optional existing ontology parent classes that are loaded from an OWL ontology. Then through the interconnected relations specified in triples, designators and their corresponding ontology concepts can be double-way queried for input information in certain tasks, eg. making a robot motion plan. " - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.317484Z", - "start_time": "2024-04-12T11:49:12.022030Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Failed to import Giskard messages, the real robot will not be available\n", - "Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "pybullet build time: Nov 28 2023 23:51:11\n" - ] - } - ], - "source": [ - "from pathlib import Path\n", - "from typing import Type, TYPE_CHECKING\n", - "import pycram\n", - "from pycram.designator import ObjectDesignatorDescription" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "# Owlready2\n", - "\n", - "[Owlready2](https://owlready2.readthedocs.io/en/latest/intro.html) is a Python package providing a transparent access to OWL ontologies. It supports various manipulation operations, including but not limited to loading, modification, saving ontologies. Built-in supported reasoners include [HermiT](http://www.hermit-reasoner.com) and [Pellet](https://github.com/stardog-union/pellet)." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.333080Z", - "start_time": "2024-04-12T11:49:13.321031Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [], - "source": [ - "import logging\n", - "try:\n", - " import owlready2\n", - " from owlready2 import *\n", - "except ImportError:\n", - " owlready2 = None\n", - " logging.error(\"Could not import owlready2, Ontology Manager could not be initialized!\")\n", - "\n", - "logging.getLogger().setLevel(logging.INFO)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "# Ontology Manager\n", - "\n", - "`OntologyManager` is the singleton class acting as the main interface between PyCram with ontologies, whereby object instances in the former could query relevant information based on the semantic connection with their corresponding ontology concepts.\n", - "\n", - "Such connection, as represented by triples (subject-predicate-object), could be also created on the fly if not pre-existing in the loaded ontology.\n", - "\n", - "Also new and updated concepts with their properties defined in runtime could be stored into an [SQLite3 file database](https://owlready2.readthedocs.io/en/latest/world.html) for reuse.\n", - "\n", - "Here we will use [SOMA ontology](https://ease-crc.github.io/soma) as the baseline to utilize the generalized concepts provided by it." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.386628Z", - "start_time": "2024-04-12T11:49:13.333901Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911825.215667]: Ontology [http://www.ease-crc.org/ont/SOMA-HOME.owl#]'s name: SOMA-HOME has been loaded\n", - "[INFO] [1716911825.216314]: - main namespace: SOMA-HOME\n", - "[INFO] [1716911825.216700]: - loaded ontologies:\n", - "[INFO] [1716911825.216988]: http://www.ease-crc.org/ont/SOMA-HOME.owl#\n", - "[INFO] [1716911825.217239]: http://www.ease-crc.org/ont/DUL.owl#\n", - "[INFO] [1716911825.217476]: http://www.ease-crc.org/ont/SOMA.owl#\n" - ] - } - ], - "source": [ - "from pycram.ontology.ontology import OntologyManager, SOMA_HOME_ONTOLOGY_IRI\n", - "from pycram.ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder\n", - "\n", - "ontology_manager = OntologyManager(SOMA_HOME_ONTOLOGY_IRI)\n", - "main_ontology = ontology_manager.main_ontology\n", - "soma = ontology_manager.soma\n", - "dul = ontology_manager.dul" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Ontology Concept Holder\n", - "__OntologyConceptHolder__ class, encapsulating an __owlready2.Thing__ instance, is used primarily as the binding connection between the `owlready2.Thing` ontology concept to PyCram designators. We make it that way, instead of creating a custom concept class that inherits from `owlready2.Thing` for the reasons below:\n", - "\n", - "- `owlready2` API does not have very robust support for client classes to inherit from theirs with added (non-semantic) attributes, particularly in our case, where classes like `DesignatorDescription` have their `metaclass` as `ABCMeta`, while it is `EntityClass` that is the metaclass used for basically all concepts (classes, properties) in `owlready2`. Since those two metaclasses just bear no relationship, for the inheritance to work, the only way is to create a child metaclass with both of those as parents, however without full support by `owlready2`, plus the second reason below will point out it's not worth the effort.\n", - "\n", - "\n", - "- Essentially, we will have new ontology concept classes created dynamically, if their types inherit from `owlready2.Thing`, all custom non-semantic (of types known only by PyCram) attributes, which are defined by their own in child classes, will apparently be not savable into the ontology by `owlready2` api. Then the next time the ontology is loaded, those same dynamic classes will not be created anymore, thus without those attributes either, causing running error.\n", - "\n", - "As such, in short, an ontology concept class, either newly created on the fly or loaded from ontologies, has to be `owlready2.Thing` or its pure derived class (without non-semantic attributes), so to make itself reusable upon reloading.\n", - "\n", - "Notable attributes:\n", - "\n", - "- `ontology_concept`: An ontology concept of `owlready2.Thing` type or its pure child class (without custom non-semantic attributes), either dynamically created, or loaded from an ontology\n", - "\n", - "- `designators`: a list of `DesignatorDescription` instances associated with `ontology_concept`\n", - "\n", - "- `resolve`: a `Callable` typically returning a list of `DesignatorDescription` as specific designators, like `designators` or its subset, inferred from the ontology concept. In fact, it can be resolved to anything else relevant, up to the caller." - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Query ontology classes and their properties\n", - "\n", - "Classes in the loaded ontology can be queried based on their exact names, or part of them, or by namespace.\n", - "Here, we can see essential info (ancestors, super/sub-classes, properties, direct instances, etc.) of the found ontology class." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.472991Z", - "start_time": "2024-04-12T11:49:13.387739Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911825.339363]: -------------------\n", - "[INFO] [1716911825.340001]: SOMA.DesignedContainer \n", - "[INFO] [1716911825.340401]: Super classes: [DUL.DesignedArtifact, DUL.DesignedArtifact, SOMA.hasDisposition.some(SOMA.Containment), SOMA.hasDisposition.some(SOMA.Containment)]\n", - "[INFO] [1716911825.340892]: Ancestors: {SOMA.DesignedContainer, DUL.DesignedArtifact, owl.Thing, DUL.PhysicalArtifact, DUL.Object, DUL.Entity, DUL.PhysicalObject}\n", - "[INFO] [1716911825.341236]: Subclasses: [SOMA.Bottle, SOMA.Crockery, SOMA.Box, SOMA.Building, SOMA.Carafe, SOMA.Cupboard, SOMA.Dishwasher, SOMA.Dispenser, SOMA.Drawer, SOMA.Jar, SOMA.Pack, SOMA.Oven, SOMA.Shaker, SOMA.Refrigerator, SOMA.TrashContainer, SOMA-HOME.CustomContainerConcept, SOMA-HOME.AnotherCustomContainerConcept, SOMA-HOME.OntologyPlaceHolderObject, SOMA-HOME.OntologyLiquidHolderObject]\n", - "[INFO] [1716911825.341683]: Properties: [rdf-schema.isDefinedBy, rdf-schema.comment, SOMA.hasDisposition, rdf-schema.label]\n", - "[INFO] [1716911825.400233]: Instances: [SOMA-HOME.ontology_custom_container_concept, SOMA-HOME.another_custom_container_concept, SOMA-HOME.table_concept, SOMA-HOME.stool_concept, SOMA-HOME.shelf_concept, SOMA-HOME.egg_tray_concept, SOMA-HOME.cup_concept, SOMA-HOME.bowl_concept, SOMA-HOME.pitcher_concept]\n", - "[INFO] [1716911825.401077]: Direct Instances: []\n", - "[INFO] [1716911825.401662]: Inverse Restrictions: []\n", - "DUL.PhysicalObject\n", - "[SOMA.Affordance, SOMA.Disposition]\n" - ] - } - ], - "source": [ - "ontology_designed_container_class = ontology_manager.get_ontology_class('DesignedContainer')\n", - "ontology_manager.print_ontology_class(ontology_designed_container_class)\n", - "classes = ontology_manager.get_ontology_classes_by_subname('PhysicalObject'); print(classes[0])\n", - "classes = ontology_manager.get_ontology_classes_by_namespace('SOMA'); print(classes[:2])" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "__Descendants__ of an ontology class can be also queried by" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.490775Z", - "start_time": "2024-04-12T11:49:13.473745Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "[SOMA.Bottle, SOMA.DesignedContainer, SOMA.Bowl, SOMA.Crockery, SOMA.Box]" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "ontology_manager.get_ontology_descendant_classes(ontology_designed_container_class)[:5]" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Create a new ontology concept class and its individual\n", - "\n", - "A new ontology class can be created dynamically as inheriting from an existing class in the loaded ontology.\n", - "Here we create the class and its instance, also known as [__individual__](https://owlready2.readthedocs.io/en/latest/class.html#creating-equivalent-classes) in ontology terms, which is then wrapped inside an `OntologyConceptHolder`." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.496929Z", - "start_time": "2024-04-12T11:49:13.491199Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [], - "source": [ - "ontology_custom_container_class = ontology_manager.create_ontology_concept_class('CustomContainerConcept',\n", - " ontology_designed_container_class)\n", - "custom_container_concept_holder = OntologyConceptHolder(ontology_custom_container_class(name='ontology_custom_container_concept',\n", - " namespace=main_ontology))" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Access ontology concept classes and individuals\n", - "All ontology classes created on the fly purely inherit (without added non-semantic attributes) from `owlready2.Thing`, and so share the same namespace with the loaded ontology instance, `main_ontology`. They can then be accessible through that namespace by __main_ontology.__.\n", - "The same applies for individuals of those classes, accessible by __main_ontology.__" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.510281Z", - "start_time": "2024-04-12T11:49:13.497378Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911825.469569]: -------------------\n", - "[INFO] [1716911825.470353]: SOMA-HOME.CustomContainerConcept \n", - "[INFO] [1716911825.470808]: Super classes: [SOMA.DesignedContainer, owl.Thing]\n", - "[INFO] [1716911825.471233]: Ancestors: {SOMA-HOME.CustomContainerConcept, SOMA.DesignedContainer, DUL.DesignedArtifact, owl.Thing, DUL.PhysicalArtifact, DUL.Object, DUL.Entity, DUL.PhysicalObject}\n", - "[INFO] [1716911825.471560]: Subclasses: []\n", - "[INFO] [1716911825.471912]: Properties: []\n", - "[INFO] [1716911825.472469]: Instances: [SOMA-HOME.ontology_custom_container_concept]\n", - "[INFO] [1716911825.472774]: Direct Instances: [SOMA-HOME.ontology_custom_container_concept]\n", - "[INFO] [1716911825.473048]: Inverse Restrictions: []\n", - "custom_container_concept is SOMA-HOME.ontology_custom_container_concept: True\n" - ] - } - ], - "source": [ - "ontology_manager.print_ontology_class(main_ontology.CustomContainerConcept)\n", - "print(f\"custom_container_concept is {main_ontology.ontology_custom_container_concept}: {custom_container_concept_holder.ontology_concept is main_ontology.ontology_custom_container_concept}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "For ones already existing in the ontology, they can only be accessed through their corresponding ontology, eg: `soma` as follows" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.530908Z", - "start_time": "2024-04-12T11:49:13.510723Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911825.485656]: -------------------\n", - "[INFO] [1716911825.486269]: SOMA.Cup \n", - "[INFO] [1716911825.486616]: Super classes: [SOMA.Crockery, SOMA.hasPhysicalComponent.some(SOMA.DesignedHandle)]\n", - "[INFO] [1716911825.486935]: Ancestors: {SOMA.Crockery, SOMA.DesignedContainer, SOMA.Tableware, DUL.DesignedArtifact, SOMA.Cup, owl.Thing, DUL.PhysicalArtifact, SOMA.DesignedTool, DUL.Object, DUL.Entity, DUL.PhysicalObject}\n", - "[INFO] [1716911825.487221]: Subclasses: []\n", - "[INFO] [1716911825.487572]: Properties: [rdf-schema.isDefinedBy, rdf-schema.comment, SOMA.hasPhysicalComponent]\n", - "[INFO] [1716911825.488186]: Instances: []\n", - "[INFO] [1716911825.488503]: Direct Instances: []\n", - "[INFO] [1716911825.488776]: Inverse Restrictions: []\n" - ] - } - ], - "source": [ - "ontology_manager.print_ontology_class(soma.Cup)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Connect ontology class individuals with designators\n", - "After creating `custom_container_concept_holder` (wrapping `custom_container_concept` as an `owlready2.Thing`), we connect it to a designator (say `obj_designator`) by:\n", - "\n", - "- Appending to `obj_designator.ontology_concept_holders` with `custom_container_concept_holder`\n", - "\n", - "- Appending to `custom_container_concept_holder.designators` with `obj_designator`" - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.544458Z", - "start_time": "2024-04-12T11:49:13.531362Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [], - "source": [ - "custom_container_designator = ObjectDesignatorDescription(names=[\"obj\"])\n", - "custom_container_designator.ontology_concept_holders.append(custom_container_concept_holder)\n", - "custom_container_concept_holder.designators.append(custom_container_designator)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "We can also automatize all the above setup with a single function call" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.555369Z", - "start_time": "2024-04-12T11:49:13.545449Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Ontology concept: another_custom_container_concept of class SOMA-HOME.AnotherCustomContainerConcept\n", - "Designator: another_custom_container of type \n" - ] - } - ], - "source": [ - "another_custom_container_designator = ontology_manager.create_ontology_linked_designator(object_name=\"another_custom_container\",\n", - " designator_class=ObjectDesignatorDescription,\n", - " ontology_concept_name=\"AnotherCustomContainerConcept\",\n", - " ontology_parent_class=ontology_designed_container_class)\n", - "another_custom_container_concept = another_custom_container_designator.ontology_concept_holders[0].ontology_concept \n", - "print(f\"Ontology concept: {another_custom_container_concept.name} of class {type(another_custom_container_concept)}\")\n", - "another_custom_container_designator = OntologyConceptHolderStore().get_ontology_concept_holder_by_name(main_ontology.AnotherCustomContainerConcept.instances()[0].name).get_default_designator()\n", - "print(f\"Designator: {another_custom_container_designator.names[0]} of type {type(another_custom_container_designator)}\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Create new ontology triple classes\n", - "\n", - "Concept classes of a triple, aka [__subject, predicate, object__], can be created dynamically. Here we will make an example creating ones for [__handheld objects__] and [__placeholder objects__], with a pair of predicate and inverse predicate signifying their mutual relation." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.565635Z", - "start_time": "2024-04-12T11:49:13.555823Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [], - "source": [ - "PLACEABLE_ON_PREDICATE_NAME = \"placeable_on\"\n", - "HOLD_OBJ_PREDICATE_NAME = \"hold_obj\"\n", - "ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer,\n", - " subject_class_name=\"OntologyPlaceHolderObject\",\n", - " ontology_object_parent_class=soma.Shape,\n", - " object_class_name=\"OntologyHandheldObject\",\n", - " predicate_name=PLACEABLE_ON_PREDICATE_NAME,\n", - " inverse_predicate_name=HOLD_OBJ_PREDICATE_NAME,\n", - " ontology_property_parent_class=soma.affordsBearer,\n", - " ontology_inverse_property_parent_class=soma.isBearerAffordedBy)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "There, we use `soma.DesignedContainer` & `soma.Shape`, existing concept in SOMA ontology, as the parent classes for the subject & object concepts respectively.\n", - "There is also a note that those classes will have the same namespace with `main_ontology`, so later on to be accessible through it.\n", - "\n", - "Then now we define some instances of the newly created triple classes, and link them to object designators, again using `ontology_manager.create_ontology_linked_designator()`" - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.586549Z", - "start_time": "2024-04-12T11:49:13.566094Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [], - "source": [ - "def create_ontology_handheld_object_designator(object_name: str, ontology_parent_class: Type[owlready2.Thing]):\n", - " return ontology_manager.create_ontology_linked_designator(object_name=object_name,\n", - " designator_class=ObjectDesignatorDescription,\n", - " ontology_concept_name=f\"Onto{object_name}\",\n", - " ontology_parent_class=ontology_parent_class)\n", - "# Holdable Objects\n", - "cookie_box = create_ontology_handheld_object_designator(\"cookie_box\", main_ontology.OntologyHandheldObject)\n", - "egg = create_ontology_handheld_object_designator(\"egg\", main_ontology.OntologyHandheldObject)\n", - " \n", - "# Placeholder objects\n", - "placeholders = [create_ontology_handheld_object_designator(object_name, main_ontology.OntologyPlaceHolderObject)\n", - " for object_name in ['table', 'stool', 'shelf']]\n", - "\n", - "egg_tray = create_ontology_handheld_object_designator(\"egg_tray\", main_ontology.OntologyPlaceHolderObject)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "### Create ontology relations\n", - "\n", - "Now we will create ontology relations or predicates between __placeholder objects__ and __handheld objects__ with `ontology_manager.set_ontology_relation()`" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.596771Z", - "start_time": "2024-04-12T11:49:13.587021Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 13, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "for place_holder in placeholders:\n", - " ontology_manager.set_ontology_relation(subject_designator=cookie_box, object_designator=place_holder,\n", - " predicate_name=PLACEABLE_ON_PREDICATE_NAME)\n", - "\n", - "ontology_manager.set_ontology_relation(subject_designator=egg_tray, object_designator=egg,\n", - " predicate_name=HOLD_OBJ_PREDICATE_NAME)" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Query designators based on their ontology-concept relations\n", - "\n", - "Now we can make queries for designators from designators, based on the relation among their corresponding ontology concepts setup above" - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.611368Z", - "start_time": "2024-04-12T11:49:13.597218Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "['cookie_box']'s placeholder candidates: [['table'], ['stool'], ['shelf']]\n", - "['egg']'s placeholder candidates: [['egg_tray']]\n", - "['table'] can hold: [['cookie_box']]\n", - "['stool'] can hold: [['cookie_box']]\n", - "['shelf'] can hold: [['cookie_box']]\n", - "['egg_tray'] can hold: [['egg']]\n" - ] - } - ], - "source": [ - "print(f\"{cookie_box.names}'s placeholder candidates:\",\n", - " f\"\"\"{[placeholder.names for placeholder in\n", - " ontology_manager.get_designators_by_subject_predicate(subject=cookie_box,\n", - " predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}\"\"\")\n", - "\n", - "print(f\"{egg.names}'s placeholder candidates:\",\n", - " f\"\"\"{[placeholder.names for placeholder in\n", - " ontology_manager.get_designators_by_subject_predicate(subject=egg,\n", - " predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}\"\"\")\n", - "\n", - "for place_holder in placeholders:\n", - " print(f\"{place_holder.names} can hold:\",\n", - " f\"\"\"{[placeholder.names for placeholder in\n", - " ontology_manager.get_designators_by_subject_predicate(subject=place_holder,\n", - " predicate_name=HOLD_OBJ_PREDICATE_NAME)]}\"\"\")\n", - "\n", - "print(f\"{egg_tray.names} can hold:\",\n", - " f\"\"\"{[placeholder.names for placeholder in\n", - " ontology_manager.get_designators_by_subject_predicate(subject=egg_tray,\n", - " predicate_name=HOLD_OBJ_PREDICATE_NAME)]}\"\"\")" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "# Practical examples\n", - "\n", - "## Example 1\n", - "How about creating ontology concept classes encapsulating `pycram.datastructures.enums.ObjectType`? We can do it by:" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:13.621797Z", - "start_time": "2024-04-12T11:49:13.611864Z" - }, - "jupyter": { - "outputs_hidden": false - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "GenericEdible object types:\n", - "SOMA-HOME.milk_concept [['milk']]\n", - "SOMA-HOME.breakfast_cereal_concept [['breakfast_cereal']]\n" - ] - } - ], - "source": [ - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "# Create a generic ontology concept class for edible objects\n", - "generic_edible_class = ontology_manager.create_ontology_concept_class('GenericEdible')\n", - "\n", - "# Create a list of object designators sharing the same concept class as [generic_edible_class]\n", - "edible_obj_types = [ObjectType.MILK, ObjectType.BREAKFAST_CEREAL]\n", - "for object_type in ObjectType:\n", - " if object_type in edible_obj_types:\n", - " # Create a designator for the edible object\n", - " ontology_manager.create_ontology_object_designator_from_type(object_type, generic_edible_class)\n", - "\n", - "print(f'{generic_edible_class.name} object types:')\n", - "for edible_ontology_concept in generic_edible_class.direct_instances():\n", - " print(edible_ontology_concept,\n", - " [des.types for des in OntologyConceptHolderStore().get_ontology_concept_holder_by_name(edible_ontology_concept.name).designators])\n" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "jupyter": { - "outputs_hidden": false - } - }, - "source": [ - "## Example 2\n", - "We could also make use of relations between ontology concepts that designators are associated with, to enable more abstract inputs in robot motion plans.\n", - "\n", - "In a similar style to the scenario of __placeholder objects__ and __handheld objects__ above, but with a bit difference, we will ask the robot to query which content holders (eg. cup, pitcher, bowl) whereby a milk box could be pourable into.\n", - "\n", - "Basically, we will provide an ontology-based implementation for the query:\n", - " \n", - "`abstract_ontology_concept -> specific_objects_in_world?`\n", - "\n", - "To achieve it, we will create triple classes and configure a customized `resolve()` for the abstract concept, which returns its associated specific designators.\n", - "These designators are then used to again resolve for the target objects of interest, which become the inputs to a robot motion plan.\n", - "\n", - "### Setup simulated environment" - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.414260Z", - "start_time": "2024-04-12T11:49:13.622258Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Scalar element defined multiple times: limit\n" - ] - } - ], - "source": [ - "from pycram.worlds.bullet_world import BulletWorld, Object\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.designators.action_designator import *\n", - "from pycram.designators.location_designator import *\n", - "\n", - "world = BulletWorld()\n", - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "kitchen_designator = ObjectDesignatorDescription(names=[\"kitchen\"])\n", - "robot_designator = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Create PourableObject-LiquidHolder triple ontology classes" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.501124Z", - "start_time": "2024-04-12T11:49:14.415241Z" - } - }, - "outputs": [], - "source": [ - "POURABLE_INTO_PREDICATE_NAME = \"pourable_into\"\n", - "HOLD_LIQUID_PREDICATE_NAME = \"hold_liquid\"\n", - "ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer,\n", - " subject_class_name=\"OntologyLiquidHolderObject\",\n", - " ontology_object_parent_class=soma.Shape,\n", - " object_class_name=\"OntologyPourableObject\",\n", - " predicate_name=POURABLE_INTO_PREDICATE_NAME,\n", - " inverse_predicate_name=HOLD_LIQUID_PREDICATE_NAME,\n", - " ontology_property_parent_class=soma.affordsBearer,\n", - " ontology_inverse_property_parent_class=soma.isBearerAffordedBy)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Spawn a pourable object & liquid holders into the world and Create their designators" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.583818Z", - "start_time": "2024-04-12T11:49:14.501719Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cup_object']/link[@name='cup_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='bowl_object']/link[@name='bowl_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cup_object']/link[@name='cup_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='bowl_object']/link[@name='bowl_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='pitcher_object']/link[@name='pitcher_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "# Holdable obj\n", - "milk_box = Object(\"milk_box\", ObjectType.MILK, \"milk.stl\")\n", - "milk_box_designator = create_ontology_handheld_object_designator(milk_box.name, main_ontology.OntologyPourableObject)\n", - "\n", - "# Liquid-holders\n", - "cup = Object(\"cup\", ObjectType.JEROEN_CUP, \"jeroen_cup.stl\", pose=Pose([1.4, 1, 0.9]))\n", - "bowl = Object(\"bowl\", ObjectType.BOWL, \"bowl.stl\", pose=Pose([1.4, 0.5, 0.9]))\n", - "pitcher = Object(\"pitcher\", ObjectType.GENERIC_OBJECT, \"Static_MilkPitcher.stl\", pose=Pose([1.4, 0, 0.9]))\n", - "milk_holders = [cup, bowl, pitcher]\n", - "milk_holder_designators = [create_ontology_handheld_object_designator(obj.name, main_ontology.OntologyLiquidHolderObject)\n", - " for obj in milk_holders]" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Create an ontology relation between the designators of the pourable object & its liquid holders" - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.602526Z", - "start_time": "2024-04-12T11:49:14.584461Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown tag \"rgba_color\" in /robot[@name='pitcher_object']/link[@name='pitcher_main']/visual[1]/material[@name='white']\n" - ] - } - ], - "source": [ - "for milk_holder_desig in milk_holder_designators:\n", - " ontology_manager.set_ontology_relation(subject_designator=milk_box_designator, object_designator=milk_holder_desig,\n", - " predicate_name=POURABLE_INTO_PREDICATE_NAME)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Set up `resolve` for the ontology concept of the pourable object" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.612923Z", - "start_time": "2024-04-12T11:49:14.603173Z" - } - }, - "outputs": [], - "source": [ - "milk_box_concept_holder = milk_box_designator.ontology_concept_holders[0]\n", - "def milk_box_concept_resolve(): \n", - " object_designator = ontology_manager.get_designators_by_subject_predicate(subject=milk_box_designator, predicate_name=POURABLE_INTO_PREDICATE_NAME)[0]\n", - " return object_designator, object_designator.resolve()\n", - "\n", - "milk_box_concept_holder.resolve = milk_box_concept_resolve" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Here, for demonstration purpose only, we specify the resolving result by `milk_box_concept_holder` as `cup`, the first-registered (default) pourable-into target milk holder, utilizing the ontology relation setup above.\n", - "\n", - "Now, we can query the milk box's target liquid holder by resolving `milk_box_concept_holder`" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:14.627982Z", - "start_time": "2024-04-12T11:49:14.613382Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Pickup target object: cup, a content holder for ['milk_box'] as in relation `pourable_into`\n" - ] - } - ], - "source": [ - "target_milk_holder_designator, target_milk_holder = milk_box_concept_holder.resolve()\n", - "print(f\"Pickup target object: {target_milk_holder.name}, a content holder for {milk_box_designator.names} as in relation `{POURABLE_INTO_PREDICATE_NAME}`\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Robot picks up the target liquid holder" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:23.283909Z", - "start_time": "2024-04-12T11:49:14.628600Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911828.077699]: Waiting for IK service: /pr2_left_arm_kinematics/get_ik\n", - "CostmapLocation.Location(pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1716911828\n", - " nsecs: 58246850\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6399999999999999\n", - " y: 1.24\n", - " z: 0.0\n", - " orientation: \n", - " x: -0.0\n", - " y: 0.0\n", - " z: 0.15234391170286138\n", - " w: -0.988327543159185, reachable_arms=['left', 'right']) left\n" - ] - } - ], - "source": [ - "with simulated_robot:\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " MoveTorsoAction([0.3]).resolve().perform()\n", - "\n", - " pickup_pose = CostmapLocation(target=target_milk_holder, reachable_for=robot_designator).resolve()\n", - " pickup_arm = pickup_pose.reachable_arms[0]\n", - "\n", - " print(pickup_pose, pickup_arm)\n", - "\n", - " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", - "\n", - " PickUpAction(object_designator_description=target_milk_holder_designator, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", kitchen_designator.resolve(), target_milk_holder_designator.resolve()).resolve()\n", - "\n", - " place_stand = CostmapLocation(place_island.pose, reachable_for=robot_designator, reachable_arm=pickup_arm).resolve()\n", - "\n", - " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", - "\n", - " PlaceAction(target_milk_holder_designator, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", - "\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Save ontologies to an OWL file\n", - "After all the above operations on our ontologies, we now can save them to an OWL file on disk" - ] - }, - { - "cell_type": "code", - "execution_count": 23, - "metadata": { - "ExecuteTime": { - "end_time": "2024-04-12T11:49:23.337691Z", - "start_time": "2024-04-12T11:49:23.286322Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1716911835.388276]: Ontologies have been saved to /home/ducthan/ontologies/NewSOMA-HOME.owl\n" - ] - }, - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 23, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "ontology_manager.save(f\"{Path.home()}/ontologies/New{main_ontology.name}.owl\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Optimize ontology loading with SQLite3\n", - "Upon the initial ontology loading from OWL, an SQLite3 file is automatically created, acting as the quadstore cache for the loaded ontologies. This allows them to be __selectively__ reusable the next time being loaded.\n", - "More info can be referenced [here](https://owlready2.readthedocs.io/en/latest/world.html). " - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/examples/ontology.md b/examples/ontology.md new file mode 100644 index 000000000..1c93353ab --- /dev/null +++ b/examples/ontology.md @@ -0,0 +1,517 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.2 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + + + +# Ontology interface + +This tutorial demonstrates basic usages of __owlready2__ API for ontology manipulation. Notably, new ontology concept +triple classes (subject, predicate, object) will be dynamically created, with optional existing ontology parent classes +that are loaded from an OWL ontology. Then through the interconnected relations specified in triples, designators and +their corresponding ontology concepts can be double-way queried for input information in certain tasks, eg. making a +robot motion plan. + + +```python jupyter={"outputs_hidden": false} +from pathlib import Path +from typing import Type +from pycram.designator import ObjectDesignatorDescription +``` + + + +# Owlready2 + +[Owlready2](https://owlready2.readthedocs.io/en/latest/intro.html) is a Python package providing a transparent access to +OWL ontologies. It supports various manipulation operations, including but not limited to loading, modification, saving +ontologies. Built-in supported reasoners include [HermiT](http://www.hermit-reasoner.com) +and [Pellet](https://github.com/stardog-union/pellet). + + +```python jupyter={"outputs_hidden": false} +import logging + +try: + from owlready2 import * +except ImportError: + owlready2 = None + logging.error("Could not import owlready2, Ontology Manager could not be initialized!") + +logging.getLogger().setLevel(logging.INFO) +``` + + + +# Ontology Manager + +{class}`~pycram.ontology.ontology.OntologyManager` is the singleton class acting as the main interface between PyCram with ontologies, whereby object +instances in the former could query relevant information based on the semantic connection with their corresponding +ontology concepts. + +Such connection, as represented by triples (subject-predicate-object), could be also created on the fly if not +pre-existing in the loaded ontology. + +Also new and updated concepts with their properties defined in runtime could be stored into +an [SQLite3 file database](https://owlready2.readthedocs.io/en/latest/world.html) for reuse. + +Here we will use [SOMA ontology](https://ease-crc.github.io/soma) as the baseline to utilize the generalized concepts +provided by it. + + +```python jupyter={"outputs_hidden": false} +from pycram.ontology.ontology import OntologyManager, SOMA_HOME_ONTOLOGY_IRI +from pycram.ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder + +ontology_manager = OntologyManager(SOMA_HOME_ONTOLOGY_IRI) +main_ontology = ontology_manager.main_ontology +soma = ontology_manager.soma +dul = ontology_manager.dul +``` + +[General class axioms](https://owlready2.readthedocs.io/en/latest/general_class_axioms.html) of the loaded ontologies +can be queried by + +```python +print(f"{main_ontology.name}: ", ontology_manager.get_ontology_general_class_axioms(main_ontology)) +print(f"{soma.name}: ", ontology_manager.get_ontology_general_class_axioms(soma)) +print(f"{dul.name}: ", ontology_manager.get_ontology_general_class_axioms(dul)) +``` + + + +## Ontology Concept Holder + +__OntologyConceptHolder__ class, encapsulating an __owlready2.Thing__ instance, is used primarily as the binding +connection between the `owlready2.Thing` ontology concept to PyCram designators. We make it that way, instead of +creating a custom concept class that inherits from `owlready2.Thing` for the reasons below: + +- `owlready2` API does not have very robust support for client classes to inherit from theirs with added (non-semantic) + attributes, particularly in our case, where classes like {class}`~pycram.designator.DesignatorDescription` have their `metaclass` as `ABCMeta`, + while it is `EntityClass` that is the metaclass used for basically all concepts (classes, properties) in `owlready2`. + Since those two metaclasses just bear no relationship, for the inheritance to work, the only way is to create a child + metaclass with both of those as parents, however without full support by `owlready2`, plus the second reason below + will point out it's not worth the effort. + + +- Essentially, we will have new ontology concept classes created dynamically, if their types inherit + from `owlready2.Thing`, all custom non-semantic (of types known only by PyCram) attributes, which are defined by their + own in child classes, will apparently be not savable into the ontology by `owlready2` api. Then the next time the + ontology is loaded, those same dynamic classes will not be created anymore, thus without those attributes either, + causing running error. + +As such, in short, an ontology concept class, either newly created on the fly or loaded from ontologies, has to +be `owlready2.Thing` or its pure derived class (without non-semantic attributes), so to make itself reusable upon +reloading. + +Notable attributes: + +- `ontology_concept`: An ontology concept of `owlready2.Thing` type or its pure child class (without custom non-semantic + attributes), either dynamically created, or loaded from an ontology + +- `designators`: a list of `DesignatorDescription` instances associated with `ontology_concept` + +- `resolve`: a `Callable` typically returning a list of `DesignatorDescription` as specific designators, + like `designators` or its subset, inferred from the ontology concept. In fact, it can be resolved to anything else + relevant, up to the caller. + + + + + +## Query ontology classes and their properties + +Classes in the loaded ontology can be queried based on their exact names, or part of them, or by namespace. +Here, we can see essential info (ancestors, super/sub-classes, properties, direct instances, etc.) of the found ontology +class. + + +```python jupyter={"outputs_hidden": false} +ontology_designed_container_class = ontology_manager.get_ontology_class('DesignedContainer') +ontology_manager.print_ontology_class(ontology_designed_container_class) +classes = ontology_manager.get_ontology_classes_by_subname('PhysicalObject'); +print(classes[0]) +classes = ontology_manager.get_ontology_classes_by_namespace('SOMA'); +print(classes[:2]) +``` + + +__Descendants__ of an ontology class can be also queried by + + +```python jupyter={"outputs_hidden": false} +ontology_manager.get_ontology_descendant_classes(ontology_designed_container_class)[:5] +``` + + + +## Create a new ontology concept class and its individual + +A new ontology class can be created dynamically as inheriting from an existing class in the loaded ontology. +Here we create the class and its instance, also known as [__individual +__](https://owlready2.readthedocs.io/en/latest/class.html#creating-equivalent-classes) in ontology terms, which is then +wrapped inside an {class}`~pycram.ontology.ontology_common.OntologyConceptHolder`. + + +```python jupyter={"outputs_hidden": false} +ontology_custom_container_class = ontology_manager.create_ontology_concept_class('CustomContainerConcept', + ontology_designed_container_class) +custom_container_concept_holder = OntologyConceptHolder( + ontology_custom_container_class(name='ontology_custom_container_concept', + namespace=main_ontology)) +``` + + + +## Access ontology concept classes and individuals + +All ontology classes created on the fly purely inherit (without added non-semantic attributes) from `owlready2.Thing`, +and so share the same namespace with the loaded ontology instance, `main_ontology`. They can then be accessible through +that namespace by __main_ontology.__. +The same applies for individuals of those classes, accessible by __main_ontology.__ + + +```python jupyter={"outputs_hidden": false} +ontology_manager.print_ontology_class(main_ontology.CustomContainerConcept) +print( + f"custom_container_concept is {main_ontology.ontology_custom_container_concept}: {custom_container_concept_holder.ontology_concept is main_ontology.ontology_custom_container_concept}") +``` + + +For ones already existing in the ontology, they can only be accessed through their corresponding ontology, eg: `soma` as +follows + + +```python jupyter={"outputs_hidden": false} +ontology_manager.print_ontology_class(soma.Cup) +``` + + + +## Connect ontology class individuals with designators + +After creating `custom_container_concept_holder` (wrapping `custom_container_concept` as an `owlready2.Thing`), we +connect it to a designator (say `obj_designator`) by: + +- Appending to `obj_designator.ontology_concept_holders` with `custom_container_concept_holder` + +- Appending to `custom_container_concept_holder.designators` with `obj_designator` + + + +```python jupyter={"outputs_hidden": false} +custom_container_designator = ObjectDesignatorDescription(names=["obj"]) +custom_container_designator.ontology_concept_holders.append(custom_container_concept_holder) +custom_container_concept_holder.designators.append(custom_container_designator) +``` + + +We can also automatize all the above setup with a single function call + + +```python jupyter={"outputs_hidden": false} +another_custom_container_designator = ontology_manager.create_ontology_linked_designator( + object_name="another_custom_container", + designator_class=ObjectDesignatorDescription, + ontology_concept_name="AnotherCustomContainerConcept", + ontology_parent_class=ontology_designed_container_class) +another_custom_container_concept = another_custom_container_designator.ontology_concept_holders[0].ontology_concept +print(f"Ontology concept: {another_custom_container_concept.name} of class {type(another_custom_container_concept)}") +another_custom_container_designator = OntologyConceptHolderStore().get_ontology_concept_holder_by_name( + main_ontology.AnotherCustomContainerConcept.instances()[0].name).get_default_designator() +print(f"Designator: {another_custom_container_designator.names[0]} of type {type(another_custom_container_designator)}") +``` + + + +## Create new ontology triple classes + +Concept classes of a triple, aka [__subject, predicate, object__], can be created dynamically. Here we will make an +example creating ones for [__handheld objects__] and [__placeholder objects__], with a pair of predicate and inverse +predicate signifying their mutual relation. + + +```python jupyter={"outputs_hidden": false} +PLACEABLE_ON_PREDICATE_NAME = "placeable_on" +HOLD_OBJ_PREDICATE_NAME = "hold_obj" +ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer, + subject_class_name="OntologyPlaceHolderObject", + ontology_object_parent_class=soma.Shape, + object_class_name="OntologyHandheldObject", + predicate_class_name=PLACEABLE_ON_PREDICATE_NAME, + inverse_predicate_class_name=HOLD_OBJ_PREDICATE_NAME, + ontology_property_parent_class=soma.affordsBearer, + ontology_inverse_property_parent_class=soma.isBearerAffordedBy) +ontology_manager.print_ontology_property(main_ontology.placeable_on) +ontology_manager.print_ontology_property(main_ontology.hold_obj) +``` + + +There, we use `soma.DesignedContainer` & `soma.Shape`, existing concept in SOMA ontology, as the parent classes for the +subject & object concepts respectively. +There is also a note that those classes will have the same namespace with `main_ontology`, so later on to be accessible +through it. + +Then now we define some instances of the newly created triple classes, and link them to object designators, again +using `ontology_manager.create_ontology_linked_designator()` + + +```python jupyter={"outputs_hidden": false} +def create_ontology_handheld_object_designator(object_name: str, ontology_parent_class: Type[owlready2.Thing]): + return ontology_manager.create_ontology_linked_designator(object_name=object_name, + designator_class=ObjectDesignatorDescription, + ontology_concept_name=f"Onto{object_name}", + ontology_parent_class=ontology_parent_class) + + +# Holdable Objects +cookie_box = create_ontology_handheld_object_designator("cookie_box", main_ontology.OntologyHandheldObject) +egg = create_ontology_handheld_object_designator("egg", main_ontology.OntologyHandheldObject) + +# Placeholder objects +placeholders = [create_ontology_handheld_object_designator(object_name, main_ontology.OntologyPlaceHolderObject) + for object_name in ['table', 'stool', 'shelf']] + +egg_tray = create_ontology_handheld_object_designator("egg_tray", main_ontology.OntologyPlaceHolderObject) +``` + + + +### Create ontology relations + +Now we will create ontology relations or predicates between __placeholder objects__ and __handheld objects__ +with `ontology_manager.set_ontology_relation()` + + +```python jupyter={"outputs_hidden": false} +for place_holder in placeholders: + ontology_manager.set_ontology_relation(subject_designator=cookie_box, object_designator=place_holder, + predicate_name=PLACEABLE_ON_PREDICATE_NAME) + +ontology_manager.set_ontology_relation(subject_designator=egg_tray, object_designator=egg, + predicate_name=HOLD_OBJ_PREDICATE_NAME) +``` + + + +## Query designators based on their ontology-concept relations + +Now we can make queries for designators from designators, based on the relation among their corresponding ontology +concepts setup above + + +```python jupyter={"outputs_hidden": false} +print(f"{cookie_box.names}'s placeholder candidates:", + f"""{[placeholder.names for placeholder in + ontology_manager.get_designators_by_subject_predicate(subject=cookie_box, + predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}""") + +print(f"{egg.names}'s placeholder candidates:", + f"""{[placeholder.names for placeholder in + ontology_manager.get_designators_by_subject_predicate(subject=egg, + predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}""") + +for place_holder in placeholders: + print(f"{place_holder.names} can hold:", + f"""{[placeholder.names for placeholder in + ontology_manager.get_designators_by_subject_predicate(subject=place_holder, + predicate_name=HOLD_OBJ_PREDICATE_NAME)]}""") + +print(f"{egg_tray.names} can hold:", + f"""{[placeholder.names for placeholder in + ontology_manager.get_designators_by_subject_predicate(subject=egg_tray, + predicate_name=HOLD_OBJ_PREDICATE_NAME)]}""") +``` + + + +# Practical examples + +## Example 1 + +How about creating ontology concept classes encapsulating {class}`pycram.datastructures.enums.ObjectType`? We can do it by: + + +```python jupyter={"outputs_hidden": false} +from pycram.datastructures.enums import ObjectType + +# Create a generic ontology concept class for edible objects +generic_edible_class = ontology_manager.create_ontology_concept_class('GenericEdible') + +# Create a list of object designators sharing the same concept class as [generic_edible_class] +edible_obj_types = [ObjectType.MILK, ObjectType.BREAKFAST_CEREAL] +for object_type in ObjectType: + if object_type in edible_obj_types: + # Create a designator for the edible object + ontology_manager.create_ontology_object_designator_from_type(object_type, generic_edible_class) + +print(f'{generic_edible_class.name} object types:') +for edible_ontology_concept in generic_edible_class.direct_instances(): + print(edible_ontology_concept, + [des.types for des in + OntologyConceptHolderStore().get_ontology_concept_holder_by_name(edible_ontology_concept.name).designators]) + +``` + + + +## Example 2 + +We could also make use of relations between ontology concepts that designators are associated with, to enable more +abstract inputs in robot motion plans. + +In a similar style to the scenario of __placeholder objects__ and __handheld objects__ above, but with a bit difference, +we will ask the robot to query which content holders (eg. cup, pitcher, bowl) whereby a milk box could be pourable into. + +Basically, we will provide an ontology-based implementation for the query: + +`abstract_ontology_concept -> specific_objects_in_world?` + +To achieve it, we will create triple classes and configure a customized `resolve()` for the abstract concept, which +returns its associated specific designators. +These designators are then used to again resolve for the target objects of interest, which become the inputs to a robot +motion plan. + +### Setup simulated environment + + + +```python +from pycram.worlds.bullet_world import BulletWorld, Object +from pycram.datastructures.pose import Pose + +from pycram.process_module import simulated_robot +from pycram.designators.action_designator import * +from pycram.designators.location_designator import * + +world = BulletWorld() +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +kitchen_designator = ObjectDesignatorDescription(names=["kitchen"]) +robot_designator = ObjectDesignatorDescription(names=["pr2"]).resolve() +``` + +### Create PourableObject-LiquidHolder triple ontology classes + +```python +POURABLE_INTO_PREDICATE_NAME = "pourable_into" +HOLD_LIQUID_PREDICATE_NAME = "hold_liquid" +ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer, + subject_class_name="OntologyLiquidHolderObject", + ontology_object_parent_class=soma.Shape, + object_class_name="OntologyPourableObject", + predicate_class_name=POURABLE_INTO_PREDICATE_NAME, + inverse_predicate_class_name=HOLD_LIQUID_PREDICATE_NAME, + ontology_property_parent_class=soma.affordsBearer, + ontology_inverse_property_parent_class=soma.isBearerAffordedBy) +``` + +### Spawn a pourable object & liquid holders into the world and Create their designators + +```python +# Holdable obj +milk_box = Object("milk_box", ObjectType.MILK, "milk.stl") +milk_box_designator = create_ontology_handheld_object_designator(milk_box.name, main_ontology.OntologyPourableObject) + +# Liquid-holders +cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=Pose([1.4, 1, 0.9])) +bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([1.4, 0.5, 0.9])) +pitcher = Object("pitcher", ObjectType.GENERIC_OBJECT, "Static_MilkPitcher.stl", pose=Pose([1.4, 0, 0.9])) +milk_holders = [cup, bowl, pitcher] +milk_holder_designators = [ + create_ontology_handheld_object_designator(obj.name, main_ontology.OntologyLiquidHolderObject) + for obj in milk_holders] +``` + +### Create an ontology relation between the designators of the pourable object & its liquid holders + +```python +for milk_holder_desig in milk_holder_designators: + ontology_manager.set_ontology_relation(subject_designator=milk_box_designator, object_designator=milk_holder_desig, + predicate_name=POURABLE_INTO_PREDICATE_NAME) +``` + +### Set up `resolve` for the ontology concept of the pourable object + +```python +milk_box_concept_holder = milk_box_designator.ontology_concept_holders[0] + + +def milk_box_concept_resolve(): + object_designator = ontology_manager.get_designators_by_subject_predicate(subject=milk_box_designator, + predicate_name=POURABLE_INTO_PREDICATE_NAME)[ + 0] + return object_designator, object_designator.resolve() + + +milk_box_concept_holder.resolve = milk_box_concept_resolve +``` + +Here, for demonstration purpose only, we specify the resolving result by `milk_box_concept_holder` as `cup`, the +first-registered (default) pourable-into target milk holder, utilizing the ontology relation setup above. + +Now, we can query the milk box's target liquid holder by resolving `milk_box_concept_holder` + +```python +target_milk_holder_designator, target_milk_holder = milk_box_concept_holder.resolve() +print( + f"Pickup target object: {target_milk_holder.name}, a content holder for {milk_box_designator.names} as in relation `{POURABLE_INTO_PREDICATE_NAME}`") +``` + +### Robot picks up the target liquid holder + +```python +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.3]).resolve().perform() + + pickup_pose = CostmapLocation(target=target_milk_holder, reachable_for=robot_designator).resolve() + pickup_arm = pickup_pose.reachable_arms[0] + + print(pickup_pose, pickup_arm) + + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() + + PickUpAction(object_designator_description=target_milk_holder_designator, arms=[pickup_arm], + grasps=[Grasp.FRONT]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_designator.resolve(), + target_milk_holder_designator.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=robot_designator, reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(target_milk_holder_designator, target_locations=[place_island.pose], + arms=[pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() +world.exit() +``` + +# Save ontologies to an OWL file + +After all the above operations on our ontologies, we now can save them to an OWL file on disk + +```python +ontology_manager.save(f"{Path.home()}/ontologies/New{main_ontology.name}.owl") +``` + +# Optimize ontology loading with SQLite3 + +Upon the initial ontology loading from OWL, an SQLite3 file is automatically created, acting as the quadstore cache for +the loaded ontologies. This allows them to be __selectively__ reusable the next time being loaded. +More info can be referenced [here](https://owlready2.readthedocs.io/en/latest/world.html). diff --git a/examples/orm_example.ipynb b/examples/orm_example.ipynb deleted file mode 100644 index f200f419e..000000000 --- a/examples/orm_example.ipynb +++ /dev/null @@ -1,584 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Hands on Object Relational Mapping in PyCram\n", - "\n", - "This tutorial will walk you through the serialization of a minimal plan in pycram.\n", - "First we will import sqlalchemy, create an in memory database and connect a session to it." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:09.128875369Z", - "start_time": "2024-01-29T16:51:08.956260897Z" - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "" - ] - }, - "execution_count": 1, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "import sqlalchemy\n", - "import sqlalchemy.orm\n", - "\n", - "engine = sqlalchemy.create_engine(\"sqlite+pysqlite:///:memory:\", echo=False)\n", - "session = sqlalchemy.orm.Session(bind=engine)\n", - "session" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Next we create the database schema using the sqlalchemy functionality. For that we need to import the base class of pycram.orm." - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:09.568683265Z", - "start_time": "2024-01-29T16:51:09.124071834Z" - } - }, - "outputs": [], - "source": [ - "import pycram.orm.base\n", - "import pycram.orm.action_designator\n", - "pycram.orm.base.Base.metadata.create_all(engine)\n", - "session.commit()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Next we will write a simple plan where the robot parks his arms and then moves somewhere. We will construct a TaskTree around it such that we can serialize it later. As usual, we first create a world and then define the plan. By doing so, we obtain the task tree." - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.305706324Z", - "start_time": "2024-01-29T16:51:09.568597034Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n", - "Scalar element defined multiple times: limit\n", - "Unknown tag \"rgba_color\" in /robot[@name='milk_object']/link[@name='milk_main']/visual[1]/material[@name='white']\n", - "Unknown tag \"rgba_color\" in /robot[@name='cereal_object']/link[@name='cereal_main']/visual[1]/material[@name='white']\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[INFO] [1719919094.609041]: Ontology [http://www.ease-crc.org/ont/SOMA-HOME.owl#]'s name: SOMA-HOME has been loaded\n", - "[INFO] [1719919094.609908]: - main namespace: SOMA-HOME\n", - "[INFO] [1719919094.610788]: - loaded ontologies:\n", - "[INFO] [1719919094.611264]: http://www.ease-crc.org/ont/SOMA-HOME.owl#\n", - "[INFO] [1719919094.611735]: http://www.ease-crc.org/ont/DUL.owl#\n", - "[INFO] [1719919094.612237]: http://www.ease-crc.org/ont/SOMA.owl#\n", - "[INFO] [1719919095.385427]: Waiting for IK service: /pr2_left_arm_kinematics/get_ik\n", - "NoOperation\n", - "└── NoOperation\n", - " ├── ParkArmsActionPerformable\n", - " ├── MoveTorsoActionPerformable\n", - " ├── NavigateActionPerformable\n", - " │ └── MoveMotion\n", - " ├── PickUpActionPerformable\n", - " │ ├── MoveTCPMotion\n", - " │ ├── MoveGripperMotion\n", - " │ ├── MoveTCPMotion\n", - " │ ├── MoveGripperMotion\n", - " │ └── MoveTCPMotion\n", - " ├── ParkArmsActionPerformable\n", - " ├── NavigateActionPerformable\n", - " │ └── MoveMotion\n", - " ├── PlaceActionPerformable\n", - " │ ├── MoveTCPMotion\n", - " │ ├── MoveGripperMotion\n", - " │ └── MoveTCPMotion\n", - " └── ParkArmsActionPerformable\n" - ] - } - ], - "source": [ - "from pycram.designators.action_designator import *\n", - "from pycram.designators.location_designator import *\n", - "from pycram.process_module import simulated_robot\n", - "from pycram.datastructures.enums import Arms, ObjectType, Grasp\n", - "from pycram.tasktree import with_tree\n", - "import pycram.tasktree\n", - "from pycram.world_concepts.world_object import Object\n", - "from pycram.designators.object_designator import *\n", - "from pycram.datastructures.pose import Pose\n", - "import anytree\n", - "\n", - "world = BulletWorld(WorldMode.GUI)\n", - "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")\n", - "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", - "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n", - "cereal = Object(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\", pose=Pose([1.3, 0.7, 0.95]))\n", - "milk_desig = ObjectDesignatorDescription(names=[\"milk\"])\n", - "cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", - "robot_desig = ObjectDesignatorDescription(names=[\"pr2\"]).resolve()\n", - "kitchen_desig = ObjectDesignatorDescription(names=[\"kitchen\"])\n", - "\n", - "@with_tree\n", - "def plan():\n", - " with simulated_robot:\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - " MoveTorsoAction([0.2]).resolve().perform()\n", - " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", - " pickup_arm = pickup_pose.reachable_arms[0]\n", - " NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform()\n", - " PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform()\n", - " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", - "\n", - " place_island = SemanticCostmapLocation(\"kitchen_island_surface\", kitchen_desig.resolve(),\n", - " cereal_desig.resolve()).resolve()\n", - "\n", - " place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve()\n", - "\n", - " NavigateAction(target_locations=[place_stand.pose]).resolve().perform()\n", - "\n", - " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", - "\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - "\n", - "plan()\n", - "\n", - "# set description of what we are doing\n", - "pycram.orm.base.ProcessMetaData().description = \"Tutorial for getting familiar with the ORM.\"\n", - "task_tree = pycram.tasktree.task_tree\n", - "print(anytree.RenderTree(task_tree))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Next we serialize the task tree by recursively inserting from its root." - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.505074180Z", - "start_time": "2024-01-29T16:51:35.307461245Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 727.06it/s]\n" - ] - }, - { - "data": { - "text/plain": [ - "TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "task_tree.root.insert(session)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can look at our experiment (Process)MetaData to get some context on the data we just created." - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.508649071Z", - "start_time": "2024-01-29T16:51:35.506023628Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ProcessMetaData(id=1, created_at=datetime.datetime(2024, 7, 2, 11, 18, 27), created_by='jdech', description='Tutorial for getting familiar with the ORM.', pycram_version='7bcd68e456d856a51e62df5e830aab0d7113f2e7')\n" - ] - } - ], - "source": [ - "from sqlalchemy import select\n", - "\n", - "print(*session.scalars(select(pycram.orm.base.ProcessMetaData)).all())" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lastly we can look at various table to see how the structures got logged.\n", - "For example, we can get all the navigate actions that occurred." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.520518056Z", - "start_time": "2024-01-29T16:51:35.508477111Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, pose_to_init=False, torso_height=0.2, type=, pose_id=3, process_metadata_id=1), pose_to_init=False, pose_id=4)\n", - "NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, pose_to_init=False, torso_height=0.2, type=, pose_id=12, process_metadata_id=1), pose_to_init=False, pose_id=13)\n" - ] - } - ], - "source": [ - "navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all()\n", - "print(*navigations, sep=\"\\n\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Due to the inheritance mapped in the ORM package, we can also obtain all executed actions with just one query. " - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.560192686Z", - "start_time": "2024-01-29T16:51:35.520628950Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, pose_to_init=False, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=)\n", - "MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, pose_to_init=False, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.2)\n", - "NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, pose_to_init=False, torso_height=0.2, type=, pose_id=3, process_metadata_id=1), pose_to_init=False, pose_id=4)\n", - "PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1)\n", - "ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, pose_to_init=False, torso_height=0.2, type=, pose_id=11, process_metadata_id=1), arm=)\n", - "NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, pose_to_init=False, torso_height=0.2, type=, pose_id=12, process_metadata_id=1), pose_to_init=False, pose_id=13)\n", - "PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, pose_to_init=False, torso_height=0.2, type=, pose_id=15, process_metadata_id=1), object_to_init=False, pose_to_init=False, arm=, pose_id=17, object_id=2)\n", - "ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, pose_to_init=False, torso_height=0.2, type=, pose_id=20, process_metadata_id=1), arm=)\n" - ] - } - ], - "source": [ - "actions = session.scalars(select(pycram.orm.action_designator.Action)).all()\n", - "print(*actions, sep=\"\\n\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Of course all relational algebra operators, such as filtering and joining also work in pycram.orm queries. Let's say we need all the poses of objects, that were picked up by a robot. Since we defined a relationship between the PickUpAction table and the Object table and between the Object table and the Pose table in the ORM class schema, we can just use the join operator without any further specification:" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.561234983Z", - "start_time": "2024-01-29T16:51:35.535162323Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Pose(id=7, orientation_to_init=False, position_to_init=False, time=datetime.datetime(2024, 7, 2, 11, 18, 21, 670262), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" - ] - } - ], - "source": [ - "object_actions = (session.scalars(select(pycram.orm.base.Pose)\n", - " .join(pycram.orm.action_designator.PickUpAction.object)\n", - " .join(pycram.orm.object_designator.Object.pose))\n", - " .all())\n", - "print(*object_actions, sep=\"\\n\")\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Did you notice, that for the joins we did not join the tables together in a typical sql kind of way, but rather used the relationships defined in the ORM classes and wrote joins like PickUpAction.object or Object.pose? This is because the ORM package automatically creates the joins for us, so we only have to join on the attributes that hold the relationship. This is a huge advantage over writing sql queries by hand, since we do not have to worry about the join conditions. \n", - "This is a strong tool, but it is crucial to use it properly. Very important to note: The order of the joins matters! For instance, if we joined the Pose table with the Object table first, and placed the join between the PickUpAction table and the Object table second, sqlalchemy would have selected the Pose not from the join between all three tables, but rather from a join between the Pose and the Object table + from a join between the PickUpAction table and the Object table. These mistakes can lead to wrong results or even to errors (the above-mentioned example would actually lead to an error due to the Object table being accessed twice in two separate joins in the same query and therefore the column names of the Object tables would have been ambiguous and could not be used by sqlalchemy to join).\n", - "\n", - "Make sure to check out the other examples of ORM querying." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "If we want to filter for all successful tasks we can just add the filter operator:" - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.600701170Z", - "start_time": "2024-01-29T16:51:35.539887927Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=3, action_id=1, action=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, pose_to_init=False, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429791), end_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 938565), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=4, action_id=2, action=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, pose_to_init=False, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.2), start_time=datetime.datetime(2024, 7, 2, 13, 18, 14, 612725), end_time=datetime.datetime(2024, 7, 2, 13, 18, 15, 116136), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=5, action_id=3, action=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, pose_to_init=False, torso_height=0.2, type=, pose_id=3, process_metadata_id=1), pose_to_init=False, pose_id=4), start_time=datetime.datetime(2024, 7, 2, 13, 18, 15, 512508), end_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 18303), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=7, action_id=11, action=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, pose_to_init=False, torso_height=0.2, type=, pose_id=11, process_metadata_id=1), arm=), start_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 511201), end_time=datetime.datetime(2024, 7, 2, 13, 18, 20, 21805), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=8, action_id=12, action=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, pose_to_init=False, torso_height=0.2, type=, pose_id=12, process_metadata_id=1), pose_to_init=False, pose_id=13), start_time=datetime.datetime(2024, 7, 2, 13, 18, 20, 619721), end_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128413), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=9, action_id=14, action=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, pose_to_init=False, torso_height=0.2, type=, pose_id=15, process_metadata_id=1), object_to_init=False, pose_to_init=False, arm=, pose_id=17, object_id=2), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128994), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694118), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=10, action_id=18, action=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, pose_to_init=False, torso_height=0.2, type=, pose_id=20, process_metadata_id=1), arm=), start_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694186), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200254), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=11, action_id=4, action=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_to_init=False, pose_id=5), start_time=datetime.datetime(2024, 7, 2, 13, 18, 15, 512553), end_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 18296), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, action_id=3, action=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, pose_to_init=False, torso_height=0.2, type=, pose_id=3, process_metadata_id=1), pose_to_init=False, pose_id=4), start_time=datetime.datetime(2024, 7, 2, 13, 18, 15, 512508), end_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 18303), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=12, action_id=6, action=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', pose_to_init=False, arm=, allow_gripper_collision=True, pose_id=8), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 229557), end_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 749725), status=, reason=None, parent_id=6, parent=TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=13, action_id=7, action=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion=, gripper=, allow_gripper_collision=None), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 749842), end_time=datetime.datetime(2024, 7, 2, 13, 18, 17, 255565), status=, reason=None, parent_id=6, parent=TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=14, action_id=8, action=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', pose_to_init=False, arm=, allow_gripper_collision=True, pose_id=9), start_time=datetime.datetime(2024, 7, 2, 13, 18, 17, 396402), end_time=datetime.datetime(2024, 7, 2, 13, 18, 17, 919758), status=, reason=None, parent_id=6, parent=TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=15, action_id=9, action=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion=, gripper=, allow_gripper_collision=None), start_time=datetime.datetime(2024, 7, 2, 13, 18, 17, 919882), end_time=datetime.datetime(2024, 7, 2, 13, 18, 18, 425650), status=, reason=None, parent_id=6, parent=TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=16, action_id=10, action=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', pose_to_init=False, arm=, allow_gripper_collision=True, pose_id=10), start_time=datetime.datetime(2024, 7, 2, 13, 18, 18, 582197), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 181567), status=, reason=None, parent_id=6, parent=TaskTreeNode(id=6, action_id=5, action=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, pose_to_init=False, torso_height=0.2, type=, pose_id=6, process_metadata_id=1), object_to_init=False, arm=, grasp=, object_id=1), start_time=datetime.datetime(2024, 7, 2, 13, 18, 16, 19500), end_time=datetime.datetime(2024, 7, 2, 13, 18, 19, 495448), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=17, action_id=13, action=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_to_init=False, pose_id=14), start_time=datetime.datetime(2024, 7, 2, 13, 18, 20, 619782), end_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128406), status=, reason=None, parent_id=8, parent=TaskTreeNode(id=8, action_id=12, action=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, pose_to_init=False, torso_height=0.2, type=, pose_id=12, process_metadata_id=1), pose_to_init=False, pose_id=13), start_time=datetime.datetime(2024, 7, 2, 13, 18, 20, 619721), end_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128413), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=18, action_id=15, action=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', pose_to_init=False, arm=, allow_gripper_collision=None, pose_id=18), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 131634), end_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 661035), status=, reason=None, parent_id=9, parent=TaskTreeNode(id=9, action_id=14, action=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, pose_to_init=False, torso_height=0.2, type=, pose_id=15, process_metadata_id=1), object_to_init=False, pose_to_init=False, arm=, pose_id=17, object_id=2), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128994), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694118), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=19, action_id=16, action=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion=, gripper=, allow_gripper_collision=None), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 661137), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 170556), status=, reason=None, parent_id=9, parent=TaskTreeNode(id=9, action_id=14, action=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, pose_to_init=False, torso_height=0.2, type=, pose_id=15, process_metadata_id=1), object_to_init=False, pose_to_init=False, arm=, pose_id=17, object_id=2), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128994), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694118), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=20, action_id=17, action=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', pose_to_init=False, arm=, allow_gripper_collision=None, pose_id=19), start_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 172993), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694110), status=, reason=None, parent_id=9, parent=TaskTreeNode(id=9, action_id=14, action=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, pose_to_init=False, torso_height=0.2, type=, pose_id=15, process_metadata_id=1), object_to_init=False, pose_to_init=False, arm=, pose_id=17, object_id=2), start_time=datetime.datetime(2024, 7, 2, 13, 18, 21, 128994), end_time=datetime.datetime(2024, 7, 2, 13, 18, 22, 694118), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 18, 13, 429749), end_time=datetime.datetime(2024, 7, 2, 13, 18, 23, 200265), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, action_id=None, action=None, start_time=datetime.datetime(2024, 7, 2, 13, 17, 43, 157947), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" - ] - } - ], - "source": [ - "from pycram.orm.tasktree import TaskTreeNode\n", - "\n", - "successful_tasks = session.scalars(select(TaskTreeNode).where(TaskTreeNode.status == \"SUCCEEDED\"))\n", - "print(*successful_tasks, sep=\"\\n\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "As expected all but the root node succeeded, since the root node is still running.\n", - "\n", - "Writing an extension to the ORM package is also done with ease. We need to create a new ActionDesignator class and its ORM equivalent, where we define our new table. Let's say we want to log all the things the robot says. We will create a new ActionDesignator class called Saying and its ORM equivalent called ORMSaying. " - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.601121928Z", - "start_time": "2024-01-29T16:51:35.600085428Z" - } - }, - "outputs": [], - "source": [ - "from sqlalchemy.orm import Mapped, mapped_column, Session\n", - "from pycram.orm.action_designator import Action\n", - "from dataclasses import dataclass\n", - "\n", - "\n", - "# define ORM class from pattern in every pycram.orm class\n", - "class ORMSaying(Action):\n", - "\n", - " id: Mapped[int] = mapped_column(sqlalchemy.ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False)\n", - " # since we do not want to add any custom specifications to our column, we don't even need to define mapped_column, sqlalchemy does this internally.\n", - " text: Mapped[str] \n", - "\n", - "# define brand new action designator\n", - "\n", - "@dataclass \n", - "class SayingActionPerformable(ActionDesignatorDescription.Action):\n", - " \n", - " text: str\n", - " \n", - " @with_tree\n", - " def perform(self) -> None:\n", - " print(self.text)\n", - "\n", - " def to_sql(self) -> ORMSaying:\n", - " return ORMSaying(self.text)\n", - "\n", - " def insert(self, session: Session, *args, **kwargs) -> ORMSaying:\n", - " action = super().insert(session)\n", - " session.add(action)\n", - " session.commit()\n", - " return action\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Now we got our new ActionDesignator called Saying and its ORM version. Since this class got created after all other classes got inserted into the database (in the beginning of the notebook) we have to insert it manually. " - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:35.601417935Z", - "start_time": "2024-01-29T16:51:35.600240790Z" - } - }, - "outputs": [], - "source": [ - "ORMSaying.metadata.create_all(bind=engine)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Now we can create and insert a Saying action. Since this is the last part where we interact with the BulletWorld, we can also close it." - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:51:36.020873055Z", - "start_time": "2024-01-29T16:51:35.600296911Z" - } - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Patchie, Patchie; Where is my Patchie?\n" - ] - }, - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 954.29it/s]\n" - ] - } - ], - "source": [ - "# create a saying action and insert it\n", - "SayingActionPerformable(\"Patchie, Patchie; Where is my Patchie?\").perform()\n", - "pycram.tasktree.task_tree.root.insert(session)\n", - "session.commit()\n", - "\n", - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "It is notable that committing the object to the session fills its primary key. Hence, there is no worries about assigning unique IDs manually.\n", - "Finally, we can double-check that our object exists in the database." - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:52:32.021378099Z", - "start_time": "2024-01-29T16:52:31.976674160Z" - } - }, - "outputs": [ - { - "data": { - "text/plain": [ - "[ORMSaying(id=37, process_metadata_id=1, dtype='ORMSaying', robot_state_id=17, robot_state=RobotState(id=17, pose_to_init=False, torso_height=0.2, type=, pose_id=41, process_metadata_id=1), text='Patchie, Patchie; Where is my Patchie?')]" - ] - }, - "execution_count": 17, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "session.scalars(select(ORMSaying)).all()" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 1 -} diff --git a/examples/orm_example.md b/examples/orm_example.md new file mode 100644 index 000000000..b39f2cb0f --- /dev/null +++ b/examples/orm_example.md @@ -0,0 +1,199 @@ +from pycram.designators.action_designator import ActionAbstract--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Hands on Object Relational Mapping in PyCram + +This tutorial will walk you through the serialization of a minimal plan in pycram. +First we will import sqlalchemy, create an in-memory database and connect a session to it. + +```python +import sqlalchemy.orm + +engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) +session = sqlalchemy.orm.Session(bind=engine) +session +``` + +Next we create the database schema using the sqlalchemy functionality. For that we need to import the base class of pycram.orm. + +```python +import pycram.orm.action_designator +pycram.orm.base.Base.metadata.create_all(engine) +session.commit() +``` + +Next we will write a simple plan where the robot parks his arms and then moves somewhere. We will construct a TaskTree around it such that we can serialize it later. As usual, we first create a world and then define the plan. By doing so, we obtain the task tree. + +```python +from pycram.designators.action_designator import * +from pycram.designators.location_designator import * +from pycram.process_module import simulated_robot +from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode +from pycram.tasktree import with_tree +import pycram.tasktree +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.designators.object_designator import * +from pycram.datastructures.pose import Pose +from pycram.orm.base import ProcessMetaData +import anytree + +world = BulletWorld(WorldMode.DIRECT) +pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) +milk_desig = ObjectDesignatorDescription(names=["milk"]) +cereal_desig = ObjectDesignatorDescription(names=["cereal"]) +robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() +kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) + +@with_tree +def plan(): + with simulated_robot: + ParkArmsActionPerformable(Arms.BOTH).perform() + MoveTorsoAction([0.2]).resolve().perform() + pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=[Grasp.FRONT]).resolve().perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + + place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_desig.resolve(), + cereal_desig.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() + + ParkArmsActionPerformable(Arms.BOTH).perform() + +plan() + +# set description of what we are doing +ProcessMetaData().description = "Tutorial for getting familiar with the ORM." +task_tree = pycram.tasktree.task_tree +print(anytree.RenderTree(task_tree.root)) +``` + +Next we serialize the task tree by recursively inserting from its root. + +```python +task_tree.root.insert(session) +``` + +We can look at our experiment (Process)MetaData to get some context on the data we just created. + +```python +from sqlalchemy import select + +print(*session.scalars(select(pycram.orm.base.ProcessMetaData)).all()) +``` + +Lastly we can look at various table to see how the structures got logged. +For example, we can get all the navigate actions that occurred. + +```python +navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all() +print(*navigations, sep="\n") +``` + +Due to the inheritance mapped in the ORM package, we can also obtain all executed actions with just one query. + +```python +actions = session.scalars(select(pycram.orm.action_designator.Action)).all() +print(*actions, sep="\n") +``` + +Of course all relational algebra operators, such as filtering and joining also work in pycram.orm queries. Let's say we need all the poses of objects, that were picked up by a robot. Since we defined a relationship between the PickUpAction table and the Object table and between the Object table and the Pose table in the ORM class schema, we can just use the join operator without any further specification: + +```python +object_actions = (session.scalars(select(pycram.orm.base.Pose) + .join(pycram.orm.action_designator.PickUpAction.object) + .join(pycram.orm.object_designator.Object.pose)) + .all()) +print(*object_actions, sep="\n") + +``` + +Did you notice, that for the joins we did not join the tables together in a typical sql kind of way, but rather used the relationships defined in the ORM classes and wrote joins like PickUpAction.object or Object.pose? This is because the ORM package automatically creates the joins for us, so we only have to join on the attributes that hold the relationship. This is a huge advantage over writing sql queries by hand, since we do not have to worry about the join conditions. +This is a strong tool, but it is crucial to use it properly. Very important to note: The order of the joins matters! For instance, if we joined the Pose table with the Object table first, and placed the join between the PickUpAction table and the Object table second, sqlalchemy would have selected the Pose not from the join between all three tables, but rather from a join between the Pose and the Object table + from a join between the PickUpAction table and the Object table. These mistakes can lead to wrong results or even to errors (the above-mentioned example would actually lead to an error due to the Object table being accessed twice in two separate joins in the same query and therefore the column names of the Object tables would have been ambiguous and could not be used by sqlalchemy to join). + +Make sure to check out the other examples of ORM querying. + + +If we want to filter for all successful tasks we can just add the filter operator: + +```python +from pycram.orm.tasktree import TaskTreeNode + +successful_tasks = session.scalars(select(TaskTreeNode).where(TaskTreeNode.status == "SUCCEEDED")) +print(*successful_tasks, sep="\n") +``` + +As expected all but the root node succeeded, since the root node is still running. + +Writing an extension to the ORM package is also done with ease. We need to create a new ActionDesignator class and its ORM equivalent, where we define our new table. Let's say we want to log all the things the robot says. We will create a new ActionDesignator class called Saying and its ORM equivalent called ORMSaying. + +```python +from sqlalchemy.orm import Mapped, mapped_column, Session +from pycram.orm.action_designator import Action +from dataclasses import dataclass + + +# define ORM class from pattern in every pycram.orm class +class ORMSaying(Action): + + id: Mapped[int] = mapped_column(sqlalchemy.ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) + # since we do not want to add any custom specifications to our column, we don't even need to define mapped_column, sqlalchemy does this internally. + text: Mapped[str] + +# define brand new action designator +# Since this class is derived from ActionAbstract, we do not need to manually define the insert() and to_sql() function, the mapping is done automatically. We just have to tell the class, which ORMclass it is supposed to use. +@dataclass +class SayingActionPerformable(ActionAbstract): + + text: str + orm_class = ORMSaying + + @with_tree + def perform(self) -> None: + print(self.text) +``` + +Now we got our new ActionDesignator called Saying and its ORM version. Since this class got created after all other classes got inserted into the database (in the beginning of the notebook) we have to insert it manually. + +```python +ORMSaying.metadata.create_all(bind=engine) +``` + +Now we can create and insert a Saying action. Since this is the last part where we interact with the BulletWorld, we can also close it. + +```python +# create a saying action and insert it +SayingActionPerformable("Patchie, Patchie; Where is my Patchie?").perform() +pycram.tasktree.task_tree.root.insert(session) +session.commit() + +world.exit() +``` + +It is notable that committing the object to the session fills its primary key. Hence, there is no worries about assigning unique IDs manually. +Finally, we can double-check that our object exists in the database. + +```python +session.scalars(select(ORMSaying)).all() +``` diff --git a/examples/orm_querying_examples.ipynb b/examples/orm_querying_examples.ipynb deleted file mode 100644 index 5569494e3..000000000 --- a/examples/orm_querying_examples.ipynb +++ /dev/null @@ -1,445 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "15d0886b", - "metadata": {}, - "source": [ - "# ORM querying examples\n", - "\n", - "In this tutorial, we will get to see more examples of ORM querying. " - ] - }, - { - "cell_type": "markdown", - "id": "31385bc6", - "metadata": {}, - "source": [ - "First, we will gather a lot of data. In order to achieve that we will write a randomized experiment for grasping a couple of objects.\n", - "In the experiment the robot will try to grasp a randomized object using random poses and torso heights.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "71de1567", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:54:39.452156158Z", - "start_time": "2024-01-29T16:54:38.500054294Z" - } - }, - "outputs": [], - "source": [ - "from tf import transformations\n", - "import itertools\n", - "import time\n", - "from typing import Optional, List, Tuple\n", - "\n", - "import numpy as np\n", - "\n", - "import sqlalchemy.orm\n", - "import tf\n", - "import tqdm\n", - "\n", - "import pycram.orm.base\n", - "from pycram.worlds.bullet_world import BulletWorld\n", - "from pycram.world_concepts.world_object import Object as BulletWorldObject\n", - "from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction\n", - "from pycram.designators.object_designator import ObjectDesignatorDescription\n", - "from pycram.plan_failures import PlanFailure\n", - "from pycram.process_module import ProcessModule\n", - "from pycram.datastructures.enums import Arms, ObjectType, Grasp\n", - "\n", - "from pycram.process_module import simulated_robot\n", - "import sqlalchemy.orm\n", - "# from pycram.resolver.location.jpt_location import JPTCostmapLocation\n", - "import pycram.orm\n", - "from pycram.orm.base import Position, RobotState\n", - "from pycram.orm.tasktree import TaskTreeNode\n", - "from pycram.orm.action_designator import PickUpAction as ORMPickUpAction\n", - "from pycram.orm.object_designator import Object\n", - "import sqlalchemy.sql\n", - "import pandas as pd\n", - "\n", - "from pycram.datastructures.pose import Pose\n", - "\n", - "np.random.seed(420)\n", - "\n", - "ProcessModule.execution_delay = False\n", - "pycram.orm.base.ProcessMetaData().description = \"Tutorial for learning from experience in a Grasping action.\"\n", - "\n", - "\n", - "class GraspingExplorer:\n", - " \"\"\"Class to try randomized grasping plans.\"\"\"\n", - "\n", - " world: Optional[BulletWorld]\n", - "\n", - " def __init__(self, robots: Optional[List[Tuple[str, str]]] = None, objects: Optional[List[Tuple[str, str]]] = None,\n", - " arms: Optional[List[Arms]] = None, grasps: Optional[List[Grasp]] = None,\n", - " samples_per_scenario: int = 1000):\n", - " \"\"\"\n", - " Create a GraspingExplorer.\n", - " :param robots: The robots to use\n", - " :param objects: The objects to try to grasp\n", - " :param arms: The arms of the robot to use\n", - " :param grasps: The grasp orientations to use\n", - " :param samples_per_scenario: The number of tries per scenario.\n", - " \"\"\"\n", - " # store exploration space\n", - " if not robots:\n", - " self.robots: List[Tuple[str, str]] = [(\"pr2\", \"pr2.urdf\")]\n", - "\n", - " if not objects:\n", - " self.objects: List[Tuple[str, ObjectType, str]] = [(\"cereal\", ObjectType.BREAKFAST_CEREAL, \"breakfast_cereal.stl\"),\n", - " (\"bowl\", ObjectType.BOWL, \"bowl.stl\"),\n", - " (\"milk\", ObjectType.MILK, \"milk.stl\"),\n", - " (\"spoon\", ObjectType.SPOON, \"spoon.stl\")]\n", - "\n", - " if not arms:\n", - " self.arms: List[str] = [Arms.LEFT, Arms.RIGHT]\n", - "\n", - " if not grasps:\n", - " self.grasps: List[str] = [Grasp.LEFT, Grasp.RIGHT, Grasp.FRONT, Grasp.TOP]\n", - "\n", - " # store trials per scenario\n", - " self.samples_per_scenario: int = samples_per_scenario\n", - "\n", - " # chain hyperparameters\n", - " self.hyper_parameters = [self.robots, self.objects, self.arms, self.grasps]\n", - "\n", - " self.total_tries = 0\n", - " self.total_failures = 0\n", - "\n", - " def perform(self, session: sqlalchemy.orm.Session):\n", - " \"\"\"\n", - " Perform all experiments.\n", - " :param session: The database-session to insert the samples in.\n", - " \"\"\"\n", - "\n", - " # create progress bar\n", - " progress_bar = tqdm.tqdm(\n", - " total=np.prod([len(p) for p in self.hyper_parameters]) * self.samples_per_scenario)\n", - "\n", - " self.world = BulletWorld(\"DIRECT\")\n", - "\n", - " # for every robot\n", - " for robot, robot_urdf in self.robots:\n", - "\n", - " # spawn it\n", - " robot = BulletWorldObject(robot, ObjectType.ROBOT, robot_urdf)\n", - "\n", - " # for every obj\n", - " for obj, obj_type, obj_stl in self.objects:\n", - "\n", - " # spawn it\n", - " bw_object = BulletWorldObject(obj, obj_type, obj_stl, pose=Pose([0, 0, 0.75], [0, 0, 0, 1]))\n", - "\n", - " # create object designator\n", - " object_designator = ObjectDesignatorDescription(names=[obj])\n", - "\n", - " # for every arm and grasp pose\n", - " for arm, grasp in itertools.product(self.arms, self.grasps):\n", - " # sample positions in 2D\n", - " positions = np.random.uniform([-2, -2], [2, 2], (self.samples_per_scenario, 2))\n", - "\n", - " # for every position\n", - " for position in positions:\n", - "\n", - " # set z axis to 0\n", - " position = [*position, 0]\n", - "\n", - " # calculate orientation for robot to face the object\n", - " angle = np.arctan2(position[1], position[0]) + np.pi\n", - " orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes=\"sxyz\"))\n", - "\n", - " # try to execute a grasping plan\n", - " with simulated_robot:\n", - "\n", - " ParkArmsActionPerformable(Arms.BOTH).perform()\n", - " # navigate to sampled position\n", - " NavigateAction([Pose(position, orientation)]).resolve().perform()\n", - "\n", - " # move torso\n", - " height = np.random.uniform(0., 0.33, 1)[0]\n", - " MoveTorsoActionPerformable(height).perform()\n", - "\n", - " # try to pick it up\n", - " try:\n", - " PickUpAction(object_designator, [arm], [grasp]).resolve().perform()\n", - "\n", - " # if it fails\n", - " except PlanFailure:\n", - "\n", - " # update failure stats\n", - " self.total_failures += 1\n", - "\n", - " # reset BulletWorld\n", - " self.world.reset_world()\n", - "\n", - " # update progress bar\n", - " self.total_tries += 1\n", - "\n", - " # insert into database\n", - " pycram.tasktree.task_tree.insert(session, use_progress_bar=False)\n", - " pycram.tasktree.reset_tree()\n", - "\n", - " progress_bar.update()\n", - " progress_bar.set_postfix(success_rate=(self.total_tries - self.total_failures) /\n", - " self.total_tries)\n", - "\n", - " bw_object.remove()\n", - " robot.remove()\n" - ] - }, - { - "cell_type": "markdown", - "id": "0182c495", - "metadata": {}, - "source": [ - "Next we have to establish a connection to a database and execute the experiment a couple of times. Note that the (few) number of samples we generate is only for demonstrations.\n", - "For robust and reliable machine learning millions of samples are required.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "6ffa0f89", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:56:53.360247565Z", - "start_time": "2024-01-29T16:54:39.452769369Z" - } - }, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - " 0%| | 0/960 [00:00 9\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(\u001b[43mTaskTreeNode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mcode\u001b[49m)\n\u001b[1;32m 10\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(Code\u001b[38;5;241m.\u001b[39mdesignator\u001b[38;5;241m.\u001b[39mof_type(ORMPickUpAction))\n\u001b[1;32m 11\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(ORMPickUpAction\u001b[38;5;241m.\u001b[39mrobot_state)\n\u001b[1;32m 12\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(RobotState\u001b[38;5;241m.\u001b[39mpose)\n\u001b[1;32m 13\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(pycram\u001b[38;5;241m.\u001b[39morm\u001b[38;5;241m.\u001b[39mbase\u001b[38;5;241m.\u001b[39mPose\u001b[38;5;241m.\u001b[39mposition)\n\u001b[1;32m 14\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(ORMPickUpAction\u001b[38;5;241m.\u001b[39mobject)\u001b[38;5;241m.\u001b[39mwhere(Object\u001b[38;5;241m.\u001b[39mtype \u001b[38;5;241m==\u001b[39m milk\u001b[38;5;241m.\u001b[39mtype)\n\u001b[1;32m 15\u001b[0m \u001b[38;5;241m.\u001b[39mwhere(TaskTreeNode\u001b[38;5;241m.\u001b[39mstatus \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mSUCCEEDED\u001b[39m\u001b[38;5;124m\"\u001b[39m))\n\u001b[1;32m 16\u001b[0m \u001b[38;5;28mprint\u001b[39m(query)\n\u001b[1;32m 18\u001b[0m df \u001b[38;5;241m=\u001b[39m pd\u001b[38;5;241m.\u001b[39mread_sql_query(query, session\u001b[38;5;241m.\u001b[39mget_bind())\n", - "\u001b[0;31mAttributeError\u001b[0m: type object 'TaskTreeNode' has no attribute 'code'" - ] - } - ], - "source": [ - "from sqlalchemy import select\n", - "from pycram.datastructures.enums import ObjectType\n", - "\n", - "milk = BulletWorldObject(\"Milk\", ObjectType.MILK, \"milk.stl\")\n", - "\n", - "# query all relative robot positions in regard to an objects position\n", - "# make sure to order the joins() correctly\n", - "query = (select(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y)\n", - " .join(TaskTreeNode.code)\n", - " .join(Code.designator.of_type(ORMPickUpAction))\n", - " .join(ORMPickUpAction.robot_state)\n", - " .join(RobotState.pose)\n", - " .join(pycram.orm.base.Pose.position)\n", - " .join(ORMPickUpAction.object).where(Object.type == milk.type)\n", - " .where(TaskTreeNode.status == \"SUCCEEDED\"))\n", - "print(query)\n", - "\n", - "df = pd.read_sql_query(query, session.get_bind())\n", - "print(df)" - ] - }, - { - "cell_type": "markdown", - "id": "2cd07b40", - "metadata": {}, - "source": [ - "If you are not familiar with sqlalchemy querying you might wonder what the of_type() function does and why we needed it in this query:\n", - "\n", - "In order to understand the importance of the of_type() function in the joins above it is crucial to understand the inheritance structure in the ORM package. The action necessary for this query is the PickUpAction. It inherits the Action class/table (which holds all the actions). The Action class itself on the other hand inherits Designator (which holds all the actions, but also all the motions). \n", - "We started our joins by joining TaskTreeNode on its relationship to Code and Code on its relationship to Designator. Next table we need is the PickUpAction table, but there is no specified relationship between Designator and PickUpAction. But we do know that a PickUpAction is actually a Designator, meaning, it inherits from Designator. So we can just \"tell\" the join to join Code on every Designator, that is \"of_type\" PickUpAction (.join(Code.designator.of_type(ORMPickUpAction))). \n", - "The effect of this function can also be seen in the printed query of above's output. " - ] - }, - { - "cell_type": "markdown", - "id": "f337365d", - "metadata": {}, - "source": [ - "Another interesting query: Let's say we want to select the torso height and positions of robots relative to the object they were trying to grasp:" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "0120dceb", - "metadata": { - "ExecuteTime": { - "end_time": "2024-01-29T16:56:53.408414208Z", - "start_time": "2024-01-29T16:56:53.408012819Z" - } - }, - "outputs": [ - { - "ename": "AttributeError", - "evalue": "type object 'Object' has no attribute 'type'", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[4], line 8\u001b[0m\n\u001b[1;32m 5\u001b[0m robot_position \u001b[38;5;241m=\u001b[39m sqlalchemy\u001b[38;5;241m.\u001b[39morm\u001b[38;5;241m.\u001b[39maliased(Position)\n\u001b[1;32m 6\u001b[0m object_position \u001b[38;5;241m=\u001b[39m sqlalchemy\u001b[38;5;241m.\u001b[39morm\u001b[38;5;241m.\u001b[39maliased(Position)\n\u001b[0;32m----> 8\u001b[0m query \u001b[38;5;241m=\u001b[39m (select(TaskTreeNode\u001b[38;5;241m.\u001b[39mstatus, \u001b[43mObject\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mtype\u001b[49m, \n\u001b[1;32m 9\u001b[0m sqlalchemy\u001b[38;5;241m.\u001b[39mlabel(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mrelative torso height\u001b[39m\u001b[38;5;124m\"\u001b[39m, object_position\u001b[38;5;241m.\u001b[39mz \u001b[38;5;241m-\u001b[39m RobotState\u001b[38;5;241m.\u001b[39mtorso_height),\n\u001b[1;32m 10\u001b[0m sqlalchemy\u001b[38;5;241m.\u001b[39mlabel(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mx\u001b[39m\u001b[38;5;124m\"\u001b[39m, robot_position\u001b[38;5;241m.\u001b[39mx \u001b[38;5;241m-\u001b[39m object_position\u001b[38;5;241m.\u001b[39mx),\n\u001b[1;32m 11\u001b[0m sqlalchemy\u001b[38;5;241m.\u001b[39mlabel(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124my\u001b[39m\u001b[38;5;124m\"\u001b[39m, robot_position\u001b[38;5;241m.\u001b[39my \u001b[38;5;241m-\u001b[39m object_position\u001b[38;5;241m.\u001b[39my))\n\u001b[1;32m 12\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(TaskTreeNode\u001b[38;5;241m.\u001b[39mcode)\n\u001b[1;32m 13\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(Code\u001b[38;5;241m.\u001b[39mdesignator\u001b[38;5;241m.\u001b[39mof_type(ORMPickUpAction))\n\u001b[1;32m 14\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(ORMPickUpAction\u001b[38;5;241m.\u001b[39mrobot_state)\n\u001b[1;32m 15\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(robot_pose, RobotState\u001b[38;5;241m.\u001b[39mpose)\n\u001b[1;32m 16\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(robot_position, robot_pose\u001b[38;5;241m.\u001b[39mposition)\n\u001b[1;32m 17\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(ORMPickUpAction\u001b[38;5;241m.\u001b[39mobject)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(object_pose, Object\u001b[38;5;241m.\u001b[39mpose)\n\u001b[1;32m 19\u001b[0m \u001b[38;5;241m.\u001b[39mjoin(object_position, object_pose\u001b[38;5;241m.\u001b[39mposition))\n\u001b[1;32m 20\u001b[0m \u001b[38;5;28mprint\u001b[39m(query)\n\u001b[1;32m 22\u001b[0m df \u001b[38;5;241m=\u001b[39m pd\u001b[38;5;241m.\u001b[39mread_sql(query, session\u001b[38;5;241m.\u001b[39mget_bind())\n", - "\u001b[0;31mAttributeError\u001b[0m: type object 'Object' has no attribute 'type'" - ] - } - ], - "source": [ - "from pycram.orm.base import Pose as ORMPose\n", - "\n", - "robot_pose = sqlalchemy.orm.aliased(ORMPose)\n", - "object_pose = sqlalchemy.orm.aliased(ORMPose)\n", - "robot_position = sqlalchemy.orm.aliased(Position)\n", - "object_position = sqlalchemy.orm.aliased(Position)\n", - "\n", - "query = (select(TaskTreeNode.status, Object.type, \n", - " sqlalchemy.label(\"relative torso height\", object_position.z - RobotState.torso_height),\n", - " sqlalchemy.label(\"x\", robot_position.x - object_position.x),\n", - " sqlalchemy.label(\"y\", robot_position.y - object_position.y))\n", - " .join(TaskTreeNode.code)\n", - " .join(Code.designator.of_type(ORMPickUpAction))\n", - " .join(ORMPickUpAction.robot_state)\n", - " .join(robot_pose, RobotState.pose)\n", - " .join(robot_position, robot_pose.position)\n", - " .join(ORMPickUpAction.object)\n", - " .join(object_pose, Object.pose)\n", - " .join(object_position, object_pose.position))\n", - "print(query)\n", - "\n", - "df = pd.read_sql(query, session.get_bind())\n", - "df[\"status\"] = df[\"status\"].apply(lambda x: str(x.name))\n", - "print(df)" - ] - }, - { - "cell_type": "markdown", - "id": "bb133d09", - "metadata": {}, - "source": [ - "Obviously the query returned every row of the database since we didn't apply any filters.\n", - "\n", - "Why is this query interesting? This query not only required more joins and the usage of the of_type() function, but we actually needed to access two of the tables twice with different purposes, namely the Pose and Position tables. We wanted to get the position of the robot relative to the object position, meaning we had to obtain all robot positions and all object positions. If we want to access the same table twice, we have to make sure to rename (one of) the occurrences in our query in order to provide proper sql syntax. This can be done by creating aliases using the sqlalchemy.orm.aliased() function. Sqlalchemy will automatically rename all the aliased tables for you during runtime." - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/orm_querying_examples.md b/examples/orm_querying_examples.md new file mode 100644 index 000000000..b24c7f740 --- /dev/null +++ b/examples/orm_querying_examples.md @@ -0,0 +1,257 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# ORM querying examples + +In this tutorial, we will get to see more examples of ORM querying. + + +First, we will gather a lot of data. In order to achieve that we will write a randomized experiment for grasping a couple of objects. +In the experiment the robot will try to grasp a randomized object using random poses and torso heights. + +```python +from tf import transformations +import itertools +from typing import Optional, List, Tuple +import numpy as np +import sqlalchemy.orm +import tqdm +import pycram.orm.base +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object as BulletWorldObject +from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction, + ParkArmsActionPerformable, MoveTorsoActionPerformable +from pycram.designators.object_designator import ObjectDesignatorDescription +from pycram.failures import PlanFailure +from pycram.process_module import ProcessModule +from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode +from pycram.process_module import simulated_robot +from pycram.orm.action_designator import PickUpAction as ORMPickUpAction +from pycram.orm.base import RobotState, Position, ProcessMetaData, Pose as ORMPose +from pycram.orm.tasktree import TaskTreeNode +from pycram.orm.object_designator import Object +from pycram.tasktree import task_tree, TaskTree +import pycram.orm +import sqlalchemy.sql +import pandas as pd + +from pycram.datastructures.pose import Pose + +np.random.seed(420) + +ProcessModule.execution_delay = False +ProcessMetaData().description = "Tutorial for learning from experience in a Grasping action." + + +class GraspingExplorer: + """Class to try randomized grasping plans.""" + + world: Optional[BulletWorld] + + def __init__(self, robots: Optional[List[Tuple[str, str]]] = None, objects: Optional[List[Tuple[str, str]]] = None, + arms: Optional[List[Arms]] = None, grasps: Optional[List[Grasp]] = None, + samples_per_scenario: int = 1000): + """ + Create a GraspingExplorer. + :param robots: The robots to use + :param objects: The objects to try to grasp + :param arms: The arms of the robot to use + :param grasps: The grasp orientations to use + :param samples_per_scenario: The number of tries per scenario. + """ + # store exploration space + if not robots: + self.robots: List[Tuple[str, str]] = [("pr2", "pr2.urdf")] + + if not objects: + self.objects: List[Tuple[str, ObjectType, str]] = [ + ("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl"), + ("bowl", ObjectType.BOWL, "bowl.stl"), + ("milk", ObjectType.MILK, "milk.stl"), + ("spoon", ObjectType.SPOON, "spoon.stl")] + + if not arms: + self.arms: List[Arms] = [Arms.LEFT, Arms.RIGHT] + + if not grasps: + self.grasps: List[Grasp] = [Grasp.LEFT, Grasp.RIGHT, Grasp.FRONT, Grasp.TOP] + + # store trials per scenario + self.samples_per_scenario: int = samples_per_scenario + + # chain hyperparameters + self.hyper_parameters = [self.robots, self.objects, self.arms, self.grasps] + + self.total_tries = 0 + self.total_failures = 0 + + def perform(self, session: sqlalchemy.orm.Session): + """ + Perform all experiments. + :param session: The database-session to insert the samples in. + """ + + # create progress bar + progress_bar = tqdm.tqdm( + total=np.prod([len(p) for p in self.hyper_parameters]) * self.samples_per_scenario) + + self.world = BulletWorld(WorldMode.DIRECT) + + # for every robot + for robot, robot_urdf in self.robots: + + # spawn it + robot = BulletWorldObject(robot, ObjectType.ROBOT, robot_urdf) + + # for every obj + for obj, obj_type, obj_stl in self.objects: + + # spawn it + bw_object = BulletWorldObject(obj, obj_type, obj_stl, pose=Pose([0, 0, 0.75], [0, 0, 0, 1])) + + # create object designator + object_designator = ObjectDesignatorDescription(names=[obj]) + + # for every arm and grasp pose + for arm, grasp in itertools.product(self.arms, self.grasps): + # sample positions in 2D + positions = np.random.uniform([-2, -2], [2, 2], (self.samples_per_scenario, 2)) + + # for every position + for position in positions: + + # set z axis to 0 + position = [*position, 0] + + # calculate orientation for robot to face the object + angle = np.arctan2(position[1], position[0]) + np.pi + orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) + + # try to execute a grasping plan + with simulated_robot: + + ParkArmsActionPerformable(Arms.BOTH).perform() + # navigate to sampled position + NavigateAction([Pose(position, orientation)]).resolve().perform() + + # move torso + height = np.random.uniform(0., 0.33, 1)[0] + MoveTorsoActionPerformable(height).perform() + + # try to pick it up + try: + PickUpAction(object_designator, [arm], [grasp]).resolve().perform() + + # if it fails + except PlanFailure: + + # update failure stats + self.total_failures += 1 + + # reset BulletWorld + self.world.reset_world() + + # update progress bar + self.total_tries += 1 + + # insert into database + task_tree.root.insert(session, use_progress_bar=False) + task_tree.reset_tree() + + progress_bar.update() + progress_bar.set_postfix(success_rate=(self.total_tries - self.total_failures) / + self.total_tries) + + bw_object.remove() + robot.remove() + +``` + +Next we have to establish a connection to a database and execute the experiment a couple of times. Note that the (few) number of samples we generate is only for demonstrations. +For robust and reliable machine learning millions of samples are required. + + +```python +engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:") +session = sqlalchemy.orm.Session(bind=engine) +pycram.orm.base.Base.metadata.create_all(bind=engine) +session.commit() + +explorer = GraspingExplorer(samples_per_scenario=30) +explorer.perform(session) +``` + +The success_rate of the output above indicates how many of our samples succeeded in trying to grasp a randomized object. + + +Now that we have data to query from and a running session, we can actually start creating queries. +Let's say we want to select positions of robots that were able to grasp a specific object (in this case a "milk" object): + +```python +from sqlalchemy import select +from pycram.datastructures.enums import ObjectType + +milk = BulletWorldObject("milk", ObjectType.MILK, "milk.stl") + +# query all relative robot positions in regard to an objects position +# make sure to order the joins() correctly +query = (select(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y) + .join(TaskTreeNode.action.of_type(ORMPickUpAction)) + .join(ORMPickUpAction.robot_state) + .join(RobotState.pose) + .join(ORMPose.position) + .join(ORMPickUpAction.object).where(Object.obj_type == milk.obj_type) + .where(TaskTreeNode.status == "SUCCEEDED")) +print(query) + +df = pd.read_sql_query(query, session.get_bind()) +print(df) +``` + +If you are not familiar with sqlalchemy querying you might wonder what the of_type() function does and why we needed it in this query: + +In order to understand the importance of the of_type() function in the joins above it is crucial to understand the inheritance structure in the ORM package. The action necessary for this query is the PickUpAction. It inherits the Action class/table (which holds all the actions). The Action class itself on the other hand inherits Designator (which holds all the actions, but also all the motions). +We started our joins by joining TaskTreeNode on its relationship to Code and Code on its relationship to Designator. Next table we need is the PickUpAction table, but there is no specified relationship between Designator and PickUpAction. But we do know that a PickUpAction is actually a Designator, meaning, it inherits from Designator. So we can just "tell" the join to join Code on every Designator, that is "of_type" PickUpAction (.join(Code.designator.of_type(ORMPickUpAction))). +The effect of this function can also be seen in the printed query of above's output. + + +Another interesting query: Let's say we want to select the torso height and positions of robots relative to the object they were trying to grasp: + +```python +robot_pose = sqlalchemy.orm.aliased(ORMPose) +object_pose = sqlalchemy.orm.aliased(ORMPose) +robot_position = sqlalchemy.orm.aliased(Position) +object_position = sqlalchemy.orm.aliased(Position) + +query = (select(TaskTreeNode.status, Object.obj_type, + sqlalchemy.label("relative torso height", object_position.z - RobotState.torso_height), + sqlalchemy.label("x", robot_position.x - object_position.x), + sqlalchemy.label("y", robot_position.y - object_position.y)) + .join(TaskTreeNode.action.of_type(ORMPickUpAction)) + .join(ORMPickUpAction.robot_state) + .join(robot_pose, RobotState.pose) + .join(robot_position, robot_pose.position) + .join(ORMPickUpAction.object) + .join(object_pose, Object.pose) + .join(object_position, object_pose.position)) +print(query) + +df = pd.read_sql(query, session.get_bind()) +df["status"] = df["status"].apply(lambda x: str(x.name)) +print(df) +``` + +Obviously the query returned every row of the database since we didn't apply any filters. + +Why is this query interesting? This query not only required more joins and the usage of the of_type() function, but we actually needed to access two of the tables twice with different purposes, namely the Pose and Position tables. We wanted to get the position of the robot relative to the object position, meaning we had to obtain all robot positions and all object positions. If we want to access the same table twice, we have to make sure to rename (one of) the occurrences in our query in order to provide proper sql syntax. This can be done by creating aliases using the sqlalchemy.orm.aliased() function. Sqlalchemy will automatically rename all the aliased tables for you during runtime. diff --git a/examples/pose.ipynb b/examples/pose.ipynb deleted file mode 100644 index 6b1afebb4..000000000 --- a/examples/pose.ipynb +++ /dev/null @@ -1,693 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "5c5c430e", - "metadata": {}, - "source": [ - "# Pose\n", - "Poses in PyCRAM are represented by the Pose class which inherits from the PoseStamped message of ROS. This makes PyCRAMs poses compatible with everything in ROS like services, topics or TF. \n", - "\n", - "This notebook will provide an overview about poses, how to use them and what they can do. We will start by simply creating a pose. " - ] - }, - { - "cell_type": "markdown", - "id": "2bf9ef26", - "metadata": {}, - "source": [ - "Before we start a few words about naming convention of Poses in PyCRAM. Naming convention is similar to the PoseStamped message so if you are familiar with that this should be easy. \n", - "\n", - "* **Position:** A position means the position in cartesian space, so the x, y, and z coordinates. \n", - "* **Orientation:** An orientation is the rotation in all three axes represented as a quaternion with x, y, z, w.\n", - "* **Pose:** A pose is the combination of a position and an orientation. Poses in PyCRAM also contain a frame of reference to which the position and orientation are relative." - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "98706668", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1719917019\n", - " nsecs: 639300107\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1\n", - " y: 2\n", - " z: 3\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n", - "print(example_pose)" - ] - }, - { - "cell_type": "markdown", - "id": "423aa431", - "metadata": {}, - "source": [ - "As you can see we created the ```example_pose``` with a position of ```[1, 2, 3]``` and an orientation of ```[0, 0, 0, 1]``` in the frame ```map```. But we don't need to provide all these parameters for a Pose, in case there is no parameter the Pose will use default parameter. " - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "2be360e2", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448769\n", - " nsecs: 128770112\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "default_pose = Pose()\n", - "print(default_pose)" - ] - }, - { - "cell_type": "markdown", - "id": "42d7d949", - "metadata": {}, - "source": [ - "In case no parameter is provided the defualt parameter are:\n", - "\n", - "* position: ```[0, 0, 0]```\n", - "* orientation: ```[o, 0, 0, 1]```\n", - "* frame: ```map```\n", - "\n", - "The following example will show how to access the data stored in a pose. " - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "27efe5d3", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Access to a component of the position: 2\n", - "Access to a component of the rotation: 0.0\n", - "Get the whole position as geometry_msgs/Pose:\n", - "x: 1\n", - "y: 2\n", - "z: 3\n", - "You can also get position or orientation as a list: [1, 2, 3]\n", - "Same with the whole pose: [[1, 2, 3], [0.0, 0.0, 0.0, 1.0]]\n", - "Access the reference frame: map\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n", - "\n", - "print(f\"Access to a component of the position: {example_pose.position.y}\")\n", - "\n", - "print(f\"Access to a component of the rotation: {example_pose.orientation.x}\")\n", - "\n", - "print(f\"Get the whole position as geometry_msgs/Pose:\\n{example_pose.position}\")\n", - "\n", - "print(f\"You can also get position or orientation as a list: {example_pose.position_as_list()}\")\n", - "\n", - "print(f\"Same with the whole pose: {example_pose.to_list()}\")\n", - "\n", - "print(f\"Access the reference frame: {example_pose.frame}\")" - ] - }, - { - "cell_type": "markdown", - "id": "ac538586", - "metadata": {}, - "source": [ - "## Editing a pose\n", - "You can also edit the data saved in a Pose, similar to how you access it.\n" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "d515f704", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Edit only one component:\n", - "x: 3\n", - "y: 2\n", - "z: 3 \n", - "\n", - "Edit the whole position:\n", - "x: 0\n", - "y: 0\n", - "z: 1 \n", - "\n", - "Set a new frame:\n", - "new_frame \n", - "\n", - "Set the position via method:\n", - "x: 3\n", - "y: 2\n", - "z: 1 \n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n", - "\n", - "# Edit a single component of the position \n", - "example_pose.position.x = 3\n", - "print(f\"Edit only one component:\\n{example_pose.position}\", \"\\n\")\n", - "\n", - "# Edit the whole position\n", - "example_pose.position = [0, 0, 1]\n", - "print(f\"Edit the whole position:\\n{example_pose.position}\", \"\\n\")\n", - "\n", - "example_pose.frame = \"new_frame\"\n", - "print(f\"Set a new frame:\\n{example_pose.frame}\", \"\\n\")\n", - "\n", - "example_pose.set_position([3, 2, 1])\n", - "print(f\"Set the position via method:\\n{example_pose.position}\", \"\\n\")" - ] - }, - { - "cell_type": "markdown", - "id": "270b30f8", - "metadata": {}, - "source": [ - "## Copy Poses\n", - "You can also copy Poses to create a new Pose with the same data. This can be useful if you have a method which would need to alter the Pose, since poses are passed by reference to a method every change done to the Pose in the method would affect the instanced passed to the method. " - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "d455c26c", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448775\n", - " nsecs: 284231901\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1\n", - " y: 2\n", - " z: 3\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0 \n", - "\n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448775\n", - " nsecs: 284231901\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1\n", - " y: 2\n", - " z: 3\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n", - "\n", - "copy_pose = example_pose.copy()\n", - "\n", - "print(example_pose, \"\\n\")\n", - "print(copy_pose)" - ] - }, - { - "cell_type": "markdown", - "id": "1c040833", - "metadata": {}, - "source": [ - "## Convert to Transform\n", - "PyCRAM also has its own transform at which we will take a look in the next section. However, here we will take a look at how to convert a Pose into a Transform. \n", - "\n", - "For this example we will take a Pose which represents the current pose of a milk object and convert it into a Transform which represents the transformation from the ```map``` frame to the ```milk``` frame." - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "b8faef0b", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448777\n", - " nsecs: 693948984\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk\"\n", - "transform: \n", - " translation: \n", - " x: 3\n", - " y: 4\n", - " z: 1\n", - " rotation: \n", - " x: 0.7071067811865476\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 0.7071067811865476\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Pose\n", - "\n", - "milk_pose = Pose([3, 4, 1], [1, 0, 0, 1], \"map\")\n", - "\n", - "milk_transform = milk_pose.to_transform(\"milk\")\n", - "\n", - "print(milk_transform)" - ] - }, - { - "cell_type": "markdown", - "id": "4d480900", - "metadata": {}, - "source": [ - "# Transforms\n", - "Transforms are similar to Poses but instead of representing a Pose in a frame of reference they represent a transformation from one frame of reference to another. For this purpose Transforms have an additional parameter called ```child_frame_id``` which is the frame of reference to which the Transform is pointing. \n", - "\n", - "Transforms in PyCRAM inherit from the TransformStamped message of ROS which makes them, like Poses, compatible to ROS services and topics that expect a TransformStamped message. Therefore, the naming conventions of Transforms are the same as of TransformStamped which. \n", - "\n", - "* **Translation:** The vector describing the transformation in cartesian space. \n", - "* **Rotation:** The quaternion describing the transformation of rotation.\n", - "* **Transform:** The combination of translation and rotation " - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "2436ec84", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448779\n", - " nsecs: 614031314\n", - " frame_id: \"map\"\n", - "child_frame_id: \"object\"\n", - "transform: \n", - " translation: \n", - " x: 1\n", - " y: 2\n", - " z: 2\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "example_transform = Transform([1, 2, 2], [0, 0, 0, 1], \"map\", \"object\")\n", - "\n", - "print(example_transform)" - ] - }, - { - "cell_type": "markdown", - "id": "6831de29", - "metadata": {}, - "source": [ - "Transforms have the same methods to get and set values as Poses have, therefore only a short showcase will be given. For more details please look at the Pose example or the API documentation." - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "313a3e03", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Access the rotation:\n", - "x: 0.0\n", - "y: 0.0\n", - "z: 0.7071067811865475\n", - "w: 0.7071067811865475 \n", - "\n", - "Access the child_frane: object \n", - "\n", - "New translation:\n", - "x: 1\n", - "y: 1\n", - "z: 1\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "example_transform = Transform([2, 5, 1], [0, 0, 1, 1], \"map\", \"object\")\n", - "\n", - "print(f\"Access the rotation:\\n{example_transform.rotation}\", \"\\n\")\n", - "\n", - "print(f\"Access the child_frane: {example_transform.child_frame_id}\", \"\\n\")\n", - "\n", - "# changing translation and rotation is exactly like with Poses.\n", - "\n", - "example_transform.translation = [1, 1, 1]\n", - "print(f\"New translation:\\n{example_transform.translation}\")" - ] - }, - { - "cell_type": "markdown", - "id": "d128bda5", - "metadata": {}, - "source": [ - "## Convert to Pose and Copy\n", - "Analog to Poses Transforms have a method that converts a Transform to a Pose, in this process the ```child_frame_id``` will be lost. \n", - "\n", - "Also like in Poses Transforms have a ```copy``` method which creates an exact copy of this Transform." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "b93306a4", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "The converted pose:\n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448783\n", - " nsecs: 267217397\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1\n", - " y: 1\n", - " z: 1\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0 \n", - "\n", - "The copied transform:\n", - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448783\n", - " nsecs: 268019437\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk\"\n", - "transform: \n", - " translation: \n", - " x: 1\n", - " y: 1\n", - " z: 1\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "milk_transform = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"milk\")\n", - "\n", - "milk_pose = milk_transform.to_pose()\n", - "\n", - "print(f\"The converted pose:\\n{milk_pose}\", \"\\n\")\n", - "\n", - "example_transform = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"milk\")\n", - "\n", - "copy_transform = example_transform.copy()\n", - "\n", - "print(f\"The copied transform:\\n{copy_transform}\")" - ] - }, - { - "cell_type": "markdown", - "id": "ceb21ff4", - "metadata": {}, - "source": [ - "## Operations on Transforms\n", - "Transforms have, unlike Poses, operations that can be done. These operations are: \n", - "\n", - "* Multiplication \n", - "* Invert\n", - "* InverseTimes\n", - "\n", - "### Multiplication\n", - "We will first take a look at the multiplication of Transforms. We will use an example were we have two Transforms, the first from ```map``` to a ```hand``` frame and the second from the ```hand``` to a ```milk``` frame. By multiplying these two we get the Transform from ```map``` to ```milk``` frame." - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "bd359dc1", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448785\n", - " nsecs: 359950304\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk\"\n", - "transform: \n", - " translation: \n", - " x: 1.1\n", - " y: 1.05\n", - " z: 1.0\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "map_to_hand = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"hand\")\n", - "\n", - "hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], \"hand\", \"milk\")\n", - "\n", - "map_to_milk = map_to_hand * hand_to_milk\n", - "\n", - "print(map_to_milk)" - ] - }, - { - "cell_type": "markdown", - "id": "c9bc5501", - "metadata": {}, - "source": [ - "### Invert \n", - "This inverts a Transform, so in we have a transform from ```map``` to ```milk``` then inverting it results in a Transform from ```milk``` to ```map``` . " - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "9a796303", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448787\n", - " nsecs: 1776695\n", - " frame_id: \"milk\"\n", - "child_frame_id: \"map\"\n", - "transform: \n", - " translation: \n", - " x: -1.0\n", - " y: -1.0\n", - " z: -0.5\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "map_to_milk = Transform([1, 1, 0.5], [0, 0, 0, 1], \"map\", \"milk\")\n", - "\n", - "milk_to_map = map_to_milk.invert()\n", - "\n", - "print(milk_to_map)" - ] - }, - { - "cell_type": "markdown", - "id": "d3f4839f", - "metadata": {}, - "source": [ - "### Inverse Times\n", - "Inverse times combines the inverting and multiplication of Transforms, this results in a 'minus' for Transforms. We will again use the example of a hand holding a milk, but this time we have the Transforms from ```map``` to ```milk``` and ```hand``` to ```milk```. " - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "a133eb99", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448788\n", - " nsecs: 592707633\n", - " frame_id: \"map\"\n", - "child_frame_id: \"hand\"\n", - "transform: \n", - " translation: \n", - " x: 1.0\n", - " y: 1.0\n", - " z: 1.0\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0\n" - ] - } - ], - "source": [ - "from pycram.datastructures.pose import Transform\n", - "\n", - "map_to_milk = Transform([1.1, 1.05, 1], [0, 0, 0, 1], \"map\", \"milk\")\n", - "\n", - "hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], \"hand\", \"milk\")\n", - "\n", - "map_to_milk = map_to_milk.inverse_times(hand_to_milk)\n", - "\n", - "print(map_to_milk)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/pose.md b/examples/pose.md new file mode 100644 index 000000000..39475cd01 --- /dev/null +++ b/examples/pose.md @@ -0,0 +1,254 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Pose + +Poses in PyCRAM are represented by the Pose class which inherits from the PoseStamped message of ROS. This makes PyCRAMs +poses compatible with everything in ROS like services, topics or TF. + +This notebook will provide an overview about poses, how to use them and what they can do. We will start by simply +creating a pose. + +Before we start a few words about naming convention of Poses in PyCRAM. Naming convention is similar to the PoseStamped +message so if you are familiar with that this should be easy. + +* **Position:** A position means the position in cartesian space, so the x, y, and z coordinates. +* **Orientation:** An orientation is the rotation in all three axes represented as a quaternion with x, y, z, w. +* **Pose:** A pose is the combination of a position and an orientation. Poses in PyCRAM also contain a frame of + reference to which the position and orientation are relative. + +```python +from pycram.datastructures.pose import Pose + +example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map") +print(example_pose) +``` + +As you can see we created the ```example_pose``` with a position of ```[1, 2, 3]``` and an orientation +of ```[0, 0, 0, 1]``` in the frame ```map```. But we don't need to provide all these parameters for a Pose, in case +there is no parameter the Pose will use default parameter. + +```python +from pycram.datastructures.pose import Pose + +default_pose = Pose() +print(default_pose) +``` + +In case no parameter is provided the defualt parameter are: + +* position: ```[0, 0, 0]``` +* orientation: ```[o, 0, 0, 1]``` +* frame: ```map``` + +The following example will show how to access the data stored in a pose. + +```python +from pycram.datastructures.pose import Pose + +example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map") + +print(f"Access to a component of the position: {example_pose.position.y}") + +print(f"Access to a component of the rotation: {example_pose.orientation.x}") + +print(f"Get the whole position as geometry_msgs/Pose:\n{example_pose.position}") + +print(f"You can also get position or orientation as a list: {example_pose.position_as_list()}") + +print(f"Same with the whole pose: {example_pose.to_list()}") + +print(f"Access the reference frame: {example_pose.frame}") +``` + +## Editing a pose + +You can also edit the data saved in a Pose, similar to how you access it. + +```python +from pycram.datastructures.pose import Pose + +example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map") + +# Edit a single component of the position +example_pose.position.x = 3 +print(f"Edit only one component:\n{example_pose.position}", "\n") + +# Edit the whole position +example_pose.position = [0, 0, 1] +print(f"Edit the whole position:\n{example_pose.position}", "\n") + +example_pose.frame = "new_frame" +print(f"Set a new frame:\n{example_pose.frame}", "\n") + +example_pose.set_position([3, 2, 1]) +print(f"Set the position via method:\n{example_pose.position}", "\n") +``` + +## Copy Poses + +You can also copy Poses to create a new Pose with the same data. This can be useful if you have a method which would +need to alter the Pose, since poses are passed by reference to a method every change done to the Pose in the method +would affect the instanced passed to the method. + +```python +from pycram.datastructures.pose import Pose + +example_pose = Pose([1, 2, 3], [0, 0, 0, 1], "map") + +copy_pose = example_pose.copy() + +print(example_pose, "\n") +print(copy_pose) +``` + +## Convert to Transform + +PyCRAM also has its own transform at which we will take a look in the next section. However, here we will take a look at +how to convert a Pose into a Transform. + +For this example we will take a Pose which represents the current pose of a milk object and convert it into a Transform +which represents the transformation from the ```map``` frame to the ```milk``` frame. + +```python +from pycram.datastructures.pose import Pose + +milk_pose = Pose([3, 4, 1], [1, 0, 0, 1], "map") + +milk_transform = milk_pose.to_transform("milk") + +print(milk_transform) +``` + +# Transforms + +Transforms are similar to Poses but instead of representing a Pose in a frame of reference they represent a +transformation from one frame of reference to another. For this purpose Transforms have an additional parameter +called ```child_frame_id``` which is the frame of reference to which the Transform is pointing. + +Transforms in PyCRAM inherit from the TransformStamped message of ROS which makes them, like Poses, compatible to ROS +services and topics that expect a TransformStamped message. Therefore, the naming conventions of Transforms are the same +as of TransformStamped which. + +* **Translation:** The vector describing the transformation in cartesian space. +* **Rotation:** The quaternion describing the transformation of rotation. +* **Transform:** The combination of translation and rotation + +```python +from pycram.datastructures.pose import Transform + +example_transform = Transform([1, 2, 2], [0, 0, 0, 1], "map", "object") + +print(example_transform) +``` + +Transforms have the same methods to get and set values as Poses have, therefore only a short showcase will be given. For +more details please look at the Pose example or the API documentation. + +```python +from pycram.datastructures.pose import Transform + +example_transform = Transform([2, 5, 1], [0, 0, 1, 1], "map", "object") + +print(f"Access the rotation:\n{example_transform.rotation}", "\n") + +print(f"Access the child_frane: {example_transform.child_frame_id}", "\n") + +# changing translation and rotation is exactly like with Poses. + +example_transform.translation = [1, 1, 1] +print(f"New translation:\n{example_transform.translation}") +``` + +## Convert to Pose and Copy + +Analog to Poses Transforms have a method that converts a Transform to a Pose, in this process the ```child_frame_id``` +will be lost. + +Also like in Poses Transforms have a ```copy``` method which creates an exact copy of this Transform. + +```python +from pycram.datastructures.pose import Transform + +milk_transform = Transform([1, 1, 1], [0, 0, 0, 1], "map", "milk") + +milk_pose = milk_transform.to_pose() + +print(f"The converted pose:\n{milk_pose}", "\n") + +example_transform = Transform([1, 1, 1], [0, 0, 0, 1], "map", "milk") + +copy_transform = example_transform.copy() + +print(f"The copied transform:\n{copy_transform}") +``` + +## Operations on Transforms + +Transforms have, unlike Poses, operations that can be done. These operations are: + +* Multiplication +* Invert +* InverseTimes + +### Multiplication + +We will first take a look at the multiplication of Transforms. We will use an example were we have two Transforms, the +first from ```map``` to a ```hand``` frame and the second from the ```hand``` to a ```milk``` frame. By multiplying +these two we get the Transform from ```map``` to ```milk``` frame. + +```python +from pycram.datastructures.pose import Transform + +map_to_hand = Transform([1, 1, 1], [0, 0, 0, 1], "map", "hand") + +hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], "hand", "milk") + +map_to_milk = map_to_hand * hand_to_milk + +print(map_to_milk) +``` + +### Invert + +This inverts a Transform, so in we have a transform from ```map``` to ```milk``` then inverting it results in a +Transform from ```milk``` to ```map``` . + +```python +from pycram.datastructures.pose import Transform + +map_to_milk = Transform([1, 1, 0.5], [0, 0, 0, 1], "map", "milk") + +milk_to_map = map_to_milk.invert() + +print(milk_to_map) +``` + +### Inverse Times + +Inverse times combines the inverting and multiplication of Transforms, this results in a 'minus' for Transforms. We will +again use the example of a hand holding a milk, but this time we have the Transforms from ```map``` to ```milk``` +and ```hand``` to ```milk```. + +```python +from pycram.datastructures.pose import Transform + +map_to_milk = Transform([1.1, 1.05, 1], [0, 0, 0, 1], "map", "milk") + +hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], "hand", "milk") + +map_to_milk = map_to_milk.inverse_times(hand_to_milk) + +print(map_to_milk) +``` diff --git a/examples/robot_description.ipynb b/examples/robot_description.ipynb deleted file mode 100644 index 08323ad5a..000000000 --- a/examples/robot_description.ipynb +++ /dev/null @@ -1,275 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "b2098e0b", - "metadata": {}, - "source": [ - "# Robot Description\n", - "The robot description contains semantic information about the robot which can not be extracted from the URDF in a general way. This inludes kinematic chains, end-effectors, cameras and their parameter, etc. \n", - "\n", - "In genral a Robot Description consists a number of different descriptions, these are: \n", - " * RobotDescription \n", - " * KinematicChainDescription\n", - " * EndEffectorDescription\n", - " * CameraDescription\n", - "\n", - "In this example we will create a robot description step-by-step and describe the different components on the way. The robot we will use as an example will be the PR2, the complete PR2 description can also be seen in ``pycram.robot_descriptions.pr2_description``.\n", - "\n", - "## Robot Description Class \n", - "We start by creating an instance of the ``RobotDescription`` class, this will serve as a the main component to which all other descriptions will be added. \n", - "\n", - "To initialize a ``RobotDescription`` we need a few parameter which are: \n", - " * Name \n", - " * base_link \n", - " * torso_link \n", - " * torso_joint\n", - " * Path to a URDF file\n", - " " - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "4857333f", - "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - } - ], - "source": [ - "from pycram.robot_description import RobotDescription\n", - "import rospkg\n", - "\n", - "rospack = rospkg.RosPack()\n", - "filename = rospack.get_path('pycram') + '/resources/robots/' + \"pr2\" + '.urdf'\n", - "\n", - "pr2_description = RobotDescription(\"pr2\", \"base_link\", \"torso_lift_link\", \"torso_lift_joint\", filename)" - ] - }, - { - "cell_type": "markdown", - "id": "4a0bd430", - "metadata": {}, - "source": [ - "## Kinematic Chain Description \n", - "The kinematic chain description describes a chain of links and joints of the robot which might be interesting when working with the robot. An example of such a chain would be the arm of the robot, when programming for the robot it is important to know which links and joints exactly make up the arm, however, these can not be extracted from the URDF automatically. \n", - "\n", - "The kinematic chain is based upon the URDF, meaning when initializing the description one only needs to specify the first and last link of the chain. \n", - "\n", - "We will now create the kinematic chain description for the right arm of the PR2. For initializing the ``KinematicChainDescription`` the following parameter are needed: \n", - " * Name\n", - " * first link\n", - " * last link \n", - " * URDF object\n", - " * Arm type\n", - " \n", - "The arm type specifies which arm this kinematic chain describes, this is needed when one wants to access only the arms of the robot." - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "9bcb35b2", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.robot_description import KinematicChainDescription\n", - "\n", - "right_arm = KinematicChainDescription(\"right\", \"torso_lift_link\", \"r_wrist_roll_link\",\n", - " pr2_description.urdf_object, arm_type=Arms.RIGHT)" - ] - }, - { - "cell_type": "markdown", - "id": "e63061a0", - "metadata": {}, - "source": [ - "The created ``KinematicChainDescription`` can now be added to the robot description." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "0dc4a9aa", - "metadata": {}, - "outputs": [], - "source": [ - "pr2_description.add_kinematic_chain_description(right_arm)" - ] - }, - { - "cell_type": "markdown", - "id": "546d5e02", - "metadata": {}, - "source": [ - "## End Effector Description\n", - "Since kinematic chains only describe a moveable chain of links and joints like arms these do not represent end-effectors which can be used to do manipulation tasks. \n", - "\n", - "To represent end-effectors we will create an ``EndEffectorDescription`` which contains the information of the respective end-effector. When creating an ``EndEffectorDescription`` we need the following parameter:\n", - " * Name \n", - " * first link \n", - " * tool_frame\n", - " * URDF object\n", - " \n", - "You might have noticed that the end-efftor only has a first link but no last link, this is the case since end-effectors are at the end of the arms. Therefore, all links and joints below a certain link can be seen as part of the end-effector. " - ] - }, - { - "cell_type": "code", - "execution_count": 12, - "id": "26e04470", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.robot_description import EndEffectorDescription\n", - "\n", - "right_gripper = EndEffectorDescription(\"right_gripper\", \"r_gripper_palm_link\", \"r_gripper_tool_frame\",\n", - " pr2_description.urdf_object)" - ] - }, - { - "cell_type": "markdown", - "id": "7676ffd4", - "metadata": {}, - "source": [ - "The gripper can no be added to the previously created ``KinematicChainDescription``. " - ] - }, - { - "cell_type": "code", - "execution_count": 13, - "id": "3543f03e", - "metadata": {}, - "outputs": [], - "source": [ - "right_arm.end_effector = right_gripper" - ] - }, - { - "cell_type": "markdown", - "id": "1a834355", - "metadata": {}, - "source": [ - "## Camera Description \n", - "The camera description contains all parameters of a camera, which is mounted on the robot. The parameter for the ``CameraDescription`` are:\n", - " * Name\n", - " * Link name \n", - " * minimal height \n", - " * maximal height\n", - " * horizontal angle \n", - " * vertical angle " - ] - }, - { - "cell_type": "code", - "execution_count": 14, - "id": "4af8825c", - "metadata": {}, - "outputs": [], - "source": [ - "from pycram.robot_description import CameraDescription\n", - "\n", - "camera = CameraDescription(\"kinect_camera\", \"wide_stereo_optical_frame\", 1.27,\n", - " 1.60, 0.99483, 0.75049)" - ] - }, - { - "cell_type": "markdown", - "id": "ccedd9b5", - "metadata": {}, - "source": [ - "The finished camera description can now be added to the robot description. " - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "c9e65919", - "metadata": {}, - "outputs": [], - "source": [ - "pr2_description.add_camera_description(camera)" - ] - }, - { - "cell_type": "markdown", - "id": "364a9628", - "metadata": {}, - "source": [ - "## Grasps \n", - "Grasps define how a robot interacts with objects. The grasps defined in the robot description define for each grasp (right, left, top, front) the orientation of the end-effector, relative to the base_frame of the robot, to achieve the respective grasp. " - ] - }, - { - "cell_type": "code", - "execution_count": 16, - "id": "73d85e49", - "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "shutdown request: [/pycram] Reason: new node registered with same name\n" - ] - } - ], - "source": [ - "pr2_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1],\n", - " Grasp.LEFT: [0, 0, -1, 1],\n", - " Grasp.RIGHT: [0, 0, 1, 1],\n", - " Grasp.TOP: [0, 1, 0, 1]})" - ] - }, - { - "cell_type": "markdown", - "id": "c47a9198", - "metadata": {}, - "source": [ - "## Register Robot Description\n", - "Lastly, you need to register the robot description to the ``RobotDescriptionManager``. As you can see the code to register the robot description has to be executed at the start of PyCRAM, if you put your file with the robot description in the `pycram.robot_descriptions` directory it will be executed upon the start of PyCRAM." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "3dce0d87", - "metadata": {}, - "outputs": [], - "source": [ - "rdm = RobotDescriptionManager()\n", - "rdm.register_description(pr2_description)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.10" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} diff --git a/examples/robot_description.md b/examples/robot_description.md new file mode 100644 index 000000000..9782b2cf7 --- /dev/null +++ b/examples/robot_description.md @@ -0,0 +1,171 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 + language: python + name: python3 +--- + +# Robot Description + +(robot_description_header)= +The robot description contains semantic information about the robot which can not be extracted from the URDF in a +general way. This inludes kinematic chains, end-effectors, cameras and their parameter, etc. + +In genral a Robot Description consists a number of different descriptions, these are: + +* RobotDescription +* KinematicChainDescription +* EndEffectorDescription +* CameraDescription + +In this example we will create a robot description step-by-step and describe the different components on the way. The +robot we will use as an example will be the PR2, the complete PR2 description can also be seen in +{meth}`pycram.robot_descriptions.pr2_description`. + +## Robot Description Class + +We start by creating an instance of the {class}`~pycram.robot_description.RobotDescription` class, this will serve as a +the main component to which all other descriptions will be added. + +To initialize a {class}`~pycram.robot_description.RobotDescription` we need a few parameter which are: + +* Name +* base_link +* torso_link +* torso_joint +* Path to a URDF file + +```python +from pycram.robot_description import RobotDescription +import rospkg + +rospack = rospkg.RosPack() +filename = rospack.get_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' + +pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", filename) +``` + +## Kinematic Chain Description + +The kinematic chain description describes a chain of links and joints of the robot which might be interesting when +working with the robot. An example of such a chain would be the arm of the robot, when programming for the robot it is +important to know which links and joints exactly make up the arm, however, these can not be extracted from the URDF +automatically. + +The kinematic chain is based upon the URDF, meaning when initializing the description one only needs to specify the +first and last link of the chain. + +We will now create the kinematic chain description for the right arm of the PR2. For initializing +the {class}`~pycram.robot_description.KinematicChainDescription` the following parameter are needed: + +* Name +* first link +* last link +* URDF object +* Arm type + +The arm type specifies which arm this kinematic chain describes, this is needed when one wants to access only the arms +of the robot. + +```python +from pycram.robot_description import KinematicChainDescription +from pycram.datastructures.enums import Arms + +right_arm = KinematicChainDescription("right", "torso_lift_link", "r_wrist_roll_link", + pr2_description.urdf_object, arm_type=Arms.RIGHT) +``` + +The created {class}`~pycram.robot_description.KinematicChainDescription` can now be added to the robot description. + +```python +pr2_description.add_kinematic_chain_description(right_arm) +``` + +## End Effector Description + +Since kinematic chains only describe a moveable chain of links and joints like arms these do not represent end-effectors +which can be used to do manipulation tasks. + +To represent end-effectors we will create an {class}`~pycram.robot_description.EndEffectorDescription` which contains the information of the respective +end-effector. When creating an {class}`~pycram.robot_description.EndEffectorDescription` we need the following parameter: + +* Name +* first link +* tool_frame +* URDF object + +You might have noticed that the end-efftor only has a first link but no last link, this is the case since end-effectors +are at the end of the arms. Therefore, all links and joints below a certain link can be seen as part of the +end-effector. + +```python +from pycram.robot_description import EndEffectorDescription + +right_gripper = EndEffectorDescription("right_gripper", "r_gripper_palm_link", "r_gripper_tool_frame", + pr2_description.urdf_object) +``` + +The gripper can no be added to the previously created {class}`~pycram.robot_description.KinematicChainDescription`. + +```python +right_arm.end_effector = right_gripper +``` + +## Camera Description + +The camera description contains all parameters of a camera, which is mounted on the robot. The parameter for +the {class}`~pycram.robot_description.CameraDescription` are: + +* Name +* Link name +* minimal height +* maximal height +* horizontal angle +* vertical angle + +```python +from pycram.robot_description import CameraDescription +from pycram.datastructures.enums import Grasp + +camera = CameraDescription("kinect_camera", "wide_stereo_optical_frame", 1.27, + 1.60, 0.99483, 0.75049) +``` + +The finished camera description can now be added to the robot description. + +```python +pr2_description.add_camera_description(camera) +``` + +## Grasps + +Grasps define how a robot interacts with objects. The grasps defined in the robot description define for each grasp ( +right, left, top, front) the orientation of the end-effector, relative to the base_frame of the robot, to achieve the +respective grasp. + +```python +pr2_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1], + Grasp.LEFT: [0, 0, -1, 1], + Grasp.RIGHT: [0, 0, 1, 1], + Grasp.TOP: [0, 1, 0, 1]}) +``` + +## Register Robot Description + +Lastly, you need to register the robot description to the {class}`~pycram.robot_description.RobotDescriptionManager`. As you can see the code to +register the robot description has to be executed at the start of PyCRAM, if you put your file with the robot +description in the {class}`pycram.robot_descriptions` directory it will be executed upon the start of PyCRAM. + +```python +from pycram.robot_description import RobotDescriptionManager + +rdm = RobotDescriptionManager() +rdm.register_description(pr2_description) +``` diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index 628fd7f82..4484eaf86 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -44,6 +44,12 @@ textfile="$(find pycram)/resources/robots/stretch_description.urdf"/> + + + + + diff --git a/package.xml b/package.xml index f3b332d63..b13066606 100644 --- a/package.xml +++ b/package.xml @@ -53,7 +53,10 @@ geometry_msgs geometry_msgs + pr2_arm_kinematics + moveit_kinematics + pr2_common diff --git a/pycram-https.rosinstall b/pycram-https.rosinstall index 64bbf0430..94d873ea8 100644 --- a/pycram-https.rosinstall +++ b/pycram-https.rosinstall @@ -11,15 +11,7 @@ repositories: type: git url: https://github.com/cram2/pycram.git version: dev - pr2_common: - type: git - url: https://@github.com/PR2/pr2_common.git - version: b34703bcca2b07cadbc3777d3c504c232a0c0c28 kdl_ik_services: type: git url: https://github.com/cram2/kdl_ik_service.git version: master - pr2_kinematics: - type: git - url: https://github.com/PR2/pr2_kinematics.git - version: kinetic-devel diff --git a/pycram.rosinstall b/pycram.rosinstall index ccbb0cd22..8628e2c22 100644 --- a/pycram.rosinstall +++ b/pycram.rosinstall @@ -11,15 +11,7 @@ repositories: type: git url: git@github.com:cram2/pycram.git version: dev - pr2_common: - type: git - url: git@github.com:PR2/pr2_common.git - version: b34703bcca2b07cadbc3777d3c504c232a0c0c28 kdl_ik_services: type: git url: git@github.com:cram2/kdl_ik_service.git version: master - pr2_kinematics: - type: git - url: git@github.com:PR2/pr2_kinematics.git - version: kinetic-devel diff --git a/requirements-resolver.txt b/requirements-resolver.txt index 9f31a1cb0..efcd1c22b 100644 --- a/requirements-resolver.txt +++ b/requirements-resolver.txt @@ -1,3 +1,2 @@ -r requirements.txt -probabilistic_model>=5.0.3 -random_events>=3.0.4 + diff --git a/requirements-setuptools.txt b/requirements-setuptools.txt new file mode 100644 index 000000000..4ac3b836c --- /dev/null +++ b/requirements-setuptools.txt @@ -0,0 +1,11 @@ +packaging>=24 +ordered-set>=3.1.1 +more_itertools>=8.8 +jaraco.text>=3.7 +importlib_resources>=5.10.2 +importlib_metadata>=6 +tomli>=2.0.1 +wheel>=0.43.0 + + +platformdirs >= 2.6.2 \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index ad3b5fb7e..8814d84c2 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,14 +1,9 @@ +-r requirements-setuptools.txt gitpython>=3.1.37 -pybullet~=3.2.5 -rospkg~=1.4.0 -roslibpy~=1.2.1 -# rospy~=1.14.11 +pycram_bullet==3.2.8 pathlib~=1.0.1 numpy==1.24.3 pytransform3d~=1.9.1 -# tf~=1.12.1 -# actionlib~=1.12.1 -urdf-parser-py~=0.0.3 graphviz anytree>=2.8.0 SQLAlchemy>=2.0.9 @@ -17,16 +12,17 @@ psutil==5.9.7 lxml==4.9.1 typing_extensions==4.9.0 owlready2>=0.45 +Pillow>=10.3.0 +pynput~=1.7.7 +playsound~=1.3.0 +pydub~=0.25.1 +gTTS~=2.5.3 +dm_control +trimesh +deprecated +probabilistic_model>=5.1.0 +random_events>=3.0.7 +sympy Pygments~=2.14.0 -rospy~=1.15.14 -tf~=1.13.2 -rosgraph~=1.15.14 -rosservice~=1.15.14 -rdflib~=6.3.2 -typeguard~=4.3.0 -matplotlib~=3.7.3 -actionlib~=1.13.2 -rosnode~=1.15.14 -roslaunch~=1.15.14 -catkin-pkg~=0.5.2 \ No newline at end of file +typeguard~=4.3.0 \ No newline at end of file diff --git a/resources/kitchen-small.urdf b/resources/kitchen-small.urdf new file mode 100644 index 000000000..18764ed70 --- /dev/null +++ b/resources/kitchen-small.urdf @@ -0,0 +1,2247 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000000 + 1000000000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/IAI_kitchen.urdf b/resources/objects/IAI_kitchen.urdf similarity index 100% rename from resources/IAI_kitchen.urdf rename to resources/objects/IAI_kitchen.urdf diff --git a/resources/Static_CokeBottle.stl b/resources/objects/Static_CokeBottle.stl similarity index 100% rename from resources/Static_CokeBottle.stl rename to resources/objects/Static_CokeBottle.stl diff --git a/resources/Static_MilkPitcher.stl b/resources/objects/Static_MilkPitcher.stl similarity index 100% rename from resources/Static_MilkPitcher.stl rename to resources/objects/Static_MilkPitcher.stl diff --git a/resources/apartment.urdf b/resources/objects/apartment.urdf similarity index 96% rename from resources/apartment.urdf rename to resources/objects/apartment.urdf index 60b5a4d01..2900accb1 100644 --- a/resources/apartment.urdf +++ b/resources/objects/apartment.urdf @@ -4,6 +4,27 @@ + + + + + + + + + + + + + + + + + + + + + @@ -480,6 +501,7 @@ + @@ -501,6 +523,7 @@ + @@ -533,7 +556,7 @@ - + @@ -562,6 +585,7 @@ + @@ -600,6 +624,7 @@ + @@ -678,6 +703,7 @@ + @@ -706,6 +732,7 @@ + @@ -744,6 +771,7 @@ + @@ -765,6 +793,7 @@ + @@ -784,6 +813,7 @@ + @@ -812,6 +842,7 @@ + @@ -858,6 +889,7 @@ + @@ -894,6 +926,7 @@ + @@ -914,6 +947,7 @@ + @@ -936,6 +970,7 @@ + @@ -972,6 +1007,7 @@ + @@ -992,6 +1028,7 @@ + @@ -1030,6 +1067,7 @@ + @@ -1068,6 +1106,7 @@ + @@ -1104,6 +1143,7 @@ + @@ -1124,6 +1164,7 @@ + @@ -1162,6 +1203,7 @@ + @@ -1200,6 +1242,7 @@ + @@ -1237,6 +1280,7 @@ + @@ -1257,6 +1301,7 @@ + @@ -1335,6 +1380,7 @@ + @@ -1355,6 +1401,7 @@ + @@ -1376,6 +1423,7 @@ + @@ -1412,6 +1460,7 @@ + @@ -1432,6 +1481,7 @@ + @@ -1470,6 +1520,7 @@ + @@ -1508,6 +1559,7 @@ + @@ -1544,6 +1596,7 @@ + @@ -1564,6 +1617,7 @@ + @@ -1602,6 +1656,7 @@ + @@ -1640,6 +1695,7 @@ + @@ -1676,6 +1732,7 @@ + @@ -1696,6 +1753,7 @@ + @@ -1734,6 +1792,7 @@ + @@ -1772,6 +1831,7 @@ + @@ -1809,6 +1869,7 @@ + @@ -1827,6 +1888,7 @@ + @@ -1869,6 +1931,7 @@ + @@ -2010,6 +2073,7 @@ + @@ -2028,6 +2092,7 @@ + @@ -2046,6 +2111,7 @@ + @@ -2064,6 +2130,7 @@ + @@ -2082,6 +2149,7 @@ + @@ -2100,6 +2168,7 @@ + diff --git a/resources/apartment_bowl.stl b/resources/objects/apartment_bowl.stl similarity index 100% rename from resources/apartment_bowl.stl rename to resources/objects/apartment_bowl.stl diff --git a/resources/apartment_without_walls.urdf b/resources/objects/apartment_without_walls.urdf similarity index 96% rename from resources/apartment_without_walls.urdf rename to resources/objects/apartment_without_walls.urdf index a807692d1..26f05aec7 100644 --- a/resources/apartment_without_walls.urdf +++ b/resources/objects/apartment_without_walls.urdf @@ -4,6 +4,27 @@ + + + + + + + + + + + + + + + + + + + + + @@ -395,6 +416,7 @@ + @@ -416,6 +438,7 @@ + @@ -448,7 +471,7 @@ - + @@ -477,6 +500,7 @@ + @@ -515,6 +539,7 @@ + @@ -593,6 +618,7 @@ + @@ -621,6 +647,7 @@ + @@ -659,6 +686,7 @@ + @@ -680,6 +708,7 @@ + @@ -699,6 +728,7 @@ + @@ -727,6 +757,7 @@ + @@ -773,6 +804,7 @@ + @@ -809,6 +841,7 @@ + @@ -829,6 +862,7 @@ + @@ -851,6 +885,7 @@ + @@ -887,6 +922,7 @@ + @@ -907,6 +943,7 @@ + @@ -945,6 +982,7 @@ + @@ -983,6 +1021,7 @@ + @@ -1019,6 +1058,7 @@ + @@ -1039,6 +1079,7 @@ + @@ -1077,6 +1118,7 @@ + @@ -1115,6 +1157,7 @@ + @@ -1152,6 +1195,7 @@ + @@ -1172,6 +1216,7 @@ + @@ -1250,6 +1295,7 @@ + @@ -1270,6 +1316,7 @@ + @@ -1291,6 +1338,7 @@ + @@ -1327,6 +1375,7 @@ + @@ -1347,6 +1396,7 @@ + @@ -1385,6 +1435,7 @@ + @@ -1423,6 +1474,7 @@ + @@ -1459,6 +1511,7 @@ + @@ -1479,6 +1532,7 @@ + @@ -1517,6 +1571,7 @@ + @@ -1555,6 +1610,7 @@ + @@ -1591,6 +1647,7 @@ + @@ -1611,6 +1668,7 @@ + @@ -1649,6 +1707,7 @@ + @@ -1687,6 +1746,7 @@ + @@ -1724,6 +1784,7 @@ + @@ -1742,6 +1803,7 @@ + @@ -1784,6 +1846,7 @@ + @@ -1925,6 +1988,7 @@ + @@ -1943,6 +2007,7 @@ + @@ -1961,6 +2026,7 @@ + @@ -1979,6 +2045,7 @@ + @@ -1997,6 +2064,7 @@ + @@ -2015,6 +2083,7 @@ + diff --git a/resources/bowl.stl b/resources/objects/bowl.stl similarity index 100% rename from resources/bowl.stl rename to resources/objects/bowl.stl diff --git a/resources/box.urdf b/resources/objects/box.urdf similarity index 100% rename from resources/box.urdf rename to resources/objects/box.urdf diff --git a/resources/breakfast_cereal.stl b/resources/objects/breakfast_cereal.stl similarity index 100% rename from resources/breakfast_cereal.stl rename to resources/objects/breakfast_cereal.stl diff --git a/resources/broken_kitchen.urdf b/resources/objects/broken_kitchen.urdf similarity index 100% rename from resources/broken_kitchen.urdf rename to resources/objects/broken_kitchen.urdf diff --git a/resources/jeroen_cup.stl b/resources/objects/jeroen_cup.stl similarity index 100% rename from resources/jeroen_cup.stl rename to resources/objects/jeroen_cup.stl diff --git a/resources/kitchen.urdf b/resources/objects/kitchen.urdf similarity index 100% rename from resources/kitchen.urdf rename to resources/objects/kitchen.urdf diff --git a/resources/milk.stl b/resources/objects/milk.stl similarity index 100% rename from resources/milk.stl rename to resources/objects/milk.stl diff --git a/resources/plane.obj b/resources/objects/plane.obj similarity index 100% rename from resources/plane.obj rename to resources/objects/plane.obj diff --git a/resources/plane.urdf b/resources/objects/plane.urdf similarity index 100% rename from resources/plane.urdf rename to resources/objects/plane.urdf diff --git a/resources/spoon.stl b/resources/objects/spoon.stl similarity index 100% rename from resources/spoon.stl rename to resources/objects/spoon.stl diff --git a/resources/robots/pr2.urdf b/resources/robots/pr2.urdf index 671407ef6..439087f3f 100644 --- a/resources/robots/pr2.urdf +++ b/resources/robots/pr2.urdf @@ -55,6 +55,9 @@ + + + @@ -826,7 +829,7 @@ - + @@ -857,7 +860,7 @@ - + @@ -1372,7 +1375,7 @@ - + @@ -1452,7 +1455,7 @@ - + @@ -1571,7 +1574,7 @@ - + @@ -1651,7 +1654,7 @@ - + @@ -1690,7 +1693,7 @@ - + @@ -1800,7 +1803,7 @@ - + @@ -1998,7 +2001,7 @@ - + @@ -2033,7 +2036,7 @@ - + @@ -2337,7 +2340,7 @@ - + @@ -2456,7 +2459,7 @@ - + @@ -2536,7 +2539,7 @@ - + @@ -2575,7 +2578,7 @@ - + @@ -2685,7 +2688,7 @@ - + @@ -2883,7 +2886,7 @@ - + @@ -2918,7 +2921,7 @@ - + diff --git a/resources/robots/turtlebot.urdf b/resources/robots/turtlebot.urdf new file mode 100644 index 000000000..b3a996afd --- /dev/null +++ b/resources/robots/turtlebot.urdf @@ -0,0 +1,371 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + 0.1 + 0.1 + 500000.0 + 10.0 + 0.001 + 0.1 + 1 0 0 + Gazebo/FlatBlack + + + 0.1 + 0.1 + 1000000.0 + 100.0 + 0.001 + 1.0 + Gazebo/FlatBlack + + + 0.1 + 0.1 + 1000000.0 + 100.0 + 0.001 + 1.0 + Gazebo/FlatBlack + + + + true + false + + Gazebo/Grey + + + + cmd_vel + odom + odom + world + true + base_footprint + false + true + true + false + 30 + wheel_left_joint + wheel_right_joint + 0.287 + 0.066 + 1 + 10 + na + + + + + true + imu_link + imu_link + imu + imu_service + 0.0 + 0 + + + gaussian + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + Gazebo/FlatBlack + + 0 0 0 0 0 0 + false + 5 + + + + 360 + 1 + 0.0 + 6.28319 + + + + 0.120 + 3.5 + 0.015 + + + gaussian + 0.0 + 0.01 + + + + scan + base_scan + + + + + + + true + false + + 1.085595 + + 640 + 480 + R8G8B8 + + + 0.03 + 100 + + + + true + 30.0 + camera + camera_rgb_optical_frame + rgb/image_raw + rgb/camera_info + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/worlds/pycram_test.muv b/resources/worlds/pycram_test.muv new file mode 100644 index 000000000..bfe6acb7d --- /dev/null +++ b/resources/worlds/pycram_test.muv @@ -0,0 +1,52 @@ +resources: + - ../cached + - ../robots + - ../worlds + - ../objects + +worlds: + pycram_test: + rtf_desired: 1 + prospection_pycram_test: + rtf_desired: 1 + +simulations: + pycram_test: + simulator: mujoco + world: + name: world + path: apartment/mjcf/apartment.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + prospection_pycram_test: + simulator: mujoco + world: + name: prospection_world + path: apartment/mjcf/apartment.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + +multiverse_server: + host: "tcp://127.0.0.1" + port: 7000 + +multiverse_clients: + pycram_test: + port: 7500 + send: + body: ["position", "quaternion", "relative_velocity"] + joint: ["joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque"] + + prospection_pycram_test: + port: 7600 + send: + body: [ "position", "quaternion", "relative_velocity" ] + joint: [ "joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque" ] \ No newline at end of file diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index 8e87af9b3..9fde2cdac 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -1,33 +1,19 @@ -"""Python3 implementation of CRAM. +from . import process_modules +from . import robot_descriptions +# from .specialized_designators import * -To use macros one must put the code in an own file and create a second file (the launcher) which activates MacroPy and then imports the file where the macros are used. -E. g. if you have a file target.py which contains your code, create a file run.py: +from .datastructures.world import World +import signal +__version__ = "0.0.2" -#!/usr/bin/env python -import macropy.activate -import target +def signal_handler(sig, frame): + if World.current_world: + World.current_world.exit() + print("Exiting...") + exit(0) -Now launch run.py to start your program. +signal.signal(signal.SIGINT, signal_handler) -Modules: -designator_description -- implementation of designators. -fluent -- implementation of fluents and the whenever macro. -helper -- implementation of helper classes and functions for internal usage only. -language -- implementation of the CRAM language. -process_module -- implementation of process modules. -""" -import logging -import logging.config -import pycram.process_modules - -logging.basicConfig(level=logging.WARNING, format='%(levelname)s - %(name)s - Line:%(lineno)d - %(message)s') - -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) -formatter = logging.Formatter('%(levelname)s - %(name)s - Line:%(lineno)d - %(message)s') -ch.setFormatter(formatter) - -#from .specialized_designators import * diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 1c7b86cea..3e6a9889d 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -1,8 +1,9 @@ import glob import os import pathlib +import shutil -from typing_extensions import List, TYPE_CHECKING +from typing_extensions import List, TYPE_CHECKING, Optional if TYPE_CHECKING: from .description import ObjectDescription @@ -14,31 +15,52 @@ class CacheManager: The CacheManager is responsible for caching object description files and managing the cache directory. """ - mesh_extensions: List[str] = [".obj", ".stl"] + cache_cleared: bool = False """ - The file extensions of mesh files. + Indicate whether the cache directory has been cleared at least once since beginning or not. """ - def __init__(self, cache_dir: str, data_directory: List[str]): + def __init__(self, cache_dir: str, data_directory: List[str], clear_cache: bool = True): """ - Initializes the CacheManager. + Initialize the CacheManager. :param cache_dir: The directory where the cached files are stored. :param data_directory: The directory where all resource files are stored. + :param clear_cache: If True, the cache directory will be cleared. """ self.cache_dir = cache_dir - self.data_directory = data_directory + self.data_directories = data_directory + if clear_cache: + self.clear_cache() + + def clear_cache(self): + """ + Clear the cache directory. + """ + self.delete_cache_dir() + self.create_cache_dir_if_not_exists() + self.cache_cleared = True + + def delete_cache_dir(self): + """ + Delete the cache directory. + """ + if pathlib.Path(self.cache_dir).exists(): + shutil.rmtree(self.cache_dir) def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - object_description: 'ObjectDescription', object_name: str) -> str: + object_description: 'ObjectDescription', object_name: str, + scale_mesh: Optional[float] = None) -> str: """ - Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + Check if the file is already in the cache directory, if not preprocess and save in the cache. :param path: The path of the file to preprocess and save in the cache directory. :param ignore_cached_files: If True, the file will be preprocessed and saved in the cache directory even if it is already cached. :param object_description: The object description of the file. :param object_name: The name of the object. + :param scale_mesh: The scale of the mesh. + :return: The path of the cached file. """ path_object = pathlib.Path(path) extension = path_object.suffix @@ -46,49 +68,24 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, self.create_cache_dir_if_not_exists() # save correct path in case the file is already in the cache directory - cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) + cache_path = os.path.join(self.cache_dir, object_description.get_file_name(path_object, extension, object_name)) if not self.is_cached(path, object_description) or ignore_cached_files: # if file is not yet cached preprocess the description file and save it in the cache directory. path = self.look_for_file_in_data_dir(path_object) - self.generate_description_and_write_to_cache(path, object_name, extension, cache_path, object_description) + object_description.generate_description_from_file(path, object_name, extension, cache_path, scale_mesh) return cache_path - def generate_description_and_write_to_cache(self, path: str, name: str, extension: str, cache_path: str, - object_description: 'ObjectDescription') -> None: - """ - Generates the description from the file at the given path and writes it to the cache directory. - - :param path: The path of the file to preprocess. - :param name: The name of the object. - :param extension: The file extension of the file to preprocess. - :param cache_path: The path of the file in the cache directory. - :param object_description: The object description of the file. - """ - description_string = object_description.generate_description_from_file(path, name, extension) - self.write_to_cache(description_string, cache_path) - - @staticmethod - def write_to_cache(description_string: str, cache_path: str) -> None: - """ - Writes the description string to the cache directory. - - :param description_string: The description string to write to the cache directory. - :param cache_path: The path of the file in the cache directory. - """ - with open(cache_path, "w") as file: - file.write(description_string) - def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: """ - Looks for a file in the data directory of the World. If the file is not found in the data directory, this method - raises a FileNotFoundError. + Look for a file in the data directory of the World. If the file is not found in the data directory, raise a + FileNotFoundError. :param path_object: The pathlib object of the file to look for. """ name = path_object.name - for data_dir in self.data_directory: + for data_dir in self.data_directories: data_path = pathlib.Path(data_dir).joinpath("**") for file in glob.glob(str(data_path), recursive=True): file_path = pathlib.Path(file) @@ -97,18 +94,18 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: return str(file_path) raise FileNotFoundError( - f"File {name} could not be found in the resource directory {self.data_directory}") + f"File {name} could not be found in the resource directory {self.data_directories}") def create_cache_dir_if_not_exists(self): """ - Creates the cache directory if it does not exist. + Create the cache directory if it does not exist. """ if not pathlib.Path(self.cache_dir).exists(): os.mkdir(self.cache_dir) def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: """ - Checks if the file in the given path is already cached or if + Check if the file in the given path is already cached or if there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from the parameter server is used. @@ -116,26 +113,26 @@ def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: :param object_description: The object description of the file. :return: True if there already exists a cached file, False in any other case. """ - return True if self.check_with_extension(path) else self.check_without_extension(path, object_description) + return self.check_with_extension(path) or self.check_without_extension(path, object_description) def check_with_extension(self, path: str) -> bool: """ - Checks if the file in the given ath exists in the cache directory including file extension. + Check if the file in the given ath exists in the cache directory including file extension. :param path: The path of the file to check. """ file_name = pathlib.Path(path).name - full_path = pathlib.Path(self.cache_dir + file_name) + full_path = pathlib.Path(os.path.join(self.cache_dir, file_name)) return full_path.exists() def check_without_extension(self, path: str, object_description: 'ObjectDescription') -> bool: """ - Checks if the file in the given path exists in the cache directory without file extension, - the extension is added after the file name manually in this case. + Check if the file in the given path exists in the cache directory the given file extension. + Instead, replace the given extension with the extension of the used ObjectDescription and check for that one. :param path: The path of the file to check. :param object_description: The object description of the file. """ file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(self.cache_dir + file_stem + object_description.get_file_extension()) + full_path = pathlib.Path(os.path.join(self.cache_dir, file_stem + object_description.get_file_extension())) return full_path.exists() diff --git a/src/pycram/config b/src/pycram/config new file mode 120000 index 000000000..899f69898 --- /dev/null +++ b/src/pycram/config @@ -0,0 +1 @@ +../../config \ No newline at end of file diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 462c60233..9ce0ecfdf 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -1,26 +1,35 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from typing_extensions import Tuple, List, Optional - -import matplotlib.pyplot as plt from dataclasses import dataclass +import matplotlib.pyplot as plt import numpy as np import psutil -import rospy +import random_events +import tf from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData - +from probabilistic_model.probabilistic_circuit.nx.distributions import UniformDistribution +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, ProductUnit +from random_events.interval import Interval, reals, closed_open, closed +from random_events.product_algebra import Event, SimpleEvent +from random_events.variable import Continuous +from typing_extensions import Tuple, List, Optional, Iterator + +from .ros.ros_tools import wait_for_message +from .datastructures.dataclasses import AxisAlignedBoundingBox +from .datastructures.pose import Pose from .datastructures.world import UseProspectionWorld -from .world_concepts.world_object import Object +from .datastructures.world import World from .description import Link from .local_transformer import LocalTransformer +from .world_concepts.world_object import Object + from .datastructures.pose import Pose, Transform from .datastructures.world import World from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color - -import pybullet as p +from tf.transformations import quaternion_from_matrix @dataclass @@ -111,42 +120,21 @@ def visualize(self) -> None: # Creation of the visual shapes, for documentation of the visual shapes # please look here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf for box in boxes: - visual = p.createVisualShape(p.GEOM_BOX, - halfExtents=[(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, - 0.001], - rgbaColor=[1, 0, 0, 0.6], - visualFramePosition=[(box[0][0] + box[1] / 2) * self.resolution, - (box[0][1] + box[2] / 2) * self.resolution, 0.]) + box = BoxVisualShape(Color(1, 0, 0, 0.6), + [(box[0][0] + box[1] / 2) * self.resolution, + (box[0][1] + box[2] / 2) * self.resolution, 0.], + [(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, 0.001]) + visual = self.world.create_visual_shape(box) cells.append(visual) # Set to 127 for since this is the maximal amount of links in a multibody for cell_parts in self._chunks(cells, 127): - # Dummy paramater since these are needed to spawn visual shapes as a - # multibody. - link_poses = [[0, 0, 0] for c in cell_parts] - link_orientations = [[0, 0, 0, 1] for c in cell_parts] - link_masses = [1.0 for c in cell_parts] - link_parent = [0 for c in cell_parts] - link_joints = [p.JOINT_FIXED for c in cell_parts] - link_collision = [-1 for c in cell_parts] - link_joint_axis = [[1, 0, 0] for c in cell_parts] - # The position at which the multibody will be spawned. Offset such that - # the origin referes to the centre of the costmap. - # origin_pose = self.origin.position_as_list() - # base_pose = [origin_pose[0] - self.height / 2 * self.resolution, - # origin_pose[1] - self.width / 2 * self.resolution, origin_pose[2]] - offset = [[-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], [0, 0, 0, 1]] - new_pose = p.multiplyTransforms(self.origin.position_as_list(), self.origin.orientation_as_list(), - offset[0], offset[1]) - - map_obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=cell_parts, - basePosition=new_pose[0], baseOrientation=new_pose[1], linkPositions=link_poses, - # [0, 0, 1, 0] - linkMasses=link_masses, linkOrientations=link_orientations, - linkInertialFramePositions=link_poses, - linkInertialFrameOrientations=link_orientations, linkParentIndices=link_parent, - linkJointTypes=link_joints, linkJointAxis=link_joint_axis, - linkCollisionShapeIndices=link_collision) + origin_transform = (Transform(self.origin.position_as_list(), self.origin.orientation_as_list()) + .get_homogeneous_matrix()) + offset_transform = (Transform(offset[0], offset[1]).get_homogeneous_matrix()) + new_pose_transform = np.dot(origin_transform, offset_transform) + new_pose = Pose(new_pose_transform[:3, 3].tolist(), quaternion_from_matrix(new_pose_transform)) + map_obj = self.world.create_multi_body_from_visual_shapes(cell_parts, new_pose) self.vis_ids.append(map_obj) def _chunks(self, lst: List, n: int) -> List: @@ -165,7 +153,7 @@ def close_visualization(self) -> None: Removes the visualization from the World. """ for v_id in self.vis_ids: - self.world.remove_object_by_id(v_id) + self.world.remove_visual_object(v_id) self.vis_ids = [] def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int: @@ -366,7 +354,7 @@ def _get_map() -> np.ndarray: :return: The costmap as a numpy array. """ print("Waiting for Map") - map = rospy.wait_for_message("/map", OccupancyGrid) + map = wait_for_message("/map", OccupancyGrid) print("Recived Map") return np.array(map.data) @@ -379,7 +367,7 @@ def _get_map_metadata() -> MapMetaData: :return: The meta-data for the costmap array. """ print("Waiting for Map Meta Data") - meta = rospy.wait_for_message("/map_metadata", MapMetaData) + meta = wait_for_message("/map_metadata", MapMetaData) print("Recived Meta Data") return meta @@ -448,8 +436,8 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray: indices = np.concatenate(np.dstack(np.mgrid[int(-size / 2):int(size / 2), int(-size / 2):int(size / 2)]), axis=0) * resolution + np.array(origin_position[:2]) # Add the z-coordinate to the grid, which is either 0 or 10 - indices_0 = np.pad(indices, (0, 1), mode='constant', constant_values=0)[:-1] - indices_10 = np.pad(indices, (0, 1), mode='constant', constant_values=10)[:-1] + indices_0 = np.pad(indices, (0, 1), mode='constant', constant_values=5)[:-1] + indices_10 = np.pad(indices, (0, 1), mode='constant', constant_values=0)[:-1] # Zips both arrays such that there are tuples for every coordinate that # only differ in the z-coordinate rays = np.dstack(np.dstack((indices_0, indices_10))).T @@ -461,10 +449,9 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray: i = 0 j = 0 for n in self._chunks(np.array(rays), 16380): - # with UseProspectionWorld(): - r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) + r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) while r_t is None: - r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) + r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) j += len(n) if World.robot: attached_objs_id = [o.id for o in self.world.robot.attachments.keys()] @@ -787,11 +774,10 @@ def generate_map(self) -> None: def get_aabb_for_link(self) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let the - AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the - identity orientation. - :return: Two points in world coordinate space, which span a rectangle + :return: The axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let + the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the + identity orientation. """ prospection_object = World.current_world.get_prospection_object_for_object(self.object) with UseProspectionWorld(): @@ -802,6 +788,160 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: return self.link.get_axis_aligned_bounding_box() +class AlgebraicSemanticCostmap(SemanticCostmap): + """ + Class for a semantic costmap that is based on an algebraic set-description of the valid area. + """ + x: Continuous = Continuous("x") + """ + The variable for height. + """ + + y: Continuous = Continuous("y") + """ + The variable for width. + """ + + original_valid_area: Optional[SimpleEvent] + """ + The original rectangle of the valid area. + """ + + valid_area: Optional[Event] + """ + A description of the valid positions as set. + """ + + number_of_samples: int + """ + The number of samples to generate for the iter. + """ + + def __init__(self, object, urdf_link_name, world=None, number_of_samples=1000): + super().__init__(object, urdf_link_name, world=world) + self.number_of_samples = number_of_samples + + def check_valid_area_exists(self): + assert self.valid_area is not None, ("The map has to be created before semantics can be applied. " + "Call 'generate_map first'") + + def left(self, margin = 0.) -> Event: + """ + Create an event left of the origins Y-Coordinate. + :param margin: The margin of the events left bound. + :return: The left event. + """ + self.check_valid_area_exists() + y_origin = self.origin.position.y + left = self.original_valid_area[self.y].simple_sets[0].lower + left += margin + event = SimpleEvent( + {self.x: reals(), self.y: random_events.interval.open(left, y_origin)}).as_composite_set() + return event + + def right(self, margin = 0.) -> Event: + """ + Create an event right of the origins Y-Coordinate. + :param margin: The margin of the events right bound. + :return: The right event. + """ + self.check_valid_area_exists() + y_origin = self.origin.position.y + right = self.original_valid_area[self.y].simple_sets[0].upper + right -= margin + event = SimpleEvent({self.x: reals(), self.y: closed_open(y_origin, right)}).as_composite_set() + return event + + def top(self, margin = 0.) -> Event: + """ + Create an event above the origins X-Coordinate. + :param margin: The margin of the events upper bound. + :return: The top event. + """ + self.check_valid_area_exists() + x_origin = self.origin.position.x + top = self.original_valid_area[self.x].simple_sets[0].upper + top -= margin + event = SimpleEvent( + {self.x: random_events.interval.closed_open(x_origin, top), self.y: reals()}).as_composite_set() + return event + + def bottom(self, margin = 0.) -> Event: + """ + Create an event below the origins X-Coordinate. + :param margin: The margin of the events lower bound. + :return: The bottom event. + """ + self.check_valid_area_exists() + x_origin = self.origin.position.x + lower = self.original_valid_area[self.x].simple_sets[0].lower + lower += margin + event = SimpleEvent( + {self.x: random_events.interval.open(lower, x_origin), self.y: reals()}).as_composite_set() + return event + + def inner(self, margin=0.2): + min_x = self.original_valid_area[self.x].simple_sets[0].lower + max_x = self.original_valid_area[self.x].simple_sets[0].upper + min_y = self.original_valid_area[self.y].simple_sets[0].lower + max_y = self.original_valid_area[self.y].simple_sets[0].upper + + min_x += margin + max_x -= margin + min_y += margin + max_y -= margin + + inner_event = SimpleEvent({self.x: closed(min_x, max_x), + self.y: closed(min_y, max_y)}).as_composite_set() + return inner_event + + def border(self, margin=0.2): + return ~self.inner(margin) + + def generate_map(self) -> None: + super().generate_map() + valid_area = Event() + for rectangle in self.partitioning_rectangles(): + # rectangle.scale(1/self.resolution, 1/self.resolution) + rectangle.translate(self.origin.position.x, self.origin.position.y) + valid_area.simple_sets.add(SimpleEvent({self.x: closed(rectangle.x_lower, rectangle.x_upper), + self.y: closed(rectangle.y_lower, rectangle.y_upper)})) + + assert len(valid_area.simple_sets) == 1, ("The map at the basis of a Semantic costmap must be an axis aligned" + "bounding box") + self.valid_area = valid_area + self.original_valid_area = self.valid_area.simple_sets[0] + + def as_distribution(self) -> ProbabilisticCircuit: + p_xy = ProductUnit() + u_x = UniformDistribution(self.x, self.original_valid_area[self.x].simple_sets[0]) + u_y = UniformDistribution(self.y, self.original_valid_area[self.y].simple_sets[0]) + p_xy.add_subcircuit(u_x) + p_xy.add_subcircuit(u_y) + + conditional, _ = p_xy.conditional(self.valid_area) + return conditional.probabilistic_circuit + + def sample_to_pose(self, sample: np.ndarray) -> Pose: + """ + Convert a sample from the costmap to a pose. + :param sample: The sample to convert + :return: The pose corresponding to the sample + """ + x = sample[0] + y = sample[1] + position = [x, y, self.origin.position.z] + angle = np.arctan2(position[1] - self.origin.position.y, position[0] - self.origin.position.x) + np.pi + orientation = list(tf.transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) + return Pose(position, orientation, self.origin.frame) + + def __iter__(self) -> Iterator[Pose]: + model = self.as_distribution() + samples = model.sample(self.number_of_samples) + for sample in samples: + yield self.sample_to_pose(sample) + + cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index aae3b82d8..cb8b1b14c 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -1,10 +1,14 @@ from __future__ import annotations +from abc import ABC, abstractmethod +from copy import deepcopy, copy from dataclasses import dataclass + from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from .enums import JointType, Shape + +from .enums import JointType, Shape, VirtualMobileBaseJointName from .pose import Pose, Point -from abc import ABC, abstractmethod +from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable if TYPE_CHECKING: from ..description import Link @@ -14,7 +18,7 @@ def get_point_as_list(point: Point) -> List[float]: """ - Returns the point as a list. + Return the point as a list. :param point: The point. :return: The point as a list @@ -37,7 +41,7 @@ class Color: @classmethod def from_list(cls, color: List[float]): """ - Sets the rgba_color from a list of RGBA values. + Set the rgba_color from a list of RGBA values. :param color: The list of RGBA values """ @@ -51,7 +55,7 @@ def from_list(cls, color: List[float]): @classmethod def from_rgb(cls, rgb: List[float]): """ - Sets the rgba_color from a list of RGB values. + Set the rgba_color from a list of RGB values. :param rgb: The list of RGB values """ @@ -60,7 +64,7 @@ def from_rgb(cls, rgb: List[float]): @classmethod def from_rgba(cls, rgba: List[float]): """ - Sets the rgba_color from a list of RGBA values. + Set the rgba_color from a list of RGBA values. :param rgba: The list of RGBA values """ @@ -68,7 +72,7 @@ def from_rgba(cls, rgba: List[float]): def get_rgba(self) -> List[float]: """ - Returns the rgba_color as a list of RGBA values. + Return the rgba_color as a list of RGBA values. :return: The rgba_color as a list of RGBA values """ @@ -76,7 +80,7 @@ def get_rgba(self) -> List[float]: def get_rgb(self) -> List[float]: """ - Returns the rgba_color as a list of RGB values. + Return the rgba_color as a list of RGB values. :return: The rgba_color as a list of RGB values """ @@ -98,7 +102,7 @@ class AxisAlignedBoundingBox: @classmethod def from_min_max(cls, min_point: List[float], max_point: List[float]): """ - Sets the axis-aligned bounding box from a minimum and maximum point. + Set the axis-aligned bounding box from a minimum and maximum point. :param min_point: The minimum point :param max_point: The maximum point @@ -107,48 +111,36 @@ def from_min_max(cls, min_point: List[float], max_point: List[float]): def get_min_max_points(self) -> Tuple[Point, Point]: """ - Returns the axis-aligned bounding box as a tuple of minimum and maximum points. - :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ return self.get_min_point(), self.get_max_point() def get_min_point(self) -> Point: """ - Returns the axis-aligned bounding box as a minimum point. - :return: The axis-aligned bounding box as a minimum point """ return Point(self.min_x, self.min_y, self.min_z) def get_max_point(self) -> Point: """ - Returns the axis-aligned bounding box as a maximum point. - :return: The axis-aligned bounding box as a maximum point """ return Point(self.max_x, self.max_y, self.max_z) def get_min_max(self) -> Tuple[List[float], List[float]]: """ - Returns the axis-aligned bounding box as a tuple of minimum and maximum points. - :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ return self.get_min(), self.get_max() def get_min(self) -> List[float]: """ - Returns the minimum point of the axis-aligned bounding box. - :return: The minimum point of the axis-aligned bounding box """ return [self.min_x, self.min_y, self.min_z] def get_max(self) -> List[float]: """ - Returns the maximum point of the axis-aligned bounding box. - :return: The maximum point of the axis-aligned bounding box """ return [self.max_x, self.max_y, self.max_z] @@ -156,12 +148,19 @@ def get_max(self) -> List[float]: @dataclass class CollisionCallbacks: + """ + Dataclass for storing the collision callbacks which are callables that get called when there is a collision + or when a collision is no longer there. + """ on_collision_cb: Callable no_collision_cb: Optional[Callable] = None @dataclass class MultiBody: + """ + Dataclass for storing the information of a multibody which consists of a base and multiple links with joints. + """ base_visual_shape_index: int base_pose: Pose link_visual_shape_indices: List[int] @@ -176,13 +175,16 @@ class MultiBody: @dataclass class VisualShape(ABC): + """ + Abstract dataclass for storing the information of a visual shape. + """ rgba_color: Color visual_frame_position: List[float] @abstractmethod def shape_data(self) -> Dict[str, Any]: """ - Returns the shape data of the visual shape (e.g. half extents for a box, radius for a sphere). + :return: the shape data of the visual shape (e.g. half extents for a box, radius for a sphere) as a dictionary. """ pass @@ -190,13 +192,16 @@ def shape_data(self) -> Dict[str, Any]: @abstractmethod def visual_geometry_type(self) -> Shape: """ - Returns the visual geometry type of the visual shape (e.g. box, sphere). + :return: The visual geometry type of the visual shape (e.g. box, sphere) as a Shape object. """ pass @dataclass class BoxVisualShape(VisualShape): + """ + Dataclass for storing the information of a box visual shape + """ half_extents: List[float] def shape_data(self) -> Dict[str, List[float]]: @@ -213,6 +218,9 @@ def size(self) -> List[float]: @dataclass class SphereVisualShape(VisualShape): + """ + Dataclass for storing the information of a sphere visual shape + """ radius: float def shape_data(self) -> Dict[str, float]: @@ -225,6 +233,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class CapsuleVisualShape(VisualShape): + """ + Dataclass for storing the information of a capsule visual shape + """ radius: float length: float @@ -238,6 +249,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class CylinderVisualShape(CapsuleVisualShape): + """ + Dataclass for storing the information of a cylinder visual shape + """ @property def visual_geometry_type(self) -> Shape: @@ -246,6 +260,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class MeshVisualShape(VisualShape): + """ + Dataclass for storing the information of a mesh visual shape + """ scale: List[float] file_name: str @@ -259,6 +276,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class PlaneVisualShape(VisualShape): + """ + Dataclass for storing the information of a plane visual shape + """ normal: List[float] def shape_data(self) -> Dict[str, List[float]]: @@ -271,33 +291,417 @@ def visual_geometry_type(self) -> Shape: @dataclass class State(ABC): + """ + Abstract dataclass for storing the state of an entity (e.g. world, object, link, joint). + """ pass @dataclass class LinkState(State): + """ + Dataclass for storing the state of a link. + """ constraint_ids: Dict[Link, int] + def __eq__(self, other: 'LinkState'): + return self.all_constraints_exist(other) and self.all_constraints_are_equal(other) + + def all_constraints_exist(self, other: 'LinkState') -> bool: + """ + Check if all constraints exist in the other link state. + + :param other: The state of the other link. + :return: True if all constraints exist, False otherwise. + """ + return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) + and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) + + def all_constraints_are_equal(self, other: 'LinkState') -> bool: + """ + Check if all constraints are equal to the ones in the other link state. + + :param other: The state of the other link. + :return: True if all constraints are equal, False otherwise. + """ + return all([cid == other_cid for cid, other_cid in zip(self.constraint_ids.values(), + other.constraint_ids.values())]) + + def __copy__(self): + return LinkState(constraint_ids=copy(self.constraint_ids)) + @dataclass class JointState(State): + """ + Dataclass for storing the state of a joint. + """ position: float + acceptable_error: float + + def __eq__(self, other: 'JointState'): + error = calculate_joint_position_error(self.position, other.position) + return is_error_acceptable(error, other.acceptable_error) + + def __copy__(self): + return JointState(position=self.position, acceptable_error=self.acceptable_error) @dataclass class ObjectState(State): + """ + Dataclass for storing the state of an object. + """ pose: Pose attachments: Dict[Object, Attachment] link_states: Dict[int, LinkState] joint_states: Dict[int, JointState] + acceptable_pose_error: Tuple[float, float] + + def __eq__(self, other: 'ObjectState'): + return (self.pose_is_almost_equal(other) + and self.all_attachments_exist(other) and self.all_attachments_are_equal(other) + and self.link_states == other.link_states + and self.joint_states == other.joint_states) + + def pose_is_almost_equal(self, other: 'ObjectState') -> bool: + """ + Check if the pose of the object is almost equal to the pose of another object. + + :param other: The state of the other object. + :return: True if the poses are almost equal, False otherwise. + """ + return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) + + def all_attachments_exist(self, other: 'ObjectState') -> bool: + """ + Check if all attachments exist in the other object state. + + :param other: The state of the other object. + :return: True if all attachments exist, False otherwise. + """ + return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) + and len(self.attachments.keys()) == len(other.attachments.keys())) + + def all_attachments_are_equal(self, other: 'ObjectState') -> bool: + """ + Check if all attachments are equal to the ones in the other object state. + + :param other: The state of the other object. + :return: True if all attachments are equal, False otherwise + """ + return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) + + def __copy__(self): + return ObjectState(pose=self.pose.copy(), attachments=copy(self.attachments), + link_states=copy(self.link_states), + joint_states=copy(self.joint_states), + acceptable_pose_error=deepcopy(self.acceptable_pose_error)) @dataclass class WorldState(State): - simulator_state_id: int + """ + Dataclass for storing the state of the world. + """ + simulator_state_id: Optional[int] object_states: Dict[str, ObjectState] + def __eq__(self, other: 'WorldState'): + return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) + and self.all_objects_states_are_equal(other)) + + def simulator_state_is_equal(self, other: 'WorldState') -> bool: + """ + Check if the simulator state is equal to the simulator state of another world state. + + :param other: The state of the other world. + :return: True if the simulator states are equal, False otherwise. + """ + return self.simulator_state_id == other.simulator_state_id + + def all_objects_exist(self, other: 'WorldState') -> bool: + """ + Check if all objects exist in the other world state. + + :param other: The state of the other world. + :return: True if all objects exist, False otherwise. + """ + return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) + and len(self.object_states.keys()) == len(other.object_states.keys())) + + def all_objects_states_are_equal(self, other: 'WorldState') -> bool: + """ + Check if all object states are equal to the ones in the other world state. + + :param other: The state of the other world. + :return: True if all object states are equal, False otherwise. + """ + return all([obj_state == other_obj_state + for obj_state, other_obj_state in zip(self.object_states.values(), + other.object_states.values())]) + + def __copy__(self): + return WorldState(simulator_state_id=self.simulator_state_id, + object_states=deepcopy(self.object_states)) + + +@dataclass +class LateralFriction: + """ + Dataclass for storing the information of the lateral friction. + """ + lateral_friction: float + lateral_friction_direction: List[float] + + +@dataclass +class ContactPoint: + """ + Dataclass for storing the information of a contact point between two objects. + """ + link_a: Link + link_b: Link + position_on_object_a: Optional[List[float]] = None + position_on_object_b: Optional[List[float]] = None + normal_on_b: Optional[List[float]] = None # normal on object b pointing towards object a + distance: Optional[float] = None + normal_force: Optional[List[float]] = None # normal force applied during last step simulation + lateral_friction_1: Optional[LateralFriction] = None + lateral_friction_2: Optional[LateralFriction] = None + force_x_in_world_frame: Optional[float] = None + force_y_in_world_frame: Optional[float] = None + force_z_in_world_frame: Optional[float] = None + + def __str__(self): + return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}" + + def __repr__(self): + return self.__str__() + + +ClosestPoint = ContactPoint +""" +The closest point between two objects which has the same structure as ContactPoint. +""" + + +class ContactPointsList(list): + """ + A list of contact points. + """ + def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: + """ + Return the links that are not in the current points list but were in the initial points list. + + :param previous_points: The initial points list. + :return: A list of Link instances that represent the links that got removed. + """ + initial_links_in_contact = previous_points.get_links_in_contact() + current_links_in_contact = self.get_links_in_contact() + return [link for link in initial_links_in_contact if link not in current_links_in_contact] + + def get_links_in_contact(self) -> List[Link]: + """ + Get the links in contact. + + :return: A list of Link instances that represent the links in contact. + """ + return [point.link_b for point in self] + + def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> bool: + """ + Check if two objects are in contact. + + :param obj_a: An instance of the Object class that represents the first object. + :param obj_b: An instance of the Object class that represents the second object. + :return: True if the objects are in contact, False otherwise. + """ + return (any([point.link_b.object == obj_b and point.link_a.object == obj_a for point in self]) or + any([point.link_a.object == obj_b and point.link_b.object == obj_a for point in self])) + + def get_normals_of_object(self, obj: Object) -> List[List[float]]: + """ + Get the normals of the object. + + :param obj: An instance of the Object class that represents the object. + :return: A list of float vectors that represent the normals of the object. + """ + return self.get_points_of_object(obj).get_normals() + + def get_normals(self) -> List[List[float]]: + """ + Get the normals of the points. + + :return: A list of float vectors that represent the normals of the contact points. + """ + return [point.normal_on_b for point in self] + + def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: + """ + Get the links in contact of the object. + + :param obj: An instance of the Object class that represents the object. + :return: A list of Link instances that represent the links in contact of the object. + """ + return [point.link_b for point in self if point.link_b.object == obj] + + def get_points_of_object(self, obj: Object) -> 'ContactPointsList': + """ + Get the points of the object. + + :param obj: An instance of the Object class that represents the object that the points are related to. + :return: A ContactPointsList instance that represents the contact points of the object. + """ + return ContactPointsList([point for point in self if point.link_b.object == obj]) + + def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: + """ + Return the object that is not in the current points list but was in the initial points list. + + :param previous_points: The initial points list. + :return: A list of Object instances that represent the objects that got removed. + """ + initial_objects_in_contact = previous_points.get_objects_that_have_points() + current_objects_in_contact = self.get_objects_that_have_points() + return [obj for obj in initial_objects_in_contact if obj not in current_objects_in_contact] + + def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: + """ + Return the object that is not in the initial points list but is in the current points list. + + :param previous_points: The initial points list. + :return: A list of Object instances that represent the new objects. + """ + initial_objects_in_contact = previous_points.get_objects_that_have_points() + current_objects_in_contact = self.get_objects_that_have_points() + return [obj for obj in current_objects_in_contact if obj not in initial_objects_in_contact] + + def is_object_in_the_list(self, obj: Object) -> bool: + """ + Check if the object is one of the objects that have points in the list. + + :param obj: An instance of the Object class that represents the object. + :return: True if the object is in the list, False otherwise. + """ + return obj in self.get_objects_that_have_points() + + def get_names_of_objects_that_have_points(self) -> List[str]: + """ + Return the names of the objects that have points in the list. + + :return: A list of strings that represent the names of the objects that have points in the list. + """ + return [obj.name for obj in self.get_objects_that_have_points()] + + def get_objects_that_have_points(self) -> List[Object]: + """ + Return the objects that have points in the list. + + :return: A list of Object instances that represent the objects that have points in the list. + """ + return list({point.link_b.object for point in self}) + + def __str__(self): + return f"ContactPointsList: {', '.join([point.__str__() for point in self])}" + + def __repr__(self): + return self.__str__() + + +ClosestPointsList = ContactPointsList +""" +The list of closest points which has same structure as ContactPointsList. +""" + + +@dataclass +class TextAnnotation: + """ + Dataclass for storing text annotations that can be displayed in the simulation. + """ + text: str + position: List[float] + id: int + color: Color = Color(0, 0, 0, 1) + size: float = 0.1 + + +@dataclass +class VirtualMobileBaseJoints: + """ + Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot. + """ + translation_x: Optional[str] = VirtualMobileBaseJointName.LINEAR_X.value + translation_y: Optional[str] = VirtualMobileBaseJointName.LINEAR_Y.value + angular_z: Optional[str] = VirtualMobileBaseJointName.ANGULAR_Z.value + + @property + def names(self) -> List[str]: + """ + Return the names of the virtual mobile base joints. + """ + return [self.translation_x, self.translation_y, self.angular_z] + + def get_types(self) -> Dict[str, JointType]: + """ + Return the joint types of the virtual mobile base joints. + """ + return {self.translation_x: JointType.PRISMATIC, + self.translation_y: JointType.PRISMATIC, + self.angular_z: JointType.REVOLUTE} + + def get_axes(self) -> Dict[str, Point]: + """ + Return the axes (i.e. The axis on which the joint moves) of the virtual mobile base joints. + """ + return {self.translation_x: Point(1, 0, 0), + self.translation_y: Point(0, 1, 0), + self.angular_z: Point(0, 0, 1)} + + +@dataclass +class MultiverseMetaData: + """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" + world_name: str = "world" + simulation_name: str = "cram" + length_unit: str = "m" + angle_unit: str = "rad" + mass_unit: str = "kg" + time_unit: str = "s" + handedness: str = "rhs" + + +@dataclass +class RayResult: + """ + A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the + distance from the ray origin to the intersection point. + """ + body_name: str + distance: float + + def intersected(self) -> bool: + """ + Check if the ray intersects with a body. + return: Whether the ray intersects with a body. + """ + return self.distance >= 0 and self.body_name != "" + + +@dataclass +class MultiverseContactPoint: + """ + A dataclass to store the contact point returned from Multiverse. + """ + body_name: str + contact_force: List[float] + contact_torque: List[float] + @dataclass class ReasoningResult: + """ + Result of a reasoning result of knowledge source + """ success: bool reasoned_parameter: Dict[str, Any] \ No newline at end of file diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index ce2ef5439..92968ba61 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -2,20 +2,24 @@ from enum import Enum, auto +from ..failures import UnsupportedJointType + + class ExecutionType(Enum): """Enum for Execution Process Module types.""" REAL = auto() SIMULATED = auto() SEMI_REAL = auto() -class Arms(Enum): + +class Arms(int, Enum): """Enum for Arms.""" - LEFT = auto() - RIGHT = auto() - BOTH = auto() + LEFT = 0 + RIGHT = 1 + BOTH = 2 -class TaskStatus(Enum): +class TaskStatus(int, Enum): """ Enum for readable descriptions of a tasks' status. """ @@ -39,7 +43,7 @@ class JointType(Enum): FLOATING = 7 -class Grasp(Enum): +class Grasp(int, Enum): """ Enum for Grasp orientations. """ @@ -49,7 +53,7 @@ class Grasp(Enum): TOP = 3 -class ObjectType(Enum): +class ObjectType(int, Enum): """ Enum for Object types to easier identify different objects """ @@ -66,7 +70,7 @@ class ObjectType(Enum): HUMAN = auto() -class State(Enum): +class State(int, Enum): """ Enumeration which describes the result of a language expression. """ @@ -125,3 +129,145 @@ class GripperType(Enum): CUSTOM = auto() +class PerceptionTechniques(Enum): + """ + Enum for techniques for perception tasks. + """ + ALL = auto() + HUMAN = auto() + TYPES = auto() + + +class ImageEnum(Enum): + """ + Enum for image switch view on hsrb display. + """ + HI = 0 + TALK = 1 + DISH = 2 + DONE = 3 + DROP = 4 + HANDOVER = 5 + ORDER = 6 + PICKING = 7 + PLACING = 8 + REPEAT = 9 + SEARCH = 10 + WAVING = 11 + FOLLOWING = 12 + DRIVINGBACK = 13 + PUSHBUTTONS = 14 + FOLLOWSTOP = 15 + JREPEAT = 16 + SOFA = 17 + INSPECT = 18 + CHAIR = 37 + + +class VirtualMobileBaseJointName(Enum): + """ + Enum for the joint names of the virtual mobile base. + """ + LINEAR_X = "odom_vel_lin_x_joint" + LINEAR_Y = "odom_vel_lin_y_joint" + ANGULAR_Z = "odom_vel_ang_z_joint" + + +class MJCFGeomType(Enum): + """ + Enum for the different geom types in a MuJoCo XML file. + """ + BOX = "box" + CYLINDER = "cylinder" + CAPSULE = "capsule" + SPHERE = "sphere" + PLANE = "plane" + MESH = "mesh" + ELLIPSOID = "ellipsoid" + HFIELD = "hfield" + SDF = "sdf" + + +MJCFBodyType = MJCFGeomType +""" +Alias for MJCFGeomType. As the body type is the same as the geom type. +""" + + +class MJCFJointType(Enum): + """ + Enum for the different joint types in a MuJoCo XML file. + """ + FREE = "free" + BALL = "ball" + SLIDE = "slide" + HINGE = "hinge" + FIXED = "fixed" # Added for compatibility with PyCRAM, but not a real joint type in MuJoCo. + + +class MultiverseAPIName(Enum): + """ + Enum for the different APIs of the Multiverse. + """ + GET_CONTACT_BODIES = "get_contact_bodies" + GET_CONSTRAINT_EFFORT = "get_constraint_effort" + ATTACH = "attach" + DETACH = "detach" + GET_RAYS = "get_rays" + EXIST = "exist" + PAUSE = "pause" + UNPAUSE = "unpause" + SAVE = "save" + LOAD = "load" + + +class MultiverseProperty(Enum): + def __str__(self): + return self.value + + +class MultiverseBodyProperty(MultiverseProperty): + """ + Enum for the different properties of a body the Multiverse. + """ + POSITION = "position" + ORIENTATION = "quaternion" + RELATIVE_VELOCITY = "relative_velocity" + + +class MultiverseJointProperty(MultiverseProperty): + pass + + +class MultiverseJointPosition(MultiverseJointProperty): + """ + Enum for the Position names of the different joint types in the Multiverse. + """ + REVOLUTE_JOINT_POSITION = "joint_rvalue" + PRISMATIC_JOINT_POSITION = "joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: JointType) -> 'MultiverseJointPosition': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointPosition.REVOLUTE_JOINT_POSITION + elif joint_type == JointType.PRISMATIC: + return MultiverseJointPosition.PRISMATIC_JOINT_POSITION + else: + raise UnsupportedJointType(joint_type) + + +class MultiverseJointCMD(MultiverseJointProperty): + """ + Enum for the Command names of the different joint types in the Multiverse. + """ + REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" + PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: JointType) -> 'MultiverseJointCMD': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointCMD.REVOLUTE_JOINT_CMD + elif joint_type == JointType.PRISMATIC: + return MultiverseJointCMD.PRISMATIC_JOINT_CMD + else: + raise UnsupportedJointType(joint_type) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 2fcdae63a..e5e226861 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -3,15 +3,19 @@ import math import datetime -from typing_extensions import List, Union, Optional + +from tf.transformations import euler_from_quaternion +from typing_extensions import List, Union, Optional, Sized, Self import numpy as np -import rospy import sqlalchemy.orm from geometry_msgs.msg import PoseStamped, TransformStamped, Vector3, Point from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) from tf import transformations from ..orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData +from ..ros.data_types import Time +from ..validation.error_checkers import calculate_pose_error +from ..ros.logging import logwarn, logerr def get_normalized_quaternion(quaternion: np.ndarray) -> GeoQuaternion: @@ -22,7 +26,7 @@ def get_normalized_quaternion(quaternion: np.ndarray) -> GeoQuaternion: :return: The normalized quaternion """ mag = math.sqrt(sum(v**2 for v in quaternion)) - normed_rotation = quaternion / mag + normed_rotation = [f / mag for f in quaternion] geo_quaternion = GeoQuaternion() geo_quaternion.x = normed_rotation[0] @@ -47,7 +51,7 @@ class Pose(PoseStamped): """ def __init__(self, position: Optional[List[float]] = None, orientation: Optional[List[float]] = None, - frame: str = "map", time: rospy.Time = None): + frame: str = "map", time: Time = None): """ Poses can be initialized by a position and orientation given as lists, this is optional. By default, Poses are initialized with the position being [0, 0, 0], the orientation being [0, 0, 0, 1] and the frame being 'map'. @@ -68,7 +72,7 @@ def __init__(self, position: Optional[List[float]] = None, orientation: Optional self.header.frame_id = frame - self.header.stamp = time if time else rospy.Time.now() + self.header.stamp = time if time else Time().now() self.frame = frame @@ -85,6 +89,32 @@ def from_pose_stamped(pose_stamped: PoseStamped) -> Pose: p.pose = pose_stamped.pose return p + def get_position_diff(self, target_pose: Self) -> Point: + """ + Get the difference between the target and the current positions. + + :param target_pose: The target pose. + :return: The difference between the two positions. + """ + return Point(target_pose.position.x - self.position.x, target_pose.position.y - self.position.y, + target_pose.position.z - self.position.z) + + def get_z_angle_difference(self, target_pose: Self) -> float: + """ + Get the difference between two z angles. + + :param target_pose: The target pose. + :return: The difference between the two z angles. + """ + return target_pose.z_angle - self.z_angle + + @property + def z_angle(self) -> float: + """ + The z angle of the orientation of this Pose in radians. + """ + return euler_from_quaternion(self.orientation_as_list())[2] + @property def frame(self) -> str: """ @@ -119,7 +149,7 @@ def position(self, value) -> None: """ if (not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoPose) and not isinstance(value, Point)): - rospy.logerr("Position can only be a list or geometry_msgs/Pose") + logerr("Position can only be a list or geometry_msgs/Pose") raise TypeError("Position can only be a list/tuple or geometry_msgs/Pose") if isinstance(value, list) or isinstance(value, tuple) and len(value) == 3: self.pose.position.x = value[0] @@ -144,21 +174,22 @@ def orientation(self, value) -> None: :param value: New orientation, either a list or geometry_msgs/Quaternion """ - if not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoQuaternion): - rospy.logwarn("Orientation can only be a list or geometry_msgs/Quaternion") + if not isinstance(value, Sized) and not isinstance(value, GeoQuaternion): + logwarn("Orientation can only be an iterable (list, tuple, ...etc.) or a geometry_msgs/Quaternion") return - if isinstance(value, list) or isinstance(value, tuple) and len(value) == 4: + if isinstance(value, Sized) and len(value) == 4: orientation = np.array(value) - else: + elif isinstance(value, GeoQuaternion): orientation = np.array([value.x, value.y, value.z, value.w]) + else: + logerr("Orientation has to be a list or geometry_msgs/Quaternion") + raise TypeError("Orientation has to be a list or geometry_msgs/Quaternion") # This is used instead of np.linalg.norm since numpy is too slow on small arrays self.pose.orientation = get_normalized_quaternion(orientation) def to_list(self) -> List[List[float]]: """ - Returns the position and orientation of this pose as a list containing two list. - :return: The position and orientation as lists """ return [[self.pose.position.x, self.pose.position.y, self.pose.position.z], @@ -186,16 +217,12 @@ def copy(self) -> Pose: def position_as_list(self) -> List[float]: """ - Returns only the position as a list of xyz. - - :return: The position as a list + :return: The position as a list of xyz values. """ return [self.position.x, self.position.y, self.position.z] def orientation_as_list(self) -> List[float]: """ - Returns only the orientation as a list of a quaternion - :return: The orientation as a quaternion with xyzw """ return [self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w] @@ -230,6 +257,22 @@ def __eq__(self, other: Pose) -> bool: return self_position == other_position and self_orient == other_orient and self.frame == other.frame + def almost_equal(self, other: Pose, position_tolerance_in_meters: float = 1e-3, + orientation_tolerance_in_degrees: float = 1) -> bool: + """ + Checks if the given Pose is almost equal to this Pose. The position and orientation can have a certain + tolerance. The position tolerance is given in meters and the orientation tolerance in degrees. The position + error is calculated as the euclidian distance between the positions and the orientation error as the angle + between the quaternions. + + :param other: The other Pose which should be compared + :param position_tolerance_in_meters: The tolerance for the position in meters + :param orientation_tolerance_in_degrees: The tolerance for the orientation in degrees + :return: True if the Poses are almost equal, False otherwise + """ + error = calculate_pose_error(self, other) + return error[0] <= position_tolerance_in_meters and error[1] <= orientation_tolerance_in_degrees * math.pi / 180 + def set_position(self, new_position: List[float]) -> None: """ Sets the position of this Pose to the given position. Position has to be given as a vector in cartesian space. @@ -285,25 +328,6 @@ def multiply_quaternions(self, quaternion: List) -> None: self.orientation = (x, y, z, w) - def set_orientation_from_euler(self, axis: List, euler_angles: List[float]) -> None: - """ - Convert axis-angle to quaternion. - - :param axis: (x, y, z) tuple representing rotation axis. - :param angle: rotation angle in degree - :return: The quaternion representing the axis angle - """ - angle = math.radians(euler_angles) - axis_length = math.sqrt(sum([i ** 2 for i in axis])) - normalized_axis = tuple(i / axis_length for i in axis) - - x = normalized_axis[0] * math.sin(angle / 2) - y = normalized_axis[1] * math.sin(angle / 2) - z = normalized_axis[2] * math.sin(angle / 2) - w = math.cos(angle / 2) - - return (x, y, z, w) - class Transform(TransformStamped): """ @@ -319,7 +343,7 @@ class Transform(TransformStamped): Rotation: A quaternion representing the conversion of rotation between both frames """ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional[List[float]] = None, - frame: Optional[str] = "map", child_frame: Optional[str] = "", time: rospy.Time = None): + frame: Optional[str] = "map", child_frame: Optional[str] = "", time: Time = None): """ Transforms take a translation, rotation, frame and child_frame as optional arguments. If nothing is given the Transform will be initialized with [0, 0, 0] for translation, [0, 0, 0, 1] for rotation, 'map' for frame and an @@ -342,10 +366,31 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional self.header.frame_id = frame self.child_frame_id = child_frame - self.header.stamp = time if time else rospy.Time.now() + self.header.stamp = time if time else Time().now() self.frame = frame + def apply_transform_to_array_of_points(self, points: np.ndarray) -> np.ndarray: + """ + Applies this Transform to an array of points. The points are given as a Nx3 matrix, where N is the number of + points. The points are transformed from the child_frame_id to the frame_id of this Transform. + + :param points: The points that should be transformed, given as a Nx3 matrix. + """ + homogeneous_transform = self.get_homogeneous_matrix() + # add the homogeneous coordinate, by adding a column of ones to the position vectors, becoming 4xN matrix + homogenous_points = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1).T + rays_end_positions = homogeneous_transform @ homogenous_points + return rays_end_positions[:3, :].T + + def get_homogeneous_matrix(self) -> np.ndarray: + """ + :return: The homogeneous matrix of this Transform + """ + translation = transformations.translation_matrix(self.translation_as_list()) + rotation = transformations.quaternion_matrix(self.rotation_as_list()) + return np.dot(translation, rotation) + @classmethod def from_pose_and_child_frame(cls, pose: Pose, child_frame_name: str) -> Transform: return cls(pose.position_as_list(), pose.orientation_as_list(), pose.frame, child_frame_name, @@ -386,7 +431,7 @@ def frame(self, value: str) -> None: self.header.frame_id = value @property - def translation(self) -> None: + def translation(self) -> Vector3: """ Property that points to the translation of this Transform """ @@ -401,7 +446,7 @@ def translation(self, value) -> None: :param value: The new value for the translation, either a list or geometry_msgs/Vector3 """ if not isinstance(value, list) and not isinstance(value, Vector3): - rospy.logwarn("Value of a translation can only be a list of a geometry_msgs/Vector3") + logwarn("Value of a translation can only be a list of a geometry_msgs/Vector3") return if isinstance(value, list) and len(value) == 3: self.transform.translation.x = value[0] @@ -411,7 +456,7 @@ def translation(self, value) -> None: self.transform.translation = value @property - def rotation(self) -> None: + def rotation(self) -> Quaternion: """ Property that points to the rotation of this Transform """ @@ -426,7 +471,7 @@ def rotation(self, value): :param value: The new value for the rotation, either a list or geometry_msgs/Quaternion """ if not isinstance(value, list) and not isinstance(value, GeoQuaternion): - rospy.logwarn("Value of the rotation can only be a list or a geometry.msgs/Quaternion") + logwarn("Value of the rotation can only be a list or a geometry.msgs/Quaternion") return if isinstance(value, list) and len(value) == 4: rotation = np.array(value) @@ -449,16 +494,12 @@ def copy(self) -> Transform: def translation_as_list(self) -> List[float]: """ - Returns the translation of this Transform as a list. - :return: The translation as a list of xyz """ return [self.transform.translation.x, self.transform.translation.y, self.transform.translation.z] def rotation_as_list(self) -> List[float]: """ - Returns the rotation of this Transform as a list representing a quaternion. - :return: The rotation of this Transform as a list with xyzw """ return [self.transform.rotation.x, self.transform.rotation.y, self.transform.rotation.z, @@ -494,7 +535,7 @@ def __mul__(self, other: Transform) -> Union[Transform, None]: :return: The resulting Transform from the multiplication """ if not isinstance(other, Transform): - rospy.logerr(f"Can only multiply two Transforms") + logerr(f"Can only multiply two Transforms") return self_trans = transformations.translation_matrix(self.translation_as_list()) self_rot = transformations.quaternion_matrix(self.rotation_as_list()) @@ -553,5 +594,3 @@ def set_rotation(self, new_rotation: List[float]) -> None: :param new_rotation: The new rotation as a quaternion with xyzw """ self.rotation = new_rotation - - diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index b3364455a..faa0ab68e 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -8,7 +8,7 @@ from .pose import Pose from typing_extensions import List, Iterable, Dict, Any, Callable, Type, TYPE_CHECKING from anytree import NodeMixin, PreOrderIter, Node -from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ +from ..failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ GripperOccupied, PlanFailure if TYPE_CHECKING: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 77f6f1c2d..9097b7e85 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -6,100 +6,39 @@ import time from abc import ABC, abstractmethod from copy import copy -from queue import Queue - import numpy as np -import rospy from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING -from typing_extensions import Union +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type from ..cache_manager import CacheManager -from .enums import JointType, ObjectType, WorldMode -from ..world_concepts.event import Event +from ..config.world_conf import WorldConfig +from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, + SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, WorldState, ClosestPointsList, + ContactPointsList, VirtualMobileBaseJoints) +from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms +from ..datastructures.pose import Pose, Transform +from ..datastructures.world_entity import StateEntity +from ..failures import ProspectionObjectNotFound, WorldObjectNotFound from ..local_transformer import LocalTransformer -from .pose import Pose, Transform +from ..robot_description import RobotDescription +from ..ros.data_types import Time +from ..ros.logging import logwarn +from ..validation.goal_validator import (MultiPoseGoalValidator, + PoseGoalValidator, JointPositionGoalValidator, + MultiJointPositionGoalValidator, GoalValidator, + validate_joint_position, validate_multiple_joint_positions, + validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint -from .dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) +from ..world_concepts.event import Event if TYPE_CHECKING: from ..world_concepts.world_object import Object - from ..description import Link, Joint - - -class StateEntity: - """ - The StateEntity class is used to store the state of an object or the physics simulator. This is used to save and - restore the state of the World. - """ - - def __init__(self): - self._saved_states: Dict[int, State] = {} - - @property - def saved_states(self) -> Dict[int, State]: - """ - Returns the saved states of this entity. - """ - return self._saved_states - - def save_state(self, state_id: int) -> int: - """ - Saves the state of this entity with the given state id. - - :param state_id: The unique id of the state. - """ - self._saved_states[state_id] = self.current_state - return state_id - - @property - @abstractmethod - def current_state(self) -> State: - """ - Returns the current state of this entity. - - :return: The current state of this entity. - """ - pass - - @current_state.setter - @abstractmethod - def current_state(self, state: State) -> None: - """ - Sets the current state of this entity. - - :param state: The new state of this entity. - """ - pass - - def restore_state(self, state_id: int) -> None: - """ - Restores the state of this entity from a saved state using the given state id. - - :param state_id: The unique id of the state. - """ - self.current_state = self.saved_states[state_id] - - def remove_saved_states(self) -> None: - """ - Removes all saved states of this entity. - """ - self._saved_states = {} - - -class WorldEntity(StateEntity, ABC): - """ - A data class that represents an entity of the world, such as an object or a link. - """ - - def __init__(self, _id: int, world: Optional[World] = None): - StateEntity.__init__(self) - self.id = _id - self.world: World = world if world is not None else World.current_world + from ..description import Link, Joint, ObjectDescription + from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription class World(StateEntity, ABC): @@ -109,17 +48,17 @@ class World(StateEntity, ABC): current_world which is managed by the World class itself. """ - simulation_frequency: float + conf: Type[WorldConfig] = WorldConfig """ - Global reference for the simulation frequency (Hz), used in calculating the equivalent real time in the simulation. + The configurations of the world, the default configurations are defined in world_conf.py in the config folder. """ current_world: Optional[World] = None """ - Global reference to the currently used World, usually this is the - graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the - prospection world. In this way you can comfortably use the current_world, which should point towards the World - used at the moment. + Global reference to the currently used World, usually this is the + graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the + prospection world. In this way you can comfortably use the current_world, which should point towards the World + used at the moment. """ robot: Optional[Object] = None @@ -128,51 +67,49 @@ class World(StateEntity, ABC): the URDF with the name of the URDF on the parameter server. """ - data_directory: List[str] = [os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources')] - """ - Global reference for the data directories, this is used to search for the description files of the robot - and the objects. - """ - - cache_dir = data_directory[0] + '/cached/' + cache_manager: CacheManager = CacheManager(conf.cache_dir, [conf.resources_path], False) """ - Global reference for the cache directory, this is used to cache the description files of the robot and the objects. + Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ - def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): + def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_cache: bool = False): """ - Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the - background. There can only be one rendered simulation. - The World object also initializes the Events for attachment, detachment and for manipulating the world. + Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The World object also initializes the Events for attachment, detachment and for manipulating the world. - :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is + "GUI" + :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + :param clear_cache: Whether to clear the cache directory. """ StateEntity.__init__(self) + if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): + self.cache_manager.clear_cache() + + GoalValidator.raise_error = self.conf.raise_goal_validator_error + if World.current_world is None: World.current_world = self - World.simulation_frequency = simulation_frequency - self.cache_manager = CacheManager(self.cache_dir, self.data_directory) + self.object_lock: threading.Lock = threading.Lock() self.id: Optional[int] = -1 # This is used to connect to the physics server (allows multiple clients) self._init_world(mode) + self.objects: List[Object] = [] + # List of all Objects in the World + self.is_prospection_world: bool = is_prospection_world self._init_and_sync_prospection_world() self.local_transformer = LocalTransformer() self._update_local_transformer_worlds() - self.objects: List[Object] = [] - # List of all Objects in the World - - - self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" @@ -182,16 +119,104 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._current_state: Optional[WorldState] = None + self._init_goal_validators() + + self.original_state_id = self.save_state() + + @classmethod + def get_cache_dir(cls) -> str: + """ + Return the cache directory. + """ + return cls.cache_manager.cache_dir + + def add_object(self, obj: Object) -> None: + """ + Add an object to the world. + + :param obj: The object to be added. + """ + self.object_lock.acquire() + self.objects.append(obj) + self.add_object_to_original_state(obj) + self.object_lock.release() + + @property + def robot_description(self) -> RobotDescription: + """ + Return the current robot description. + """ + return RobotDescription.current_robot_description + + @property + def robot_has_actuators(self) -> bool: + """ + Return whether the robot has actuators. + """ + return self.robot_description.has_actuators + + def get_actuator_for_joint(self, joint: Joint) -> str: + """ + Get the actuator name for a given joint. + """ + return self.robot_joint_actuators[joint.name] + + def joint_has_actuator(self, joint: Joint) -> bool: + """ + Return whether the joint has an actuator. + """ + return joint.name in self.robot_joint_actuators + + @property + def robot_joint_actuators(self) -> Dict[str, str]: + """ + Return the joint actuators of the robot. + """ + return self.robot_description.joint_actuators + + def _init_goal_validators(self): + """ + Initialize the goal validators for the World objects' poses, positions, and orientations. + """ + + # Objects Pose goal validators + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.get_pose_tolerance(), + self.conf.acceptable_percentage_of_goal) + self.multi_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(self.get_multiple_object_poses(x).values()), + self.conf.get_pose_tolerance(), self.conf.acceptable_percentage_of_goal) + + # Joint Goal validators + self.joint_position_goal_validator = JointPositionGoalValidator( + self.get_joint_position, + acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) + self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.get_multiple_joint_positions(x).values()), + acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) + + def check_object_exists(self, obj: Object) -> bool: + """ + Check if the object exists in the simulator. + + :param obj: The object to check. + :return: True if the object is in the world, False otherwise. + """ + raise NotImplementedError + @abstractmethod def _init_world(self, mode: WorldMode): """ - Initializes the physics simulation. + Initialize the physics simulation. """ raise NotImplementedError def _init_events(self): """ - Initializes dynamic events that can be used to react to changes in the World. + Initialize dynamic events that can be used to react to changes in the World. """ self.detachment_event: Event = Event() self.attachment_event: Event = Event() @@ -199,82 +224,108 @@ def _init_events(self): def _init_and_sync_prospection_world(self): """ - Initializes the prospection world and the synchronization between the main and the prospection world. + Initialize the prospection world and the synchronization between the main and the prospection world. """ self._init_prospection_world() self._sync_prospection_world() def _update_local_transformer_worlds(self): """ - Updates the local transformer worlds with the current world and prospection world. + Update the local transformer worlds with the current world and prospection world. """ self.local_transformer.world = self self.local_transformer.prospection_world = self.prospection_world def _init_prospection_world(self): """ - Initializes the prospection world, if this is a prospection world itself it will not create another prospection, + Initialize the prospection world, if this is a prospection world itself it will not create another prospection, world, but instead set the prospection world to None, else it will create a prospection world. """ if self.is_prospection_world: # then no need to add another prospection world self.prospection_world = None else: self.prospection_world: World = self.__class__(WorldMode.DIRECT, - True, - World.simulation_frequency) + True) def _sync_prospection_world(self): """ - Synchronizes the prospection world with the main world, this means that every object in the main world will be + Synchronize the prospection world with the main world, this means that every object in the main world will be added to the prospection world and vice versa. """ if self.is_prospection_world: # then no need to add another prospection world self.world_sync = None else: self.world_sync: WorldSync = WorldSync(self, self.prospection_world) + self.pause_world_sync() self.world_sync.start() - def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - obj: Object) -> str: + def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, + description: ObjectDescription, name: str, + scale_mesh: Optional[float] = None) -> str: """ - Updates the cache directory with the given object. + Update the cache directory with the given object. :param path: The path to the object. :param ignore_cached_files: If the cached files should be ignored. - :param obj: The object to be added to the cache directory. + :param description: The object description. + :param name: The name of the object. + :param scale_mesh: The scale of the mesh. + :return: The path of the cached object. """ - return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, obj.description, obj.name) + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name, scale_mesh) @property def simulation_time_step(self): """ The time step of the simulation in seconds. """ - return 1 / World.simulation_frequency + return 1 / self.__class__.conf.simulation_frequency @abstractmethod - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: """ - Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. + Load a description file (e.g. URDF) at the given pose and returns the id of the loaded object. :param path: The path to the description file, if None the description file is assumed to be already loaded. :param pose: The pose at which the object should be loaded. + :param obj_type: The type of the object. :return: The id of the loaded object. """ pass - def get_object_by_name(self, name: str) -> List[Object]: + def load_generic_object_and_get_id(self, description: GenericObjectDescription, + pose: Optional[Pose] = None) -> int: """ - Returns a list of all Objects in this World with the same name as the given one. + Create a visual and collision box in the simulation and returns the id of the loaded object. + + :param description: The object description. + :param pose: The pose at which the object should be loaded. + """ + raise NotImplementedError + + def get_object_names(self) -> List[str]: + """ + Return the names of all objects in the World. + + :return: A list of object names. + """ + return [obj.name for obj in self.objects] + + def get_object_by_name(self, name: str) -> Optional[Object]: + """ + Return the object with the given name. If there is no object with the given name, None is returned. :param name: The name of the returned Objects. - :return: A list of all Objects with the name 'name'. + :return: The object with the given name, if there is one. """ - return list(filter(lambda obj: obj.name == name, self.objects))[0] + + matching_objects = list(filter(lambda obj: obj.name == name, self.objects)) + return matching_objects[0] if len(matching_objects) > 0 else None def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ - Returns a list of all Objects which have the type 'obj_type'. + Return a list of all Objects which have the type 'obj_type'. :param obj_type: The type of the returned Objects. :return: A list of all Objects that have the type 'obj_type'. @@ -283,59 +334,92 @@ def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: def get_object_by_id(self, obj_id: int) -> Object: """ - Returns the single Object that has the unique id. + Return the single Object that has the unique id. :param obj_id: The unique id for which the Object should be returned. :return: The Object with the id 'id'. """ return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] - @abstractmethod - def remove_object_by_id(self, obj_id: int) -> None: + def remove_visual_object(self, obj_id: int) -> bool: """ - Removes the object with the given id from the world. + Remove the object with the given id from the world, and saves a new original state for the world. :param obj_id: The unique id of the object to be removed. + :return: Whether the object was removed successfully. + """ + + removed = self._remove_visual_object(obj_id) + if removed: + self.update_simulator_state_id_in_original_state() + else: + logwarn(f"Object with id {obj_id} could not be removed.") + return removed + + @abstractmethod + def _remove_visual_object(self, obj_id: int) -> bool: + """ + Remove the visual object with the given id from the world, and update the simulator state in the original state. + + :param obj_id: The unique id of the visual object to be removed. + :return: Whether the object was removed successfully. """ pass @abstractmethod - def remove_object_from_simulator(self, obj: Object) -> None: + def remove_object_from_simulator(self, obj: Object) -> bool: """ - Removes an object from the physics simulator. + Remove an object from the physics simulator. :param obj: The object to be removed. + :return: Whether the object was removed successfully. """ pass def remove_object(self, obj: Object) -> None: """ - Removes this object from the current world. + Remove this object from the current world. For the object to be removed it has to be detached from all objects it is currently attached to. After this is done a call to world remove object is done to remove this Object from the simulation/world. :param obj: The object to be removed. """ - obj.detach_all() - - self.objects.remove(obj) + self.object_lock.acquire() - # This means the current world of the object is not the prospection world, since it - # has a reference to the prospection world - if self.prospection_world is not None: - self.world_sync.remove_obj_queue.put(obj) - self.world_sync.remove_obj_queue.join() + obj.detach_all() - self.remove_object_from_simulator(obj) + if self.remove_object_from_simulator(obj): + self.objects.remove(obj) + self.remove_object_from_original_state(obj) if World.robot == obj: World.robot = None + self.object_lock.release() + + def remove_object_from_original_state(self, obj: Object) -> None: + """ + Remove an object from the original state of the world. + + :param obj: The object to be removed. + """ + self.original_state.object_states.pop(obj.name) + self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=True) + + def add_object_to_original_state(self, obj: Object) -> None: + """ + Add an object to the original state of the world. + + :param obj: The object to be added. + """ + self.original_state.object_states[obj.name] = obj.current_state + self.update_simulator_state_id_in_original_state() + def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ - Creates a fixed joint constraint between the given parent and child links, + Create a fixed joint constraint between the given parent and child links, the joint frame will be at the origin of the child link frame, and would have the same orientation as the child link frame. @@ -386,7 +470,7 @@ def get_joint_position(self, joint: Joint) -> float: @abstractmethod def get_object_joint_names(self, obj: Object) -> List[str]: """ - Returns the names of all joints of this object. + Return the names of all joints of this object. :param obj: The object. :return: A list of joint names. @@ -403,10 +487,60 @@ def get_link_pose(self, link: Link) -> Pose: """ pass + @abstractmethod + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and Pose objects as values. + """ + pass + + @abstractmethod + def get_link_position(self, link: Link) -> List[float]: + """ + Get the position of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The position of the link as a list of floats. + """ + pass + + @abstractmethod + def get_link_orientation(self, link: Link) -> List[float]: + """ + Get the orientation of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The orientation of the link as a list of floats. + """ + pass + + @abstractmethod + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + + @abstractmethod + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + @abstractmethod def get_object_link_names(self, obj: Object) -> List[str]: """ - Returns the names of all links of this object. + Return the names of all links of this object. :param obj: The object. :return: A list of link names. @@ -415,7 +549,7 @@ def get_object_link_names(self, obj: Object) -> List[str]: def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: """ - Simulates Physics in the World for a given amount of seconds. Usually this simulation is faster than real + Simulate Physics in the World for a given amount of seconds. Usually this simulation is faster than real time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real time. @@ -423,24 +557,24 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: :param real_time: If the simulation should happen in real time or faster. """ self.set_realtime(real_time) - for i in range(0, int(seconds * self.simulation_frequency)): - curr_time = rospy.Time.now() + for i in range(0, int(seconds * self.conf.simulation_frequency)): + curr_time = Time().now() self.step() for objects, callbacks in self.coll_callbacks.items(): contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) - if contact_points != (): + if len(contact_points) > 0: callbacks.on_collision_cb() elif callbacks.no_collision_cb is not None: callbacks.no_collision_cb() if real_time: - loop_time = rospy.Time.now() - curr_time + loop_time = Time().now() - curr_time time_diff = self.simulation_time_step - loop_time.to_sec() time.sleep(max(0, time_diff)) self.update_all_objects_poses() def update_all_objects_poses(self) -> None: """ - Updates the positions of all objects in the world. + Update the positions of all objects in the world. """ for obj in self.objects: obj.update_pose() @@ -449,20 +583,89 @@ def update_all_objects_poses(self) -> None: def get_object_pose(self, obj: Object) -> Pose: """ Get the pose of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. + """ + pass + + @abstractmethod + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + """ + Get the poses of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. + """ + pass + + @abstractmethod + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the positions of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. + """ + pass + + @abstractmethod + def get_object_position(self, obj: Object) -> List[float]: + """ + Get the position of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. + """ + pass + + @abstractmethod + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. """ pass + @abstractmethod + def get_object_orientation(self, obj: Object) -> List[float]: + """ + Get the orientation of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. + """ + pass + + @property + def robot_virtual_joints(self) -> List[Joint]: + """ + The virtual joints of the robot. + """ + return [self.robot.joints[name] for name in self.robot_virtual_joints_names] + + @property + def robot_virtual_joints_names(self) -> List[str]: + """ + The names of the virtual joints of the robot. + """ + return self.robot_description.virtual_mobile_base_joints.names + + def get_robot_mobile_base_joints(self) -> VirtualMobileBaseJoints: + """ + Get the mobile base joints of the robot. + + :return: The mobile base joints. + """ + return self.robot_description.virtual_mobile_base_joints + @abstractmethod def perform_collision_detection(self) -> None: """ - Checks for collisions between all objects in the World and updates the contact points. + Check for collisions between all objects in the World and updates the contact points. """ pass @abstractmethod - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ - Returns a list of contact points of this Object with all other Objects. + Return a list of contact points of this Object with all other Objects. :param obj: The object. :return: A list of all contact points with other objects @@ -470,9 +673,9 @@ def get_object_contact_points(self, obj: Object) -> List: pass @abstractmethod - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Returns a list of contact points between obj1 and obj2. + Return a list of contact points between obj_a and obj_b. :param obj1: The first object. :param obj2: The second object. @@ -480,24 +683,97 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass + def get_object_closest_points(self, obj: Object, max_distance: float) -> ClosestPointsList: + """ + Return the closest points of this object with all other objects in the world. + + :param obj: The object. + :param max_distance: The maximum distance between the points. + :return: A list of the closest points. + """ + all_obj_closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in + self.objects + if other_obj != obj] + return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points]) + + def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \ + -> ClosestPointsList: + """ + Return the closest points between two objects. + + :param object_a: The first object. + :param object_b: The second object. + :param max_distance: The maximum distance between the points. + :return: A list of the closest points. + """ + raise NotImplementedError + + @validate_joint_position @abstractmethod - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation + .. note:: + It is recommended to use the validate_joint_position decorator to validate the joint position for + the implementation of this method. + :param joint: The joint to reset the position for. :param joint_position: The new joint pose. + :return: True if the reset was successful, False otherwise """ pass + @validate_multiple_joint_positions @abstractmethod - def reset_object_base_pose(self, obj: Object, pose: Pose): + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints of an articulated object. + + .. note:: + It is recommended to use the validate_multiple_joint_positions decorator to validate the + joint positions for the implementation of this method. + + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + :return: True if the set was successful, False otherwise. + """ + pass + + @abstractmethod + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + """ + Get the positions of multiple joints of an articulated object. + + :param joints: The joints as a list of Joint objects. + """ + pass + + @validate_object_pose + @abstractmethod + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. + .. note:: + It is recommended to use the validate_object_pose decorator to validate the object pose for the + implementation of this method. + :param obj: The object. :param pose: The new pose as a Pose object. + :return: True if the reset was successful, False otherwise. + """ + pass + + @validate_multiple_object_poses + @abstractmethod + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: + """ + Reset the world position and orientation of the base of multiple objects instantaneously, + not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. + + :param objects: A dictionary with objects as keys and poses as values. + :return: True if the reset was successful, False otherwise. """ pass @@ -508,10 +784,20 @@ def step(self): """ pass + def get_arm_tool_frame_link(self, arm: Arms) -> Link: + """ + Get the tool frame link of the arm of the robot. + + :param arm: The arm for which the tool frame link should be returned. + :return: The tool frame link of the arm. + """ + ee_link_name = self.robot_description.get_arm_tool_frame(arm) + return self.robot.get_link(ee_link_name) + @abstractmethod def set_link_color(self, link: Link, rgba_color: Color): """ - Changes the rgba_color of a link of this object, the rgba_color has to be given as Color object. + Change the rgba_color of a link of this object, the rgba_color has to be given as Color object. :param link: The link which should be colored. :param rgba_color: The rgba_color as Color object with RGBA values between 0 and 1. @@ -541,7 +827,7 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: @abstractmethod def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of this object. The return of this method are two points in + Return the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. @@ -552,7 +838,7 @@ def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundi @abstractmethod def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of the link. The return of this method are two points in + Return the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ pass @@ -560,7 +846,7 @@ def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingB @abstractmethod def set_realtime(self, real_time: bool) -> None: """ - Enables the real time simulation of Physics in the World. By default, this is disabled and Physics is only + Enable the real time simulation of Physics in the World. By default, this is disabled and Physics is only simulated to reason about it. :param real_time: Whether the World should simulate Physics in real time. @@ -570,7 +856,7 @@ def set_realtime(self, real_time: bool) -> None: @abstractmethod def set_gravity(self, gravity_vector: List[float]) -> None: """ - Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). + Set the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. :param gravity_vector: The gravity vector that should be used in the World. @@ -579,7 +865,7 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def set_robot_if_not_set(self, robot: Object) -> None: """ - Sets the robot if it is not set yet. + Set the robot if it is not set yet. :param robot: The Object reference to the Object representing the robot. """ @@ -589,7 +875,7 @@ def set_robot_if_not_set(self, robot: Object) -> None: @staticmethod def set_robot(robot: Union[Object, None]) -> None: """ - Sets the global variable for the robot Object This should be set on spawning the robot. + Set the global variable for the robot Object This should be set on spawning the robot. :param robot: The Object reference to the Object representing the robot. """ @@ -598,17 +884,21 @@ def set_robot(robot: Union[Object, None]) -> None: @staticmethod def robot_is_set() -> bool: """ - Returns whether the robot has been set or not. + Return whether the robot has been set or not. :return: True if the robot has been set, False otherwise. """ return World.robot is not None - def exit(self) -> None: + def exit(self, remove_saved_states: bool = True) -> None: """ - Closes the World as well as the prospection world, also collects any other thread that is running. + Close the World as well as the prospection world, also collects any other thread that is running. + + :param remove_saved_states: Whether to remove the saved states. """ self.exit_prospection_world_if_exists() + self.reset_world(remove_saved_states) + self.remove_all_objects() self.disconnect_from_physics_server() self.reset_robot() self.join_threads() @@ -617,7 +907,7 @@ def exit(self) -> None: def exit_prospection_world_if_exists(self) -> None: """ - Exits the prospection world if it exists. + Exit the prospection world if it exists. """ if self.prospection_world: self.terminate_world_sync() @@ -626,21 +916,21 @@ def exit_prospection_world_if_exists(self) -> None: @abstractmethod def disconnect_from_physics_server(self) -> None: """ - Disconnects the world from the physics server. + Disconnect the world from the physics server. """ pass def reset_current_world(self) -> None: """ - Resets the pose of every object in the World to the pose it was spawned in and sets every joint to 0. + Reset the pose of every object in the World to the pose it was spawned in and sets every joint to 0. """ for obj in self.objects: obj.set_pose(obj.original_pose) - obj.set_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) + obj.set_multiple_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) def reset_robot(self) -> None: """ - Sets the robot class variable to None. + Set the robot class variable to None. """ self.set_robot(None) @@ -653,19 +943,22 @@ def join_threads(self) -> None: def terminate_world_sync(self) -> None: """ - Terminates the world sync thread. + Terminate the world sync thread. """ self.world_sync.terminate = True + self.resume_world_sync() self.world_sync.join() - def save_state(self, state_id: Optional[int] = None) -> int: + def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: """ - Returns the id of the saved state of the World. The saved state contains the states of all the objects and + Return the id of the saved state of the World. The saved state contains the states of all the objects and the state of the physics simulator. + :param state_id: The id of the saved state. + :param use_same_id: Whether to use the same current state id for the new saved state. :return: A unique id of the state """ - state_id = self.save_physics_simulator_state() + state_id = self.save_physics_simulator_state(state_id=state_id, use_same_id=use_same_id) self.save_objects_state(state_id) self._current_state = WorldState(state_id, self.object_states) return super().save_state(state_id) @@ -673,18 +966,24 @@ def save_state(self, state_id: Optional[int] = None) -> int: @property def current_state(self) -> WorldState: if self._current_state is None: - self._current_state = WorldState(self.save_physics_simulator_state(), self.object_states) - return self._current_state + simulator_state = None if self.conf.use_physics_simulator_state else ( + self.save_physics_simulator_state(use_same_id=True)) + self._current_state = WorldState(simulator_state, self.object_states) + return WorldState(self._current_state.simulator_state_id, self.object_states) @current_state.setter def current_state(self, state: WorldState) -> None: - self.restore_physics_simulator_state(state.simulator_state_id) - self.object_states = state.object_states + if self.current_state != state: + if self.conf.use_physics_simulator_state: + self.restore_physics_simulator_state(state.simulator_state_id) + else: + for obj in self.objects: + self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] @property def object_states(self) -> Dict[str, ObjectState]: """ - Returns the states of all objects in the World. + Return the states of all objects in the World. :return: A dictionary with the object id as key and the object state as value. """ @@ -693,14 +992,14 @@ def object_states(self) -> Dict[str, ObjectState]: @object_states.setter def object_states(self, states: Dict[str, ObjectState]) -> None: """ - Sets the states of all objects in the World. + Set the states of all objects in the World. """ for obj_name, obj_state in states.items(): self.get_object_by_name(obj_name).current_state = obj_state def save_objects_state(self, state_id: int) -> None: """ - Saves the state of all objects in the World according to the given state using the unique state id. + Save the state of all objects in the World according to the given state using the unique state id. :param state_id: The unique id representing the state. """ @@ -708,10 +1007,12 @@ def save_objects_state(self, state_id: int) -> None: obj.save_state(state_id) @abstractmethod - def save_physics_simulator_state(self) -> int: + def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: """ - Saves the state of the physics simulator and returns the unique id of the state. + Save the state of the physics simulator and returns the unique id of the state. + :param state_id: The used specified unique id representing the state. + :param use_same_id: If the same id should be used for the state. :return: The unique id representing the state. """ pass @@ -719,7 +1020,7 @@ def save_physics_simulator_state(self) -> int: @abstractmethod def remove_physics_simulator_state(self, state_id: int) -> None: """ - Removes the state of the physics simulator with the given id. + Remove the state of the physics simulator with the given id. :param state_id: The unique id representing the state. """ @@ -728,7 +1029,7 @@ def remove_physics_simulator_state(self, state_id: int) -> None: @abstractmethod def restore_physics_simulator_state(self, state_id: int) -> None: """ - Restores the objects and environment state in the physics simulator according to + Restore the objects and environment state in the physics simulator according to the given state using the unique state id. :param state_id: The unique id representing the state. @@ -740,7 +1041,7 @@ def get_images_for_target(self, cam_pose: Pose, size: Optional[int] = 256) -> List[np.ndarray]: """ - Calculates the view and projection Matrix and returns 3 images: + Calculate the view and projection Matrix and returns 3 images: 1. An RGB image 2. A depth image @@ -759,7 +1060,7 @@ def register_two_objects_collision_callbacks(self, on_collision_callback: Callable, on_collision_removal_callback: Optional[Callable] = None) -> None: """ - Registers callback methods for contact between two Objects. There can be a callback for when the two Objects + Register callback methods for contact between two Objects. There can be a callback for when the two Objects get in contact and, optionally, for when they are not in contact anymore. :param object_a: An object in the World @@ -771,80 +1072,115 @@ def register_two_objects_collision_callbacks(self, on_collision_removal_callback) @classmethod - def add_resource_path(cls, path: str) -> None: + def get_data_directories(cls) -> List[str]: """ - Adds a resource path in which the World will search for files. This resource directory is searched if an + The resources directories where the objects, robots, and environments are stored. + """ + return cls.cache_manager.data_directories + + @classmethod + def add_resource_path(cls, path: str, prepend: bool = False) -> None: + """ + Add a resource path in which the World will search for files. This resource directory is searched if an Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. + :param prepend: Put the new path at the beginning of the list such that it is searched first. + """ + if prepend: + cls.cache_manager.data_directories = [path] + cls.cache_manager.data_directories + else: + cls.cache_manager.data_directories.append(path) + + @classmethod + def remove_resource_path(cls, path: str) -> None: + """ + Remove the given path from the data_directories list. + + :param path: The path to remove. + """ + cls.cache_manager.data_directories.remove(path) + + @classmethod + def change_cache_dir_path(cls, path: str) -> None: + """ + Change the cache directory to the given path + + :param path: The new path for the cache directory. """ - cls.data_directory.append(path) + cls.cache_manager.cache_dir = os.path.join(path, cls.conf.cache_dir_name) def get_prospection_object_for_object(self, obj: Object) -> Object: """ - Returns the corresponding object from the prospection world for a given object in the main world. + Return the corresponding object from the prospection world for a given object in the main world. If the given Object is already in the prospection world, it is returned. :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. """ - self.world_sync.add_obj_queue.join() - try: - return self.world_sync.object_mapping[obj] - except KeyError: - prospection_world = self if self.is_prospection_world else self.prospection_world - if obj in prospection_world.objects: - return obj - else: - raise ValueError( - f"There is no prospection object for the given object: {obj}, this could be the case if" - f" the object isn't anymore in the main (graphical) World" - f" or if the given object is already a prospection object. ") + with UseProspectionWorld(): + return self.world_sync.get_prospection_object(obj) def get_object_for_prospection_object(self, prospection_object: Object) -> Object: """ - Returns the corresponding object from the main World for a given + Return the corresponding object from the main World for a given object in the prospection world. If the given object is not in the prospection world an error will be raised. :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. """ - object_map = self.world_sync.object_mapping - try: - return list(object_map.keys())[list(object_map.values()).index(prospection_object)] - except ValueError: - raise ValueError("The given object is not in the prospection world.") + with UseProspectionWorld(): + return self.world_sync.get_world_object(prospection_object) + + def remove_all_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: + """ + Remove all objects from the World. - def reset_world(self, remove_saved_states=True) -> None: + :param exclude_objects: A list of objects that should not be removed. """ - Resets the World to the state it was first spawned in. + objs_copy = [obj for obj in self.objects] + exclude_objects = [] if exclude_objects is None else exclude_objects + [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] + + def reset_world(self, remove_saved_states=False) -> None: + """ + Reset the World to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and all objects will be set to the position and orientation in which they were spawned. :param remove_saved_states: If the saved states should be removed. """ - + self.restore_state(self.original_state_id) if remove_saved_states: self.remove_saved_states() - - for obj in self.objects: - obj.reset(remove_saved_states) + self.original_state_id = self.save_state() def remove_saved_states(self) -> None: """ - Removes all saved states of the World. + Remove all saved states of the World. """ - for state_id in self.saved_states: - self.remove_physics_simulator_state(state_id) + if self.conf.use_physics_simulator_state: + for state_id in self.saved_states: + self.remove_physics_simulator_state(state_id) + else: + self.remove_objects_saved_states() super().remove_saved_states() + self.original_state_id = None + + def remove_objects_saved_states(self) -> None: + """ + Remove all saved states of the objects in the World. + """ + for obj in self.objects: + obj.remove_saved_states() def update_transforms_for_objects_in_current_world(self) -> None: """ Updates transformations for all objects that are currently in :py:attr:`~pycram.world.World.current_world`. """ - curr_time = rospy.Time.now() + curr_time = Time().now() for obj in list(self.current_world.objects): obj.update_link_transforms(curr_time) @@ -879,6 +1215,12 @@ def create_visual_shape(self, visual_shape: VisualShape) -> int: :param visual_shape: The visual shape to be created, uses the VisualShape dataclass defined in world_dataclasses :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_visual_shape, visual_shape) + + def _create_visual_shape(self, visual_shape: VisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_visual_shape` + """ raise NotImplementedError def create_multi_body_from_visual_shapes(self, visual_shape_ids: List[int], pose: Pose) -> int: @@ -916,51 +1258,92 @@ def create_multi_body(self, multi_body: MultiBody) -> int: :param multi_body: The multi body to be created, uses the MultiBody dataclass defined in world_dataclasses. :return: The unique id of the created multi body. """ + return self._simulator_object_creator(self._create_multi_body, multi_body) + + def _create_multi_body(self, multi_body: MultiBody) -> int: + """ + See :py:meth:`~pycram.world.World.create_multi_body` + """ raise NotImplementedError def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: """ Creates a box visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_box_visual_shape, shape_data) + + def _create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_box_visual_shape` + """ raise NotImplementedError def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: """ Creates a cylinder visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the cylinder visual shape to be created, uses the CylinderVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the cylinder visual shape to be created, uses the + CylinderVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_cylinder_visual_shape, shape_data) + + def _create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_cylinder_visual_shape` + """ raise NotImplementedError def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: """ Creates a sphere visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_sphere_visual_shape, shape_data) + + def _create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_sphere_visual_shape` + """ raise NotImplementedError def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: """ Creates a capsule visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the capsule visual shape to be created, uses the CapsuleVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the capsule visual shape to be created, uses the + CapsuleVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_capsule_visual_shape, shape_data) + + def _create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_capsule_visual_shape` + """ raise NotImplementedError def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: """ Creates a plane visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_plane_visual_shape, shape_data) + + def _create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_plane_visual_shape` + """ raise NotImplementedError def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: @@ -971,6 +1354,12 @@ def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: uses the MeshVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_mesh_visual_shape, shape_data) + + def _create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_mesh_visual_shape` + """ raise NotImplementedError def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, @@ -981,14 +1370,26 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :param text: The text to be added. :param position: The position of the text in the world. - :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a text orientation (quaternion), the orientation will be fixed in world space or local space (when parent is specified). + :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a + text orientation (quaternion), the orientation will be fixed in world space or local space + (when parent is specified). :param size: The size of the text. :param color: The color of the text. - :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the world until it is removed manually. + :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the + world until it is removed manually. :param parent_object_id: The id of the object to which the text should be attached. :param parent_link_id: The id of the link to which the text should be attached. :return: The id of the added text. """ + return self._simulator_object_creator(self._add_text, text, position, orientation, size, color, life_time, + parent_object_id, parent_link_id) + + def _add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, + color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: + """ + See :py:meth:`~pycram.world.World.add_text` + """ raise NotImplementedError def remove_text(self, text_id: Optional[int] = None) -> None: @@ -997,6 +1398,12 @@ def remove_text(self, text_id: Optional[int] = None) -> None: :param text_id: The id of the text to be removed. """ + self._simulator_object_remover(self._remove_text, text_id) + + def _remove_text(self, text_id: Optional[int] = None) -> None: + """ + See :py:meth:`~pycram.world.World.remove_text` + """ raise NotImplementedError def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: @@ -1023,7 +1430,7 @@ def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: """ - Returns the joint reaction forces and torques of the specified joint. + Get the joint reaction forces and torques of the specified joint. :param obj: The object in which the joint is located. :param joint_id: The id of the joint for which the force torque should be returned. @@ -1033,7 +1440,7 @@ def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[fl def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ - Returns the applied torque by a joint motor. + Get the applied torque by a joint motor. :param obj: The object in which the joint is located. :param joint_id: The id of the joint for which the applied motor torque should be returned. @@ -1041,6 +1448,85 @@ def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ raise NotImplementedError + def pause_world_sync(self) -> None: + """ + Pause the world synchronization. + """ + self.world_sync.sync_lock.acquire() + + def resume_world_sync(self) -> None: + """ + Resume the world synchronization. + """ + self.world_sync.sync_lock.release() + + def add_vis_axis(self, pose: Pose) -> int: + """ + Add a visual axis to the world. + + :param pose: The pose of the visual axis. + :return: The id of the added visual axis. + """ + return self._simulator_object_creator(self._add_vis_axis, pose) + + def _add_vis_axis(self, pose: Pose) -> None: + """ + See :py:meth:`~pycram.world.World.add_vis_axis` + """ + logwarn(f"Visual axis is not supported in {self.__class__.__name__}") + + def remove_vis_axis(self) -> None: + """ + Remove the visual axis from the world. + """ + self._simulator_object_remover(self._remove_vis_axis) + + def _remove_vis_axis(self) -> None: + """ + See :py:meth:`~pycram.world.World.remove_vis_axis` + """ + logwarn(f"Visual axis is not supported in {self.__class__.__name__}") + + def _simulator_object_creator(self, creator_func: Callable, *args, **kwargs) -> int: + """ + Create an object in the physics simulator and returns the created object id. + + :param creator_func: The function that creates the object in the physics simulator. + :param args: The arguments for the creator function. + :param kwargs: The keyword arguments for the creator function. + :return: The created object id. + """ + obj_id = creator_func(*args, **kwargs) + self.update_simulator_state_id_in_original_state() + return obj_id + + def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) -> None: + """ + Remove an object from the physics simulator. + + :param remover_func: The function that removes the object from the physics simulator. + :param args: The arguments for the remover function. + :param kwargs: The keyword arguments for the remover function. + """ + remover_func(*args, **kwargs) + self.update_simulator_state_id_in_original_state() + + def update_simulator_state_id_in_original_state(self, use_same_id: bool = False) -> None: + """ + Update the simulator state id in the original state if use_physics_simulator_state is True in the configuration. + + :param use_same_id: If the same id should be used for the state. + """ + if self.conf.use_physics_simulator_state: + self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=use_same_id) + + @property + def original_state(self) -> WorldState: + """ + The saved original state of the world. + """ + return self.saved_states[self.original_state_id] + def __del__(self): self.exit() @@ -1055,10 +1541,6 @@ class UseProspectionWorld: NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ - WAIT_TIME_FOR_ADDING_QUEUE = 20 - """ - The time in seconds to wait for the adding queue to be ready. - """ def __init__(self): self.prev_world: Optional[World] = None @@ -1068,14 +1550,12 @@ def __enter__(self): """ This method is called when entering the with block, it will set the current world to the prospection world """ + # Please do not edit this function, it works as it is now! if not World.current_world.is_prospection_world: - time.sleep(self.WAIT_TIME_FOR_ADDING_QUEUE * World.current_world.simulation_time_step) - # blocks until the adding queue is ready - World.current_world.world_sync.add_obj_queue.join() - self.prev_world = World.current_world - World.current_world.world_sync.pause_sync = True World.current_world = World.current_world.prospection_world + # This is also a join statement since it is called from the main thread. + World.current_world.world_sync.sync_worlds() def __exit__(self, *args): """ @@ -1083,7 +1563,6 @@ def __exit__(self, *args): """ if self.prev_world is not None: World.current_world = self.prev_world - World.current_world.world_sync.pause_sync = False class WorldSync(threading.Thread): @@ -1091,12 +1570,15 @@ class WorldSync(threading.Thread): Synchronizes the state between the World and its prospection world. Meaning the cartesian and joint position of everything in the prospection world will be synchronized with the main World. - Adding and removing objects is done via queues, such that loading times of objects - in the prospection world does not affect the World. The class provides the possibility to pause the synchronization, this can be used if reasoning should be done in the prospection world. """ + WAIT_TIME_AS_N_SIMULATION_STEPS = 20 + """ + The time in simulation steps to wait between each iteration of the syncing loop. + """ + def __init__(self, world: World, prospection_world: World): threading.Thread.__init__(self) self.world: World = world @@ -1104,50 +1586,110 @@ def __init__(self, world: World, prospection_world: World): self.prospection_world.world_sync = self self.terminate: bool = False - self.add_obj_queue: Queue = Queue() - self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False # Maps world to prospection world objects - self.object_mapping: Dict[Object, Object] = {} + self.object_to_prospection_object_map: Dict[Object, Object] = {} + self.prospection_object_to_object_map: Dict[Object, Object] = {} self.equal_states = False + self.sync_lock: threading.Lock = threading.Lock() - def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): + def run(self): """ Main method of the synchronization, this thread runs in a loop until the terminate flag is set. While this loop runs it continuously checks the cartesian and joint position of every object in the World and updates the corresponding object in the - prospection world. When there are entries in the adding or removing queue the corresponding objects will - be added or removed in the same iteration. - - :param wait_time_as_n_simulation_steps: The time in simulation steps to wait between each iteration of - the syncing loop. + prospection world. """ while not self.terminate: - self.check_for_pause() - while not self.add_obj_queue.empty(): - obj = self.add_obj_queue.get() - # Maps the World object to the prospection world object - self.object_mapping[obj] = copy(obj) - self.add_obj_queue.task_done() - while not self.remove_obj_queue.empty(): - obj = self.remove_obj_queue.get() - # Get prospection world object reference from object mapping - prospection_obj = self.object_mapping[obj] - prospection_obj.remove() - del self.object_mapping[obj] - self.remove_obj_queue.task_done() - for world_obj, prospection_obj in self.object_mapping.items(): - prospection_obj.current_state = world_obj.current_state - self.check_for_pause() - time.sleep(wait_time_as_n_simulation_steps * self.world.simulation_time_step) - - def check_for_pause(self) -> None: - """ - Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. - """ - while self.pause_sync: - time.sleep(0.1) + self.sync_lock.acquire() + if not self.terminate: + self.sync_worlds() + self.sync_lock.release() + time.sleep(WorldSync.WAIT_TIME_AS_N_SIMULATION_STEPS * self.world.simulation_time_step) + + def get_world_object(self, prospection_object: Object) -> Object: + """ + Get the corresponding object from the main World for a given object in the prospection world. + + :param prospection_object: The object for which the corresponding object in the main World should be found. + :return: The object in the main World. + """ + try: + return self.prospection_object_to_object_map[prospection_object] + except KeyError: + if prospection_object in self.world.objects: + return prospection_object + raise WorldObjectNotFound(prospection_object) + + def get_prospection_object(self, obj: Object) -> Object: + """ + Get the corresponding object from the prospection world for a given object in the main world. + + :param obj: The object for which the corresponding object in the prospection World should be found. + :return: The corresponding object in the prospection world. + """ + try: + return self.object_to_prospection_object_map[obj] + except KeyError: + if obj in self.prospection_world.objects: + return obj + raise ProspectionObjectNotFound(obj) + + def sync_worlds(self): + """ + Syncs the prospection world with the main world by adding and removing objects and synchronizing their states. + """ + self.remove_objects_not_in_world() + self.add_objects_not_in_prospection_world() + self.prospection_object_to_object_map = {prospection_obj: obj for obj, prospection_obj in + self.object_to_prospection_object_map.items()} + self.sync_objects_states() + + def remove_objects_not_in_world(self): + """ + Removes all objects that are not in the main world from the prospection world. + """ + obj_map_copy = copy(self.object_to_prospection_object_map) + [self.remove_object(obj) for obj in obj_map_copy.keys() if obj not in self.world.objects] + + def add_objects_not_in_prospection_world(self): + """ + Adds all objects that are in the main world but not in the prospection world to the prospection world. + """ + obj_map_copy = copy(self.object_to_prospection_object_map) + [self.add_object(obj) for obj in self.world.objects if obj not in obj_map_copy.keys()] + + def add_object(self, obj: Object) -> None: + """ + Adds an object to the prospection world. + + :param obj: The object to be added. + """ + self.object_to_prospection_object_map[obj] = obj.copy_to_prospection() + + def remove_object(self, obj: Object) -> None: + """ + Removes an object from the prospection world. + + :param obj: The object to be removed. + """ + prospection_obj = self.object_to_prospection_object_map[obj] + prospection_obj.remove() + del self.object_to_prospection_object_map[obj] + + def sync_objects_states(self) -> None: + """ + Synchronizes the state of all objects in the World with the prospection world. + """ + # Set the pose of the prospection objects to the pose of the world objects + obj_pose_dict = {prospection_obj: obj.pose + for obj, prospection_obj in self.object_to_prospection_object_map.items()} + self.world.prospection_world.reset_multiple_objects_base_poses(obj_pose_dict) + for obj, prospection_obj in self.object_to_prospection_object_map.items(): + prospection_obj.set_attachments(obj.attachments) + prospection_obj.link_states = obj.link_states + prospection_obj.joint_states = obj.joint_states def check_for_equal(self) -> bool: """ @@ -1157,7 +1699,12 @@ def check_for_equal(self) -> bool: :return: True if both Worlds have the same state, False otherwise. """ eql = True - for obj, prospection_obj in self.object_mapping.items(): + prospection_names = self.prospection_world.get_object_names() + eql = eql and [name in prospection_names for name in self.world.get_object_names()] + eql = eql and len(prospection_names) == len(self.world.get_object_names()) + if not eql: + return False + for obj, prospection_obj in self.object_to_prospection_object_map.items(): eql = eql and obj.get_pose().dist(prospection_obj.get_pose()) < 0.001 self.equal_states = eql return eql diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py new file mode 100644 index 000000000..1e7c61e06 --- /dev/null +++ b/src/pycram/datastructures/world_entity.py @@ -0,0 +1,77 @@ +from abc import ABC, abstractmethod + +from typing_extensions import TYPE_CHECKING, Dict + +from .dataclasses import State + +if TYPE_CHECKING: + from ..datastructures.world import World + + +class StateEntity: + """ + The StateEntity class is used to store the state of an object or the physics simulator. This is used to save and + restore the state of the World. + """ + + def __init__(self): + self._saved_states: Dict[int, State] = {} + + @property + def saved_states(self) -> Dict[int, State]: + """ + :return: the saved states of this entity. + """ + return self._saved_states + + def save_state(self, state_id: int) -> int: + """ + Saves the state of this entity with the given state id. + + :param state_id: The unique id of the state. + """ + self._saved_states[state_id] = self.current_state + return state_id + + @property + @abstractmethod + def current_state(self) -> State: + """ + :return: The current state of this entity. + """ + pass + + @current_state.setter + @abstractmethod + def current_state(self, state: State) -> None: + """ + Sets the current state of this entity. + + :param state: The new state of this entity. + """ + pass + + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this entity from a saved state using the given state id. + + :param state_id: The unique id of the state. + """ + self.current_state = self.saved_states[state_id] + + def remove_saved_states(self) -> None: + """ + Removes all saved states of this entity. + """ + self._saved_states = {} + + +class WorldEntity(StateEntity, ABC): + """ + A data class that represents an entity of the world, such as an object or a link. + """ + + def __init__(self, _id: int, world: 'World'): + StateEntity.__init__(self) + self.id = _id + self.world: 'World' = world diff --git a/src/pycram/description.py b/src/pycram/description.py index 0ad05d7f1..689bec2f5 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -1,34 +1,36 @@ from __future__ import annotations import logging +import os import pathlib from abc import ABC, abstractmethod -import rospy +from .ros.data_types import Time +import trimesh from geometry_msgs.msg import Point, Quaternion -from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING +from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated +from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape from .datastructures.enums import JointType -from .local_transformer import LocalTransformer from .datastructures.pose import Pose, Transform -from .datastructures.world import WorldEntity -from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from .datastructures.world_entity import WorldEntity +from .failures import ObjectDescriptionNotFound +from .local_transformer import LocalTransformer if TYPE_CHECKING: from .world_concepts.world_object import Object class EntityDescription(ABC): - """ - A class that represents a description of an entity. This can be a link, joint or object description. + A description of an entity. This can be a link, joint or object description. """ @property @abstractmethod def origin(self) -> Pose: """ - Returns the origin of this entity. + :return: the origin of this entity. """ pass @@ -36,14 +38,14 @@ def origin(self) -> Pose: @abstractmethod def name(self) -> str: """ - Returns the name of this entity. + :return: the name of this entity. """ pass class LinkDescription(EntityDescription): """ - A class that represents a link description of an object. + A link description of an object. """ def __init__(self, parsed_link_description: Any): @@ -53,7 +55,7 @@ def __init__(self, parsed_link_description: Any): @abstractmethod def geometry(self) -> Union[VisualShape, None]: """ - Returns the geometry type of the collision element of this link. + The geometry type of the collision element of this link. """ pass @@ -63,8 +65,13 @@ class JointDescription(EntityDescription): A class that represents the description of a joint. """ - def __init__(self, parsed_joint_description: Any): + def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: bool = False): + """ + :param parsed_joint_description: The parsed description of the joint (e.g. from urdf or mjcf file). + :param is_virtual: True if the joint is virtual (i.e. not a physically existing joint), False otherwise. + """ self.parsed_description = parsed_joint_description + self.is_virtual: Optional[bool] = is_virtual @property @abstractmethod @@ -86,8 +93,6 @@ def axis(self) -> Point: @abstractmethod def has_limits(self) -> bool: """ - Checks if this joint has limits. - :return: True if the joint has limits, False otherwise. """ pass @@ -120,7 +125,7 @@ def upper_limit(self) -> Union[float, None]: @property @abstractmethod - def parent_link_name(self) -> str: + def parent(self) -> str: """ :return: The name of the parent link of this joint. """ @@ -128,7 +133,7 @@ def parent_link_name(self) -> str: @property @abstractmethod - def child_link_name(self) -> str: + def child(self) -> str: """ :return: The name of the child link of this joint. """ @@ -159,6 +164,13 @@ def __init__(self, _id: int, obj: Object): WorldEntity.__init__(self, _id, obj.world) self.object: Object = obj + @property + def object_name(self) -> str: + """ + The name of the object to which this joint belongs. + """ + return self.object.name + @property @abstractmethod def pose(self) -> Pose: @@ -170,7 +182,7 @@ def pose(self) -> Pose: @property def transform(self) -> Transform: """ - Returns the transform of this entity. + The transform of this entity. :return: The transform of this entity. """ @@ -180,7 +192,7 @@ def transform(self) -> Transform: @abstractmethod def tf_frame(self) -> str: """ - Returns the tf frame of this entity. + The tf frame of this entity. :return: The tf frame of this entity. """ @@ -196,7 +208,7 @@ def object_id(self) -> int: class Link(ObjectEntity, LinkDescription, ABC): """ - Represents a link of an Object in the World. + A link of an Object in the World. """ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): @@ -204,7 +216,48 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): LinkDescription.__init__(self, link_description.parsed_description) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} - self._update_pose() + self._current_pose: Optional[Pose] = None + self.update_pose() + + def set_pose(self, pose: Pose) -> None: + """ + Set the pose of this link to the given pose. + NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints + that can allow the link to be at the given pose. + + :param pose: The target pose for this link. + """ + self.object.set_pose(self.get_object_pose_given_link_pose(pose)) + + def get_object_pose_given_link_pose(self, pose): + """ + Get the object pose given the link pose, which could be a hypothetical link pose to see what would be the object + pose in that case (assuming that the object itself moved not the joints). + + :param pose: The link pose. + """ + return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() + + def get_pose_given_object_pose(self, pose): + """ + Get the link pose given the object pose, which could be a hypothetical object pose to see what would be the link + pose in that case (assuming that the object itself moved not the joints). + + :param pose: The object pose. + """ + return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() + + def get_transform_from_root_link(self) -> Transform: + """ + Return the transformation from the root link of the object to this link. + """ + return self.get_transform_from_link(self.object.root_link) + + def get_transform_to_root_link(self) -> Transform: + """ + Return the transformation from this link to the root link of the object. + """ + return self.get_transform_to_link(self.object.root_link) @property def current_state(self) -> LinkState: @@ -212,25 +265,28 @@ def current_state(self) -> LinkState: @current_state.setter def current_state(self, link_state: LinkState) -> None: - self.constraint_ids = link_state.constraint_ids + if self.current_state != link_state: + self.constraint_ids = link_state.constraint_ids - def add_fixed_constraint_with_link(self, child_link: 'Link') -> int: + def add_fixed_constraint_with_link(self, child_link: Self, + child_to_parent_transform: Optional[Transform] = None) -> int: """ - Adds a fixed constraint between this link and the given link, used to create attachments for example. + Add a fixed constraint between this link and the given link, to create attachments for example. :param child_link: The child link to which a fixed constraint should be added. + :param child_to_parent_transform: The transformation between the two links. :return: The unique id of the constraint. """ - constraint_id = self.world.add_fixed_constraint(self, - child_link, - child_link.get_transform_from_link(self)) + if child_to_parent_transform is None: + child_to_parent_transform = child_link.get_transform_to_link(self) + constraint_id = self.world.add_fixed_constraint(self, child_link, child_to_parent_transform) self.constraint_ids[child_link] = constraint_id child_link.constraint_ids[self] = constraint_id return constraint_id def remove_constraint_with_link(self, child_link: 'Link') -> None: """ - Removes the constraint between this link and the given link. + Remove the constraint between this link and the given link. :param child_link: The child link of the constraint that should be removed. """ @@ -240,17 +296,22 @@ def remove_constraint_with_link(self, child_link: 'Link') -> None: del child_link.constraint_ids[self] @property - def is_root(self) -> bool: + def is_only_link(self) -> bool: + """ + :return: True if this link is the only link, False otherwise. """ - Returns whether this link is the root link of the object. + return self.object.has_one_link + @property + def is_root(self) -> bool: + """ :return: True if this link is the root link, False otherwise. """ return self.object.get_root_link_id() == self.id - def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: + def update_transform(self, transform_time: Optional[Time] = None) -> None: """ - Updates the transformation of this link at the given time. + Update the transformation of this link at the given time. :param transform_time: The time at which the transformation should be updated. """ @@ -258,8 +319,6 @@ def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: def get_transform_to_link(self, link: 'Link') -> Transform: """ - Returns the transformation from this link to the given link. - :param link: The link to which the transformation should be returned. :return: A Transform object with the transformation from this link to the given link. """ @@ -267,8 +326,6 @@ def get_transform_to_link(self, link: 'Link') -> Transform: def get_transform_from_link(self, link: 'Link') -> Transform: """ - Returns the transformation from the given link to this link. - :param link: The link from which the transformation should be returned. :return: A Transform object with the transformation from the given link to this link. """ @@ -276,8 +333,6 @@ def get_transform_from_link(self, link: 'Link') -> Transform: def get_pose_wrt_link(self, link: 'Link') -> Pose: """ - Returns the pose of this link with respect to the given link. - :param link: The link with respect to which the pose should be returned. :return: A Pose object with the pose of this link with respect to the given link. """ @@ -285,8 +340,6 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of this link. - :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. """ return self.world.get_link_axis_aligned_bounding_box(self) @@ -294,8 +347,6 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: @property def position(self) -> Point: """ - The getter for the position of the link relative to the world frame. - :return: A Point object containing the position of the link relative to the world frame. """ return self.pose.position @@ -303,8 +354,6 @@ def position(self) -> Point: @property def position_as_list(self) -> List[float]: """ - The getter for the position of the link relative to the world frame as a list. - :return: A list containing the position of the link relative to the world frame. """ return self.pose.position_as_list() @@ -312,8 +361,6 @@ def position_as_list(self) -> List[float]: @property def orientation(self) -> Quaternion: """ - The getter for the orientation of the link relative to the world frame. - :return: A Quaternion object containing the orientation of the link relative to the world frame. """ return self.pose.orientation @@ -321,55 +368,58 @@ def orientation(self) -> Quaternion: @property def orientation_as_list(self) -> List[float]: """ - The getter for the orientation of the link relative to the world frame as a list. - :return: A list containing the orientation of the link relative to the world frame. """ return self.pose.orientation_as_list() - def _update_pose(self) -> None: + def update_pose(self) -> None: """ - Updates the current pose of this link from the world. + Update the current pose of this link from the world. """ self._current_pose = self.world.get_link_pose(self) @property def pose(self) -> Pose: """ - The pose of the link relative to the world frame. - :return: A Pose object containing the pose of the link relative to the world frame. """ + if self.world.conf.update_poses_from_sim_on_get: + self.update_pose() return self._current_pose @property def pose_as_list(self) -> List[List[float]]: """ - The pose of the link relative to the world frame as a list. - :return: A list containing the position and orientation of the link relative to the world frame. """ return self.pose.to_list() def get_origin_transform(self) -> Transform: """ - Returns the transformation between the link frame and the origin frame of this link. + :return: the transformation between the link frame and the origin frame of this link. """ return self.origin.to_transform(self.tf_frame) @property def color(self) -> Color: """ - The getter for the rgba_color of this link. - :return: A Color object containing the rgba_color of this link. """ return self.world.get_link_color(self) + @deprecated("Use color property setter instead") + def set_color(self, color: Color) -> None: + """ + Set the color of this link, could be rgb or rgba. + + :param color: The color as a list of floats, either rgb or rgba. + """ + self.color = color + @color.setter def color(self, color: Color) -> None: """ - The setter for the color of this link, could be rgb or rgba. + Set the color of this link, could be rgb or rgba. :param color: The color as a list of floats, either rgb or rgba. """ @@ -401,8 +451,8 @@ def __hash__(self): class RootLink(Link, ABC): """ - Represents the root link of an Object in the World. - It differs from the normal AbstractLink class in that the pose ande the tf_frame is the same as that of the object. + The root link of an Object in the World. + This differs from the normal AbstractLink class in that the pose and the tf_frame is the same as that of the object. """ def __init__(self, obj: Object): @@ -411,12 +461,12 @@ def __init__(self, obj: Object): @property def tf_frame(self) -> str: """ - Returns the tf frame of the root link, which is the same as the tf frame of the object. + :return: the tf frame of the root link, which is the same as the tf frame of the object. """ return self.object.tf_frame - def _update_pose(self) -> None: - self._current_pose = self.object.get_pose() + def update_pose(self) -> None: + self._current_pose = self.world.get_object_pose(self.object) def __copy__(self): return RootLink(self.object) @@ -424,14 +474,16 @@ def __copy__(self): class Joint(ObjectEntity, JointDescription, ABC): """ - Represents a joint of an Object in the World. + Represent a joint of an Object in the World. """ def __init__(self, _id: int, joint_description: JointDescription, - obj: Object): + obj: Object, is_virtual: Optional[bool] = False): ObjectEntity.__init__(self, _id, obj) - JointDescription.__init__(self, joint_description.parsed_description) + JointDescription.__init__(self, joint_description.parsed_description, is_virtual) + self.acceptable_error = (self.world.conf.revolute_joint_position_tolerance if self.type == JointType.REVOLUTE + else self.world.conf.prismatic_joint_position_tolerance) self._update_position() @property @@ -444,38 +496,34 @@ def tf_frame(self) -> str: @property def pose(self) -> Pose: """ - Returns the pose of this joint. The pose is the pose of the child link of this joint. - - :return: The pose of this joint. + :return: The pose of this joint. The pose is the pose of the child link of this joint. """ return self.child_link.pose def _update_position(self) -> None: """ - Updates the current position of the joint from the physics simulator. + Update the current position of the joint from the physics simulator. """ self._current_position = self.world.get_joint_position(self) @property def parent_link(self) -> Link: """ - Returns the parent link of this joint. - :return: The parent link as a AbstractLink object. """ - return self.object.get_link(self.parent_link_name) + return self.object.get_link(self.parent) @property def child_link(self) -> Link: """ - Returns the child link of this joint. - :return: The child link as a AbstractLink object. """ - return self.object.get_link(self.child_link_name) + return self.object.get_link(self.child) @property def position(self) -> float: + if self.world.conf.update_poses_from_sim_on_get: + self._update_position() return self._current_position def reset_position(self, position: float) -> None: @@ -484,8 +532,6 @@ def reset_position(self, position: float) -> None: def get_object_id(self) -> int: """ - Returns the id of the object to which this joint belongs. - :return: The integer id of the object to which this joint belongs. """ return self.object.id @@ -493,8 +539,8 @@ def get_object_id(self) -> int: @position.setter def position(self, joint_position: float) -> None: """ - Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, - an error will be printed. However, the joint will be set either way. + Set the position of the given joint to the given joint pose. If the pose is outside the joint limits, + issue a warning. However, set the joint either way. :param joint_position: The target pose for this joint """ @@ -524,16 +570,16 @@ def get_applied_motor_torque(self) -> float: @property def current_state(self) -> JointState: - return JointState(self.position) + return JointState(self.position, self.acceptable_error) @current_state.setter def current_state(self, joint_state: JointState) -> None: """ - Updates the current state of this joint from the given joint state if the position is different. + Update the current state of this joint from the given joint state if the position is different. :param joint_state: The joint state to update from. """ - if self._current_position != joint_state.position: + if self.current_state != joint_state: self.position = joint_state.position def __copy__(self): @@ -547,12 +593,11 @@ def __hash__(self): class ObjectDescription(EntityDescription): - """ A class that represents the description of an object. """ - mesh_extensions: Tuple[str] = (".obj", ".stl", ".dae") + mesh_extensions: Tuple[str] = (".obj", ".stl", ".dae", ".ply") """ The file extensions of the mesh files that can be used to generate a description file. """ @@ -570,23 +615,107 @@ def __init__(self, path: Optional[str] = None): """ :param path: The path of the file to update the description data from. """ + + self._links: Optional[List[LinkDescription]] = None + self._joints: Optional[List[JointDescription]] = None + self._link_map: Optional[Dict[str, Any]] = None + self._joint_map: Optional[Dict[str, Any]] = None + if path: self.update_description_from_file(path) else: self._parsed_description = None + self.virtual_joint_names: List[str] = [] + + @property + @abstractmethod + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + pass + + @property + @abstractmethod + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + pass + + @property + @abstractmethod + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + pass + + @property + @abstractmethod + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + pass + + def is_joint_virtual(self, name: str) -> bool: + """ + :param name: The name of the joint. + :return: True if the joint is virtual, False otherwise. + """ + return name in self.virtual_joint_names + + @abstractmethod + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: + """ + Add a joint to this object. + + :param name: The name of the joint. + :param child: The name of the child link. + :param joint_type: The type of the joint. + :param axis: The axis of the joint. + :param parent: The name of the parent link. + :param origin: The origin of the joint. + :param lower_limit: The lower limit of the joint. + :param upper_limit: The upper limit of the joint. + :param is_virtual: True if the joint is virtual, False otherwise. + """ + pass + def update_description_from_file(self, path: str) -> None: """ - Updates the description of this object from the file at the given path. + Update the description of this object from the file at the given path. :param path: The path of the file to update from. """ self._parsed_description = self.load_description(path) + def update_description_from_string(self, description_string: str) -> None: + """ + Update the description of this object from the given description string. + + :param description_string: The description string to update from. + """ + self._parsed_description = self.load_description_from_string(description_string) + + def load_description_from_string(self, description_string: str) -> Any: + """ + Load the description from the given string. + + :param description_string: The description string to load from. + """ + raise NotImplementedError + @property def parsed_description(self) -> Any: """ - Return the object parsed from the description file. + :return: The object parsed from the description file. """ return self._parsed_description @@ -600,46 +729,74 @@ def parsed_description(self, parsed_description: Any): @abstractmethod def load_description(self, path: str) -> Any: """ - Loads the description from the file at the given path. + Load the description from the file at the given path. :param path: The path to the source file, if only a filename is provided then the resources directories will be searched. """ pass - def generate_description_from_file(self, path: str, name: str, extension: str) -> str: + def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str, + scale_mesh: Optional[float] = None) -> None: """ - Generates and preprocesses the description from the file at the given path and returns the preprocessed - description as a string. + Generate and preprocess the description from the file at the given path and save the preprocessed + description. The generated description will be saved at the given save path. :param path: The path of the file to preprocess. :param name: The name of the object. :param extension: The file extension of the file to preprocess. - :return: The processed description string. + :param save_path: The path to save the generated description file. + :param scale_mesh: The scale of the mesh. + :raises ObjectDescriptionNotFound: If the description file could not be found/read. """ - description_string = None if extension in self.mesh_extensions: - description_string = self.generate_from_mesh_file(path, name) + if extension == ".ply": + mesh = trimesh.load(path) + path = path.replace(extension, ".obj") + if scale_mesh is not None: + mesh.apply_scale(scale_mesh) + mesh.export(path) + self.generate_from_mesh_file(path, name, save_path=save_path) elif extension == self.get_file_extension(): - description_string = self.generate_from_description_file(path) + self.generate_from_description_file(path, save_path=save_path) else: try: # Using the description from the parameter server - description_string = self.generate_from_parameter_server(path) + self.generate_from_parameter_server(path, save_path=save_path) except KeyError: - logging.warning(f"Couldn't find dile data in the ROS parameter server") - if description_string is None: - logging.error(f"Could not find file with path {path} in the resources directory nor" - f" in the ros parameter server.") - raise FileNotFoundError + logging.warning(f"Couldn't find file data in the ROS parameter server") - return description_string + if not self.check_description_file_exists_and_can_be_read(save_path): + raise ObjectDescriptionNotFound(name, path, extension) - def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: + @staticmethod + def check_description_file_exists_and_can_be_read(path: str) -> bool: """ - Returns the file name of the description file. + Check if the description file exists at the given path. + :param path: The path to the description file. + :return: True if the file exists, False otherwise. + """ + exists = os.path.exists(path) + if exists: + with open(path, "r") as file: + exists = bool(file.read()) + return exists + + @staticmethod + def write_description_to_file(description_string: str, save_path: str) -> None: + """ + Write the description string to the file at the given path. + + :param description_string: The description string to write. + :param save_path: The path of the file to write to. + """ + with open(save_path, "w") as file: + file.write(description_string) + + def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: + """ :param path_object: The path object of the description file or the mesh file. :param extension: The file extension of the description file or the mesh file. :param object_name: The name of the object. @@ -656,36 +813,39 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @classmethod @abstractmethod - def generate_from_mesh_file(cls, path: str, name: str) -> str: + def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> None: """ - Generates a description file from one of the mesh types defined in the mesh_extensions and - returns the path of the generated file. + Generate a description file from one of the mesh types defined in the mesh_extensions and + return the path of the generated file. The generated file will be saved at the given save_path. :param path: The path to the .obj file. :param name: The name of the object. - :return: The path of the generated description file. + :param save_path: The path to save the generated description file. """ pass @classmethod @abstractmethod - def generate_from_description_file(cls, path: str) -> str: + def generate_from_description_file(cls, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: """ - Preprocesses the given file and returns the preprocessed description string. + Preprocess the given file and return the preprocessed description string. The preprocessed description will be + saved at the given save_path. :param path: The path of the file to preprocess. - :return: The preprocessed description string. + :param save_path: The path to save the preprocessed description file. + :param make_mesh_paths_absolute: Whether to make the mesh paths absolute. """ pass @classmethod @abstractmethod - def generate_from_parameter_server(cls, name: str) -> str: + def generate_from_parameter_server(cls, name: str, save_path: str) -> None: """ - Preprocesses the description from the ROS parameter server and returns the preprocessed description string. + Preprocess the description from the ROS parameter server and return the preprocessed description string. + The preprocessed description will be saved at the given save_path. :param name: The name of the description on the parameter server. - :return: The preprocessed description string. + :param save_path: The path to save the preprocessed description file. """ pass @@ -697,12 +857,11 @@ def links(self) -> List[LinkDescription]: """ pass - @abstractmethod def get_link_by_name(self, link_name: str) -> LinkDescription: """ :return: The link description with the given name. """ - pass + return self.link_map[link_name] @property @abstractmethod @@ -712,12 +871,11 @@ def joints(self) -> List[JointDescription]: """ pass - @abstractmethod def get_joint_by_name(self, joint_name: str) -> JointDescription: """ :return: The joint description with the given name. """ - pass + return self.joint_map[joint_name] @abstractmethod def get_root(self) -> str: @@ -726,8 +884,15 @@ def get_root(self) -> str: """ pass + def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + """ + raise NotImplementedError + @abstractmethod - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: """ :return: the chain of links from 'start_link_name' to 'end_link_name'. """ diff --git a/src/pycram/designator.py b/src/pycram/designator.py index e5db34489..30490e28c 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -6,7 +6,8 @@ from abc import ABC, abstractmethod from inspect import isgenerator, isgeneratorfunction -import rospy +from .ros.logging import logwarn, loginfo + import inspect from .knowledge.knowledge_engine import KnowledgeEngine @@ -15,7 +16,7 @@ import owlready2 except ImportError: owlready2 = None - rospy.logwarn("owlready2 is not installed!") + logwarn("owlready2 is not installed!") from sqlalchemy.orm.session import Session @@ -24,7 +25,7 @@ from .utils import GeneratorList, bcolors from threading import Lock from time import time -from typing_extensions import Type, List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable, TYPE_CHECKING +from typing_extensions import Type, List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable, TYPE_CHECKING, get_args, get_origin from .local_transformer import LocalTransformer from .language import Language @@ -36,6 +37,7 @@ from .orm.action_designator import (Action as ORMAction) from .orm.object_designator import (Object as ORMObjectDesignator) +from .orm.motion_designator import Motion as ORMMotionDesignator from .orm.base import RobotState, ProcessMetaData from .tasktree import with_tree @@ -113,9 +115,8 @@ def ground(self) -> Any: def get_slots(self) -> List[str]: """ - Returns a list of all slots of this description. Can be used for inspecting different descriptions and debugging. - - :return: A list of all slots. + :return: a list of all slots of this description. Can be used for inspecting different descriptions and + debugging. """ return list(self.__dict__.keys()) @@ -124,7 +125,7 @@ def copy(self) -> DesignatorDescription: def get_default_ontology_concept(self) -> owlready2.Thing | None: """ - Returns the first element of ontology_concept_holders if there is, else None + :return: The first element of ontology_concept_holders if there is, else None """ return self.ontology_concept_holders[0].ontology_concept if self.ontology_concept_holders else None @@ -393,7 +394,7 @@ def to_sql(self) -> ORMObjectDesignator: :return: The created ORM object. """ - return ORMObjectDesignator(self.obj_type, self.name) + return ORMObjectDesignator(name=self.name, obj_type=self.obj_type) def insert(self, session: Session) -> ORMObjectDesignator: """ @@ -451,7 +452,7 @@ def __repr__(self): def special_knowledge_adjustment_pose(self, grasp: Grasp, pose: Pose) -> Pose: """ - Returns the adjusted target pose based on special knowledge for "grasp front". + Get the adjusted target pose based on special knowledge for "grasp front". :param grasp: From which side the object should be grasped :param pose: Pose at which the object should be grasped, before adjustment @@ -470,7 +471,7 @@ def special_knowledge_adjustment_pose(self, grasp: Grasp, pose: Pose) -> Pose: pose_in_object.pose.position.x += value[0] pose_in_object.pose.position.y += value[1] pose_in_object.pose.position.z += value[2] - rospy.loginfo("Adjusted target pose based on special knowledge for grasp: %s", grasp) + loginfo("Adjusted target pose based on special knowledge for grasp: %s", grasp) return pose_in_object return pose @@ -513,3 +514,69 @@ def __iter__(self) -> Iterable[Object]: continue yield self.Object(obj.name, obj.obj_type, obj) + +@dataclass +class BaseMotion(ABC): + + @abstractmethod + def perform(self): + """ + 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. Will be overwritten by each motion. + + :return: The created ORM object. + """ + return ORMMotionDesignator() + + @abstractmethod + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + """ + 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 args: Possible extra arguments + :param kwargs: Possible extra keyword arguments + :return: The completely instanced ORM motion. + """ + metadata = ProcessMetaData().insert(session) + + motion = self.to_sql() + motion.process_metadata = metadata + + return motion + + def __post_init__(self): + """ + Checks if types are missing or wrong + """ + right_types = get_type_hints(self) + attributes = self.__dict__.copy() + + missing = [] + wrong_type = {} + current_type = {} + + for k in attributes.keys(): + attribute = attributes[k] + attribute_type = type(attributes[k]) + right_type = right_types[k] + types = get_args(right_type) + if attribute is None: + 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 [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 != {}: + raise ResolutionError(missing, wrong_type, current_type, self.__class__) + diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index aa908925d..3e85fd808 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -8,9 +8,7 @@ import numpy as np from sqlalchemy.orm import Session from tf import transformations -from typing_extensions import Any, List, Union, Callable, Optional, Type - -import rospy +from typing_extensions import List, Union, Callable, Optional, Type from .location_designator import CostmapLocation from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ @@ -21,8 +19,7 @@ VisibleProperty from ..knowledge.knowledge_engine import ReasoningInstance from ..local_transformer import LocalTransformer -from ..plan_failures import ObjectUnfetchable, ReachabilityFailure -# from ..robot_descriptions import robot_description +from ..failures import ObjectUnfetchable, ReachabilityFailure from ..robot_description import RobotDescription from ..tasktree import with_tree @@ -80,11 +77,11 @@ def to_sql(self) -> Action: :return: An instance of the ORM equivalent of the action with the parameters set """ - # get all class parameters (ignore inherited ones) + # get all class parameters class_variables = {key: value for key, value in vars(self).items() if key in inspect.getfullargspec(self.__init__).args} - # get all orm class parameters (ignore inherited ones) + # get all orm class parameters orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args # list of parameters that will be passed to the ORM class. If the name does not match the orm_class equivalent @@ -108,11 +105,11 @@ def insert(self, session: Session, **kwargs) -> Action: action = super().insert(session) - # get all class parameters (ignore inherited ones) + # get all class parameters class_variables = {key: value for key, value in vars(self).items() if key in inspect.getfullargspec(self.__init__).args} - # get all orm class parameters (ignore inherited ones) + # get all orm class parameters orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args # loop through all class parameters and insert them into the session unless they are already added by the ORM @@ -262,10 +259,13 @@ class PickUpActionPerformable(ActionAbstract): """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMPickUpAction) - @with_tree - def plan(self) -> None: + def __post_init__(self): + super(ActionAbstract, self).__post_init__() # Store the object's data copy at execution self.object_at_execution = self.object_designator.frozen_copy() + + @with_tree + def plan(self) -> None: robot = World.robot # Retrieve object and robot from designators object = self.object_designator.world_object @@ -322,6 +322,17 @@ def plan(self) -> None: # Remove the vis axis from the world World.current_world.remove_vis_axis() + #TODO find a way to use object_at_execution instead of object_designator in the automatic orm mapping in ActionAbstract + def to_sql(self) -> Action: + return ORMPickUpAction(arm=self.arm, grasp=self.grasp) + + def insert(self, session: Session, **kwargs) -> Action: + action = super(ActionAbstract, self).insert(session) + action.object = self.object_at_execution.insert(session) + + session.add(action) + return action + @dataclass class PlaceActionPerformable(ActionAbstract): @@ -408,7 +419,7 @@ def plan(self) -> None: ParkArmsActionPerformable(Arms.BOTH).perform() pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), reachable_arm=self.arm) - # Tries to find a pick-up posotion for the robot that uses the given arm + # Tries to find a pick-up position for the robot that uses the given arm pickup_pose = None for pose in pickup_loc: if self.arm in pose.reachable_arms: diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 4bb4fcc79..9e854a19f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -174,6 +174,7 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object test_robot = World.current_world.get_prospection_object_for_object(robot_object) + with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): res = True @@ -244,7 +245,6 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = World.current_world.get_prospection_object_for_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree @@ -278,8 +278,10 @@ def __iter__(self) -> Location: valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose, allowed_collision={test_robot: hand_links}) - if valid_init and valid_goal: - yield self.Location(maybe_pose, list(set(arms_init).intersection(set(arms_goal)))) + arms_list = list(set(arms_init).intersection(set(arms_goal))) + + if valid_init and valid_goal and len(arms_list) > 0: + yield self.Location(maybe_pose, arms_list) class SemanticCostmapLocation(LocationDesignatorDescription): diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 92b1bd283..589d5f9ad 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -5,84 +5,19 @@ from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from ..designator import ResolutionError from ..orm.base import ProcessMetaData -from ..plan_failures import PerceptionObjectNotFound +from ..failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion, Motion as ORMMotionDesignator) -from ..datastructures.enums import ObjectType, Arms, GripperState +from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType -from typing_extensions import Dict, Optional, get_type_hints, get_args, get_origin +from typing_extensions import Dict, Optional, get_type_hints from ..datastructures.pose import Pose from ..tasktree import with_tree - - -@dataclass -class BaseMotion(ABC): - - @abstractmethod - def perform(self): - """ - 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. Will be overwritten by each motion. - - :return: The created ORM object. - """ - return ORMMotionDesignator() - - @abstractmethod - def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: - """ - 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 args: Possible extra arguments - :param kwargs: Possible extra keyword arguments - :return: The completely instanced ORM motion. - """ - metadata = ProcessMetaData().insert(session) - - motion = self.to_sql() - motion.process_metadata = metadata - - return motion - - def __post_init__(self): - """ - Checks if types are missing or wrong - """ - right_types = get_type_hints(self) - attributes = self.__dict__.copy() - - missing = [] - wrong_type = {} - current_type = {} - - for k in attributes.keys(): - attribute = attributes[k] - attribute_type = type(attributes[k]) - right_type = right_types[k] - types = get_args(right_type) - if attribute is None: - 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 [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 != {}: - raise ResolutionError(missing, wrong_type, current_type, self.__class__) +from ..designator import BaseMotion @dataclass @@ -226,9 +161,9 @@ def perform(self): if not world_object: raise PerceptionObjectNotFound( f"Could not find an object with the type {self.object_type} in the FOV of the robot") - if ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return RealObject.Object(world_object.name, world_object.obj_type, - world_object, world_object.get_pose()) + world_object, world_object.get_pose()) return ObjectDesignatorDescription.Object(world_object.name, world_object.obj_type, world_object) @@ -379,3 +314,26 @@ def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: session.add(motion) return motion + + +@dataclass +class TalkingMotion(BaseMotion): + """ + Talking Motion, lets the robot say a sentence. + """ + + cmd: str + """ + Talking Motion, let the robot say a sentence. + """ + + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.talk().execute(self) + + def to_sql(self) -> ORMMotionDesignator: + pass + + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 6b3be1c95..2fd07a99d 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -3,13 +3,14 @@ import dataclasses from typing_extensions import List, Optional, Callable, TYPE_CHECKING import sqlalchemy.orm +from ..datastructures.enums import ObjectType from ..datastructures.world import World from ..world_concepts.world_object import Object as WorldObject from ..designator import ObjectDesignatorDescription from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) from ..datastructures.pose import Pose -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * if TYPE_CHECKING: import owlready2 @@ -27,7 +28,7 @@ class Object(ObjectDesignatorDescription.Object): """ def to_sql(self) -> ORMBelieveObject: - return ORMBelieveObject(self.obj_type, self.name) + return ORMBelieveObject(name=self.name, obj_type=self.obj_type) def insert(self, session: sqlalchemy.orm.session.Session) -> ORMBelieveObject: metadata = ProcessMetaData().insert(session) @@ -50,7 +51,7 @@ class Object(ObjectDesignatorDescription.Object): part_pose: Pose def to_sql(self) -> ORMObjectPart: - return ORMObjectPart(self.obj_type, self.name) + return ORMObjectPart(obj_type=self.obj_type, name=self.name) def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectPart: metadata = ProcessMetaData().insert(session) @@ -64,7 +65,7 @@ def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectPart: def __init__(self, names: List[str], part_of: ObjectDesignatorDescription.Object, - type: Optional[str] = None): + type: Optional[ObjectType] = None): """ Describing the relationship between an object and a specific part of it. @@ -77,7 +78,7 @@ def __init__(self, names: List[str], if not part_of: raise AttributeError("part_of cannot be None.") - self.type: Optional[str] = type + self.type: Optional[ObjectType] = type self.names: Optional[List[str]] = names self.part_of = part_of @@ -135,6 +136,8 @@ def __init__(self, names: List[str], types: List[str], self.timestamps: List[float] = timestamps +@DeprecationWarning +# Depricated class this will be done differently class RealObject(ObjectDesignatorDescription): """ Object designator representing an object in the real world, when resolving this object designator description ] @@ -152,9 +155,9 @@ class Object(ObjectDesignatorDescription.Object): def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None, world_object: WorldObject = None): """ - - :param names: - :param types: + + :param names: + :param types: :param world_object: """ super().__init__() diff --git a/src/pycram/designators/specialized_designators/action/__init__.py b/src/pycram/designators/specialized_designators/action/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py b/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py new file mode 100644 index 000000000..6347b0a72 --- /dev/null +++ b/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py @@ -0,0 +1,78 @@ +from typing_extensions import List, Union, Optional +from numpy.linalg import norm +from numpy import array +from geometry_msgs.msg import Vector3 + +from owlready2 import Thing + +from ...action_designator import PickUpAction, PickUpActionPerformable +from ....local_transformer import LocalTransformer +from ....datastructures.world import World +from ....datastructures.pose import Pose, Transform +from ....datastructures.enums import Arms, Grasp +from ....robot_description import RobotDescription, KinematicChainDescription +from ....designator import ObjectDesignatorDescription +from ....ros.logging import loginfo + + +class DualArmPickupAction(PickUpAction): + """ + Specialization version of the PickUpAction designator which uses heuristics to solve for a dual pickup solution. + """ + + def __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + grasps: List[Grasp], resolver=None, + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Specialized version of the PickUpAction designator which uses heuristics to solve for a dual pickup problem. The + designator will choose the arm which is closest to the object that is to be picked up. + + :param object_designator_description: List of object designator which should be picked up + :param grasps: List of possible grasps which should be used for the pickup + :param resolver: Optional specialized_designators that returns a performable designator with elements from the + lists of possible parameter + :param ontology_concept_holders: List of ontology concepts that the action is categorized as or associated with + """ + super().__init__(object_designator_description, + arms=[Arms.LEFT, Arms.RIGHT], + grasps=grasps, + resolver=resolver, + ontology_concept_holders=ontology_concept_holders) + + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + + left_gripper = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT) + right_gripper = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT) + self.gripper_list: List[KinematicChainDescription] = [left_gripper, right_gripper] + + + def ground(self) -> PickUpActionPerformable: + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object): + obj_desig = self.object_designator_description + else: + obj_desig = self.object_designator_description.resolve() + + loginfo("Calculating closest gripper to object {}".format(obj_desig.name)) + + local_transformer = LocalTransformer() + + object_pose: Pose = obj_desig.world_object.pose + distances = [] + # Iterate over possible grippers + for gripper in self.gripper_list: + # Object pose in gripper frame + gripper_frame = World.robot.get_link_tf_frame(gripper.get_tool_frame()) + + object_T_gripper: Pose = local_transformer.transform_pose(object_pose, gripper_frame) + object_V_gripper: Vector3 = object_T_gripper.pose.position # translation vector + distance = norm(array([object_V_gripper.x, object_V_gripper.y, object_V_gripper.z])) + loginfo(f"Distance between {gripper} and {obj_desig.name}: {distance}") + distances.append(distance) + + min_index = distances.index(min(distances)) + winner = self.gripper_list[min_index] + loginfo(f"Winner is {winner.arm_type.name} with distance {min(distances):.2f}") + + return PickUpActionPerformable(object_designator=obj_desig, arm=winner.arm_type, grasp=self.grasps[0]) diff --git a/src/pycram/designators/specialized_designators/location/giskard_location.py b/src/pycram/designators/specialized_designators/location/giskard_location.py index 1400a8e63..de0d0a6e8 100644 --- a/src/pycram/designators/specialized_designators/location/giskard_location.py +++ b/src/pycram/designators/specialized_designators/location/giskard_location.py @@ -53,7 +53,7 @@ def __iter__(self) -> CostmapLocation.Location: prospection_robot = World.current_world.get_prospection_object_for_object(World.robot) with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) gripper_pose = prospection_robot.get_link_pose(chain.get_tool_frame()) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index fe6668bee..606969672 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -1,8 +1,8 @@ import numpy as np import tqdm -from probabilistic_model.probabilistic_circuit.distributions import GaussianDistribution, SymbolicDistribution -from probabilistic_model.probabilistic_circuit.probabilistic_circuit import ProbabilisticCircuit, \ - DecomposableProductUnit +from probabilistic_model.probabilistic_circuit.nx.distributions import GaussianDistribution, SymbolicDistribution +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, \ + ProductUnit from probabilistic_model.utils import MissingDict from random_events.interval import * from random_events.product_algebra import Event, SimpleEvent @@ -19,7 +19,7 @@ from ....designator import ActionDesignatorDescription, ObjectDesignatorDescription from ....local_transformer import LocalTransformer from ....orm.views import PickUpWithContextView -from ....plan_failures import ObjectUnreachable, PlanFailure +from ....failures import ObjectUnreachable, PlanFailure class Grasp(SetElement): @@ -124,7 +124,7 @@ def create_model_with_center(self) -> ProbabilisticCircuit: """ Create a fully factorized gaussian at the center of the map. """ - centered_model = DecomposableProductUnit() + centered_model = ProductUnit() centered_model.add_subcircuit(GaussianDistribution(self.relative_x, 0., np.sqrt(self.variance))) centered_model.add_subcircuit(GaussianDistribution(self.relative_y, 0., np.sqrt(self.variance))) @@ -206,7 +206,7 @@ def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: pose = Pose(position, frame=self.object_designator.world_object.tf_frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 - action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms(int(arm)), EGrasp(int(grasp))) + action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], EGrasp(int(grasp))) return action def events_from_occupancy_and_visibility_costmap(self) -> Event: @@ -300,8 +300,6 @@ def query_for_database(): def batch_rollout(self): """ Try the policy without conditioning on visibility and occupancy and count the successful tries. - - :amount: The amount of tries """ # initialize statistics diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 925584fa5..9fd922866 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -2,17 +2,18 @@ import threading import time -import rospy import sys -import rosnode + +from ..ros.data_types import Time +from ..ros.logging import logwarn, loginfo_once +from ..ros.ros_tools import get_node_names from ..datastructures.enums import JointType, ObjectType from ..datastructures.pose import Pose -# from ..robot_descriptions import robot_description from ..datastructures.world import World from ..datastructures.dataclasses import MeshVisualShape +from ..ros.service import get_service_proxy from ..world_concepts.world_object import Object -# from ..robot_description import ManipulatorDescription from ..robot_description import RobotDescription from typing_extensions import List, Dict, Callable, Optional @@ -22,9 +23,8 @@ try: from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry - from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse except ModuleNotFoundError as e: - rospy.logwarn("Failed to import Giskard messages, the real robot will not be available") + logwarn("Failed to import Giskard messages, the real robot will not be available") giskard_wrapper = None giskard_update_service = None @@ -66,28 +66,27 @@ def wrapper(*args, **kwargs): global giskard_wrapper global giskard_update_service global is_init - if is_init and "/giskard" in rosnode.get_node_names(): + if is_init and "/giskard" in get_node_names(): return func(*args, **kwargs) - elif is_init and "/giskard" not in rosnode.get_node_names(): - rospy.logwarn("Giskard node is not available anymore, could not initialize giskard interface") + elif is_init and "/giskard" not in get_node_names(): + logwarn("Giskard node is not available anymore, could not initialize giskard interface") is_init = False giskard_wrapper = None return if "giskard_msgs" not in sys.modules: - rospy.logwarn("Could not initialize the Giskard interface since the giskard_msgs are not imported") + logwarn("Could not initialize the Giskard interface since the giskard_msgs are not imported") return - if "/giskard" in rosnode.get_node_names(): + if "/giskard" in get_node_names(): giskard_wrapper = GiskardWrapper() - giskard_update_service = rospy.ServiceProxy("/giskard/update_world", UpdateWorld) - rospy.loginfo_once("Successfully initialized Giskard interface") + giskard_update_service = get_service_proxy("/giskard/update_world", UpdateWorld) + loginfo_once("Successfully initialized Giskard interface") is_init = True else: - rospy.logwarn("Giskard is not running, could not initialize Giskard interface") + logwarn("Giskard is not running, could not initialize Giskard interface") return return func(*args, **kwargs) - return wrapper @@ -169,7 +168,7 @@ def spawn_object(object: Object) -> None: :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: - geometry = object.get_link_geometry(object.root_link_name) + geometry = object.get_link_geometry(object.root_link.name) if isinstance(geometry, MeshVisualShape): filename = geometry.file_name spawn_mesh(object.name, filename, object.get_pose()) @@ -318,7 +317,7 @@ def achieve_joint_goal(goal_poses: Dict[str, float]) -> 'MoveResult': @init_giskard_interface @thread_safe -def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'MoveResult': +def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str, position_threshold: float = 0.02, orientation_threshold: float = 0.02) -> 'MoveResult': """ Takes a cartesian position and tries to move the tip_link to this position using the chain defined by tip_link and root_link. @@ -326,6 +325,8 @@ def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'M :param goal_pose: The position which should be achieved with tip_link :param tip_link: The end link of the chain as well as the link which should achieve the goal_pose :param root_link: The starting link of the chain which should be used to achieve this goal + :param position_threshold: Position distance at which the goal is successfully reached + :param orientation_threshold: Orientation distance at which the goal is successfully reached :return: MoveResult message for this goal """ sync_worlds() @@ -334,8 +335,19 @@ def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'M if par_return: return par_return - giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link) - # giskard_wrapper.add_default_end_motion_conditions() + cart_monitor1 = giskard_wrapper.monitors.add_cartesian_pose(root_link=root_link, tip_link=tip_link, + goal_pose=_pose_to_pose_stamped(goal_pose), + position_threshold=position_threshold, orientation_threshold=orientation_threshold, + name='cart goal 1') + end_monitor = giskard_wrapper.monitors.add_local_minimum_reached(start_condition=cart_monitor1) + + giskard_wrapper.motion_goals.add_cartesian_pose(name='g1', root_link=root_link, tip_link=tip_link, + goal_pose=_pose_to_pose_stamped(goal_pose), + end_condition=cart_monitor1) + + giskard_wrapper.monitors.add_end_motion(start_condition=end_monitor) + giskard_wrapper.motion_goals.avoid_all_collisions() + giskard_wrapper.motion_goals.allow_collision(group1='gripper', group2=CollisionEntry.ALL) return giskard_wrapper.execute() @@ -578,9 +590,7 @@ def allow_gripper_collision(gripper: str) -> None: @init_giskard_interface def get_gripper_group_names() -> List[str]: """ - Returns a list of groups that are registered in giskard which have 'gripper' in their name. - - :return: The list of gripper groups + :return: The list of groups that are registered in giskard which have 'gripper' in their name. """ groups = giskard_wrapper.get_group_names() return list(filter(lambda elem: "gripper" in elem, groups)) @@ -589,7 +599,7 @@ def get_gripper_group_names() -> List[str]: @init_giskard_interface def add_gripper_groups() -> None: """ - Adds the gripper links as a group for collision avoidance. + Add the gripper links as a group for collision avoidance. :return: Response of the RegisterGroup Service """ @@ -633,7 +643,7 @@ def avoid_collisions(object1: Object, object2: Object) -> None: @init_giskard_interface def make_world_body(object: Object) -> 'WorldBody': """ - Creates a WorldBody message for a World Object. The WorldBody will contain the URDF of the World Object + Create a WorldBody message for a World Object. The WorldBody will contain the URDF of the World Object :param object: The World Object :return: A WorldBody message for the World Object @@ -656,7 +666,7 @@ def make_point_stamped(point: List[float]) -> PointStamped: :return: A PointStamped message """ msg = PointStamped() - msg.header.stamp = rospy.Time.now() + msg.header.stamp = Time().now() msg.header.frame_id = "map" msg.point.x = point[0] @@ -674,7 +684,7 @@ def make_quaternion_stamped(quaternion: List[float]) -> QuaternionStamped: :return: A QuaternionStamped message """ msg = QuaternionStamped() - msg.header.stamp = rospy.Time.now() + msg.header.stamp = Time().now() msg.header.frame_id = "map" msg.quaternion.x = quaternion[0] @@ -693,7 +703,7 @@ def make_vector_stamped(vector: List[float]) -> Vector3Stamped: :return: A Vector3Stamped message """ msg = Vector3Stamped() - msg.header.stamp = rospy.Time.now() + msg.header.stamp = Time().now() msg.header.frame_id = "map" msg.vector.x = vector[0] diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 17ceca769..5a89a679f 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -2,7 +2,9 @@ import tf from typing_extensions import List, Union, Tuple, Dict -import rospy +from ..ros.data_types import Duration, ServiceException +from ..ros.logging import loginfo_once, logerr +from ..ros.service import get_service_proxy, wait_for_service from moveit_msgs.msg import PositionIKRequest from moveit_msgs.msg import RobotState from moveit_msgs.srv import GetPositionIK @@ -14,7 +16,7 @@ from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose from ..robot_description import RobotDescription -from ..plan_failures import IKError +from ..failures import IKError from ..external_interfaces.giskard import projection_cartesian_goal, allow_gripper_collision @@ -49,7 +51,7 @@ def _make_request_msg(root_link: str, tip_link: str, target_pose: Pose, robot_ob msg_request.pose_stamped = target_pose msg_request.avoid_collisions = False msg_request.robot_state = robot_state - msg_request.timeout = rospy.Duration(secs=1000) + msg_request.timeout = Duration(1000) # msg_request.attempts = 1000 return msg_request @@ -74,15 +76,15 @@ def call_ik(root_link: str, tip_link: str, target_pose: Pose, robot_object: Obje else: ik_service = "/kdl_ik_service/get_ik" - rospy.loginfo_once(f"Waiting for IK service: {ik_service}") - rospy.wait_for_service(ik_service) + loginfo_once(f"Waiting for IK service: {ik_service}") + wait_for_service(ik_service) req = _make_request_msg(root_link, tip_link, target_pose, robot_object, joints) req.pose_stamped.header.frame_id = root_link - ik = rospy.ServiceProxy(ik_service, GetPositionIK) + ik = get_service_proxy(ik_service, GetPositionIK) try: resp = ik(req) - except rospy.ServiceException as e: + except ServiceException as e: if RobotDescription.current_robot_description.name == "pr2": raise IKError(target_pose, root_link, tip_link) else: @@ -151,7 +153,7 @@ def try_to_reach(pose_or_object: Union[Pose, Object], prospection_robot: Object, try: inv = request_ik(input_pose, prospection_robot, joints, gripper_name) except IKError as e: - rospy.logerr(f"Pose is not reachable: {e}") + logerr(f"Pose is not reachable: {e}") return None _apply_ik(prospection_robot, inv) @@ -213,7 +215,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[ :param gripper: Name of the tool frame which should grasp, this should be at the end of the given joint chain. :return: A list of joint values. """ - rospy.loginfo_once(f"Using Giskard for full body IK") + loginfo_once(f"Using Giskard for full body IK") local_transformer = LocalTransformer() target_map = local_transformer.transform_pose(target_pose, "map") @@ -234,7 +236,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[ robot_joint_states[joint_name] = state with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) tip_pose = prospection_robot.get_link_pose(gripper) diff --git a/src/pycram/external_interfaces/knowrob.py b/src/pycram/external_interfaces/knowrob.py index 9bd5055b6..f58a2556e 100644 --- a/src/pycram/external_interfaces/knowrob.py +++ b/src/pycram/external_interfaces/knowrob.py @@ -123,4 +123,4 @@ def knowrob_string_to_pose(pose_as_string: str) -> List[float]: pos, ori = pose_as_string[1+i+2:-2].split("],[") xyz = list(map(float, pos.split(","))) qxyzw = list(map(float, ori.split(","))) - return xyz + qxyzw + return xyz + qxyzw \ No newline at end of file diff --git a/src/pycram/external_interfaces/move_base.py b/src/pycram/external_interfaces/move_base.py index d443ef739..a63ed0bec 100644 --- a/src/pycram/external_interfaces/move_base.py +++ b/src/pycram/external_interfaces/move_base.py @@ -1,15 +1,16 @@ import sys -import rospy -import actionlib -import rosnode +from ..ros.action_lib import create_action_client, SimpleActionClient +from ..ros.logging import logwarn, loginfo +from ..ros.ros_tools import get_node_names + from geometry_msgs.msg import PoseStamped from typing import Callable try: from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import MoveBase messages, Navigation interface could not be initialized") + logwarn(f"Could not import MoveBase messages, Navigation interface could not be initialized") # Global variables for shared resources @@ -17,10 +18,10 @@ is_init = False -def create_nav_action_client() -> actionlib.SimpleActionClient: +def create_nav_action_client() -> SimpleActionClient: """Creates a new action client for the move_base interface.""" - client = actionlib.SimpleActionClient("move_base", MoveBaseAction) - rospy.loginfo("Waiting for move_base action server") + client = create_action_client("move_base", MoveBaseAction) + loginfo("Waiting for move_base action server") client.wait_for_server() return client @@ -36,15 +37,15 @@ def wrapper(*args, **kwargs): return func(*args, **kwargs) if "move_base_msgs" not in sys.modules: - rospy.logwarn("Could not initialize the navigation interface: move_base_msgs not imported") + logwarn("Could not initialize the navigation interface: move_base_msgs not imported") return - if "/move_base" in rosnode.get_node_names(): + if "/move_base" in get_node_names(): nav_action_client = create_nav_action_client() - rospy.loginfo("Successfully initialized navigation interface") + loginfo("Successfully initialized navigation interface") is_init = True else: - rospy.logwarn("Move_base is not running, could not initialize navigation interface") + logwarn("Move_base is not running, could not initialize navigation interface") return return func(*args, **kwargs) @@ -59,10 +60,10 @@ def query_pose_nav(navpose: PoseStamped): global query_result def active_callback(): - rospy.loginfo("Sent query to move_base") + loginfo("Sent query to move_base") def done_callback(state, result): - rospy.loginfo("Finished moving") + loginfo("Finished moving") global query_result query_result = result diff --git a/src/pycram/external_interfaces/robokudo.py b/src/pycram/external_interfaces/robokudo.py index a1a7d618a..9a71c05bb 100644 --- a/src/pycram/external_interfaces/robokudo.py +++ b/src/pycram/external_interfaces/robokudo.py @@ -1,135 +1,168 @@ import sys -from typing_extensions import Callable +from threading import Lock, RLock +from typing import Any -import rospy -import actionlib -import rosnode +from ..ros.action_lib import create_action_client +from ..ros.logging import logwarn, loginfo, loginfo_once +from ..ros.ros_tools import get_node_names + +from geometry_msgs.msg import PointStamped +from typing_extensions import List, Callable, Optional -from ..designator import ObjectDesignatorDescription from ..datastructures.pose import Pose -from ..local_transformer import LocalTransformer -from ..datastructures.world import World -from ..datastructures.enums import ObjectType +from ..designator import ObjectDesignatorDescription try: - from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjetDesignator + from robokudo_msgs.msg import ObjectDesignator as robokudo_ObjectDesignator from robokudo_msgs.msg import QueryAction, QueryGoal, QueryResult except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import RoboKudo messages, RoboKudo interface could not be initialized") + logwarn("Failed to import Robokudo messages, the real robot will not be available") + +is_init = False + +number_of_par_goals = 0 +robokudo_lock = Lock() +robokudo_rlock = RLock() +with robokudo_rlock: + par_threads = {} + par_motion_goal = {} + + +def thread_safe(func: Callable) -> Callable: + """ + Adds thread safety to a function via a decorator. This uses the robokudo_lock + + :param func: Function that should be thread safe + :return: A function with thread safety + """ + + def wrapper(*args, **kwargs): + with robokudo_rlock: + return func(*args, **kwargs) -robokudo_action_client = None + return wrapper def init_robokudo_interface(func: Callable) -> Callable: """ - Tries to import the RoboKudo messages and with that initialize the RoboKudo interface. + Checks if the ROS messages are available and if Robokudo is running, if that is the case the interface will be + initialized. + + :param func: Function this decorator should be wrapping + :return: A callable function which initializes the interface and then calls the wrapped function """ + def wrapper(*args, **kwargs): - global robokudo_action_client - topics = list(map(lambda x: x[0], rospy.get_published_topics())) + global is_init + if is_init and "/robokudo" in get_node_names(): + return func(*args, **kwargs) + elif is_init and "/robokudo" not in get_node_names(): + logwarn("Robokudo node is not available anymore, could not initialize robokudo interface") + is_init = False + return + if "robokudo_msgs" not in sys.modules: - rospy.logwarn("Could not initialize the RoboKudo interface since the robokudo_msgs are not imported") + logwarn("Could not initialize the Robokudo interface since the robokudo_msgs are not imported") return - if "/robokudo" in rosnode.get_node_names(): - robokudo_action_client = create_robokudo_action_client() - rospy.loginfo("Successfully initialized robokudo interface") + if "/robokudo" in get_node_names(): + loginfo_once("Successfully initialized Robokudo interface") + is_init = True else: - rospy.logwarn("RoboKudo is not running, could not initialize RoboKudo interface") + logwarn("Robokudo is not running, could not initialize Robokudo interface") return - return func(*args, **kwargs) + return wrapper -def create_robokudo_action_client() -> Callable: - """ - Creates a new action client for the RoboKudo query interface and returns a function encapsulating the action client. - The returned function can be called with an ObjectDesigantor as parameter and returns the result of the action client. - - :return: A callable function encapsulating the action client - """ - client = actionlib.SimpleActionClient('robokudo/query', QueryAction) - rospy.loginfo("Waiting for action server") +@init_robokudo_interface +def send_query(obj_type: Optional[str] = None, region: Optional[str] = None, + attributes: Optional[List[str]] = None) -> Any: + """Generic function to send a query to RoboKudo.""" + goal = QueryGoal() + + if obj_type: + goal.obj.type = obj_type + if region: + goal.obj.location = region + if attributes: + goal.obj.attribute = attributes + + # client = actionlib.SimpleActionClient('robokudo/query', QueryAction) + client = create_action_client("robokudo/query", QueryAction) + loginfo("Waiting for action server") client.wait_for_server() - def action_client(object_desc): - global query_result - - def active_callback(): - rospy.loginfo("Send query to Robokudo") - - def done_callback(state, result): - rospy.loginfo("Finished perceiving") - global query_result - query_result = result + query_result = None - def feedback_callback(msg): - pass + def done_callback(state, result): + nonlocal query_result + query_result = result + loginfo("Query completed") - object_goal = make_query_goal_msg(object_desc) - client.send_goal(object_goal, active_cb=active_callback, done_cb=done_callback, feedback_cb=feedback_callback) - wait = client.wait_for_result() - return query_result + client.send_goal(goal, done_cb=done_callback) + client.wait_for_result() + return query_result - return action_client +@init_robokudo_interface +def query_object(obj_desc: ObjectDesignatorDescription) -> dict: + """Query RoboKudo for an object that fits the description.""" + goal = QueryGoal() + goal.obj.uid = str(id(obj_desc)) + goal.obj.type = str(obj_desc.types[0].name) -def msg_from_obj_desig(obj_desc: ObjectDesignatorDescription) -> 'robokudo_ObjetDesignator': - """ - Creates a RoboKudo Object designator from a PyCRAM Object Designator description + result = send_query(obj_type=goal.obj.type) - :param obj_desc: The PyCRAM Object designator that should be converted - :return: The RobotKudo Object Designator for the given PyCRAM designator - """ - obj_msg = robokudo_ObjetDesignator() - obj_msg.uid = str(id(obj_desc)) - obj_msg.obj_type = obj_desc.types[0] # For testing purposes + pose_candidates = {} + if result and result.res: + for i in range(len(result.res[0].pose)): + pose = Pose.from_pose_stamped(result.res[0].pose[i]) + source = result.res[0].pose_source[0] + pose_candidates[source] = pose + return pose_candidates - return obj_msg +@init_robokudo_interface +def query_human() -> PointStamped: + """Query RoboKudo for human detection and return the detected human's pose.""" + result = send_query(obj_type='human') + if result: + return result # Assuming result is of type PointStamped or similar. + return None -def make_query_goal_msg(obj_desc: ObjectDesignatorDescription) -> 'QueryGoal': - """ - Creates a QueryGoal message from a PyCRAM Object designator description for the use of Querying RobotKudo. - :param obj_desc: The PyCRAM object designator description that should be converted - :return: The RoboKudo QueryGoal for the given object designator description - """ - goal_msg = QueryGoal() - goal_msg.obj.uid = str(id(obj_desc)) - goal_msg.obj.obj_type = str(obj_desc.types[0].name) # For testing purposes - if ObjectType.JEROEN_CUP == obj_desc.types[0]: - goal_msg.obj.color.append("blue") - elif ObjectType.BOWL == obj_desc.types[0]: - goal_msg.obj.color.append("red") - return goal_msg +@init_robokudo_interface +def stop_query(): + """Stop any ongoing query to RoboKudo.""" + #client = actionlib.SimpleActionClient('robokudo/query', QueryAction) + client = create_action_client('robokudo/query', QueryAction) + client.wait_for_server() + client.cancel_all_goals() + loginfo("Cancelled current RoboKudo query goal") @init_robokudo_interface -def query(object_desc: ObjectDesignatorDescription) -> ObjectDesignatorDescription.Object: - """ - Sends a query to RoboKudo to look for an object that fits the description given by the Object designator description. - For sending the query to RoboKudo a simple action client will be created and the Object designator description is - sent as a goal. +def query_specific_region(region: str) -> Any: + """Query RoboKudo to scan a specific region.""" + return send_query(region=region) - :param object_desc: The object designator description which describes the object that should be perceived - :return: An object designator for the found object, if there was an object that fitted the description. - """ - query_result = robokudo_action_client(object_desc) - pose_candidates = {} - if query_result.res == []: - rospy.logwarn("No suitable object could be found") - return - for i in range(0, len(query_result.res[0].pose)): - pose = Pose.from_pose_stamped(query_result.res[0].pose[i]) - pose.frame = World.current_world.robot.get_link_tf_frame(pose.frame) # TODO: pose.frame is a link name? - source = query_result.res[0].poseSource[i] - - lt = LocalTransformer() - pose = lt.transform_pose(pose, "map") +@init_robokudo_interface +def query_human_attributes() -> Any: + """Query RoboKudo for human attributes like brightness of clothes, headgear, and gender.""" + return send_query(obj_type='human', attributes=["attributes"]) - pose_candidates[source] = pose - return pose_candidates +@init_robokudo_interface +def query_waving_human() -> Pose: + """Query RoboKudo for detecting a waving human.""" + result = send_query(obj_type='human') + if result and result.res: + try: + pose = Pose.from_pose_stamped(result.res[0].pose[0]) + return pose + except IndexError: + pass + return None diff --git a/src/pycram/external_interfaces/tmc.py b/src/pycram/external_interfaces/tmc.py new file mode 100644 index 000000000..daf3384e0 --- /dev/null +++ b/src/pycram/external_interfaces/tmc.py @@ -0,0 +1,60 @@ +from typing_extensions import Optional + +from ..datastructures.enums import GripperState +from ..designators.motion_designator import MoveGripperMotion, TalkingMotion +from ..ros.logging import loginfo +from ..ros.publisher import create_publisher +from ..ros.data_types import Rate + +is_init = False + + +def init_tmc_interface(): + global is_init + if is_init: + return + try: + from tmc_control_msgs.msg import GripperApplyEffortActionGoal + from tmc_msgs.msg import Voice + is_init = True + loginfo("Successfully initialized tmc interface") + except ModuleNotFoundError as e: + logwarn(f"Could not import TMC messages, tmc interface could not be initialized") + + +def tmc_gripper_control(designator: MoveGripperMotion, topic_name: Optional[str] = '/hsrb/gripper_controller/grasp/goal'): + """ + Publishes a message to the gripper controller to open or close the gripper for the HSR. + + :param designator: The designator containing the motion to be executed + :param topic_name: The topic name to publish the message to + """ + if (designator.motion == GripperState.OPEN): + pub_gripper = create_publisher(topic_name, GripperApplyEffortActionGoal, 10) + rate = Rate(10) + msg = GripperApplyEffortActionGoal() + msg.goal.effort = 0.8 + pub_gripper.publish(msg) + + elif (designator.motion == GripperState.CLOSE): + pub_gripper = create_publisher(topic_name, GripperApplyEffortActionGoal, 10) + rate = Rate(10) + msg = GripperApplyEffortActionGoal() + msg.goal.effort = -0.8 + pub_gripper.publish(msg) + + +def tmc_talk(designator: TalkingMotion, topic_name: Optional[str] = '/talk_request'): + """ + Publishes a sentence to the talk_request topic of the HSRB robot + + :param designator: The designator containing the sentence to be spoken + :param topic_name: The topic name to publish the sentence to + """ + pub = create_publisher(topic_name, Voice, 10) + texttospeech = Voice() + # language 1 = english (0 = japanese) + texttospeech.language = 1 + texttospeech.sentence = designator.cmd + + pub.publish(texttospeech) diff --git a/src/pycram/failure_handling.py b/src/pycram/failure_handling.py index 8fb266282..1c53061a4 100644 --- a/src/pycram/failure_handling.py +++ b/src/pycram/failure_handling.py @@ -1,8 +1,12 @@ +from .datastructures.enums import State from .designator import DesignatorDescription -from .plan_failures import PlanFailure +from .failures import PlanFailure +from threading import Lock +from typing_extensions import Union, Tuple, Any, List +from .language import Language, Monitor -class FailureHandling: +class FailureHandling(Language): """ Base class for failure handling mechanisms in automated systems or workflows. @@ -11,11 +15,12 @@ class FailureHandling: to be extended by subclasses that implement specific failure handling behaviors. """ - def __init__(self, designator_description: DesignatorDescription): + def __init__(self, designator_description: Union[DesignatorDescription, Monitor]): """ Initializes a new instance of the FailureHandling class. - :param designator_description: The description or context of the task or process for which the failure handling is being set up. + :param Union[DesignatorDescription, Monitor] designator_description: The description or context of the task + or process for which the failure handling is being set up. """ self.designator_description = designator_description @@ -37,15 +42,10 @@ class Retry(FailureHandling): This class represents a specific failure handling strategy where the system attempts to retry a failed action a certain number of times before giving up. - - Attributes: - max_tries (int): The maximum number of attempts to retry the action. - - Inherits: - All attributes and methods from the FailureHandling class. - - Overrides: - perform(): Implements the retry logic. + """ + max_tries: int + """ + The maximum number of attempts to retry the action. """ def __init__(self, designator_description: DesignatorDescription, max_tries: int = 3): @@ -58,7 +58,7 @@ def __init__(self, designator_description: DesignatorDescription, max_tries: int super().__init__(designator_description) self.max_tries = max_tries - def perform(self): + def perform(self) -> Tuple[State, List[Any]]: """ Implementation of the retry mechanism. @@ -79,5 +79,93 @@ def perform(self): raise e +class RetryMonitor(FailureHandling): + """ + A subclass of FailureHandling that implements a retry mechanism that works with a Monitor. + This class represents a specific failure handling strategy that allows us to retry a demo that is + being monitored, in case that monitoring condition is triggered. + """ + max_tries: int + """ + The maximum number of attempts to retry the action. + """ + recovery: dict + """ + A dictionary that maps exception types to recovery actions + """ + def __init__(self, designator_description: Monitor, max_tries: int = 3, recovery: dict = None): + """ + Initializes a new instance of the RetryMonitor class. + :param Monitor designator_description: The Monitor instance to be used. + :param int max_tries: The maximum number of attempts to retry. Defaults to 3. + :param dict recovery: A dictionary that maps exception types to recovery actions. Defaults to None. + """ + super().__init__(designator_description) + self.max_tries = max_tries + self.lock = Lock() + if recovery is None: + self.recovery = {} + else: + if not isinstance(recovery, dict): + raise ValueError( + "Recovery must be a dictionary with exception types as keys and Language instances as values.") + for key, value in recovery.items(): + if not issubclass(key, BaseException): + raise TypeError("Keys in the recovery dictionary must be exception types.") + if not isinstance(value, Language): + raise TypeError("Values in the recovery dictionary must be instances of the Language class.") + self.recovery = recovery + + def perform(self) -> Tuple[State, List[Any]]: + """ + This method attempts to perform the Monitor + plan specified in the designator_description. If the action + fails, it is retried up to max_tries times. If all attempts fail, the last exception is raised. In every + loop, we need to clear the kill_event, and set all relevant 'interrupted' variables to False, to make sure + the Monitor and plan are executed properly again. + + :raises PlanFailure: If all retry attempts fail. + + :return: The state of the execution performed, as well as a flattened list of the + results, in the correct order + """ + + def reset_interrupted(child): + child.interrupted = False + try: + for sub_child in child.children: + reset_interrupted(sub_child) + except AttributeError: + pass + + def flatten(result): + flattened_list = [] + if result: + for item in result: + if isinstance(item, list): + flattened_list.extend(item) + else: + flattened_list.append(item) + return flattened_list + return None + + status, res = None, None + with self.lock: + tries = 0 + while True: + self.designator_description.kill_event.clear() + self.designator_description.interrupted = False + for child in self.designator_description.children: + reset_interrupted(child) + try: + status, res = self.designator_description.perform() + break + except PlanFailure as e: + tries += 1 + if tries >= self.max_tries: + raise e + exception_type = type(e) + if exception_type in self.recovery: + self.recovery[exception_type].perform() + return status, flatten(res) diff --git a/src/pycram/plan_failures.py b/src/pycram/failures.py similarity index 79% rename from src/pycram/plan_failures.py rename to src/pycram/failures.py index b50507afe..effe849d4 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/failures.py @@ -1,3 +1,12 @@ +from pathlib import Path + +from typing_extensions import TYPE_CHECKING, List + +if TYPE_CHECKING: + from .world_concepts.world_object import Object + from .datastructures.enums import JointType + + class PlanFailure(Exception): """Implementation of plan failures.""" @@ -141,8 +150,10 @@ def __init__(self, *args, **kwargs): class IKError(PlanFailure): """Thrown when no inverse kinematics solution could be found""" + def __init__(self, pose, base_frame, tip_frame): - self.message = "Position {} in frame '{}' is not reachable for end effector: '{}'".format(pose, base_frame, tip_frame) + self.message = "Position {} in frame '{}' is not reachable for end effector: '{}'".format(pose, base_frame, + tip_frame) super(IKError, self).__init__(self.message) @@ -416,3 +427,66 @@ def __init__(self, *args, **kwargs): class CollisionError(PlanFailure): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) + + +""" +The following exceptions are used in the PyCRAM framework to handle errors related to the world and the objects in it. +They are usually related to a bug in the code or a misuse of the framework (e.g. logical errors in the code). +""" + + +class ProspectionObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the prospection world.") + + +class WorldObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the main world.") + + +class ObjectAlreadyExists(Exception): + def __init__(self, obj: 'Object'): + super().__init__(f"An object with the name {obj.name} already exists in the world.") + + +class ObjectDescriptionNotFound(KeyError): + def __init__(self, object_name: str, path: str, extension: str): + super().__init__(f"{object_name} with path {path} and extension {extension} is not in supported extensions, and" + f" the description data was not found on the ROS parameter server") + + +class WorldMismatchErrorBetweenObjects(Exception): + def __init__(self, obj_1: 'Object', obj_2: 'Object'): + super().__init__(f"World mismatch between the attached objects {obj_1.name} and {obj_2.name}," + f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") + + +class ObjectFrameNotFoundError(KeyError): + def __init__(self, frame_name: str): + super().__init__(f"Frame {frame_name} does not belong to any of the objects in the world.") + + +class MultiplePossibleTipLinks(Exception): + def __init__(self, object_name: str, start_link: str, tip_links: List[str]): + super().__init__(f"Multiple possible tip links found for object {object_name} with start link {start_link}:" + f" {tip_links}") + + +class UnsupportedFileExtension(Exception): + def __init__(self, object_name: str, path: str): + extension = Path(path).suffix + super().__init__(f"Unsupported file extension for object {object_name} with path {path}" + f"and extension {extension}") + + +class ObjectDescriptionUndefined(Exception): + def __init__(self, object_name: str): + super().__init__(f"Object description for object {object_name} is not defined, eith a path or a description" + f"object should be provided.") + + +class UnsupportedJointType(Exception): + def __init__(self, joint_type: 'JointType'): + super().__init__(f"Unsupported joint type: {joint_type}") + diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 73cf77dbc..fb2f78927 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -3,6 +3,13 @@ Classes: Singleton -- implementation of singleton metaclass """ +import os +from typing_extensions import Dict, Optional +import xml.etree.ElementTree as ET + +from pycram.ros.logging import logwarn + + class Singleton(type): """ Metaclass for singletons @@ -16,4 +23,94 @@ class Singleton(type): def __call__(cls, *args, **kwargs): if cls not in cls._instances: cls._instances[cls] = super(Singleton, cls).__call__(*args, **kwargs) - return cls._instances[cls] \ No newline at end of file + return cls._instances[cls] + + +def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: + """ + Parse the actuator elements from an MJCF file. + + :param file_path: The path to the MJCF file. + """ + tree = ET.parse(file_path) + root = tree.getroot() + + joint_actuators = {} + + # Iterate through all actuator elements + for actuator in root.findall(".//actuator/*"): + name = actuator.get('name') + joint = actuator.get('joint') + if name and joint: + joint_actuators[joint] = name + + return joint_actuators + + +def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: + """ + Get the path to the MJCF file of a robot. + + :param robot_relative_dir: The relative directory of the robot in the Multiverse resources/robots directory. + :param robot_name: The name of the robot. + :param xml_name: The name of the XML file of the robot. + :return: The path to the MJCF file of the robot if it exists, otherwise None. + """ + xml_name = xml_name if xml_name is not None else robot_name + if '.xml' not in xml_name: + xml_name = xml_name + '.xml' + multiverse_resources = find_multiverse_resources_path() + try: + robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name) + except TypeError: + logwarn("Multiverse resources path not found.") + return None + if multiverse_resources is not None: + list_dir = os.listdir(robot_folder) + if 'mjcf' in list_dir: + if xml_name in os.listdir(robot_folder + '/mjcf'): + return os.path.join(robot_folder, 'mjcf', xml_name) + elif xml_name in os.listdir(robot_folder): + return os.path.join(robot_folder, xml_name) + return None + + +def find_multiverse_resources_path() -> Optional[str]: + """ + :return: The path to the Multiverse resources directory. + """ + # Get the path to the Multiverse installation + multiverse_path = find_multiverse_path() + + # Check if the path to the Multiverse installation was found + if multiverse_path: + # Construct the path to the resources directory + resources_path = os.path.join(multiverse_path, 'resources') + + # Check if the resources directory exists + if os.path.exists(resources_path): + return resources_path + + return None + + +def find_multiverse_path() -> Optional[str]: + """ + :return: the path to the Multiverse installation. + """ + # Get the value of PYTHONPATH environment variable + pythonpath = os.getenv('PYTHONPATH') + multiverse_relative_path = "Multiverse/multiverse" + + # Check if PYTHONPATH is set + if pythonpath: + # Split the PYTHONPATH into individual paths using the platform-specific path separator + paths = pythonpath.split(os.pathsep) + + # Iterate through each path and check if 'Multiverse' is in it + for path in paths: + if multiverse_relative_path in path: + multiverse_path = path.split(multiverse_relative_path)[0] + return multiverse_path + multiverse_relative_path + + diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 0f2b18e08..a8aa95011 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -11,7 +11,7 @@ from .knowledge_source import KnowledgeSource from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict, Any -from ..plan_failures import KnowledgeNotAvailable, ReasoningError +from ..failures import KnowledgeNotAvailable, ReasoningError # This import is needed since the subclasses of KnowledgeSource need to be imported to be known at runtime from .knowledge_sources import * diff --git a/src/pycram/knowledge/knowledge_source.py b/src/pycram/knowledge/knowledge_source.py index 0a5d422af..f72adfe82 100644 --- a/src/pycram/knowledge/knowledge_source.py +++ b/src/pycram/knowledge/knowledge_source.py @@ -3,7 +3,7 @@ from abc import ABC, abstractmethod from typing_extensions import TYPE_CHECKING -from ..plan_failures import KnowledgeNotAvailable +from ..failures import KnowledgeNotAvailable if TYPE_CHECKING: from ..designator import DesignatorDescription diff --git a/src/pycram/language.py b/src/pycram/language.py index 3b6520f0b..d781a2e7c 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -1,16 +1,17 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import time -from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union +from queue import Queue +from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union, Tuple from anytree import NodeMixin, Node, PreOrderIter -from pycram.datastructures.enums import State +from .datastructures.enums import State import threading from .fluent import Fluent -from .plan_failures import PlanFailure, NotALanguageExpression +from .failures import PlanFailure, NotALanguageExpression from .external_interfaces import giskard +from .ros.ros_tools import sleep class Language(NodeMixin): @@ -260,6 +261,7 @@ def __init__(self, condition: Union[Callable, Fluent] = None): """ super().__init__(None, None) self.kill_event = threading.Event() + self.exception_queue = Queue() if callable(condition): self.condition = Fluent(condition) elif isinstance(condition, Fluent): @@ -267,27 +269,43 @@ def __init__(self, condition: Union[Callable, Fluent] = None): else: raise AttributeError("The condition of a Monitor has to be a Callable or a Fluent") - def perform(self): + def perform(self) -> Tuple[State, Any]: """ Behavior of the Monitor, starts a new Thread which checks the condition and then performs the attached language expression - :return: The result of the attached language expression + :return: The state of the attached language expression, as well as a list of the results of the children """ def check_condition(): - while not self.condition.get_value() and not self.kill_event.is_set(): - time.sleep(0.1) - if self.kill_event.is_set(): - return - for child in self.children: - child.interrupt() + while not self.kill_event.is_set(): + try: + cond = self.condition.get_value() + if cond: + for child in self.children: + try: + child.interrupt() + except NotImplementedError: + pass + if isinstance(cond, type) and issubclass(cond, Exception): + self.exception_queue.put(cond) + else: + self.exception_queue.put(PlanFailure("Condition met in Monitor")) + return + except Exception as e: + self.exception_queue.put(e) + return + sleep(0.1) t = threading.Thread(target=check_condition) t.start() - res = self.children[0].perform() - self.kill_event.set() - t.join() - return res + try: + state, result = self.children[0].perform() + if not self.exception_queue.empty(): + raise self.exception_queue.get() + finally: + self.kill_event.set() + t.join() + return state, result def interrupt(self) -> None: """ @@ -303,28 +321,35 @@ class Sequential(Language): Instead, the exception is saved to a list of all exceptions thrown during execution and returned. Behaviour: - Return the state :py:attr:`~State.SUCCEEDED` *iff* all children are executed without exception. - In any other case the State :py:attr:`~State.FAILED` will be returned. + Returns a tuple containing the final state of execution (SUCCEEDED, FAILED) and a list of results from each + child's perform() method. The state is :py:attr:`~State.SUCCEEDED` *iff* all children are executed without + exception. In any other case the State :py:attr:`~State.FAILED` will be returned. """ - def perform(self) -> State: + def perform(self) -> Tuple[State, List[Any]]: """ Behaviour of Sequential, calls perform() on each child sequentially - :return: The state according to the behaviour described in :func:`Sequential` + :return: The state and list of results according to the behaviour described in :func:`Sequential` """ + children_return_values = [None] * len(self.children) try: - for child in self.children: + for index, child in enumerate(self.children): if self.interrupted: if threading.get_ident() in self.block_list: self.block_list.remove(threading.get_ident()) - return + return State.FAILED, children_return_values self.root.executing_thread[child] = threading.get_ident() - child.resolve().perform() + ret_val = child.resolve().perform() + if isinstance(ret_val, tuple): + child_state, child_result = ret_val + children_return_values[index] = child_result + else: + children_return_values[index] = ret_val except PlanFailure as e: self.root.exceptions[self] = e - return State.FAILED - return State.SUCCEEDED + return State.FAILED, children_return_values + return State.SUCCEEDED, children_return_values def interrupt(self) -> None: """ @@ -343,33 +368,40 @@ class TryInOrder(Language): Instead, the exception is saved to a list of all exceptions thrown during execution and returned. Behaviour: - Returns the State :py:attr:`~State.SUCCEEDED` if one or more children are executed without + Returns a tuple containing the final state of execution (SUCCEEDED, FAILED) and a list of results from each + child's perform() method. The state is :py:attr:`~State.SUCCEEDED` if one or more children are executed without exception. In the case that all children could not be executed the State :py:attr:`~State.FAILED` will be returned. """ - def perform(self) -> State: + def perform(self) -> Tuple[State, List[Any]]: """ Behaviour of TryInOrder, calls perform() on each child sequentially and catches raised exceptions. - :return: The state according to the behaviour described in :func:`TryInOrder` + :return: The state and list of results according to the behaviour described in :func:`TryInOrder` """ failure_list = [] - for child in self.children: + children_return_values = [None] * len(self.children) + for index, child in enumerate(self.children): if self.interrupted: if threading.get_ident() in self.block_list: self.block_list.remove(threading.get_ident()) - return + return State.INTERRUPTED, children_return_values try: - child.resolve().perform() + ret_val = child.resolve().perform() + if isinstance(ret_val, tuple): + child_state, child_result = ret_val + children_return_values[index] = child_result + else: + children_return_values[index] = ret_val except PlanFailure as e: failure_list.append(e) if len(failure_list) > 0: self.root.exceptions[self] = failure_list if len(failure_list) == len(self.children): self.root.exceptions[self] = failure_list - return State.FAILED + return State.FAILED, children_return_values else: - return State.SUCCEEDED + return State.SUCCEEDED, children_return_values def interrupt(self) -> None: """ @@ -388,19 +420,27 @@ class Parallel(Language): exceptions during execution will be caught, saved to a list and returned upon end. Behaviour: - Returns the State :py:attr:`~State.SUCCEEDED` *iff* all children could be executed without an exception. In any - other case the State :py:attr:`~State.FAILED` will be returned. + Returns a tuple containing the final state of execution (SUCCEEDED, FAILED) and a list of results from + each child's perform() method. The state is :py:attr:`~State.SUCCEEDED` *iff* all children could be executed without + an exception. In any other case the State :py:attr:`~State.FAILED` will be returned. + """ - def perform(self) -> State: + def perform(self) -> Tuple[State, List[Any]]: """ Behaviour of Parallel, creates a new thread for each child and calls perform() of the child in the respective thread. - :return: The state according to the behaviour described in :func:`Parallel` + :return: The state and list of results according to the behaviour described in :func:`Parallel` + """ + results = [None] * len(self.children) + self.threads: List[threading.Thread] = [] + state = State.SUCCEEDED + results_lock = threading.Lock() - def lang_call(child_node): + def lang_call(child_node, index): + nonlocal state if ("DesignatorDescription" in [cls.__name__ for cls in child_node.__class__.__mro__] and self.__class__.__name__ not in self.do_not_use_giskard): if self not in giskard.par_threads.keys(): @@ -409,26 +449,39 @@ def lang_call(child_node): giskard.par_threads[self].append(threading.get_ident()) try: self.root.executing_thread[child] = threading.get_ident() - child_node.resolve().perform() + result = child_node.resolve().perform() + if isinstance(result, tuple): + child_state, child_result = result + with results_lock: + results[index] = child_result + else: + with results_lock: + results[index] = result except PlanFailure as e: + nonlocal state + with results_lock: + state = State.FAILED if self in self.root.exceptions.keys(): self.root.exceptions[self].append(e) else: self.root.exceptions[self] = [e] - for child in self.children: + for index, child in enumerate(self.children): if self.interrupted: + state = State.FAILED break - t = threading.Thread(target=lambda: lang_call(child)) + t = threading.Thread(target=lambda: lang_call(child, index)) t.start() self.threads.append(t) for thread in self.threads: thread.join() - if thread.ident in self.block_list: - self.block_list.remove(thread.ident) + with results_lock: + for thread in self.threads: + if thread.ident in self.block_list: + self.block_list.remove(thread.ident) if self in self.root.exceptions.keys() and len(self.root.exceptions[self]) != 0: - return State.FAILED - return State.SUCCEEDED + state = State.FAILED + return state, results def interrupt(self) -> None: """ @@ -448,20 +501,24 @@ class TryAll(Language): exceptions during execution will be caught, saved to a list and returned upon end. Behaviour: - Returns the State :py:attr:`~State.SUCCEEDED` if one or more children could be executed without raising an - exception. If all children fail the State :py:attr:`~State.FAILED` will be returned. + Returns a tuple containing the final state of execution (SUCCEEDED, FAILED) and a list of results from each + child's perform() method. The state is :py:attr:`~State.SUCCEEDED` if one or more children could be executed + without raising an exception. If all children fail the State :py:attr:`~State.FAILED` will be returned. """ - def perform(self) -> State: + def perform(self) -> Tuple[State, List[Any]]: """ Behaviour of TryAll, creates a new thread for each child and executes all children in their respective threads. - :return: The state according to the behaviour described in :func:`TryAll` + :return: The state and list of results according to the behaviour described in :func:`TryAll` """ + results = [None] * len(self.children) + results_lock = threading.Lock() + state = State.SUCCEEDED self.threads: List[threading.Thread] = [] failure_list = [] - def lang_call(child_node): + def lang_call(child_node, index): if ("DesignatorDescription" in [cls.__name__ for cls in child_node.__class__.__mro__] and self.__class__.__name__ not in self.do_not_use_giskard): if self not in giskard.par_threads.keys(): @@ -469,27 +526,37 @@ def lang_call(child_node): else: giskard.par_threads[self].append(threading.get_ident()) try: - child_node.resolve().perform() + result = child_node.resolve().perform() + if isinstance(result, tuple): + child_state, child_result = result + with results_lock: + results[index] = child_result + else: + with results_lock: + results[index] = result except PlanFailure as e: failure_list.append(e) if self in self.root.exceptions.keys(): self.root.exceptions[self].append(e) else: self.root.exceptions[self] = [e] - - for child in self.children: - t = threading.Thread(target=lambda: lang_call(child)) - t.start() + for index, child in enumerate(self.children): + if self.interrupted: + state = State.FAILED + break + t = threading.Thread(target=lambda: lang_call(child, index)) self.threads.append(t) + t.start() for thread in self.threads: thread.join() - if thread.ident in self.block_list: - self.block_list.remove(thread.ident) + with results_lock: + for thread in self.threads: + if thread.ident in self.block_list: + self.block_list.remove(thread.ident) if len(self.children) == len(failure_list): self.root.exceptions[self] = failure_list - return State.FAILED - else: - return State.SUCCEEDED + state = State.FAILED + return state, results def interrupt(self) -> None: """ @@ -529,9 +596,16 @@ def execute(self) -> Any: """ Execute the code with its arguments - :returns: Anything that the function associated with this object will return. + :returns: State.SUCCEEDED, and anything that the function associated with this object will return. """ - return self.function(**self.kwargs) + child_state = State.SUCCEEDED + ret_val = self.function(**self.kwargs) + if isinstance(ret_val, tuple): + child_state, child_result = ret_val + else: + child_result = ret_val + + return child_state, child_result def interrupt(self) -> None: raise NotImplementedError diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index e045d0148..9b5520ccb 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -1,14 +1,14 @@ import sys import logging +from .ros.data_types import Time, Duration +from .ros.logging import logerr + if 'world' in sys.modules: logging.warning("(publisher) Make sure that you are not loading this module from pycram.world.") -import rospy from tf import TransformerROS -from rospy import Duration -from geometry_msgs.msg import TransformStamped from .datastructures.pose import Pose, Transform from typing_extensions import List, Optional, Union, Iterable @@ -29,6 +29,7 @@ class LocalTransformer(TransformerROS): """ _instance = None + prospection_prefix: str = "prospection/" def __new__(cls, *args, **kwargs): if not cls._instance: @@ -48,7 +49,8 @@ def __init__(self): self._initialized = True def transform_to_object_frame(self, pose: Pose, - world_object: 'world_concepts.world_object.Object', link_name: str = None) -> Union[Pose, None]: + world_object: 'world_concepts.world_object.Object', link_name: str = None) -> Union[ + Pose, None]: """ Transforms the given pose to the coordinate frame of the given World object. If no link name is given the base frame of the Object is used, otherwise the link frame is used as target for the transformation. @@ -64,7 +66,16 @@ def transform_to_object_frame(self, pose: Pose, target_frame = world_object.tf_frame return self.transform_pose(pose, target_frame) - def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: + def update_transforms_for_objects(self, object_names: List[str]) -> None: + """ + Updates the transforms for objects affected by the transformation. The objects are identified by their names. + + :param object_names: List of object names for which the transforms should be updated + """ + objects = list(map(self.world.get_object_by_name, object_names)) + [obj.update_link_transforms() for obj in objects] + + def transform_pose(self, pose: Pose, target_frame: str) -> Optional[Pose]: """ Transforms a given pose to the target frame after updating the transforms for all objects in the current world. @@ -72,49 +83,80 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: :param target_frame: Name of the TF frame into which the Pose should be transformed :return: A transformed pose in the target frame """ - self.world.update_transforms_for_objects_in_current_world() + objects = list(map(self.get_object_name_for_frame, [pose.frame, target_frame])) + self.update_transforms_for_objects([obj for obj in objects if obj is not None]) + copy_pose = pose.copy() - copy_pose.header.stamp = rospy.Time(0) - if not self.canTransform(target_frame, pose.frame, rospy.Time(0)): - rospy.logerr( - f"Can not transform pose: \n {pose}\n to frame: {target_frame}.\n Maybe try calling 'update_transforms_for_object'") + copy_pose.header.stamp = Time(0) + if not self.canTransform(target_frame, pose.frame, Time(0)): + logerr( + f"Can not transform pose: \n {pose}\n to frame: {target_frame}." + f"\n Maybe try calling 'update_transforms_for_object'") return new_pose = super().transformPose(target_frame, copy_pose) copy_pose.pose = new_pose.pose copy_pose.header.frame_id = new_pose.header.frame_id - copy_pose.header.stamp = rospy.Time.now() + copy_pose.header.stamp = Time().now() return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) + def get_object_name_for_frame(self, frame: str) -> Optional[str]: + """ + Get the name of the object that is associated with the given frame. + + :param frame: The frame for which the object name should be returned + :return: The name of the object associated with the frame + """ + world = self.prospection_world if self.prospection_prefix in frame else self.world + if frame == "map": + return None + obj_name = [obj.name for obj in world.objects if frame == obj.tf_frame] + return obj_name[0] if len(obj_name) > 0 else self.get_object_name_for_link_frame(frame) + + def get_object_name_for_link_frame(self, link_frame: str) -> Optional[str]: + """ + Get the name of the object that is associated with the given link frame. + + :param link_frame: The frame of the link for which the object name should be returned + :return: The name of the object associated with the link frame + """ + world = self.prospection_world if self.prospection_prefix in link_frame else self.world + object_name = [obj.name for obj in world.objects for link in obj.links.values() + if link_frame in (link.name, link.tf_frame)] + return object_name[0] if len(object_name) > 0 else None + def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, - time: Optional[rospy.rostime.Time] = None) -> Transform: + time: Optional[Time] = None) -> Transform: """ Update the transforms for all world objects then Look up for the latest known transform that transforms a point from source frame to target frame. If no time is given the last common time between the two frames is used. + :param source_frame: The frame in which the point is currently represented + :param target_frame: The frame in which the point should be represented :param time: Time at which the transform should be looked up + :return: The transform from source_frame to target_frame """ - self.world.update_transforms_for_objects_in_current_world() + objects = list(map(self.get_object_name_for_frame, [source_frame, target_frame])) + self.update_transforms_for_objects([obj for obj in objects if obj is not None]) + tf_time = time if time else self.getLatestCommonTime(source_frame, target_frame) translation, rotation = self.lookupTransform(source_frame, target_frame, tf_time) return Transform(translation, rotation, source_frame, target_frame) - def update_transforms(self, transforms: Iterable[Transform], time: rospy.Time = None) -> None: + def update_transforms(self, transforms: Iterable[Transform], time: Time = None) -> None: """ Updates transforms by updating the time stamps of the header of each transform. If no time is given the current time is used. """ - time = time if time else rospy.Time.now() + time = time if time else Time().now() for transform in transforms: transform.header.stamp = time self.setTransform(transform) def get_all_frames(self) -> List[str]: """ - Returns all know coordinate frames as a list with human-readable entries. - - :return: A list of all know coordinate frames. + :return: A list of all known coordinate frames as a list with human-readable entries. """ frames = self.allFramesAsString().split("\n") frames.remove("") @@ -126,4 +168,3 @@ def transformPose(self, target_frame, ps) -> Pose: exists in the super class. """ return self.transform_pose(ps, target_frame) - diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py new file mode 100644 index 000000000..fb2b456ec --- /dev/null +++ b/src/pycram/object_descriptors/generic.py @@ -0,0 +1,189 @@ +from typing import Optional, Tuple + +from typing_extensions import List, Any, Union, Dict + +from geometry_msgs.msg import Point + +from ..datastructures.dataclasses import VisualShape, BoxVisualShape, Color +from ..datastructures.enums import JointType +from ..datastructures.pose import Pose +from ..description import JointDescription as AbstractJointDescription, LinkDescription as AbstractLinkDescription, \ + ObjectDescription as AbstractObjectDescription + + +class NamedBoxVisualShape(BoxVisualShape): + def __init__(self, name: str, color: Color, visual_frame_position: List[float], half_extents: List[float]): + super().__init__(color, visual_frame_position, half_extents) + self._name: str = name + + @property + def name(self) -> str: + return self._name + + +class LinkDescription(AbstractLinkDescription): + + def __init__(self, name: str, visual_frame_position: List[float], half_extents: List[float], + color: Color = Color()): + super().__init__(NamedBoxVisualShape(name, color, visual_frame_position, half_extents)) + + @property + def geometry(self) -> Union[VisualShape, None]: + return self.parsed_description + + @property + def origin(self) -> Pose: + return Pose(self.parsed_description.visual_frame_position) + + @property + def name(self) -> str: + return self.parsed_description.name + + @property + def color(self) -> Color: + return self.parsed_description.rgba_color + + +class JointDescription(AbstractJointDescription): + + @property + def parent(self) -> str: + raise NotImplementedError + + @property + def child(self) -> str: + raise NotImplementedError + + @property + def type(self) -> JointType: + return JointType.UNKNOWN + + @property + def axis(self) -> Point: + return Point(0, 0, 0) + + @property + def has_limits(self) -> bool: + return False + + @property + def lower_limit(self) -> Union[float, None]: + return 0 + + @property + def upper_limit(self) -> Union[float, None]: + return 0 + + @property + def parent_link_name(self) -> str: + raise NotImplementedError + + @property + def child_link_name(self) -> str: + raise NotImplementedError + + @property + def origin(self) -> Pose: + raise NotImplementedError + + @property + def name(self) -> str: + raise NotImplementedError + + +class ObjectDescription(AbstractObjectDescription): + """ + A generic description of an object in the environment. This description can be applied to any object. + The current use case involves perceiving objects using RoboKudo and spawning them with specified size and color. + """ + + class Link(AbstractObjectDescription.Link, LinkDescription): + ... + + class RootLink(AbstractObjectDescription.RootLink, Link): + ... + + class Joint(AbstractObjectDescription.Joint, JointDescription): + ... + + def __init__(self, *args, **kwargs): + self._links = [LinkDescription(*args, **kwargs)] + + def load_description(self, path: str) -> Any: + ... + + @classmethod + def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> str: + raise NotImplementedError + + @classmethod + def generate_from_description_file(cls, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> str: + raise NotImplementedError + + @classmethod + def generate_from_parameter_server(cls, name: str, save_path: str) -> str: + raise NotImplementedError + + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + return {} + + @property + def link_map(self) -> Dict[str, LinkDescription]: + return {self._links[0].name: self._links[0]} + + @property + def joint_map(self) -> Dict[str, JointDescription]: + return {} + + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + return {} + + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: + ... + + @property + def shape_data(self) -> List[float]: + return self._links[0].geometry.shape_data()['halfExtents'] + + @property + def color(self) -> Color: + return self._links[0].color + + @property + def links(self) -> List[LinkDescription]: + return self._links + + def get_link_by_name(self, link_name: str) -> LinkDescription: + if link_name == self._links[0].name: + return self._links[0] + + @property + def joints(self) -> List[JointDescription]: + return [] + + def get_joint_by_name(self, joint_name: str) -> JointDescription: + ... + + def get_root(self) -> str: + return self._links[0].name + + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: + raise NotImplementedError("Do Not Do This on generic objects as they have no chains") + + @staticmethod + def get_file_extension() -> str: + raise NotImplementedError("Do Not Do This on generic objects as they have no extensions") + + @property + def origin(self) -> Pose: + return self._links[0].origin + + @property + def name(self) -> str: + return self._links[0].name diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py new file mode 100644 index 000000000..36e895372 --- /dev/null +++ b/src/pycram/object_descriptors/mjcf.py @@ -0,0 +1,502 @@ +import os +import pathlib + +import numpy as np +from dm_control import mjcf +from geometry_msgs.msg import Point +from typing_extensions import Union, List, Optional, Dict, Tuple +from xml.etree import ElementTree as ET + +from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ + SphereVisualShape, MeshVisualShape +from ..datastructures.enums import JointType, MJCFGeomType, MJCFJointType +from ..datastructures.pose import Pose +from ..description import JointDescription as AbstractJointDescription, \ + LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription +from ..failures import MultiplePossibleTipLinks +from ..ros.ros_tools import get_parameter + +try: + from multiverse_parser import Configuration, Factory, InertiaSource, GeomBuilder + from multiverse_parser import (WorldBuilder, + GeomType, GeomProperty, + MeshProperty, + MaterialProperty) + from multiverse_parser import MjcfExporter + from pxr import Usd, UsdGeom +except ImportError: + # do not import this module if multiverse is not found + raise ImportError("Multiverse not found.") + + +class LinkDescription(AbstractLinkDescription): + """ + A class that represents a link description of an object. + """ + + def __init__(self, mjcf_description: mjcf.Element): + super().__init__(mjcf_description) + + @property + def geometry(self) -> Union[VisualShape, None]: + """ + :return: The geometry type of the collision element of this link. + """ + return self._get_visual_shape(self.parsed_description.find_all('geom')[0]) + + @staticmethod + def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: + """ + :param mjcf_geometry: The MJCFGeometry to get the visual shape for. + :return: The VisualShape of the given MJCFGeometry object. + """ + if mjcf_geometry.type == MJCFGeomType.BOX.value: + return BoxVisualShape(Color(), [0, 0, 0], mjcf_geometry.size) + if mjcf_geometry.type == MJCFGeomType.CYLINDER.value: + return CylinderVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0], mjcf_geometry.size[1] * 2) + if mjcf_geometry.type == MJCFGeomType.SPHERE.value: + return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0]) + if mjcf_geometry.type == MJCFGeomType.MESH.value: + return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.scale, mjcf_geometry.filename) + return None + + @property + def origin(self) -> Union[Pose, None]: + """ + :return: The origin of this link. + """ + return parse_pose_from_body_element(self.parsed_description) + + @property + def name(self) -> str: + return self.parsed_description.name + + +class JointDescription(AbstractJointDescription): + + mjcf_type_map = { + MJCFJointType.HINGE.value: JointType.REVOLUTE, + MJCFJointType.BALL.value: JointType.SPHERICAL, + MJCFJointType.SLIDE.value: JointType.PRISMATIC, + MJCFJointType.FREE.value: JointType.FLOATING + } + """ + A dictionary mapping the MJCF joint types to the PyCRAM joint types. + """ + + pycram_type_map = {pycram_type: mjcf_type for mjcf_type, pycram_type in mjcf_type_map.items()} + """ + A dictionary mapping the PyCRAM joint types to the MJCF joint types. + """ + + def __init__(self, mjcf_description: mjcf.Element, is_virtual: Optional[bool] = False): + super().__init__(mjcf_description, is_virtual=is_virtual) + + @property + def origin(self) -> Pose: + return parse_pose_from_body_element(self.parsed_description) + + @property + def name(self) -> str: + return self.parsed_description.name + + @property + def has_limits(self) -> bool: + return self.parsed_description.limited + + @property + def type(self) -> JointType: + """ + :return: The type of this joint. + """ + if hasattr(self.parsed_description, 'type'): + return self.mjcf_type_map[self.parsed_description.type] + else: + return self.mjcf_type_map[MJCFJointType.FREE.value] + + @property + def axis(self) -> Point: + """ + :return: The axis of this joint, for example the rotation axis for a revolute joint. + """ + return Point(*self.parsed_description.axis) + + @property + def lower_limit(self) -> Union[float, None]: + """ + :return: The lower limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.range[0] + else: + return None + + @property + def upper_limit(self) -> Union[float, None]: + """ + :return: The upper limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.range[1] + else: + return None + + @property + def parent(self) -> str: + """ + :return: The name of the parent link of this joint. + """ + return self._parent_link_element.parent.name + + @property + def child(self) -> str: + """ + :return: The name of the child link of this joint. + """ + return self._parent_link_element.name + + @property + def _parent_link_element(self) -> mjcf.Element: + return self.parsed_description.parent + + @property + def damping(self) -> float: + """ + :return: The damping of this joint. + """ + return self.parsed_description.damping + + @property + def friction(self) -> float: + raise NotImplementedError("Friction is not implemented for MJCF joints.") + + +class ObjectFactory(Factory): + """ + Create MJCF object descriptions from mesh files. + """ + def __init__(self, object_name: str, file_path: str, config: Configuration, texture_type: str = "png"): + super().__init__(file_path, config) + + self._world_builder = WorldBuilder(usd_file_path=self.tmp_usd_file_path) + + body_builder = self._world_builder.add_body(body_name=object_name) + + tmp_usd_mesh_file_path, tmp_origin_mesh_file_path = self.import_mesh( + mesh_file_path=file_path, merge_mesh=True) + mesh_stage = Usd.Stage.Open(tmp_usd_mesh_file_path) + for idx, mesh_prim in enumerate([prim for prim in mesh_stage.Traverse() if prim.IsA(UsdGeom.Mesh)]): + mesh_name = mesh_prim.GetName() + mesh_path = mesh_prim.GetPath() + mesh_property = MeshProperty.from_mesh_file_path(mesh_file_path=tmp_usd_mesh_file_path, + mesh_path=mesh_path) + # mesh_property._texture_coordinates = None # TODO: See if needed otherwise remove it. + geom_property = GeomProperty(geom_type=GeomType.MESH, + is_visible=False, + is_collidable=True) + geom_builder = body_builder.add_geom(geom_name=f"SM_{object_name}_mesh_{idx}", + geom_property=geom_property) + geom_builder.add_mesh(mesh_name=mesh_name, mesh_property=mesh_property) + + # Add texture if available + texture_file_path = file_path.replace(pathlib.Path(file_path).suffix, f".{texture_type}") + if pathlib.Path(texture_file_path).exists(): + self.add_material_with_texture(geom_builder=geom_builder, material_name=f"M_{object_name}_{idx}", + texture_file_path=texture_file_path) + + geom_builder.build() + + body_builder.compute_and_set_inertial(inertia_source=InertiaSource.FROM_COLLISION_MESH) + + @staticmethod + def add_material_with_texture(geom_builder: GeomBuilder, material_name: str, texture_file_path: str): + """ + Add a material with a texture to the geom builder. + + :param geom_builder: The geom builder to add the material to. + :param material_name: The name of the material. + :param texture_file_path: The path to the texture file. + """ + material_property = MaterialProperty(diffuse_color=texture_file_path, + opacity=None, + emissive_color=None, + specular_color=None) + geom_builder.add_material(material_name=material_name, + material_property=material_property) + + def export_to_mjcf(self, output_file_path: str): + """ + Export the object to a MJCF file. + + :param output_file_path: The path to the output file. + """ + exporter = MjcfExporter(self, output_file_path) + exporter.build() + exporter.export(keep_usd=False) + + +class ObjectDescription(AbstractObjectDescription): + """ + A class that represents an object description of an object. + """ + + COMPILER_TAG = 'compiler' + """ + The tag of the compiler element in the MJCF file. + """ + MESH_DIR_ATTR = 'meshdir' + TEXTURE_DIR_ATTR = 'texturedir' + """ + The attributes of the compiler element in the MJCF file. The meshdir attribute is the directory where the mesh files + are stored and the texturedir attribute is the directory where the texture files are stored.""" + + class Link(AbstractObjectDescription.Link, LinkDescription): + ... + + class RootLink(AbstractObjectDescription.RootLink, Link): + ... + + class Joint(AbstractObjectDescription.Joint, JointDescription): + ... + + def __init__(self): + super().__init__() + self._link_map = None + self._joint_map = None + self._child_map = None + self._parent_map = None + self._links = None + self._joints = None + self.virtual_joint_names = [] + + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + if self._child_map is None: + self._child_map = self._construct_child_map() + return self._child_map + + def _construct_child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + Construct the child map of the object. + """ + child_map = {} + for joint in self.joints: + if joint.parent not in child_map: + child_map[joint.parent] = [(joint.name, joint.child)] + else: + child_map[joint.parent].append((joint.name, joint.child)) + return child_map + + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + if self._parent_map is None: + self._parent_map = self._construct_parent_map() + return self._parent_map + + def _construct_parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + Construct the parent map of the object. + """ + child_map = self.child_map + parent_map = {} + for parent, children in child_map.items(): + for child in children: + parent_map[child[1]] = (child[0], parent) + return parent_map + + @property + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + if self._link_map is None: + self._link_map = {link.name: link for link in self.links} + return self._link_map + + @property + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + if self._joint_map is None: + self._joint_map = {joint.name: joint for joint in self.joints} + return self._joint_map + + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: + """ + Finds the child link and adds a joint to it in the object description. + for arguments documentation see :meth:`pycram.description.ObjectDescription.add_joint` + """ + + position: Optional[List[float]] = None + quaternion: Optional[List[float]] = None + lower_limit: float = 0.0 if lower_limit is None else lower_limit + upper_limit: float = 0.0 if upper_limit is None else upper_limit + limit = [lower_limit, upper_limit] + + if origin is not None: + position = origin.position_as_list() + quaternion = origin.orientation_as_list() + quaternion = [quaternion[1], quaternion[2], quaternion[3], quaternion[0]] + if axis is not None: + axis = [axis.x, axis.y, axis.z] + self.parsed_description.find(child).add('joint', name=name, type=JointDescription.pycram_type_map[joint_type], + axis=axis, pos=position, quat=quaternion, range=limit) + if is_virtual: + self.virtual_joint_names.append(name) + + def load_description(self, path) -> mjcf.RootElement: + return mjcf.from_file(path, model_dir=pathlib.Path(path).parent) + + def load_description_from_string(self, description_string: str) -> mjcf.RootElement: + return mjcf.from_xml_string(description_string) + + def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color(), + save_path: Optional[str] = None) -> None: + """ + Generate a mjcf xml file with the given .obj or .stl file as mesh. In addition, use the given rgba_color + to create a material tag in the xml. + + :param path: The path to the mesh file. + :param name: The name of the object. + :param color: The color of the object. + :param save_path: The path to save the generated xml file. + """ + factory = ObjectFactory(object_name=name, file_path=path, + config=Configuration(model_name=name, + fixed_base=False, + default_rgba=np.array(color.get_rgba()))) + factory.export_to_mjcf(output_file_path=save_path) + + def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: + model_str = self.replace_relative_paths_with_absolute_paths(path) + self.write_description_to_file(model_str, save_path) + + def replace_relative_paths_with_absolute_paths(self, model_path: str) -> str: + """ + Replace the relative paths in the xml file to be absolute paths. + + :param model_path: The path to the xml file. + """ + tree = ET.parse(model_path) + root = tree.getroot() + compiler = root.find(self.COMPILER_TAG) + model_dir = pathlib.Path(model_path).parent + for rel_dir_attrib in [self.MESH_DIR_ATTR, self.TEXTURE_DIR_ATTR]: + rel_dir = compiler.get(rel_dir_attrib) + abs_dir = str(pathlib.Path(os.path.join(model_dir, rel_dir)).resolve()) + compiler.set(rel_dir_attrib, abs_dir) + return ET.tostring(root, encoding='unicode', method='xml') + + def generate_from_parameter_server(self, name: str, save_path: str) -> None: + mjcf_string = get_parameter(name) + self.write_description_to_file(mjcf_string, save_path) + + @property + def joints(self) -> List[JointDescription]: + """ + :return: A list of joints descriptions of this object. + """ + if self._joints is None: + self._joints = [JointDescription(joint) for joint in self.parsed_description.find_all('joint')] + return self._joints + + @property + def links(self) -> List[LinkDescription]: + """ + :return: A list of link descriptions of this object. + """ + if self._links is None: + self._links = [LinkDescription(link) for link in self.parsed_description.find_all('body')] + return self._links + + def get_root(self) -> str: + """ + :return: the name of the root link of this object. + """ + if len(self.links) == 1: + return self.links[0].name + elif len(self.links) > 1: + return self.links[1].name + else: + raise ValueError("No links found in the object description.") + + def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + :raises MultiplePossibleTipLinks: If there are multiple possible tip links. + """ + link = self.get_root() + while link in self.child_map: + children = self.child_map[link] + if len(children) > 1: + # Multiple children, can't decide which one to take (e.g. fingers of a hand) + raise MultiplePossibleTipLinks(self.name, link, [child[1] for child in children]) + else: + child = children[0][1] + link = child + return link + + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: + """ + :param start_link_name: The name of the start link of the chain. + :param end_link_name: The name of the end link of the chain. + :param joints: Whether to include joints in the chain. + :param links: Whether to include links in the chain. + :param fixed: Whether to include fixed joints in the chain (Note: not used in MJCF). + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + chain = [] + if links: + chain.append(end_link_name) + link = end_link_name + while link != start_link_name: + (joint, parent) = self.parent_map[link] + if joints: + chain.append(joint) + if links: + chain.append(parent) + link = parent + chain.reverse() + return chain + + @staticmethod + def get_file_extension() -> str: + """ + :return: The file extension of the URDF file. + """ + return '.xml' + + @property + def origin(self) -> Pose: + return parse_pose_from_body_element(self.parsed_description) + + @property + def name(self) -> str: + return self.parsed_description.name + + +def parse_pose_from_body_element(body: mjcf.Element) -> Pose: + """ + Parse the pose from a body element. + + :param body: The body element. + :return: The pose of the body. + """ + position = body.pos + quaternion = body.quat + position = [0, 0, 0] if position is None else position + quaternion = [1, 0, 0, 0] if quaternion is None else quaternion + quaternion = [quaternion[1], quaternion[2], quaternion[3], quaternion[0]] + return Pose(position, quaternion) diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 75ab98a03..694d421be 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -1,11 +1,14 @@ +import os import pathlib -from xml.etree import ElementTree +import xml.etree.ElementTree as ET -import rospkg -import rospy +import numpy as np + +from ..ros.logging import logerr +from ..ros.ros_tools import create_ros_pack, ResourceNotFound, get_parameter from geometry_msgs.msg import Point -from tf.transformations import quaternion_from_euler -from typing_extensions import Union, List, Optional +from tf.transformations import quaternion_from_euler, euler_from_quaternion +from typing_extensions import Union, List, Optional, Dict, Tuple from urdf_parser_py import urdf from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) @@ -16,6 +19,7 @@ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ SphereVisualShape, MeshVisualShape +from ..failures import MultiplePossibleTipLinks from ..utils import suppress_stdout_stderr @@ -30,7 +34,7 @@ def __init__(self, urdf_description: urdf.Link): @property def geometry(self) -> Union[VisualShape, None]: """ - Returns the geometry type of the URDF collision element of this link. + :return: The geometry type of the URDF collision element of this link. """ if self.collision is None: return None @@ -40,10 +44,12 @@ def geometry(self) -> Union[VisualShape, None]: @staticmethod def _get_visual_shape(urdf_geometry) -> Union[VisualShape, None]: """ - Returns the VisualShape of the given URDF geometry. + :param urdf_geometry: The URDFGeometry for which the visual shape is returned. + :return: the VisualShape of the given URDF geometry. """ if isinstance(urdf_geometry, URDF_Box): - return BoxVisualShape(Color(), [0, 0, 0], urdf_geometry.size) + half_extents = np.array(urdf_geometry.size) / 2 + return BoxVisualShape(Color(), [0, 0, 0], half_extents.tolist()) if isinstance(urdf_geometry, URDF_Cylinder): return CylinderVisualShape(Color(), [0, 0, 0], urdf_geometry.radius, urdf_geometry.length) if isinstance(urdf_geometry, URDF_Sphere): @@ -79,8 +85,10 @@ class JointDescription(AbstractJointDescription): 'planar': JointType.PLANAR, 'fixed': JointType.FIXED} - def __init__(self, urdf_description: urdf.Joint): - super().__init__(urdf_description) + pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} + + def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False): + super().__init__(urdf_description, is_virtual=is_virtual) @property def origin(self) -> Pose: @@ -130,14 +138,14 @@ def upper_limit(self) -> Union[float, None]: return None @property - def parent_link_name(self) -> str: + def parent(self) -> str: """ :return: The name of the parent link of this joint. """ return self.parsed_description.parent @property - def child_link_name(self) -> str: + def child(self) -> str: """ :return: The name of the child link of this joint. """ @@ -172,21 +180,83 @@ class RootLink(AbstractObjectDescription.RootLink, Link): class Joint(AbstractObjectDescription.Joint, JointDescription): ... + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + return self.parsed_description.child_map + + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + return self.parsed_description.parent_map + + @property + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + if self._link_map is None: + self._link_map = {link.name: link for link in self.links} + return self._link_map + + @property + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + if self._joint_map is None: + self._joint_map = {joint.name: joint for joint in self.joints} + return self._joint_map + + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: + """ + Add a joint to the object description, could be a virtual joint as well. + For documentation of the parameters, see :meth:`pycram.description.ObjectDescription.add_joint`. + """ + if lower_limit is not None or upper_limit is not None: + limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) + else: + limit = None + if origin is not None: + origin = urdf.Pose(origin.position_as_list(), euler_from_quaternion(origin.orientation_as_list())) + if axis is not None: + axis = [axis.x, axis.y, axis.z] + if parent is None: + parent = self.get_root() + else: + parent = self.get_link_by_name(parent).parsed_description + joint = urdf.Joint(name, + parent, + self.get_link_by_name(child).parsed_description, + JointDescription.pycram_type_map[joint_type], + axis, origin, limit) + self.parsed_description.add_joint(joint) + if is_virtual: + self.virtual_joint_names.append(name) + def load_description(self, path) -> URDF: with open(path, 'r') as file: # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them with suppress_stdout_stderr(): return URDF.from_xml_string(file.read()) - def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: + def generate_from_mesh_file(self, path: str, name: str, save_path: str, color: Optional[Color] = Color()) -> None: """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be - used to create a material tag in the URDF. + Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a + material tag in the URDF. The URDF file will be saved to the given save_path. :param path: The path to the mesh file. :param name: The name of the object. + :param save_path: The path to save the URDF file to. :param color: The color of the object. - :return: The absolute path of the created file """ urdf_template = ' \n \ \n \ @@ -211,55 +281,45 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - return content + self.write_description_to_file(content, save_path) - def generate_from_description_file(self, path: str) -> str: + def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: with open(path, mode="r") as f: urdf_string = self.fix_missing_inertial(f.read()) - urdf_string = self.remove_error_tags(urdf_string) - urdf_string = self.fix_link_attributes(urdf_string) - try: - urdf_string = self.correct_urdf_string(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - return urdf_string - - def generate_from_parameter_server(self, name: str) -> str: - urdf_string = rospy.get_param(name) - return self.correct_urdf_string(urdf_string) - - def get_link_by_name(self, link_name: str) -> LinkDescription: - """ - :return: The link description with the given name. - """ - for link in self.links: - if link.name == link_name: - return link - raise ValueError(f"Link with name {link_name} not found") + urdf_string = self.remove_error_tags(urdf_string) + urdf_string = self.fix_link_attributes(urdf_string) + try: + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) + except ResourceNotFound as e: + logerr(f"Could not find resource package linked in this URDF") + raise e + urdf_string = self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string + self.write_description_to_file(urdf_string, save_path) + + def generate_from_parameter_server(self, name: str, save_path: str) -> None: + urdf_string = get_parameter(name) + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) + self.write_description_to_file(urdf_string, save_path) @property - def links(self) -> List[LinkDescription]: - """ - :return: A list of links descriptions of this object. - """ - return [LinkDescription(link) for link in self.parsed_description.links] - - def get_joint_by_name(self, joint_name: str) -> JointDescription: + def joints(self) -> List[JointDescription]: """ - :return: The joint description with the given name. + :return: A list of joints descriptions of this object. """ - for joint in self.joints: - if joint.name == joint_name: - return joint - raise ValueError(f"Joint with name {joint_name} not found") + if self._joints is None: + self._joints = [JointDescription(joint) for joint in self.parsed_description.joints] + return self._joints @property - def joints(self) -> List[JointDescription]: + def links(self) -> List[LinkDescription]: """ - :return: A list of joints descriptions of this object. + :return: A list of link descriptions of this object. """ - return [JointDescription(joint) for joint in self.parsed_description.joints] + if self._links is None: + self._links = [LinkDescription(link) for link in self.parsed_description.links] + return self._links def get_root(self) -> str: """ @@ -267,21 +327,44 @@ def get_root(self) -> str: """ return self.parsed_description.get_root() - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: - """ + def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + :raises MultiplePossibleTipLinks: If there are multiple possible tip links. + """ + link = self.get_root() + while link in self.parsed_description.child_map: + children = self.parsed_description.child_map[link] + if len(children) > 1: + # Multiple children, can't decide which one to take (e.g. fingers of a hand) + raise MultiplePossibleTipLinks(self.parsed_description.name, link, [child[1] for child in children]) + else: + child = children[0][1] + link = child + return link + + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: + """ + :param start_link_name: The name of the start link of the chain. + :param end_link_name: The name of the end link of the chain. + :param joints: Whether to include joints in the chain. + :param links: Whether to include links in the chain. + :param fixed: Whether to include fixed joints in the chain. :return: the chain of links from 'start_link_name' to 'end_link_name'. """ - return self.parsed_description.get_chain(start_link_name, end_link_name) + return self.parsed_description.get_chain(start_link_name, end_link_name, joints, links, fixed) - def correct_urdf_string(self, urdf_string: str) -> str: + @staticmethod + def replace_relative_references_with_absolute_paths(urdf_string: str) -> str: """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) - can't deal with ROS package paths. + Change paths for files in the URDF from ROS paths and file dir references to paths in the file system. Since + World (PyBullet legacy) can't deal with ROS package paths. :param urdf_string: The name of the URDf on the parameter server :return: The URDF string with paths in the filesystem instead of ROS packages """ - r = rospkg.RosPack() + r = create_ros_pack() new_urdf_string = "" for line in urdf_string.split('\n'): if "package://" in line: @@ -289,9 +372,37 @@ def correct_urdf_string(self, urdf_string: str) -> str: s1 = s[1].split('/') path = r.get_path(s1[0]) line = line.replace("package://" + s1[0], path) + if 'file://' in line: + line = line.replace("file://", './') new_urdf_string += line + '\n' - return self.fix_missing_inertial(new_urdf_string) + return new_urdf_string + + @staticmethod + def make_mesh_paths_absolute(urdf_string: str, urdf_file_path: str) -> str: + """ + Convert all relative mesh paths in the URDF to absolute paths. + + :param urdf_string: The URDF description as string + :param urdf_file_path: The path to the URDF file + :returns: The new URDF description as string. + """ + # Parse the URDF file + root = ET.fromstring(urdf_string) + + # Iterate through all mesh tags + for mesh in root.findall('.//mesh'): + filename = mesh.attrib.get('filename', '') + if filename: + # If the filename is a relative path, convert it to an absolute path + if not os.path.isabs(filename): + # Deduce the base path from the relative path + base_path = os.path.dirname( + os.path.abspath(os.path.join(os.path.dirname(urdf_file_path), filename))) + abs_path = os.path.abspath(os.path.join(base_path, os.path.basename(filename))) + mesh.set('filename', abs_path) + + return ET.tostring(root, encoding='unicode') @staticmethod def fix_missing_inertial(urdf_string: str) -> str: @@ -303,10 +414,10 @@ def fix_missing_inertial(urdf_string: str) -> str: :returns: The new, corrected URDF description as string. """ - inertia_tree = ElementTree.ElementTree(ElementTree.Element("inertial")) - inertia_tree.getroot().append(ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(ElementTree.Element("inertia", {"ixx": "0.01", + inertia_tree = ET.ElementTree(ET.Element("inertial")) + inertia_tree.getroot().append(ET.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(ET.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(ET.Element("inertia", {"ixx": "0.01", "ixy": "0", "ixz": "0", "iyy": "0.01", @@ -314,48 +425,48 @@ def fix_missing_inertial(urdf_string: str) -> str: "izz": "0.01"})) # create tree from string - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) for link_element in tree.iter("link"): inertial = [*link_element.iter("inertial")] if len(inertial) == 0: link_element.append(inertia_tree.getroot()) - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def remove_error_tags(urdf_string: str) -> str: """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + Remove all tags in the removing_tags list from the URDF since these tags are known to cause errors with the URDF_parser :param urdf_string: String of the URDF from which the tags should be removed :return: The URDF string with the tags removed """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) removing_tags = ["gazebo", "transmission"] for tag_name in removing_tags: all_tags = tree.findall(tag_name) for tag in all_tags: tree.getroot().remove(tag) - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def fix_link_attributes(urdf_string: str) -> str: """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. + Remove the attribute 'type' from links since this is not parsable by the URDF parser. :param urdf_string: The string of the URDF from which the attributes should be removed :return: The URDF string with the attributes removed """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) for link in tree.iter("link"): if "type" in link.attrib.keys(): del link.attrib["type"] - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def get_file_extension() -> str: diff --git a/src/pycram/ontology/ontology.py b/src/pycram/ontology/ontology.py index 3b5ac3350..f768d9315 100644 --- a/src/pycram/ontology/ontology.py +++ b/src/pycram/ontology/ontology.py @@ -3,19 +3,27 @@ import inspect import itertools import logging -from pathlib import Path -from typing import Callable, Dict, List, Optional, Type +import os.path +import sqlite3 -import rospy +from pathlib import Path +from typing import Callable, Dict, List, Optional, Type, Tuple, Union -from owlready2 import (Namespace, Ontology, World as OntologyWorld, onto_path, Thing, get_namespace, Property, - ObjectProperty, destroy_entity, types) +from owlready2 import (Namespace, Ontology, World as OntologyWorld, Thing, EntityClass, Imp, + Property, ObjectProperty, OwlReadyError, types, + onto_path, default_world, get_namespace, get_ontology, destroy_entity, + sync_reasoner_pellet, sync_reasoner_hermit, + OwlReadyOntologyParsingError) +from owlready2.class_construct import GeneralClassAxiom from ..datastructures.enums import ObjectType from ..helper import Singleton from ..designator import DesignatorDescription, ObjectDesignatorDescription -from ..ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder +from ..ontology.ontology_common import (OntologyConceptHolderStore, OntologyConceptHolder, + ONTOLOGY_SQL_BACKEND_FILE_EXTENSION, + ONTOLOGY_SQL_IN_MEMORY_BACKEND) +from ..ros.logging import loginfo, logerr, logwarn SOMA_HOME_ONTOLOGY_IRI = "http://www.ease-crc.org/ont/SOMA-HOME.owl" SOMA_ONTOLOGY_IRI = "http://www.ease-crc.org/ont/SOMA.owl" @@ -28,12 +36,16 @@ class OntologyManager(object, metaclass=Singleton): Singleton class as the adapter accessing data of an OWL ontology, largely based on owlready2. """ - def __init__(self, main_ontology_iri: Optional[str] = None, ontology_search_path: Optional[str] = None): + def __init__(self, main_ontology_iri: Optional[str] = None, main_sql_backend_filename: Optional[str] = None, + ontology_search_path: Optional[str] = None, + use_global_default_world: bool = True): """ Create the singleton object of OntologyManager class :param main_ontology_iri: Ontology IRI (Internationalized Resource Identifier), either a URL to a remote OWL file or the full name path of a local one + :param main_sql_backend_filename: a full file path (no need to already exist) being used as SQL backend for the ontology world. If None, in-memory is used instead :param ontology_search_path: directory path from which a possibly existing ontology is searched. This is appended to `owlready2.onto_path`, a global variable containing a list of directories for searching local copies of ontologies (similarly to python `sys.path` for modules/packages). If not specified, the path is "$HOME/ontologies" + :param use_global_default_world: whether or not using the owlready2-provided global default persistent world """ if not ontology_search_path: ontology_search_path = f"{Path.home()}/ontologies" @@ -43,37 +55,36 @@ def __init__(self, main_ontology_iri: Optional[str] = None, ontology_search_path #: A dictionary of OWL ontologies, keyed by ontology name (same as its namespace name), eg. 'SOMA' self.ontologies: Dict[str, Ontology] = {} - #: The main ontology instance as the result of an ontology loading operation + #: The main ontology instance created by Ontology Manager at initialization as the result of loading from `main_ontology_iri` self.main_ontology: Optional[Ontology] = None - #: The SOMA ontology instance, referencing :attr:`ontology` in case of ontology loading from `SOMA.owl`. + #: The SOMA ontology instance, referencing :attr:`main_ontology` in case of ontology loading from `SOMA.owl`. # Ref: http://www.ease-crc.org/ont/SOMA.owl self.soma: Optional[Ontology] = None - #: The DUL ontology instance, referencing :attr:`ontology` in case of ontology loading from `DUL.owl`. + #: The DUL ontology instance, referencing :attr:`main_ontology` in case of ontology loading from `DUL.owl`. # Ref: http://www.ease-crc.org/ont/DUL.owl self.dul: Optional[Ontology] = None - #: Ontology world, the placeholder of triples stored by owlready2. + #: The main ontology world, the placeholder of triples created in :attr:`main_ontology`. # Ref: https://owlready2.readthedocs.io/en/latest/world.html - self.ontology_world: Optional[OntologyWorld] = None + self.main_ontology_world: Optional[OntologyWorld] = None - # Ontology IRI (Internationalized Resource Identifier), either a URL to a remote OWL file or the full - # name path of a local one + #: Ontology IRI (Internationalized Resource Identifier), either a URL to a remote OWL file or the full name path of a local one + # Ref: https://owlready2.readthedocs.io/en/latest/onto.html self.main_ontology_iri: str = main_ontology_iri if main_ontology_iri else SOMA_HOME_ONTOLOGY_IRI #: Namespace of the main ontology self.main_ontology_namespace: Optional[Namespace] = None - # Create an ontology world with parallelized file parsing enabled - self.ontology_world = OntologyWorld( - filename=f"{ontology_search_path}/{Path(self.main_ontology_iri).stem}.sqlite3", - exclusive=False, enable_thread_parallelism=True) + #: SQL backend for :attr:`main_ontology_world`, being either "memory" or a full file path (no need to already exist) + self.main_ontology_sql_backend = main_sql_backend_filename if main_sql_backend_filename else ONTOLOGY_SQL_IN_MEMORY_BACKEND - self.main_ontology, self.main_ontology_namespace = self.load_ontology(self.main_ontology_iri) - if self.main_ontology.loaded: - self.soma = self.ontologies.get(SOMA_ONTOLOGY_NAMESPACE) - self.dul = self.ontologies.get(DUL_ONTOLOGY_NAMESPACE) + # Create the main ontology world holding triples + self.create_main_ontology_world(use_global_default_world=use_global_default_world) + + # Create the main ontology & its namespace, fetching :attr:`soma`, :attr:`dul` if loading from SOMA ontology + self.create_main_ontology() @staticmethod def print_ontology_class(ontology_class: Type[Thing]): @@ -84,38 +95,199 @@ def print_ontology_class(ontology_class: Type[Thing]): """ if ontology_class is None: return - rospy.loginfo("-------------------") - rospy.loginfo(f"{ontology_class} {type(ontology_class)}") - rospy.loginfo(f"Super classes: {ontology_class.is_a}") - rospy.loginfo(f"Ancestors: {ontology_class.ancestors()}") - rospy.loginfo(f"Subclasses: {list(ontology_class.subclasses())}") - rospy.loginfo(f"Properties: {list(ontology_class.get_class_properties())}") - rospy.loginfo(f"Instances: {list(ontology_class.instances())}") - rospy.loginfo(f"Direct Instances: {list(ontology_class.direct_instances())}") - rospy.loginfo(f"Inverse Restrictions: {list(ontology_class.inverse_restrictions())}") - - def load_ontology(self, ontology_iri: str) -> tuple[Ontology, Namespace]: + loginfo(f"{ontology_class} {type(ontology_class)}") + loginfo(f"Defined class: {ontology_class.get_defined_class()}") + loginfo(f"Super classes: {ontology_class.is_a}") + loginfo(f"Equivalent to: {EntityClass.get_equivalent_to(ontology_class)}") + loginfo(f"Indirectly equivalent to: {ontology_class.get_indirect_equivalent_to()}") + loginfo(f"Ancestors: {list(ontology_class.ancestors())}") + loginfo(f"Subclasses: {list(ontology_class.subclasses())}") + loginfo(f"Disjoint unions: {ontology_class.get_disjoint_unions()}") + loginfo(f"Properties: {list(ontology_class.get_class_properties())}") + loginfo(f"Indirect Properties: {list(ontology_class.INDIRECT_get_class_properties())}") + loginfo(f"Instances: {list(ontology_class.instances())}") + loginfo(f"Direct Instances: {list(ontology_class.direct_instances())}") + loginfo(f"Inverse Restrictions: {list(ontology_class.inverse_restrictions())}") + loginfo("-------------------") + + @staticmethod + def print_ontology_property(ontology_property: Property): + """ + Print information (subjects, objects, relations, etc.) of an ontology property + + :param ontology_property: An ontology property + """ + if ontology_property is None: + return + property_class = type(ontology_property) + loginfo(f"{ontology_property} {property_class}") + loginfo(f"Relations: {list(ontology_property.get_relations())}") + loginfo(f"Domain: {ontology_property.get_domain()}") + loginfo(f"Range: {ontology_property.get_range()}") + if hasattr(property_class, "_equivalent_to"): + loginfo(f"Equivalent classes: {EntityClass.get_equivalent_to(property_class)}") + if hasattr(property_class, "_indirect"): + loginfo(f"Indirectly equivalent classes: {EntityClass.get_indirect_equivalent_to(property_class)}") + loginfo(f"Property chain: {ontology_property.get_property_chain()}") + loginfo(f"Class property type: {ontology_property.get_class_property_type()}") + loginfo("-------------------") + + @staticmethod + def get_default_ontology_search_path() -> Optional[str]: + """ + Get the first ontology search path from owlready2.onto_path + + :return: the path to the ontology search path if existing, otherwise None + """ + if onto_path: + return onto_path[0] + else: + logerr("No ontology search path has been configured!") + return None + + def get_main_ontology_dir(self) -> Optional[str]: + """ + Get path to the directory of :attr:`main_ontology_iri` if it is a local absolute path, + otherwise path to the default ontology search directory + + :return: the path to the directory of the main ontology IRI + """ + return os.path.dirname(self.main_ontology_iri) if os.path.isabs( + self.main_ontology_iri) else self.get_default_ontology_search_path() + + def is_main_ontology_sql_backend_in_memory(self) -> bool: + """ + Whether the main ontology's SQL backend is in-memory + + :return: true if the main ontology's SQL backend is in-memory + """ + return self.main_ontology_sql_backend == ONTOLOGY_SQL_IN_MEMORY_BACKEND + + def create_main_ontology_world(self, use_global_default_world: bool = True) -> None: + """ + Create the main ontology world, either reusing the owlready2-provided global default ontology world or create a new one + A backend sqlite3 file of same name with `main_ontology` is also created at the same folder with :attr:`main_ontology_iri` + (if it is a local absolute path). The file is automatically registered as cache for the main ontology world. + + :param use_global_default_world: whether or not using the owlready2-provided global default persistent world + :param sql_backend_filename: a full file path (no need to already exist) being used as SQL backend for the ontology world. If None, memory is used instead + """ + self.main_ontology_world = self.create_ontology_world( + sql_backend_filename=self.main_ontology_sql_backend, + use_global_default_world=use_global_default_world) + + @staticmethod + def create_ontology_world(use_global_default_world: bool = False, + sql_backend_filename: Optional[str] = None) -> OntologyWorld: + """ + Either reuse the owlready2-provided global default ontology world or create a new one. + + :param use_global_default_world: whether or not using the owlready2-provided global default persistent world + :param sql_backend_filename: an absolute file path (no need to already exist) being used as SQL backend for the ontology world. If it is None or non-absolute path, in-memory is used instead + :return: owlready2-provided global default ontology world or a newly created ontology world + """ + world = default_world + sql_backend_path_absolute = (sql_backend_filename and os.path.isabs(sql_backend_filename)) + if sql_backend_filename and (sql_backend_filename != ONTOLOGY_SQL_IN_MEMORY_BACKEND): + if not sql_backend_path_absolute: + logerr(f"For ontology world accessing, either f{ONTOLOGY_SQL_IN_MEMORY_BACKEND}" + f"or an absolute path to its SQL file backend is expected: {sql_backend_filename}") + return default_world + elif not sql_backend_filename.endswith(ONTOLOGY_SQL_BACKEND_FILE_EXTENSION): + logerr( + f"Ontology world SQL backend file path, {sql_backend_filename}," + f"is expected to be of extension {ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}!") + return default_world + + sql_backend_path_valid = sql_backend_path_absolute + sql_backend_name = sql_backend_filename if sql_backend_path_valid else ONTOLOGY_SQL_IN_MEMORY_BACKEND + try: + if use_global_default_world: + # Reuse default world + if sql_backend_path_valid: + world.set_backend(filename=sql_backend_filename, exclusive=False, enable_thread_parallelism=True) + else: + world.set_backend(exclusive=False, enable_thread_parallelism=True) + loginfo(f"Using global default ontology world with SQL backend: {sql_backend_name}") + else: + # Create a new world with parallelized file parsing enabled + if sql_backend_path_valid: + world = OntologyWorld(filename=sql_backend_filename, exclusive=False, enable_thread_parallelism=True) + else: + world = OntologyWorld(exclusive=False, enable_thread_parallelism=True) + loginfo(f"Created a new ontology world with SQL backend: {sql_backend_name}") + except sqlite3.Error as e: + logerr(f"Failed accessing the SQL backend of ontology world: {sql_backend_name}", + e.sqlite_errorcode, e.sqlite_errorname) + return world + + def create_main_ontology(self) -> bool: + """ + Load ontologies from :attr:`main_ontology_iri` to :attr:`main_ontology_world` + If `main_ontology_iri` is a remote URL, Owlready2 first searches for a local copy of the OWL file (from `onto_path`), + if not found, tries to download it from the Internet. + + :return: True if loading succeeds + """ + ontology_info = self.load_ontology(self.main_ontology_iri) + if ontology_info: + self.main_ontology, self.main_ontology_namespace = ontology_info + if self.main_ontology and self.main_ontology.loaded: + self.soma = self.ontologies.get(SOMA_ONTOLOGY_NAMESPACE) + self.dul = self.ontologies.get(DUL_ONTOLOGY_NAMESPACE) + return ontology_info is not None + + def load_ontology(self, ontology_iri: str) -> Optional[Tuple[Ontology, Namespace]]: """ Load an ontology from an IRI :param ontology_iri: An ontology IRI :return: A tuple including an ontology instance & its namespace """ - ontology = self.ontology_world.get_ontology(ontology_iri).load(reload_if_newer=True) + if not ontology_iri: + logerr("Ontology IRI is empty") + return None + + is_local_ontology_iri = not (ontology_iri.startswith("http:") or ontology_iri.startswith("https:")) + + # If `ontology_iri` is a local path + if is_local_ontology_iri and not Path(ontology_iri).exists(): + # -> Create an empty ontology file if not existing + ontology_path = ontology_iri if os.path.isabs(ontology_iri) else ( + os.path.join(self.get_main_ontology_dir(), ontology_iri)) + with open(ontology_path, 'w'): + pass + + # Load ontology from `ontology_iri` + ontology = None + try: + if self.main_ontology_world: + ontology = self.main_ontology_world.get_ontology(ontology_iri).load(reload_if_newer=True) + else: + ontology = get_ontology(ontology_iri).load(reload_if_newer=True) + except OwlReadyOntologyParsingError as error: + logwarn(error) + if is_local_ontology_iri: + logerr(f"Main ontology failed being loaded from {ontology_iri}") + else: + logwarn(f"Main ontology failed being downloaded from the remote {ontology_iri}") + return None + + # Browse loaded `ontology`, fetching sub-ontologies ontology_namespace = get_namespace(ontology_iri) - if ontology.loaded: - rospy.loginfo( + if ontology and ontology.loaded: + loginfo( f'Ontology [{ontology.base_iri}]\'s name: {ontology.name} has been loaded') - rospy.loginfo(f'- main namespace: {ontology_namespace.name}') - rospy.loginfo(f'- loaded ontologies:') + loginfo(f'- main namespace: {ontology_namespace.name}') + loginfo(f'- loaded ontologies:') def fetch_ontology(ontology__): self.ontologies[ontology__.name] = ontology__ - rospy.loginfo(ontology__.base_iri) + loginfo(ontology__.base_iri) self.browse_ontologies(ontology, condition=None, func=lambda ontology__: fetch_ontology(ontology__)) else: - rospy.logerr(f"Ontology [{ontology.base_iri}]\'s name: {ontology.name} failed being loaded") + logerr(f"Ontology [{ontology.base_iri}]\'s name: {ontology.name} failed being loaded") return ontology, ontology_namespace def initialized(self) -> bool: @@ -124,7 +296,7 @@ def initialized(self) -> bool: :return: True if loaded, otherwise False """ - return hasattr(self, "main_ontology") and self.main_ontology.loaded + return hasattr(self, "main_ontology") and self.main_ontology and self.main_ontology.loaded @staticmethod def browse_ontologies(ontology: Ontology, @@ -137,10 +309,10 @@ def browse_ontologies(ontology: Ontology, :param func: a Callable specifying the operations to perform on all the loaded ontologies if condition is None, otherwise only the first ontology which meets the condition """ if ontology is None: - rospy.logerr(f"Ontology {ontology=} is None!") + logerr(f"Ontology {ontology=} is None!") return elif not ontology.loaded: - rospy.logerr(f"Ontology {ontology} was not loaded!") + logerr(f"Ontology {ontology} was not loaded!") return will_do_func = func is not None @@ -159,71 +331,91 @@ def browse_ontologies(ontology: Ontology, func(sub_onto, **kwargs) break - def save(self, target_filename: str = "", overwrite: bool = False) -> bool: + def save(self, target_filename: Optional[str] = None, overwrite: bool = False) -> bool: """ - Save the current ontology to disk + Save :attr:`main_ontology` to a file on disk, also caching :attr:`main_ontology_world` to a sqlite3 file :param target_filename: full name path of a file which the ontologies are saved into. :param overwrite: overwrite an existing file if it exists. If empty, they are saved to the same original OWL file from which the main ontology was loaded, or a file at the same folder with ontology search path specified at constructor if it was loaded from a remote IRI. :return: True if the ontology was successfully saved, False otherwise """ - # Commit the whole graph data of the current ontology world, saving it into SQLite3, to be reused the next time - # the ontologies are loaded - self.ontology_world.save() - # Save ontologies to OWL - is_current_ontology_local = Path(self.main_ontology_iri).exists() + is_current_ontology_local = os.path.isfile(self.main_ontology_iri) current_ontology_filename = self.main_ontology_iri if is_current_ontology_local \ - else f"{Path(self.ontology_world.filename).parent.absolute()}/{Path(self.main_ontology_iri).stem}.owl" + else f"{self.get_main_ontology_dir()}/{Path(self.main_ontology_iri).name}" save_to_same_file = is_current_ontology_local and (target_filename == current_ontology_filename) if save_to_same_file and not overwrite: - rospy.logerr( + logerr( f"Ontologies cannot be saved to the originally loaded [{target_filename}] if not by overwriting") return False else: save_filename = target_filename if target_filename else current_ontology_filename self.main_ontology.save(save_filename) if save_to_same_file and overwrite: - rospy.logwarn(f"Ontologies have been overwritten to {save_filename}") + logwarn(f"Main ontology {self.main_ontology.name} has been overwritten to {save_filename}") else: - rospy.loginfo(f"Ontologies have been saved to {save_filename}") + loginfo(f"Main ontology {self.main_ontology.name} has been saved to {save_filename}") + + # Commit the whole graph data of the current ontology world, saving it into SQLite3, to be reused the next time + # the ontologies are loaded + main_ontology_sql_filename = self.main_ontology_world.filename + self.main_ontology_world.save() + if os.path.isfile(main_ontology_sql_filename): + loginfo( + f"Main ontology world for {self.main_ontology.name} has been cached and saved to SQL: {main_ontology_sql_filename}") + #else: it could be using memory cache as SQL backend return True def create_ontology_concept_class(self, class_name: str, - ontology_parent_concept_class: Optional[Thing] = None) \ - -> Type[Thing]: + ontology_parent_concept_class: Optional[Thing] = None, + ontology: Optional[Ontology] = None) \ + -> Optional[Type[Thing]]: """ - Create a new concept class in ontology + Create a new concept class in a given ontology :param class_name: A given name to the new class :param ontology_parent_concept_class: An optional parent ontology class of the new class + :param ontology: an owlready2.Ontology in which the concept class is created :return: The created ontology class """ - ontology_concept_class = self.get_ontology_class_by_ontology(self.main_ontology, class_name) + ontology = ontology if ontology else self.main_ontology + ontology_concept_class = self.get_ontology_class_by_ontology(ontology, class_name) if ontology_concept_class: return ontology_concept_class - with self.main_ontology: + if getattr(ontology, class_name, None): + logerr(f"Ontology concept class {ontology.name}.{class_name} already exists") + return None + + with ontology: return types.new_class(class_name, (Thing, ontology_parent_concept_class,) if inspect.isclass(ontology_parent_concept_class) else (Thing,)) - @staticmethod - def create_ontology_property_class(class_name: str, - ontology_parent_property_class: Optional[Type[Property]] = None) \ + def create_ontology_property_class(self, class_name: str, + ontology_parent_property_class: Optional[Type[Property]] = None, + ontology: Optional[Ontology] = None) \ -> Optional[Type[Property]]: """ - Create a new property class in ontology + Create a new property class in a given ontology :param class_name: A given name to the new class :param ontology_parent_property_class: An optional parent ontology property class of the new class + :param ontology: an owlready2.Ontology in which the concept class is created :return: The created ontology class """ + ontology = ontology if ontology else self.main_ontology parent_class = ontology_parent_property_class if (ontology_parent_property_class and issubclass(ontology_parent_property_class, Property)) \ else None - return types.new_class(class_name, (parent_class,) if parent_class else (Property,)) + + if getattr(ontology, class_name, None): + logerr(f"Ontology property class {ontology.name}.{class_name} already exists") + return None + + with ontology: + return types.new_class(class_name, (parent_class,) if parent_class else (Property,)) def get_ontology_classes_by_condition(self, condition: Callable, first_match_only=False, **kwargs) \ -> List[Type[Thing]]: @@ -249,19 +441,19 @@ def get_ontology_classes_by_condition(self, condition: Callable, first_match_onl return out_classes if not out_classes: - rospy.loginfo(f"No class with {kwargs} is found in the ontology {self.main_ontology}") + loginfo(f"No class with {kwargs} is found in the ontology {self.main_ontology}") return out_classes @staticmethod - def get_ontology_class_by_ontology(ontology: Ontology, class_name: str) -> Optional[ - Type[Thing]]: + def get_ontology_class_by_ontology(ontology: Ontology, class_name: str) -> Optional[Type[Thing]]: """ Get an ontology class if it exists in a given ontology :param ontology: an ontology instance + :param class_name: name of the searched-for ontology class :return: The ontology class if it exists under the namespace of the given ontology, None otherwise """ - return getattr(ontology, class_name) if ontology and hasattr(ontology, class_name) else None + return getattr(ontology, class_name, None) if ontology else None def get_ontology_class(self, class_name: str) -> Optional[Type[Thing]]: """ @@ -279,8 +471,7 @@ def is_matching_class_name(ontology_class: Type[Thing], ontology_class_name: str first_match_only=True) return found_classes[0] if len(found_classes) > 0 else None - def get_ontology_classes_by_namespace(self, ontology_namespace: str) -> List[ - Type[Thing]]: + def get_ontology_classes_by_namespace(self, ontology_namespace: str) -> List[Type[Thing]]: """ Get all ontologies classes by namespace @@ -320,53 +511,102 @@ def get_ontology_descendant_classes(self, ancestor_class: Type[Thing], class_sub if (class_subname.lower() in ontology_class.name.lower()) and (ancestor_class in ontology_class.ancestors())] + def get_ontology_general_class_axioms(self, ontology: Optional[Ontology] = None) -> List[GeneralClassAxiom]: + """ + Get general class axioms of an ontology + Ref: https://owlready2.readthedocs.io/en/latest/general_class_axioms.html + + :param ontology: an ontology instance + :return: A list of ontology axioms in the ontology + """ + ontology = ontology if ontology else self.main_ontology + return list(ontology.general_class_axioms()) + def create_ontology_triple_classes(self, subject_class_name: str, object_class_name: str, - predicate_name: str, inverse_predicate_name: str, + predicate_class_name: str, inverse_predicate_class_name: Optional[str] = None, + predicate_python_attribute_name: Optional[str] = None, + inverse_predicate_python_attribute_name: Optional[str] = None, ontology_subject_parent_class: Optional[Type[Thing]] = None, - ontology_object_parent_class: Optional[Type[Thing]] = None, - ontology_property_parent_class: Optional[Type[ - Property]] = ObjectProperty, - ontology_inverse_property_parent_class: Optional[Type[ - Property]] = ObjectProperty) -> None: + ontology_object_parent_class: Optional[Type[Union[Thing, object]]] = None, + ontology_property_parent_class: Type[Property] = ObjectProperty, + ontology_inverse_property_parent_class: Type[Property] = ObjectProperty, + ontology: Optional[Ontology] = None) -> bool: """ Dynamically create ontology triple classes under same namespace with the main ontology, as known as {subject, predicate, object}, with the relations among them :param subject_class_name: name of the subject class :param object_class_name: name of the object class - :param predicate_name: name of predicate class, also used as a Python attribute of the subject class to query object instances - :param inverse_predicate_name: name of inverse predicate + :param predicate_class_name: name of predicate class, also used as a Python attribute of the subject class to query object instances + :param predicate_python_attribute_name: python attribute name designated for the predicate instance + :param inverse_predicate_class_name: name of inverse predicate + :param inverse_predicate_python_attribute_name: python attribute name designated for the inverse predicate instance :param ontology_subject_parent_class: a parent class of the subject class :param ontology_object_parent_class: a parent class of the object class :param ontology_property_parent_class: a parent ontology property class, default: owlready2.ObjectProperty :param ontology_inverse_property_parent_class: a parent ontology inverse property class, default: owlready2.ObjectProperty + :param ontology: an owlready2.Ontology in which triples are created + :return: True if the ontology triple classes are created successfully """ + if not predicate_python_attribute_name: + predicate_python_attribute_name = predicate_class_name + if not inverse_predicate_python_attribute_name: + inverse_predicate_python_attribute_name = inverse_predicate_class_name + ontology = ontology if ontology else self.main_ontology + # This context manager ensures all classes created here-in share the same namepsace with `self.main_ontology` - with self.main_ontology: + with ontology: # Subject ontology_subject_class = self.create_ontology_concept_class(subject_class_name, - ontology_subject_parent_class) + ontology_subject_parent_class, + ontology=ontology) + if not ontology_subject_class: + logerr(f"{ontology.name}: Failed creating ontology subject class named {subject_class_name}") + return False # Object - ontology_object_class = self.create_ontology_concept_class(object_class_name, ontology_object_parent_class) + if not ontology_object_parent_class or issubclass(ontology_object_parent_class, Thing): + ontology_object_class = self.create_ontology_concept_class(object_class_name, + ontology_object_parent_class, + ontology=ontology) \ + if (object_class_name != subject_class_name) else ontology_subject_class + else: + ontology_object_class = ontology_object_parent_class + + if not ontology_object_class: + logerr(f"{ontology.name}: Failed creating ontology object class named {object_class_name}") + return False # Predicate - ontology_predicate_class = self.create_ontology_property_class("OntologyPredicate", - ontology_property_parent_class) + ontology_predicate_class = self.create_ontology_property_class(predicate_class_name, + ontology_property_parent_class, + ontology=ontology) + if not ontology_predicate_class: + logerr(f"{ontology.name}: Failed creating ontology predicate class named {predicate_class_name}") + return False ontology_predicate_class.domain = [ontology_subject_class] ontology_predicate_class.range = [ontology_object_class] - ontology_predicate_class.python_name = predicate_name + ontology_predicate_class.python_name = predicate_python_attribute_name # Inverse Predicate - ontology_inverse_predicate = self.create_ontology_property_class("OntologyInversePredicate", - ontology_inverse_property_parent_class) - ontology_inverse_predicate.inverse_property = ontology_predicate_class - ontology_inverse_predicate.python_name = inverse_predicate_name + if inverse_predicate_class_name: + ontology_inverse_predicate_class = self.create_ontology_property_class(inverse_predicate_class_name, + ontology_inverse_property_parent_class, + ontology=ontology) + if not ontology_inverse_predicate_class: + logerr( + f"{ontology.name}: Failed creating ontology inverse-predicate class named {inverse_predicate_class_name}") + return False + ontology_inverse_predicate_class.inverse_property = ontology_predicate_class + ontology_inverse_predicate_class.domain = [ontology_object_class] + ontology_inverse_predicate_class.range = [ontology_subject_class] + ontology_inverse_predicate_class.python_name = inverse_predicate_python_attribute_name + return True def create_ontology_linked_designator(self, designator_class: Type[DesignatorDescription], ontology_concept_name: str, - object_name: Optional[str] = "", + object_name: str, ontology_parent_class: Optional[Type[Thing]] = None) \ -> Optional[DesignatorDescription]: """ @@ -385,7 +625,7 @@ def create_ontology_linked_designator(self, designator_class: Type[DesignatorDes def create_ontology_linked_designator_by_concept(self, designator_class: Type[DesignatorDescription], ontology_concept_class: Type[Thing], - object_name: Optional[str] = "") \ + object_name: str) \ -> Optional[DesignatorDescription]: """ Create a designator that belongs to a given ontology concept class @@ -397,7 +637,7 @@ def create_ontology_linked_designator_by_concept(self, designator_class: Type[De """ ontology_concept_name = f'{object_name}_concept' if len(OntologyConceptHolderStore().get_designators_of_ontology_concept(ontology_concept_name)) > 0: - rospy.logerr( + logerr( f"A designator named [{object_name}] is already created for ontology concept [{ontology_concept_name}]") return None @@ -405,7 +645,7 @@ def create_ontology_linked_designator_by_concept(self, designator_class: Type[De is_object_designator = issubclass(designator_class, ObjectDesignatorDescription) if is_object_designator: if not object_name: - rospy.logerr( + logerr( f"An empty object name was given as creating its Object designator for ontology concept class [{ontology_concept_class.name}]") return None designator = designator_class(names=[object_name]) @@ -458,7 +698,7 @@ def set_ontology_relation(subject_designator: DesignatorDescription, object_concepts_list.append(holder.ontology_concept) return True else: - rospy.logerr(f"Ontology concept [{subject_concept.name}] has no predicate named [{predicate_name}]") + logerr(f"Ontology concept [{subject_concept.name}] has no predicate named [{predicate_name}]") return False @staticmethod @@ -474,8 +714,7 @@ def get_designators_by_subject_predicate(subject: DesignatorDescription, return list(itertools.chain( *[OntologyConceptHolderStore().get_designators_of_ontology_concept(object_concept.name) for subject_concept_holder in subject.ontology_concept_holders - for object_concept in getattr(subject_concept_holder.ontology_concept, predicate_name) - if hasattr(subject_concept_holder.ontology_concept, predicate_name)])) + for object_concept in getattr(subject_concept_holder.ontology_concept, predicate_name, [])])) def create_ontology_object_designator_from_type(self, object_type: ObjectType, ontology_concept_class: Type[Thing]) \ @@ -508,3 +747,101 @@ def destroy_ontology_class(ontology_class, destroy_instances: bool = True): destroy_entity(ontology_individual) OntologyConceptHolderStore().remove_ontology_concept(ontology_class.name) destroy_entity(ontology_class) + + def create_rule_reflexivity(self, ontology_concept_class_name: str, + predicate_name: str, + ontology: Optional[Ontology] = None) -> Imp: + """ + Create the rule of reflexivity for a given ontology concept class. + Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.ReflexiveProperty. + Ref: https://en.wikipedia.org/wiki/Reflexive_relation + + :param ontology_concept_class_name: Name of the ontology concept class having the relation defined + :param predicate_name: Name of the ontology predicate signifying the reflexive relation + :param ontology: The ontology for which the rule is created + :return: Rule of transitivity + """ + ontology = ontology if ontology else self.main_ontology + with ontology: + rule = Imp() + rule.set_as_rule(f"""{ontology_concept_class_name}(?a) + -> {predicate_name}(?a, ?a)""") + return rule + + def create_rule_symmetry(self, ontology_concept_class_name: str, + predicate_name: str, + ontology: Optional[Ontology] = None) -> Imp: + """ + Create the rule of transitivity for a given ontology concept class. + Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.SymmetricProperty. + Ref: https://en.wikipedia.org/wiki/Symmetric_relation + + :param ontology_concept_class_name: Name of the ontology concept class having the relation defined + :param predicate_name: Name of the ontology predicate signifying the symmetric relation + :param ontology: The ontology for which the rule is created + :return: Rule of symmetry + """ + ontology = ontology if ontology else self.main_ontology + with ontology: + rule = Imp() + rule.set_as_rule(f"""{ontology_concept_class_name}(?a), {ontology_concept_class_name}(?b), + {predicate_name}(?a, ?b) + -> {predicate_name}(?b, ?a)""") + return rule + + def create_rule_transitivity(self, ontology_concept_class_name: str, + predicate_name: str, + ontology: Optional[Ontology] = None) -> Imp: + """ + Create the rule of transitivity for a given ontology concept class. + Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.TransitiveProperty. + Ref: + - https://en.wikipedia.org/wiki/Transitive_relation + - https://owlready2.readthedocs.io/en/latest/properties.html#obtaining-indirect-relations-considering-subproperty-transitivity-etc + + :param ontology_concept_class_name: Name of the ontology concept class having the relation defined + :param predicate_name: Name of the ontology predicate signifying the transitive relation + :param ontology: The ontology for which the rule is created + :return: Rule of transitivity + """ + ontology = ontology if ontology else self.main_ontology + with ontology: + rule = Imp() + rule.set_as_rule( + f"""{ontology_concept_class_name}(?a), {ontology_concept_class_name}(?b), {ontology_concept_class_name}(?c), + {predicate_name}(?a, ?b), + {predicate_name}(?b, ?c) + -> {predicate_name}(?a, ?c)""") + return rule + + def reason(self, world: OntologyWorld = None, use_pellet_reasoner: bool = True) -> bool: + """ + Run the reasoning on a given ontology world or :attr:`main_ontology_world` with Pellet or HermiT reasoner, + the two currently supported by owlready2 + - By default, the reasoning works on `owlready2.default_world` + - The reasoning also automatically save ontologies (to either in-memory cache or a temporary sqlite3 file) + Ref: + - https://owlready2.readthedocs.io/en/latest/reasoning.html + - https://owlready2.readthedocs.io/en/latest/rule.html + - https://www.researchgate.net/publication/200758993_Benchmarking_OWL_reasoners + - https://www.researchgate.net/publication/345959058_OWL2Bench_A_Benchmark_for_OWL_2_Reasoners + + :param world: An owlready2.World to reason about. If None, use :attr:`main_ontology_world` + :param use_pellet_reasoner: Use Pellet reasoner, otherwise HermiT + :return: True if the reasoning was successful, otherwise False + """ + reasoner_name = None + reasoning_world = world if world else self.main_ontology_world + try: + if use_pellet_reasoner: + reasoner_name = "Pellet" + sync_reasoner_pellet(x=reasoning_world, infer_property_values=True, + infer_data_property_values=True) + else: + reasoner_name = "HermiT" + sync_reasoner_hermit(x=reasoning_world, infer_property_values=True) + except OwlReadyError as error: + logerr(f"{reasoner_name} reasoning failed: {error}") + return False + loginfo(f"{reasoner_name} reasoning finishes!") + return True diff --git a/src/pycram/ontology/ontology_common.py b/src/pycram/ontology/ontology_common.py index 931145ed1..0df9ef570 100644 --- a/src/pycram/ontology/ontology_common.py +++ b/src/pycram/ontology/ontology_common.py @@ -2,14 +2,19 @@ import itertools from typing import Callable, Dict, List, Optional, Type, TYPE_CHECKING -import rospy from ..helper import Singleton +from ..ros.logging import logerr + if TYPE_CHECKING: from ..designator import DesignatorDescription from owlready2 import issubclass, Thing +ONTOLOGY_SQL_BACKEND_FILE_EXTENSION = ".sqlite3" +ONTOLOGY_SQL_IN_MEMORY_BACKEND = "memory" +ONTOLOGY_OWL_FILE_EXTENSION = ".owl" + class OntologyConceptHolderStore(object, metaclass=Singleton): """ @@ -32,7 +37,7 @@ def add_ontology_concept_holder(self, ontology_concept_name: str, ontology_conce :return: True if the ontology concept can be added into the concept store (if not already existing), otherwise False """ if ontology_concept_name in self.__all_ontology_concept_holders: - rospy.logerr(f"OntologyConceptHolder for `{ontology_concept_name}` was already created!") + logerr(f"OntologyConceptHolder for `{ontology_concept_name}` was already created!") return False else: self.__all_ontology_concept_holders.setdefault(ontology_concept_name, ontology_concept_holder) diff --git a/src/pycram/orm/action_designator.py b/src/pycram/orm/action_designator.py index c84a16ea2..90d4ba5de 100644 --- a/src/pycram/orm/action_designator.py +++ b/src/pycram/orm/action_designator.py @@ -47,7 +47,7 @@ class SetGripperAction(Action): motion: Mapped[GripperState] -class Release(ObjectMixin, Action): +class ReleaseAction(ObjectMixin, Action): """ORM Class of pycram.designators.action_designator.Release.""" id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) @@ -102,7 +102,6 @@ class OpenAction(ObjectMixin, Action): id: Mapped[int] = mapped_column(ForeignKey(f'{Action.__tablename__}.id'), primary_key=True, init=False) arm: Mapped[Arms] - # distance: Mapped[float] = mapped_column(init=False) class CloseAction(ObjectMixin, Action): diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index 0e064bd33..06918ebe2 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -120,7 +120,7 @@ class PoseMixin(MappedAsDataclass): @declared_attr def pose_id(self) -> Mapped[int]: - return mapped_column(ForeignKey(f'{Pose.__tablename__}.id'), init=self.pose_to_init) + return mapped_column(ForeignKey(f'{Pose.__tablename__}.id'), init=self.pose_to_init, nullable=True) @declared_attr def pose(self): diff --git a/src/pycram/orm/utils.py b/src/pycram/orm/utils.py index 624773a88..adcc5ebcc 100644 --- a/src/pycram/orm/utils.py +++ b/src/pycram/orm/utils.py @@ -1,12 +1,11 @@ import traceback -import rospy import sqlalchemy -import pycram.orm.base -from pycram.designators.object_designator import * +from .base import Base +from ..designators.object_designator import * import json -from pycram.designators.action_designator import * -import pycram.orm +from ..ros.logging import loginfo, logwarn + def write_database_to_file(in_sessionmaker: sqlalchemy.orm.sessionmaker, filename: str, @@ -21,7 +20,7 @@ def write_database_to_file(in_sessionmaker: sqlalchemy.orm.sessionmaker, filenam with in_sessionmaker() as session: with open("whatever.txt", "w") as f: to_json_dict = dict() - for table in pycram.orm.base.Base.metadata.sorted_tables: + for table in Base.metadata.sorted_tables: list_of_row = list() for column_object in session.query(table).all(): list_of_row.append(column_object) @@ -37,13 +36,13 @@ def print_database(in_sessionmaker: sqlalchemy.orm.sessionmaker): :param in_sessionmaker: Database Session which should be printed """ with in_sessionmaker() as session: - for table in pycram.orm.base.Base.metadata.sorted_tables: + for table in Base.metadata.sorted_tables: try: smt = sqlalchemy.select('*').select_from(table) result = session.execute(smt).all() - rospy.loginfo("Table: {}\tcontent:{}".format(table, result)) + loginfo("Table: {}\tcontent:{}".format(table, result)) except sqlalchemy.exc.ArgumentError as e: - rospy.logwarn(e) + logwarn(e) def update_primary_key(source_session_maker: sqlalchemy.orm.sessionmaker, @@ -60,7 +59,7 @@ def update_primary_key(source_session_maker: sqlalchemy.orm.sessionmaker, """ destination_session = destination_session_maker() source_session = source_session_maker() - sortedTables = pycram.orm.base.Base.metadata.sorted_tables + sortedTables = Base.metadata.sorted_tables for table in sortedTables: try: list_of_primary_keys_of_this_table = table.primary_key.columns.values() @@ -77,7 +76,7 @@ def update_primary_key(source_session_maker: sqlalchemy.orm.sessionmaker, results = destination_session.execute(sqlalchemy.select(table)) for column_object in results: # iterate over all columns if column_object.__getattr__(key.name) in all_source_key_values: - rospy.loginfo( + loginfo( "Found primary_key collision in table {} value: {} max value in memory {}".format(table, column_object.__getattr__( key.name), @@ -90,8 +89,8 @@ def update_primary_key(source_session_maker: sqlalchemy.orm.sessionmaker, highest_free_key_value += 1 destination_session.commit() # commit after every table except AttributeError as e: - rospy.logwarn("Possible found abstract ORM class {}".format(e.__name__)) - rospy.logwarn(e) + logwarn("Possible found abstract ORM class {}".format(e.__name__)) + logwarn(e) destination_session.close() @@ -112,7 +111,7 @@ def copy_database(source_session_maker: sqlalchemy.orm.sessionmaker, """ with source_session_maker() as source_session, destination_session_maker() as destination_session: - sorted_tables = pycram.orm.base.Base.metadata.sorted_tables + sorted_tables = Base.metadata.sorted_tables for table in sorted_tables: for value in source_session.query(table).all(): insert_statement = sqlalchemy.insert(table).values(value) @@ -121,7 +120,7 @@ def copy_database(source_session_maker: sqlalchemy.orm.sessionmaker, def update_primary_key_constrains(session_maker: sqlalchemy.orm.sessionmaker): - ''' + """ Iterates through all tables related to any ORM Class and sets in their corresponding foreign keys in the given endpoint to "ON UPDATE CASCADING". @@ -130,15 +129,15 @@ def update_primary_key_constrains(session_maker: sqlalchemy.orm.sessionmaker): :param session_maker: :return: empty - ''' + """ with session_maker() as session: - for table in pycram.orm.base.Base.metadata.sorted_tables: + for table in Base.metadata.sorted_tables: try: foreign_key_statement = sqlalchemy.text( "SELECT con.oid, con.conname, con.contype, con.confupdtype, con.confdeltype, con.confmatchtype, pg_get_constraintdef(con.oid) FROM pg_catalog.pg_constraint con INNER JOIN pg_catalog.pg_class rel ON rel.oid = con.conrelid INNER JOIN pg_catalog.pg_namespace nsp ON nsp.oid = connamespace WHERE rel.relname = '{}';".format( table)) response = session.execute(foreign_key_statement) - rospy.loginfo(25 * '~' + "{}".format(table) + 25 * '~') + loginfo(25 * '~' + "{}".format(table) + 25 * '~') for line in response: if line.conname.endswith("fkey"): if 'a' in line.confupdtype: # a --> no action | if there is no action we set it to cascading @@ -157,7 +156,7 @@ def update_primary_key_constrains(session_maker: sqlalchemy.orm.sessionmaker): alter_statement) # There is no real data coming back for this session.commit() except AttributeError: - rospy.loginfo("Attribute Error: {} has no attribute __tablename__".format(table)) + loginfo("Attribute Error: {} has no attribute __tablename__".format(table)) def migrate_neems(source_session_maker: sqlalchemy.orm.sessionmaker, diff --git a/src/pycram/orm/views.py b/src/pycram/orm/views.py index 51c9136f2..999a4aabe 100644 --- a/src/pycram/orm/views.py +++ b/src/pycram/orm/views.py @@ -1,4 +1,4 @@ -from sqlalchemy.orm import declarative_base +from sqlalchemy.orm import declarative_base, Mapped, column_property from typing_extensions import Union import sqlalchemy.orm from sqlalchemy import table, inspect, event, select, engine, MetaData, Select, TableClause, ExecutableDDLElement @@ -122,23 +122,11 @@ class PickUpWithContextView(base): 3D Vector for object position """ - __relative_x = (__robot_position.x - __object_position.x) - """ - Distance on x axis between robot and object - """ - - __relative_y = (__robot_position.y - __object_position.y) - """ - Distance on y axis between robot and object - """ - __table__ = view("PickUpWithContextView", Base.metadata, - (select(PickUpAction.id.label("id"), PickUpAction.arm.label("arm"), - PickUpAction.grasp.label("grasp"), RobotState.torso_height.label("torso_height"), - __relative_x.label("relative_x"), __relative_y.label("relative_y"), - Quaternion.x.label("quaternion_x"), Quaternion.y.label("quaternion_y"), - Quaternion.z.label("quaternion_z"), Quaternion.w.label("quaternion_w"), - Object.obj_type.label("obj_type"), TaskTreeNode.status.label("status")) + (select(PickUpAction.id, PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, + (__robot_position.x-__object_position.x).label("relative_x"), + (__robot_position.y-__object_position.y).label("relative_y"), Quaternion.x, Quaternion.y, + Quaternion.z, Quaternion.w, Object.obj_type, TaskTreeNode.status) .join(TaskTreeNode.action.of_type(PickUpAction)) .join(PickUpAction.robot_state) .join(__robot_pose, RobotState.pose) @@ -147,3 +135,16 @@ class PickUpWithContextView(base): .join(PickUpAction.object) .join(Object.pose) .join(__object_position, Pose.position))) + + id: Mapped[int] = __table__.c.id + arm: Mapped[str] = __table__.c.arm + grasp: Mapped[str] = __table__.c.grasp + torso_height: Mapped[float] = __table__.c.torso_height + relative_x: Mapped[float] = column_property(__table__.c.relative_x) + relative_y: Mapped[float] = column_property(__table__.c.relative_y) + quaternion_x: Mapped[float] = __table__.c.x + quaternion_y: Mapped[float] = __table__.c.y + quaternion_z: Mapped[float] = __table__.c.z + quaternion_w: Mapped[float] = __table__.c.w + obj_type: Mapped[str] = __table__.c.obj_type + status: Mapped[str] = __table__.c.status diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 87b2d477f..6672b6c6c 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,5 +1,5 @@ -import tf import numpy as np +import tf from .datastructures.world import World from .world_concepts.world_object import Object @@ -9,8 +9,7 @@ from .datastructures.pose import Pose, Transform from .robot_description import RobotDescription from .external_interfaces.ik import request_ik -from .plan_failures import IKError -from .utils import _apply_ik +from .failures import IKError from typing_extensions import Tuple, List, Union, Dict, Iterable @@ -120,13 +119,13 @@ def visibility_validator(pose: Pose, robot_pose = robot.get_pose() if isinstance(object_or_pose, Object): robot.set_pose(pose) - camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) ray = world.ray_test(camera_pose.position_as_list(), object_or_pose.get_position_as_list()) res = ray == object_or_pose.id else: robot.set_pose(pose) - camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) # TODO: Check if this is correct ray = world.ray_test(camera_pose.position_as_list(), object_or_pose) @@ -186,7 +185,8 @@ def reachability_validator(pose: Pose, res = False arms = [] for description in manipulator_descs: - retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(description.end_effector.tool_frame)) + retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( + description.end_effector.tool_frame)) retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class # retract_pose needs to be in world frame? @@ -203,14 +203,14 @@ def reachability_validator(pose: Pose, # test the possible solution and apply it to the robot pose, joint_states = request_ik(target, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: # only check for retract pose if pose worked pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: @@ -218,7 +218,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_positions(joint_state_before_ik) + robot.set_multiple_joint_positions(joint_state_before_ik) if arms: res = True return res, arms @@ -245,8 +245,7 @@ def collision_check(robot: Object, allowed_collision: Dict[Object, List]): for obj in World.current_world.objects: if obj.name == "floor": continue - in_contact= _in_contact(robot, obj, allowed_collision, allowed_robot_links) - + in_contact = _in_contact(robot, obj, allowed_collision, allowed_robot_links) + if in_contact: + break return in_contact - - diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index f6d984396..957d0f5e9 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -11,12 +11,11 @@ from abc import ABC from typing_extensions import Callable, Type, Any, Union -import rospy - from .language import Language from .robot_description import RobotDescription from typing_extensions import TYPE_CHECKING from .datastructures.enums import ExecutionType +from .ros.logging import logerr, logwarn_once if TYPE_CHECKING: from .designators.motion_designator import BaseMotion @@ -27,7 +26,7 @@ class ProcessModule: Implementation of process modules. Process modules are the part that communicate with the outer world to execute designators. """ - execution_delay = True + execution_delay = False """ Adds a delay of 0.5 seconds after executing a process module, to make the execution in simulation more realistic """ @@ -90,7 +89,7 @@ def __enter__(self): sets it to 'real' """ self.pre = ProcessModuleManager.execution_type - ProcessModuleManager.execution_type = "real" + ProcessModuleManager.execution_type = ExecutionType.REAL self.pre_delay = ProcessModule.execution_delay ProcessModule.execution_delay = False @@ -128,7 +127,7 @@ def __enter__(self): sets it to 'simulated' """ self.pre = ProcessModuleManager.execution_type - ProcessModuleManager.execution_type = "simulated" + ProcessModuleManager.execution_type = ExecutionType.SIMULATED def __exit__(self, _type, value, traceback): """ @@ -141,6 +140,41 @@ def __call__(self): return self +class SemiRealRobot: + """ + Management class for executing designators on the semi-real robot. This is intended to be used in a with environment. + When importing this class an instance is imported instead. + + Example: + + .. code-block:: python + + with semi_real_robot: + some designators + """ + + def __init__(self): + self.pre: str = "" + + def __enter__(self): + """ + Entering function for 'with' scope, saves the previously set :py:attr:`~ProcessModuleManager.execution_type` and + sets it to 'semi_real' + """ + self.pre = ProcessModuleManager.execution_type + ProcessModuleManager.execution_type = ExecutionType.SEMI_REAL + + def __exit__(self, type, value, traceback): + """ + Exit method for the 'with' scope, sets the :py:attr:`~ProcessModuleManager.execution_type` to the previously + used one. + """ + ProcessModuleManager.execution_type = self.pre + + def __call__(self): + return self + + def with_real_robot(func: Callable) -> Callable: """ Decorator to execute designators in the decorated class on the real robot. @@ -159,7 +193,7 @@ def plan(): def wrapper(*args, **kwargs): pre = ProcessModuleManager.execution_type - ProcessModuleManager.execution_type = "real" + ProcessModuleManager.execution_type = ExecutionType.REAL ret = func(*args, **kwargs) ProcessModuleManager.execution_type = pre return ret @@ -196,6 +230,7 @@ def wrapper(*args, **kwargs): # These are imported, so they don't have to be initialized when executing with simulated_robot = SimulatedRobot() real_robot = RealRobot() +semi_real_robot = SemiRealRobot() class ProcessModuleManager(ABC): @@ -241,14 +276,12 @@ def __init__(self, robot_name): @staticmethod def get_manager() -> Union[ProcessModuleManager, None]: """ - Returns the Process Module manager for the currently loaded robot or None if there is no Manager. - :return: ProcessModuleManager instance of the current robot """ manager = None _default_manager = None if not ProcessModuleManager.execution_type: - rospy.logerr( + logerr( f"No execution_type is set, did you use the with_simulated_robot or with_real_robot decorator?") return @@ -261,17 +294,17 @@ def get_manager() -> Union[ProcessModuleManager, None]: if manager: return manager elif _default_manager: - rospy.logwarn_once(f"No Process Module Manager found for robot: '{RobotDescription.current_robot_description.name}'" + logwarn_once(f"No Process Module Manager found for robot: '{RobotDescription.current_robot_description.name}'" f", using default process modules") return _default_manager else: - rospy.logerr(f"No Process Module Manager found for robot: '{RobotDescription.current_robot_description.name}'" + logerr(f"No Process Module Manager found for robot: '{RobotDescription.current_robot_description.name}'" f", and no default process modules available") return None def navigate(self) -> Type[ProcessModule]: """ - Returns the Process Module for navigating the robot with respect to + Get the Process Module for navigating the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for navigating @@ -281,7 +314,7 @@ def navigate(self) -> Type[ProcessModule]: def pick_up(self) -> Type[ProcessModule]: """ - Returns the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Get the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for picking up an object """ @@ -290,7 +323,7 @@ def pick_up(self) -> Type[ProcessModule]: def place(self) -> Type[ProcessModule]: """ - Returns the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Get the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for placing an Object """ @@ -299,7 +332,7 @@ def place(self) -> Type[ProcessModule]: def looking(self) -> Type[ProcessModule]: """ - Returns the Process Module for looking at a point with respect to + Get the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for looking at a specific point @@ -309,7 +342,7 @@ def looking(self) -> Type[ProcessModule]: def detecting(self) -> Type[ProcessModule]: """ - Returns the Process Module for detecting an object with respect to + Get the Process Module for detecting an object with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for detecting an object @@ -319,7 +352,7 @@ def detecting(self) -> Type[ProcessModule]: def move_tcp(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the Tool Center Point with respect to + Get the Process Module for moving the Tool Center Point with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the TCP @@ -329,7 +362,7 @@ def move_tcp(self) -> Type[ProcessModule]: def move_arm_joints(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the joints of the robot arm + Get the Process Module for moving the joints of the robot arm with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the arm joints @@ -339,7 +372,7 @@ def move_arm_joints(self) -> Type[ProcessModule]: def world_state_detecting(self) -> Type[ProcessModule]: """ - Returns the Process Module for detecting an object using the world state with respect to the + Get the Process Module for detecting an object using the world state with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for world state detecting @@ -349,7 +382,7 @@ def world_state_detecting(self) -> Type[ProcessModule]: def move_joints(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving any joint of the robot with respect to the + Get the Process Module for moving any joint of the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving joints @@ -359,7 +392,7 @@ def move_joints(self) -> Type[ProcessModule]: def move_gripper(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the gripper with respect to + Get the Process Module for moving the gripper with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the gripper @@ -369,7 +402,7 @@ def move_gripper(self) -> Type[ProcessModule]: def open(self) -> Type[ProcessModule]: """ - Returns the Process Module for opening drawers with respect to + Get the Process Module for opening drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for opening drawers @@ -379,7 +412,7 @@ def open(self) -> Type[ProcessModule]: def close(self) -> Type[ProcessModule]: """ - Returns the Process Module for closing drawers with respect to + Get the Process Module for closing drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for closing drawers diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 5dc25d8f9..4cb8b6e89 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -89,13 +89,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) @@ -115,7 +115,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) class BoxyDetecting(ProcessModule): @@ -160,9 +160,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class BoxyWorldStateDetecting(ProcessModule): @@ -200,29 +200,29 @@ def __init__(self): self._close_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyNavigation(self._navigate_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyMoveHead(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyDetecting(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyMoveTCP(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyMoveArmJoints(self._move_arm_joints_lock) def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyWorldStateDetecting(self._world_state_detecting_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return BoxyMoveGripper(self._move_gripper_lock) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 7c9a76dd5..40a3dbdc7 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -196,41 +196,41 @@ def __init__(self): self._close_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultNavigation(self._navigate_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultMoveHead(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultDetecting(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultMoveTCP(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultMoveArmJoints(self._move_arm_joints_lock) def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultWorldStateDetecting(self._world_state_detecting_lock) def move_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultMoveJoints(self._move_joints_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultMoveGripper(self._move_gripper_lock) def open(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultOpen(self._open_lock) def close(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DefaultClose(self._close_lock) diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index e9ff45edc..e39b5bcb3 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -69,13 +69,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) @@ -94,7 +94,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) class DonbotMoveTCP(ProcessModule): @@ -118,7 +118,7 @@ class DonbotMoveJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class DonbotWorldStateDetecting(ProcessModule): @@ -149,33 +149,33 @@ def __init__(self): self._close_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotNavigation(self._navigate_lock) def place(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotPlace(self._place_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotMoveHead(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotDetecting(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotMoveTCP(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotMoveJoints(self._move_arm_joints_lock) def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotWorldStateDetecting(self._world_state_detecting_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return DonbotMoveGripper(self._move_gripper_lock) diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index 5b66d6ec2..ffd1f4287 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/src/pycram/process_modules/hsrb_process_modules.py @@ -1,45 +1,22 @@ import numpy as np -import rospy from threading import Lock -from typing import Any +from typing_extensions import Any -from ..datastructures.enums import JointType +from ..datastructures.enums import ExecutionType +from ..external_interfaces.tmc import tmc_gripper_control, tmc_talk from ..robot_description import RobotDescription from ..process_module import ProcessModule -from ..datastructures.pose import Point -from ..utils import _apply_ik -from ..external_interfaces.ik import request_ik -from .. import world_reasoning as btr from ..local_transformer import LocalTransformer from ..designators.motion_designator import * from ..external_interfaces import giskard -from ..world_concepts.world_object import Object from ..datastructures.world import World +from pydub import AudioSegment +from pydub.playback import play +from gtts import gTTS +import io -def calculate_and_apply_ik(robot, gripper: str, target_position: Point, max_iterations: Optional[int] = None): - """ - Calculates the inverse kinematics for the given target pose and applies it to the robot. - """ - target_position_l = [target_position.x, target_position.y, target_position.z] - # TODO: Check if this is correct (getting the arm and using its joints), previously joints was not provided. - arm = "right" if gripper == RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame() else "left" - inv = request_ik(Pose(target_position_l, [0, 0, 0, 1]), - robot, RobotDescription.current_robot_description.kinematic_chains[arm].joints, gripper) - _apply_ik(robot, inv) - - -def _park_arms(arm): - """ - Defines the joint poses for the parking positions of the arms of HSRB and applies them to the - in the World defined robot. - :return: None - """ - - robot = World.robot - if arm == "left": - for joint, pose in RobotDescription.current_robot_description.get_static_joint_chain("left", "park").items(): - robot.set_joint_position(joint, pose) +from ..ros.logging import logdebug class HSRBNavigation(ProcessModule): @@ -52,176 +29,14 @@ def _execute(self, desig: MoveMotion): robot.set_pose(desig.target) -class HSRBMoveHead(ProcessModule): - """ - This process module moves the head to look at a specific point in the world coordinate frame. - This point can either be a position or an object. - """ - - def _execute(self, desig: LookingMotion): - target = desig.target - robot = World.robot - - local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) - - new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) - new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 - - current_pan = robot.get_joint_position("head_pan_joint") - current_tilt = robot.get_joint_position("head_tilt_joint") - - robot.set_joint_position("head_pan_joint", new_pan + current_pan) - robot.set_joint_position("head_tilt_joint", new_tilt + current_tilt) - - -class HSRBMoveGripper(ProcessModule): - """ - This process module controls the gripper of the robot. They can either be opened or closed. - Furthermore, it can only moved one gripper at a time. - """ - - def _execute(self, desig: MoveGripperMotion): - robot = World.robot - gripper = desig.gripper - motion = desig.motion - for joint, state in RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion).items(): - robot.set_joint_position(joint, state) - - class HSRBDetecting(ProcessModule): """ This process module tries to detect an object with the given type. To be detected the object has to be in the field of view of the robot. """ - - def _execute(self, desig: DetectingMotion): - rospy.loginfo("Detecting technique: {}".format(desig.technique)) - robot = World.robot - object_type = desig.object_type - # Should be "wide_stereo_optical_frame" - cam_frame_name = RobotDescription.current_robot_description.get_camera_frame() - # should be [0, 0, 1] - front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis - # if desig.technique == 'all': - # rospy.loginfo("Fake detecting all generic objects") - # objects = BulletWorld.current_bullet_world.get_all_objets_not_robot() - # elif desig.technique == 'human': - # rospy.loginfo("Fake detecting human -> spawn 0,0,0") - # human = [] - # human.append(Object("human", ObjectType.HUMAN, "human_male.stl", pose=Pose([0, 0, 0]))) - # object_dict = {} - # - # # Iterate over the list of objects and store each one in the dictionary - # for i, obj in enumerate(human): - # object_dict[obj.name] = obj - # return object_dict - # - # else: - # rospy.loginfo("Fake -> Detecting specific object type") - objects = World.current_world.get_object_by_type(object_type) - - object_dict = {} - - perceived_objects = [] - for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): - return obj - - -class HSRBMoveTCP(ProcessModule): - """ - This process moves the tool center point of either the right or the left arm. - """ - - def _execute(self, desig: MoveTCPMotion): - target = desig.target - robot = World.robot - - _move_arm_tcp(target, robot, desig.arm) - - -class HSRBMoveArmJoints(ProcessModule): - """ - This process modules moves the joints of either the right or the left arm. The joint states can be given as - list that should be applied or a pre-defined position can be used, such as "parking" - """ - - def _execute(self, desig: MoveArmJointsMotion): - - robot = World.robot - if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) - if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) - - -class HSRBMoveJoints(ProcessModule): - """ - Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot - """ - - def _execute(self, desig: MoveJointsMotion): - robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) - - -class HSRBWorldStateDetecting(ProcessModule): - """ - This process module detectes an object even if it is not in the field of view of the robot. - """ - - def _execute(self, desig: WorldStateDetectingMotion): - obj_type = desig.object_type - return list(filter(lambda obj: obj.obj_type == obj_type, World.current_world.objects))[0] - - -class HSRBOpen(ProcessModule): - """ - Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. - """ - - def _execute(self, desig: OpeningMotion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above_link(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[1] - 0.05}, desig.object_part.name) - - _move_arm_tcp(goal_pose, World.robot, desig.arm) - - desig.object_part.world_object.set_joint_position(container_joint, - part_of_object.get_joint_limits(container_joint)[1]) - - -class HSRBClose(ProcessModule): - """ - Low-level implementation that lets the robot close a grasped container, in simulation - """ - - def _execute(self, desig: ClosingMotion): - part_of_object = desig.object_part.world_object - - container_joint = part_of_object.find_joint_above_link(desig.object_part.name, JointType.PRISMATIC) - - goal_pose = btr.link_pose_for_joint_config(part_of_object, { - container_joint: part_of_object.get_joint_limits(container_joint)[0]}, desig.object_part.name) - - _move_arm_tcp(goal_pose, World.robot, desig.arm) - - desig.object_part.world_object.set_joint_position(container_joint, - part_of_object.get_joint_limits(container_joint)[0]) - - -def _move_arm_tcp(target: Pose, robot: Object, arm: Arms) -> None: - gripper = RobotDescription.current_robot_description.get_arm_chain(arm).get_tool_frame() - - joints = RobotDescription.current_robot_description.get_arm_chain(arm).joints - - inv = request_ik(target, robot, joints, gripper) - _apply_ik(robot, inv) + # pass + def _execute(self, desig: DetectingMotion) -> Any: + pass ########################################################### @@ -235,47 +50,20 @@ class HSRBNavigationReal(ProcessModule): """ def _execute(self, designator: MoveMotion) -> Any: - rospy.logdebug(f"Sending goal to giskard to Move the robot") + logdebug(f"Sending goal to giskard to Move the robot") # giskard.achieve_cartesian_goal(designator.target, robot_description.base_link, "map") - queryPoseNav(designator.target) - - -class HSRBNavigationSemiReal(ProcessModule): - """ - Process module for the real HSRB that sends a cartesian goal to giskard to move the robot base - """ - - def _execute(self, designator: MoveMotion) -> Any: - rospy.logdebug(f"Sending goal to giskard to Move the robot") - giskard.achieve_cartesian_goal(designator.target, RobotDescription.current_robot_description.base_link, "map") + # todome fix this # queryPoseNav(designator.target) class HSRBMoveHeadReal(ProcessModule): """ - Process module for the real robot to move that such that it looks at the given position. Uses the same calculation - as the simulated one + Process module for the real HSRB that sends a pose goal to giskard to move the robot head """ def _execute(self, desig: LookingMotion): target = desig.target - robot = World.robot - - local_transformer = LocalTransformer() - pose_in_pan = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_pan_link")) - pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) - - new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) - new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x + pose_in_tilt.position.y) - - current_pan = robot.get_joint_position("head_pan_joint") - current_tilt = robot.get_joint_position("head_tilt_joint") - - giskard.avoid_all_collisions() - giskard.achieve_joint_goal( - {"head_pan_joint": new_pan + current_pan, "head_tilt_joint": new_tilt + current_tilt}) - giskard.achieve_joint_goal( - {"head_pan_joint": new_pan + current_pan, "head_tilt_joint": new_tilt + current_tilt}) + giskard.move_head_to_pose(target) class HSRBDetectingReal(ProcessModule): @@ -285,98 +73,12 @@ class HSRBDetectingReal(ProcessModule): """ def _execute(self, desig: DetectingMotion) -> Any: - # todo at the moment perception ignores searching for a specific object type so we do as well on real - if desig.technique == 'human' and (desig.state == "start" or desig.state == None): - human_pose = queryHuman() - pose = Pose.from_pose_stamped(human_pose) - pose.position.z = 0 - human = [] - human.append(Object("human", ObjectType.HUMAN, "human_male.stl", pose=pose)) - object_dict = {} - - # Iterate over the list of objects and store each one in the dictionary - for i, obj in enumerate(human): - object_dict[obj.name] = obj - return object_dict - - return human_pose - elif desig.technique == 'human' and desig.state == "stop": - stop_queryHuman() - return "stopped" - - query_result = queryEmpty(ObjectDesignatorDescription(types=[desig.object_type])) - perceived_objects = [] - for i in range(0, len(query_result.res)): - # this has to be pose from pose stamped since we spawn the object with given header - obj_pose = Pose.from_pose_stamped(query_result.res[i].pose[0]) - # obj_pose.orientation = [0, 0, 0, 1] - # obj_pose_tmp = query_result.res[i].pose[0] - obj_type = query_result.res[i].type - obj_size = query_result.res[i].shape_size - obj_color = query_result.res[i].color[0] - color_switch = { - "red": [1, 0, 0, 1], - "green": [0, 1, 0, 1], - "blue": [0, 0, 1, 1], - "black": [0, 0, 0, 1], - "white": [1, 1, 1, 1], - # add more colors if needed - } - color = color_switch.get(obj_color) - if color is None: - color = [0, 0, 0, 1] - - # atm this is the string size that describes the object but it is not the shape size thats why string - def extract_xyz_values(input_string): - # Split the input string by commas and colon to separate key-value pairs - # key_value_pairs = input_string.split(', ') - - # Initialize variables to store the X, Y, and Z values - x_value = None - y_value = None - z_value = None - - for key in input_string: - x_value = key.dimensions.x - y_value = key.dimensions.y - z_value = key.dimensions.z - - # - # # Iterate through the key-value pairs to extract the values - # for pair in key_value_pairs: - # key, value = pair.split(': ') - # if key == 'x': - # x_value = float(value) - # elif key == 'y': - # y_value = float(value) - # elif key == 'z': - # z_value = float(value) - - return x_value, y_value, z_value - - x, y, z = extract_xyz_values(obj_size) - size = (x, z / 2, y) - size_box = (x / 2, z / 2, y / 2) - hard_size = (0.02, 0.02, 0.03) - id = World.current_world.add_rigid_box(obj_pose, hard_size, color) - box_object = Object(obj_type + "_" + str(rospy.get_time()), obj_type, pose=obj_pose, color=color, id=id, - customGeom={"size": [hard_size[0], hard_size[1], hard_size[2]]}) - box_object.set_pose(obj_pose) - box_desig = ObjectDesignatorDescription.Object(box_object.name, box_object.type, box_object) - - perceived_objects.append(box_desig) - - object_dict = {} - - # Iterate over the list of objects and store each one in the dictionary - for i, obj in enumerate(perceived_objects): - object_dict[obj.name] = obj - return object_dict + pass class HSRBMoveTCPReal(ProcessModule): """ - Moves the tool center point of the real HSRB while avoiding all collisions + Moves the tool center point of the real HSRB while avoiding all collisions via giskard """ def _execute(self, designator: MoveTCPMotion) -> Any: @@ -385,13 +87,13 @@ def _execute(self, designator: MoveTCPMotion) -> Any: giskard.avoid_all_collisions() if designator.allow_gripper_collision: giskard.allow_gripper_collision(designator.arm) - giskard.achieve_cartesian_goal(pose_in_map, RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame(), - "map") + giskard.achieve_cartesian_goal(pose_in_map, RobotDescription.current_robot_description.get_arm_chain( + designator.arm).get_tool_frame(), "map") class HSRBMoveArmJointsReal(ProcessModule): """ - Moves the arm joints of the real HSRB to the given configuration while avoiding all collisions + Moves the arm joints of the real HSRB to the given configuration while avoiding all collisions via giskard """ def _execute(self, designator: MoveArmJointsMotion) -> Any: @@ -419,67 +121,84 @@ class HSRBMoveGripperReal(ProcessModule): """ def _execute(self, designator: MoveGripperMotion) -> Any: - if (designator.motion == "open"): - pub_gripper = rospy.Publisher('/hsrb/gripper_controller/grasp/goal', GripperApplyEffortActionGoal, - queue_size=10) - rate = rospy.Rate(10) - rospy.sleep(2) - msg = GripperApplyEffortActionGoal() # sprechen joint gripper_controll_manager an, indem wir goal publishen type den giskard fürs greifen erwartet - msg.goal.effort = 0.8 - pub_gripper.publish(msg) - - elif (designator.motion == "close"): - pub_gripper = rospy.Publisher('/hsrb/gripper_controller/grasp/goal', GripperApplyEffortActionGoal, - queue_size=10) - rate = rospy.Rate(10) - rospy.sleep(2) - msg = GripperApplyEffortActionGoal() - msg.goal.effort = -0.8 - pub_gripper.publish(msg) - - # if designator.allow_gripper_collision: - # giskard.allow_gripper_collision("left") - # giskard.achieve_gripper_motion_goal(designator.motion) + tmc_gripper_control(designator) class HSRBOpenReal(ProcessModule): """ - Tries to open an already grasped container + This process Modules tries to open an already grasped container via giskard """ def _execute(self, designator: OpeningMotion) -> Any: - giskard.achieve_open_container_goal(RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame(), - designator.object_part.name) + giskard.achieve_open_container_goal( + RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame(), + designator.object_part.name) class HSRBCloseReal(ProcessModule): """ - Tries to close an already grasped container + This process module executes close a an already grasped container via giskard """ def _execute(self, designator: ClosingMotion) -> Any: - giskard.achieve_close_container_goal(RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame(), - designator.object_part.name) - - -# class HSRBTalkReal(ProcessModule): -# """ -# Tries to close an already grasped container -# """ -# -# def _execute(self, designator: TalkingMotion.Motion) -> Any: -# pub = rospy.Publisher('/talk_request', Voice, queue_size=10) -# -# # fill message of type Voice with required data: -# texttospeech = Voice() -# # language 1 = english (0 = japanese) -# texttospeech.language = 1 -# texttospeech.sentence = designator.cmd -# -# rospy.sleep(1) -# pub.publish(texttospeech) + giskard.achieve_close_container_goal( + RobotDescription.current_robot_description.get_arm_chain(designator.arm).get_tool_frame(), + designator.object_part.name) + + +class HSRBTalkReal(ProcessModule): + """ + Let the robot speak over tmc interface. + """ + + def _execute(self, designator: TalkingMotion) -> Any: + tmc_talk(designator) + + +########################################################### +########## Process Modules for the Semi Real HSRB ############### +########################################################### +class HSRBNavigationSemiReal(ProcessModule): + """ + Process module for the real HSRB that sends a cartesian goal to giskard to move the robot base + """ + def _execute(self, designator: MoveMotion) -> Any: + logdebug(f"Sending goal to giskard to Move the robot") + giskard.teleport_robot(designator.target) + + +class HSRBTalkSemiReal(ProcessModule): + """ + Low Level implementation to let the robot talk using gTTS and pydub. + """ + + def _execute(self, designator: TalkingMotion) -> Any: + """ + Convert text to speech using gTTS, modify the pitch and play it without saving to disk. + """ + sentence = designator.cmd + # Create a gTTS object + tts = gTTS(text=sentence, lang='en', slow=False) + + # Save the speech to an in-memory file + mp3_fp = io.BytesIO() + tts.write_to_fp(mp3_fp) + mp3_fp.seek(0) + # Load the audio into pydub from the in-memory file + audio = AudioSegment.from_file(mp3_fp, format="mp3") + + # Speed up the audio slightly + faster_audio = audio.speedup(playback_speed=1.2) + + # Play the modified audio + play(faster_audio) + + +########################################################### +########## HSRB MANAGER ############### +########################################################### class HSRBManager(ProcessModuleManager): def __init__(self): @@ -499,85 +218,65 @@ def __init__(self): self._talk_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return HSRBNavigation(self._navigate_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBNavigationReal(self._navigate_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBNavigationSemiReal(self._navigate_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBMoveHead(self._looking_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBMoveHeadReal(self._looking_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBMoveHeadReal(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return HSRBDetecting(self._detecting_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBDetectingReal(self._detecting_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBDetecting(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBMoveTCP(self._move_tcp_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBMoveTCPReal(self._move_tcp_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBMoveTCPReal(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBMoveArmJoints(self._move_arm_joints_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBMoveArmJointsReal(self._move_arm_joints_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBMoveArmJointsReal(self._move_arm_joints_lock) - def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated" or ProcessModuleManager.execution_type == "real": - return HSRBWorldStateDetecting(self._world_state_detecting_lock) - elif ProcessModuleManager.execution_type == "semi_real": - return HSRBWorldStateDetecting(self._world_state_detecting_lock) - def move_joints(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBMoveJoints(self._move_joints_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBMoveJointsReal(self._move_joints_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBMoveJointsReal(self._move_joints_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBMoveGripper(self._move_gripper_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBMoveGripperReal(self._move_gripper_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBMoveGripperReal(self._move_gripper_lock) def open(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBOpen(self._open_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBOpenReal(self._open_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBOpenReal(self._open_lock) def close(self): - if ProcessModuleManager.execution_type == "simulated": - return HSRBClose(self._close_lock) - elif ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.REAL: return HSRBCloseReal(self._close_lock) - elif ProcessModuleManager.execution_type == "semi_real": + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: return HSRBCloseReal(self._close_lock) - # def talk(self): - # if ProcessModuleManager.execution_type == "real": - # return HSRBTalkReal(self._talk_lock) - # elif ProcessModuleManager.execution_type == "semi_real": - # return HSRBTalkReal(self._talk_lock) + def talk(self): + if ProcessModuleManager.execution_type == ExecutionType.REAL: + return HSRBTalkReal(self._talk_lock) + elif ProcessModuleManager.execution_type == ExecutionType.SEMI_REAL: + return HSRBTalkSemiReal(self._talk_lock) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 0ac65ba8d..29898ba97 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,17 +1,17 @@ from threading import Lock -from typing_extensions import Any +from typing_extensions import Any, TYPE_CHECKING import actionlib from .. import world_reasoning as btr import numpy as np -import rospy from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik +from ..ros.logging import logdebug from ..utils import _apply_ik from ..local_transformer import LocalTransformer -from ..designators.object_designator import ObjectDesignatorDescription + from ..designators.motion_designator import MoveMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion @@ -19,9 +19,12 @@ from ..datastructures.world import World from ..world_concepts.world_object import Object from ..datastructures.pose import Pose -from ..datastructures.enums import JointType, ObjectType, Arms +from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType from ..external_interfaces import giskard -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * + +if TYPE_CHECKING: + from ..designators.object_designator import ObjectDesignatorDescription try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 @@ -87,7 +90,7 @@ def _execute(self, desig: DetectingMotion): robot = World.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" - cam_frame_name = RobotDescription.current_robot_description.get_camera_frame() + camera_link_name = RobotDescription.current_robot_description.get_camera_link() # should be [0, 0, 1] camera_description = RobotDescription.current_robot_description.cameras[ list(RobotDescription.current_robot_description.cameras.keys())[0]] @@ -95,7 +98,7 @@ def _execute(self, desig: DetectingMotion): objects = World.current_world.get_object_by_type(object_type) for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): + if btr.visible(obj, robot.get_link_pose(camera_link_name), front_facing_axis): return obj @@ -121,9 +124,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class PR2MoveJoints(ProcessModule): @@ -133,7 +136,7 @@ class PR2MoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion): robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) + robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): @@ -206,7 +209,7 @@ class Pr2NavigationReal(ProcessModule): """ def _execute(self, designator: MoveMotion) -> Any: - rospy.logdebug(f"Sending goal to giskard to Move the robot") + logdebug(f"Sending goal to giskard to Move the robot") giskard.achieve_cartesian_goal(designator.target, RobotDescription.current_robot_description.base_link, "map") @@ -315,10 +318,10 @@ class Pr2MoveGripperReal(ProcessModule): def _execute(self, designator: MoveGripperMotion) -> Any: def activate_callback(): - rospy.loginfo("Started gripper Movement") + loginfo("Started gripper Movement") def done_callback(state, result): - rospy.loginfo(f"Reached goal {designator.motion}: {result.reached_goal}") + loginfo(f"Reached goal {designator.motion}: {result.reached_goal}") def feedback_callback(msg): pass @@ -331,7 +334,7 @@ def feedback_callback(msg): else: controller_topic = "l_gripper_controller/gripper_action" client = actionlib.SimpleActionClient(controller_topic, Pr2GripperCommandAction) - rospy.loginfo("Waiting for action server") + loginfo("Waiting for action server") client.wait_for_server() client.send_goal(goal, active_cb=activate_callback, done_cb=done_callback, feedback_cb=feedback_callback) wait = client.wait_for_result() @@ -375,59 +378,60 @@ def __init__(self): self._close_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2Navigation(self._navigate_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2NavigationReal(self._navigate_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2MoveHead(self._looking_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2MoveHeadReal(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2Detecting(self._detecting_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2DetectingReal(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2MoveTCP(self._move_tcp_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2MoveTCPReal(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2MoveArmJoints(self._move_arm_joints_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2MoveArmJointsReal(self._move_arm_joints_lock) def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated" or ProcessModuleManager.execution_type == "real": + if (ProcessModuleManager.execution_type == ExecutionType.SIMULATED or + ProcessModuleManager.execution_type == ExecutionType.REAL): return Pr2WorldStateDetecting(self._world_state_detecting_lock) def move_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return PR2MoveJoints(self._move_joints_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2MoveJointsReal(self._move_joints_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2MoveGripper(self._move_gripper_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2MoveGripperReal(self._move_gripper_lock) def open(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2Open(self._open_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2OpenReal(self._open_lock) def close(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return Pr2Close(self._close_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return Pr2CloseReal(self._close_lock) diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py index 77b33596a..028b8f333 100644 --- a/src/pycram/process_modules/stretch_process_modules.py +++ b/src/pycram/process_modules/stretch_process_modules.py @@ -1,8 +1,7 @@ from typing import Any -import rospy - -from ..external_interfaces.robokudo import query +from ..external_interfaces.robokudo import * +from ..ros.logging import logdebug from ..utils import _apply_ik from ..external_interfaces import giskard from .default_process_modules import * @@ -132,7 +131,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: Arms) -> None: # inv = request_ik(target, robot, joints, gripper) pose, joint_states = request_giskard_ik(target, robot, gripper) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) ########################################################### @@ -146,7 +145,7 @@ class StretchNavigationReal(ProcessModule): """ def _execute(self, designator: MoveMotion) -> Any: - rospy.logdebug(f"Sending goal to giskard to Move the robot") + logdebug(f"Sending goal to giskard to Move the robot") giskard.achieve_cartesian_goal(designator.target, RobotDescription.current_robot_description.base_link, "map") @@ -295,59 +294,59 @@ def __init__(self): self._close_lock = Lock() def navigate(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchNavigate(self._navigate_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchNavigationReal(self._navigate_lock) def looking(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchMoveHead(self._looking_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchMoveHeadReal(self._looking_lock) def detecting(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchDetecting(self._detecting_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchDetectingReal(self._detecting_lock) def move_tcp(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchMoveTCP(self._move_tcp_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchMoveTCPReal(self._move_tcp_lock) def move_arm_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchMoveArmJoints(self._move_arm_joints_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchMoveArmJointsReal(self._move_arm_joints_lock) def world_state_detecting(self): - if ProcessModuleManager.execution_type == "simulated" or ProcessModuleManager.execution_type == "real": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED or ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchWorldStateDetecting(self._world_state_detecting_lock) def move_joints(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchMoveJoints(self._move_joints_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchMoveJointsReal(self._move_joints_lock) def move_gripper(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchMoveGripper(self._move_gripper_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchMoveGripperReal(self._move_gripper_lock) def open(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchOpen(self._open_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchOpenReal(self._open_lock) def close(self): - if ProcessModuleManager.execution_type == "simulated": + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return StretchClose(self._close_lock) - elif ProcessModuleManager.execution_type == "real": + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return StretchCloseReal(self._close_lock) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index ce9805395..495267a3a 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -1,12 +1,13 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations - -import rospy from typing_extensions import List, Dict, Union, Optional -from urdf_parser_py.urdf import URDF +from .datastructures.dataclasses import VirtualMobileBaseJoints +from .datastructures.enums import Arms, Grasp, GripperState, GripperType, JointType +from .object_descriptors.urdf import ObjectDescription as URDFObject +from .ros.logging import logerr from .utils import suppress_stdout_stderr -from .datastructures.enums import Arms, Grasp, GripperState, GripperType +from .helper import parse_mjcf_actuators class RobotDescriptionManager: @@ -42,7 +43,12 @@ def load_description(self, name: str): RobotDescription.current_robot_description = self.descriptions[name] return self.descriptions[name] else: - rospy.logerr(f"Robot description {name} not found") + for key in self.descriptions.keys(): + if key in name.lower(): + RobotDescription.current_robot_description = self.descriptions[key] + return self.descriptions[key] + else: + logerr(f"Robot description {name} not found") def register_description(self, description: RobotDescription): """ @@ -81,7 +87,7 @@ class RobotDescription: """ Torso joint of the robot """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -105,8 +111,14 @@ class RobotDescription: """ All joints defined in the URDF, by default fixed joints are not included """ + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None + """ + Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to + move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) + """ - def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str): + def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -116,6 +128,8 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param torso_link: Torso link of the robot :param torso_joint: Torso joint of the robot, this is the joint that moves the torso upwards if there is one :param urdf_path: Path to the URDF file of the robot + :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots + :param mjcf_path: Path to the MJCF file of the robot """ self.name = name self.base_link = base_link @@ -123,12 +137,35 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.torso_joint = torso_joint with suppress_stdout_stderr(): # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them - self.urdf_object = URDF.from_xml_file(urdf_path) + self.urdf_object = URDFObject(urdf_path) + self.joint_types = {joint.name: joint.type for joint in self.urdf_object.joints} + self.joint_actuators: Optional[Dict] = parse_mjcf_actuators(mjcf_path) if mjcf_path is not None else None self.kinematic_chains: Dict[str, KinematicChainDescription] = {} self.cameras: Dict[str, CameraDescription] = {} self.grasps: Dict[Grasp, List[float]] = {} self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] + self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints + + @property + def has_actuators(self): + """ + Property to check if the robot has actuators defined in the MJCF file. + + :return: True if the robot has actuators, False otherwise + """ + return self.joint_actuators is not None + + def get_actuator_for_joint(self, joint: str) -> Optional[str]: + """ + Get the actuator name for a given joint. + + :param joint: Name of the joint + :return: Name of the actuator + """ + if self.has_actuators: + return self.joint_actuators.get(joint) + return None def add_kinematic_chain_description(self, chain: KinematicChainDescription): """ @@ -200,7 +237,7 @@ def add_grasp_orientations(self, orientations: Dict[Grasp, List[float]]): def get_manipulator_chains(self) -> List[KinematicChainDescription]: """ - Returns a list of all manipulator chains of the robot which posses an end effector. + Get a list of all manipulator chains of the robot which posses an end effector. :return: A list of KinematicChainDescription objects """ @@ -210,7 +247,7 @@ def get_manipulator_chains(self) -> List[KinematicChainDescription]: result.append(chain) return result - def get_camera_frame(self) -> str: + def get_camera_link(self) -> str: """ Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. @@ -218,9 +255,17 @@ def get_camera_frame(self) -> str: """ return self.cameras[list(self.cameras.keys())[0]].link_name + def get_camera_frame(self) -> str: + """ + Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. + + :return: A name of the link of a camera + """ + return f"{self.name}/{self.cameras[list(self.cameras.keys())[0]].link_name}" + def get_default_camera(self) -> CameraDescription: """ - Returns the first camera in the list of cameras. + Get the first camera in the list of cameras. :return: A CameraDescription object """ @@ -228,7 +273,7 @@ def get_default_camera(self) -> CameraDescription: def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: str): """ - Returns the static joint states of a kinematic chain for a specific configuration. When trying to access one of + Get the static joint states of a kinematic chain for a specific configuration. When trying to access one of the robot arms the function `:func: get_arm_chain` should be used. :param kinematic_chain_name: @@ -246,7 +291,7 @@ def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: def get_parent(self, name: str) -> str: """ - Returns the parent of a link or joint in the URDF. Always returns the imeadiate parent, for a link this is a joint + Get the parent of a link or joint in the URDF. Always returns the imeadiate parent, for a link this is a joint and vice versa. :param name: Name of the link or joint in the URDF @@ -266,7 +311,7 @@ def get_parent(self, name: str) -> str: def get_child(self, name: str, return_multiple_children: bool = False) -> Union[str, List[str]]: """ - Returns the child of a link or joint in the URDF. Always returns the immediate child, for a link this is a joint + Get the child of a link or joint in the URDF. Always returns the immediate child, for a link this is a joint and vice versa. Since a link can have multiple children, the return_multiple_children parameter can be set to True to get a list of all children. @@ -293,9 +338,19 @@ def get_child(self, name: str, return_multiple_children: bool = False) -> Union[ child_link = self.urdf_object.joint_map[name].child return child_link + def get_arm_tool_frame(self, arm: Arms) -> str: + """ + Get the name of the tool frame of a specific arm. + + :param arm: Arm for which the tool frame should be returned + :return: The name of the link of the tool frame in the URDF. + """ + chain = self.get_arm_chain(arm) + return chain.get_tool_frame() + def get_arm_chain(self, arm: Arms) -> Union[KinematicChainDescription, List[KinematicChainDescription]]: """ - Returns the kinematic chain of a specific arm. If the arm is set to BOTH, all kinematic chains are returned. + Get the kinematic chain of a specific arm. If the arm is set to BOTH, all kinematic chains are returned. :param arm: Arm for which the chain should be returned :return: KinematicChainDescription object of the arm @@ -329,7 +384,7 @@ class KinematicChainDescription: """ Last link of the chain """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -358,7 +413,7 @@ class KinematicChainDescription: Dictionary of static joint states for the chain """ - def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDF, arm_type: Arms = None, + def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDFObject, arm_type: Arms = None, include_fixed_joints=False): """ Initialize the KinematicChainDescription object. @@ -373,7 +428,7 @@ def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDF, self.name: str = name self.start_link: str = start_link self.end_link: str = end_link - self.urdf_object: URDF = urdf_object + self.urdf_object: URDFObject = urdf_object self.include_fixed_joints: bool = include_fixed_joints self.link_names: List[str] = [] self.joint_names: List[str] = [] @@ -395,11 +450,12 @@ def _init_joints(self): Initializes the joints of the chain by getting the chain from the URDF object. """ joints = self.urdf_object.get_chain(self.start_link, self.end_link, links=False) - self.joint_names = list(filter(lambda j: self.urdf_object.joint_map[j].type != "fixed" or self.include_fixed_joints, joints)) + self.joint_names = list(filter(lambda j: self.urdf_object.joint_map[j].type != JointType.FIXED + or self.include_fixed_joints, joints)) def get_joints(self) -> List[str]: """ - Returns a list of all joints of the chain. + Get a list of all joints of the chain. :return: List of joint names """ @@ -407,9 +463,7 @@ def get_joints(self) -> List[str]: def get_links(self) -> List[str]: """ - Returns a list of all links of the chain. - - :return: List of link names + :return: A list of all links of the chain. """ return self.link_names @@ -445,7 +499,7 @@ def add_static_joint_states(self, name: str, states: dict): def get_static_joint_states(self, name: str) -> Dict[str, float]: """ - Returns the dictionary of static joint states for a given name of the static joint states. + Get the dictionary of static joint states for a given name of the static joint states. :param name: Name of the static joint states :return: Dictionary of joint names and their values @@ -453,11 +507,11 @@ def get_static_joint_states(self, name: str) -> Dict[str, float]: try: return self.static_joint_states[name] except KeyError: - rospy.logerr(f"Static joint states for chain {name} not found") + logerr(f"Static joint states for chain {name} not found") def get_tool_frame(self) -> str: """ - Returns the name of the tool frame of the end effector of this chain, if it has an end effector. + Get the name of the tool frame of the end effector of this chain, if it has an end effector. :return: The name of the link of the tool frame in the URDF. """ @@ -468,7 +522,7 @@ def get_tool_frame(self) -> str: def get_static_gripper_state(self, state: GripperState) -> Dict[str, float]: """ - Returns the static joint states for the gripper of the chain. + Get the static joint states for the gripper of the chain. :param state: Name of the static joint states :return: Dictionary of joint names and their values @@ -552,7 +606,7 @@ class EndEffectorDescription: """ Name of the tool frame link in the URDf """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -577,7 +631,7 @@ class EndEffectorDescription: Distance the gripper can open, in cm """ - def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDF): + def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject): """ Initialize the EndEffectorDescription object. @@ -589,7 +643,7 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD self.name: str = name self.start_link: str = start_link self.tool_frame: str = tool_frame - self.urdf_object: URDF = urdf_object + self.urdf_object: URDFObject = urdf_object self.link_names: List[str] = [] self.joint_names: List[str] = [] self.static_joint_states: Dict[GripperState, Dict[str, float]] = {} diff --git a/src/pycram/robot_descriptions/__init__.py b/src/pycram/robot_descriptions/__init__.py index 33dc54ca9..1ec759998 100644 --- a/src/pycram/robot_descriptions/__init__.py +++ b/src/pycram/robot_descriptions/__init__.py @@ -9,7 +9,8 @@ class DeprecatedRobotDescription: def raise_error(self): - raise DeprecationWarning("Robot description moved, please use RobotDescription.current_robot_description from pycram.robot_description") + raise DeprecationWarning("Robot description moved, please use RobotDescription.current_robot_description from" + " pycram.robot_description") @property def name(self): diff --git a/src/pycram/robot_descriptions/boxy_description.py b/src/pycram/robot_descriptions/boxy_description.py index 6af022b8e..f4dc7cfc1 100644 --- a/src/pycram/robot_descriptions/boxy_description.py +++ b/src/pycram/robot_descriptions/boxy_description.py @@ -1,10 +1,9 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, CameraDescription, KinematicChainDescription, \ EndEffectorDescription, RobotDescriptionManager from ..datastructures.enums import Arms, Grasp, GripperState -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "boxy" + '.urdf' +filename = get_ros_package_path('pycram') + '/resources/robots/' + "boxy" + '.urdf' boxy_description = RobotDescription("boxy", "base_link", "triangle_base_link", "triangle_base_joint", filename) diff --git a/src/pycram/robot_descriptions/donbot_description.py b/src/pycram/robot_descriptions/donbot_description.py index 69d50ad02..f37958440 100644 --- a/src/pycram/robot_descriptions/donbot_description.py +++ b/src/pycram/robot_descriptions/donbot_description.py @@ -1,10 +1,9 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import Arms, Grasp, GripperState -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "iai_donbot" + '.urdf' +filename = get_ros_package_path('pycram') + '/resources/robots/' + "iai_donbot" + '.urdf' donbot_description = RobotDescription("iai_donbot", "base_link", "ur5_base_link", "arm_base_mounting_joint", filename) diff --git a/src/pycram/robot_descriptions/hsrb_description.py b/src/pycram/robot_descriptions/hsrb_description.py index f83f23191..ae452e92e 100644 --- a/src/pycram/robot_descriptions/hsrb_description.py +++ b/src/pycram/robot_descriptions/hsrb_description.py @@ -1,11 +1,10 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import GripperState, Grasp, Arms -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "hsrb" + '.urdf' +filename = get_ros_package_path('pycram') + '/resources/robots/' + "hsrb" + '.urdf' hsrb_description = RobotDescription("hsrb", "base_link", "arm_lift_link", "arm_lift_joint", filename) diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 402125f2a..35ee28838 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -1,13 +1,17 @@ +from ..datastructures.dataclasses import VirtualMobileBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import Arms, Grasp, GripperState, GripperType -import rospkg +from ..ros.ros_tools import get_ros_package_path -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' +from ..helper import get_robot_mjcf_path + +filename = get_ros_package_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' + +mjcf_filename = get_robot_mjcf_path("", "pr2") pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", - filename) + filename, virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link", diff --git a/src/pycram/robot_descriptions/stretch_description.py b/src/pycram/robot_descriptions/stretch_description.py index fad1c7427..3c86be24f 100644 --- a/src/pycram/robot_descriptions/stretch_description.py +++ b/src/pycram/robot_descriptions/stretch_description.py @@ -1,11 +1,10 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ CameraDescription, RobotDescriptionManager from ..datastructures.enums import GripperState, Arms, Grasp -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "stretch_description" + '.urdf' +filename = get_ros_package_path('pycram') + '/resources/robots/' + "stretch_description" + '.urdf' stretch_description = RobotDescription("stretch_description", "base_link", "link_lift", "joint_lift", filename) diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index 6a92d47ec..ef39e8a8b 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -1,13 +1,19 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path + +from ..datastructures.dataclasses import VirtualMobileBaseJoints +from ..datastructures.enums import GripperState, Arms, Grasp from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import GripperState, Arms, Grasp +from ..helper import get_robot_mjcf_path + +filename = get_ros_package_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' +mjcf_filename = get_robot_mjcf_path("pal_robotics", "tiago_dual") tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", - filename) + filename, + virtual_mobile_base_joints=VirtualMobileBaseJoints(), + mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "torso_lift_link", "arm_left_7_link", diff --git a/src/pycram/robot_descriptions/turtlebot_description.py b/src/pycram/robot_descriptions/turtlebot_description.py new file mode 100644 index 000000000..34dbe79a9 --- /dev/null +++ b/src/pycram/robot_descriptions/turtlebot_description.py @@ -0,0 +1,13 @@ +from ..ros.ros_tools import get_ros_package_path + +from ..robot_description import RobotDescriptionManager, RobotDescription + +# Description for turtlebot3_waffle_pi +filename = get_ros_package_path('pycram') + '/resources/robots/' + "turtlebot" + '.urdf' + +turtlebot = RobotDescription("turtlebot", "world", "base_link", "base_joint", + filename) + +# Add to RobotDescriptionManager +rdm = RobotDescriptionManager() +rdm.register_description(turtlebot) diff --git a/src/pycram/robot_descriptions/ur5_description.py b/src/pycram/robot_descriptions/ur5_description.py index d50f189fd..3a931dbc3 100644 --- a/src/pycram/robot_descriptions/ur5_description.py +++ b/src/pycram/robot_descriptions/ur5_description.py @@ -1,10 +1,9 @@ -import rospkg +from ..ros.ros_tools import get_ros_package_path from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager from ..datastructures.enums import Arms, Grasp, GripperState -rospack = rospkg.RosPack() -filename = rospack.get_path('pycram') + '/resources/robots/' + "ur5_robotiq" + '.urdf' +filename = get_ros_package_path('pycram') + '/resources/robots/' + "ur5_robotiq" + '.urdf' ur5_description = RobotDescription("ur5_robotiq", "world", "base_link", "ee_link", filename) diff --git a/src/pycram/ros/__init__.py b/src/pycram/ros/__init__.py index e69de29bb..c22224413 100644 --- a/src/pycram/ros/__init__.py +++ b/src/pycram/ros/__init__.py @@ -0,0 +1,6 @@ +import rospy +from .ros_tools import is_master_online + +# Check is for sphinx autoAPI to be able to work in a CI workflow +if is_master_online(): + rospy.init_node("pycram") diff --git a/src/pycram/ros/action_lib.py b/src/pycram/ros/action_lib.py new file mode 100644 index 000000000..efc5f3048 --- /dev/null +++ b/src/pycram/ros/action_lib.py @@ -0,0 +1,7 @@ +import actionlib + +from actionlib import SimpleActionClient + +def create_action_client(topic_name: str, action_message) -> SimpleActionClient: + return actionlib.SimpleActionClient(topic_name, action_message) + diff --git a/src/pycram/ros/data_types.py b/src/pycram/ros/data_types.py new file mode 100644 index 000000000..4b6956279 --- /dev/null +++ b/src/pycram/ros/data_types.py @@ -0,0 +1,11 @@ +import rospy + +from rospy import ServiceException +def Time(time=0.0): + return rospy.Time(time) + +def Duration(duration=0.0): + return rospy.Duration(duration) + +def Rate(rate): + return rospy.Rate(rate) \ No newline at end of file diff --git a/src/pycram/ros/logging.py b/src/pycram/ros/logging.py new file mode 100644 index 000000000..184f22841 --- /dev/null +++ b/src/pycram/ros/logging.py @@ -0,0 +1,64 @@ +import rospy +import inspect +from pathlib import Path + + +def _get_caller_method_name(): + """ + Get the name of the method that called the function from which this function is called. It is intended as a helper + function for the log functions. + + :return: Name of the method that called the function from which this function is called. + """ + return inspect.stack()[2][3] + +def _get_caller_method_line(): + """ + Get the line of the method that called the function from which this function is called. It is intended as a helper + function for the log functions. + + :return: Line number of the method that called the function from which this function is called. + """ + return inspect.stack()[2][2] + +def _get_caller_file_name(): + """ + Get the file name of the method that called the function from which this function is called. It is intended as a helper + function for the log functions. + + :return: File name of the method that called the function from which this function is called. + """ + path = Path(inspect.stack()[2][1]) + return path.name + + +def logwarn(message: str): + rospy.logwarn(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def loginfo(message: str): + rospy.loginfo(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def logerr(message: str): + rospy.logerr(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def logdebug(message: str): + rospy.logdebug(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def logwarn_once(message: str): + rospy.logwarn_once(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def loginfo_once(message: str): + rospy.loginfo_once(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def logerr_once(message: str): + rospy.logerr_once(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") + + +def logdebug_once(message: str): + rospy.logdebug_once(f"[{_get_caller_file_name()}:{_get_caller_method_line()}:{_get_caller_method_name()}] {message}") diff --git a/src/pycram/ros/publisher.py b/src/pycram/ros/publisher.py new file mode 100644 index 000000000..533a7c89b --- /dev/null +++ b/src/pycram/ros/publisher.py @@ -0,0 +1,4 @@ +import rospy + +def create_publisher(topic, msg_type, queue_size=10) -> rospy.Publisher: + return rospy.Publisher(topic, msg_type, queue_size=queue_size) \ No newline at end of file diff --git a/src/pycram/ros/ros_tools.py b/src/pycram/ros/ros_tools.py new file mode 100644 index 000000000..96bcf7ce6 --- /dev/null +++ b/src/pycram/ros/ros_tools.py @@ -0,0 +1,45 @@ +import rosgraph +import rosnode +import rospy +# import rospkg + +from rospkg import RosPack, ResourceNotFound +from typing_extensions import Any + + +def get_node_names(namespace=None): + return rosnode.get_node_names(namespace) + + +def create_ros_pack(ros_paths: Any = None) -> RosPack: + """ + Creates a RosPack instance to search for resources of ros packages. + + :param ros_paths: An ordered list of paths to search for resources. + :return: An instance of RosPack + """ + return RosPack(ros_paths) + + +def get_ros_package_path(package_name: str) -> str: + rospack = create_ros_pack() + return rospack.get_path(package_name) + + +def get_parameter(name: str) -> Any: + return rospy.get_param(name) + + +def wait_for_message(topic_name: str): + return rospy.wait_for_message(topic_name) + + +def is_master_online(): + return rosgraph.is_master_online() + + +def sleep(duration: float): + rospy.sleep(duration) + +def create_timer(duration: int, callback, oneshot=False): + return rospy.Timer(rospy.Duration(duration), callback, oneshot=oneshot) \ No newline at end of file diff --git a/src/pycram/ros/service.py b/src/pycram/ros/service.py new file mode 100644 index 000000000..be2f84f4d --- /dev/null +++ b/src/pycram/ros/service.py @@ -0,0 +1,11 @@ +import rosservice +import rospy + + +def get_service_proxy(topic_name: str, service_message) -> rospy.ServiceProxy: + return rospy.ServiceProxy(topic_name, service_message) + + +def wait_for_service(topic_name: str): + rospy.loginfo_once(f"Waiting for service: {topic_name}") + rospy.wait_for_service(topic_name) diff --git a/src/pycram/ros/subscriber.py b/src/pycram/ros/subscriber.py new file mode 100644 index 000000000..d4ab3dd8b --- /dev/null +++ b/src/pycram/ros/subscriber.py @@ -0,0 +1,4 @@ +import rospy + +def create_subscriber(topic, msg_type, callback, queue_size=10) -> rospy.Subscriber: + return rospy.Subscriber(topic, msg_type, callback, queue_size=queue_size) \ No newline at end of file diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 0aa149e9b..ab9a01c89 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -1,321 +1,3 @@ -import atexit -import threading -import time -from typing import List, Optional, Tuple - -import rospy -from geometry_msgs.msg import Vector3 -from std_msgs.msg import ColorRGBA -from visualization_msgs.msg import Marker, MarkerArray - -from ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape -from ..datastructures.pose import Pose, Transform -from ..designator import ObjectDesignatorDescription -from ..datastructures.world import World - - class VizMarkerPublisher: - """ - Publishes an Array of visualization marker which represent the situation in the World - """ - - def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): - """ - The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the - World. This Array is published with a rate of interval. - - :param topic_name: The name of the topic to which the Visualization Marker should be published. - :param interval: The interval at which the visualization marker should be published, in seconds. - """ - self.topic_name = topic_name - self.interval = interval - - self.pub = rospy.Publisher(self.topic_name, MarkerArray, queue_size=10) - - self.thread = threading.Thread(target=self._publish) - self.kill_event = threading.Event() - self.main_world = World.current_world if not World.current_world.is_prospection_world else World.current_world.world_sync.world - - self.thread.start() - atexit.register(self._stop_publishing) - - def _publish(self) -> None: - """ - Constantly publishes the Marker Array. To the given topic name at a fixed rate. - """ - while not self.kill_event.is_set(): - marker_array = self._make_marker_array() - - self.pub.publish(marker_array) - time.sleep(self.interval) - - def _make_marker_array(self) -> MarkerArray: - """ - Creates the Marker Array to be published. There is one Marker for link for each object in the Array, each Object - creates a name space in the visualization Marker. The type of Visualization Marker is decided by the collision - tag of the URDF. - - :return: An Array of Visualization Marker - """ - marker_array = MarkerArray() - for obj in self.main_world.objects: - if obj.name == "floor": - continue - for link in obj.link_name_to_id.keys(): - geom = obj.get_link_geometry(link) - if not geom: - continue - msg = Marker() - msg.header.frame_id = "map" - msg.ns = obj.name - msg.id = obj.link_name_to_id[link] - msg.type = Marker.MESH_RESOURCE - msg.action = Marker.ADD - link_pose = obj.get_link_transform(link) - if obj.get_link_origin(link) is not None: - link_origin = obj.get_link_origin_transform(link) - else: - link_origin = Transform() - link_pose_with_origin = link_pose * link_origin - msg.pose = link_pose_with_origin.to_pose().pose - - color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_link_color(link).get_rgba() - - msg.color = ColorRGBA(*color) - msg.lifetime = rospy.Duration(1) - - if isinstance(geom, MeshVisualShape): - msg.type = Marker.MESH_RESOURCE - msg.mesh_resource = "file://" + geom.file_name - msg.scale = Vector3(1, 1, 1) - msg.mesh_use_embedded_materials = True - elif isinstance(geom, CylinderVisualShape): - msg.type = Marker.CYLINDER - msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.length) - elif isinstance(geom, BoxVisualShape): - msg.type = Marker.CUBE - msg.scale = Vector3(*geom.size) - elif isinstance(geom, SphereVisualShape): - msg.type = Marker.SPHERE - msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.radius * 2) - - marker_array.markers.append(msg) - return marker_array - - def _stop_publishing(self) -> None: - """ - Stops the publishing of the Visualization Marker update by setting the kill event and collecting the thread. - """ - self.kill_event.set() - self.thread.join() - - -class ManualMarkerPublisher: - """ - Class to manually add and remove marker of objects and poses. - """ - - def __init__(self, topic_name: str = '/pycram/manual_marker', interval: float = 0.1): - """ - The Publisher creates an Array of Visualization marker with a marker for a pose or object. - This Array is published with a rate of interval. - - :param topic_name: Name of the marker topic - :param interval: Interval at which the marker should be published - """ - self.start_time = None - self.marker_array_pub = rospy.Publisher(topic_name, MarkerArray, queue_size=10) - - self.marker_array = MarkerArray() - self.marker_overview = {} - self.current_id = 0 - - self.interval = interval - self.log_message = None - - def publish(self, pose: Pose, color: Optional[List] = None, bw_object: Optional[ObjectDesignatorDescription] = None, - name: Optional[str] = None): - """ - Publish a pose or an object into the MarkerArray. - Priorities to add an object if possible - - :param pose: Pose of the marker - :param color: Color of the marker if no object is given - :param bw_object: Object to add as a marker - :param name: Name of the marker - """ - - if color is None: - color = [1, 0, 1, 1] - - self.start_time = time.time() - thread = threading.Thread(target=self._publish, args=(pose, bw_object, name, color)) - thread.start() - rospy.loginfo(self.log_message) - thread.join() - - def _publish(self, pose: Pose, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None, - color: Optional[List] = None): - """ - Publish the marker into the MarkerArray - """ - stop_thread = False - duration = 2 - - while not stop_thread: - if time.time() - self.start_time > duration: - stop_thread = True - if bw_object is None: - self._publish_pose(name=name, pose=pose, color=color) - else: - self._publish_object(name=name, pose=pose, bw_object=bw_object) - - rospy.sleep(self.interval) - - def _publish_pose(self, name: str, pose: Pose, color: Optional[List] = None): - """ - Publish a Pose as a marker - - :param name: Name of the marker - :param pose: Pose of the marker - :param color: Color of the marker - """ - - if name is None: - name = 'pose_marker' - - if name in self.marker_overview.keys(): - self._update_marker(self.marker_overview[name], new_pose=pose) - return - - color_rgba = ColorRGBA(*color) - self._make_marker_array(name=name, marker_type=Marker.ARROW, marker_pose=pose, - marker_scales=(0.05, 0.05, 0.05), color_rgba=color_rgba) - self.marker_array_pub.publish(self.marker_array) - self.log_message = f"Pose '{name}' published" - - def _publish_object(self, name: Optional[str], pose: Pose, bw_object: ObjectDesignatorDescription): - """ - Publish an Object as a marker - - :param name: Name of the marker - :param pose: Pose of the marker - :param bw_object: ObjectDesignatorDescription for the marker - """ - - bw_real = bw_object.resolve() - - if name is None: - name = bw_real.name - - if name in self.marker_overview.keys(): - self._update_marker(self.marker_overview[name], new_pose=pose) - return - - path = bw_real.world_object.root_link.geometry.file_name - - self._make_marker_array(name=name, marker_type=Marker.MESH_RESOURCE, marker_pose=pose, - path_to_resource=path) - - self.marker_array_pub.publish(self.marker_array) - self.log_message = f"Object '{name}' published" - - def _make_marker_array(self, name, marker_type: int, marker_pose: Pose, marker_scales: Tuple = (1.0, 1.0, 1.0), - color_rgba: ColorRGBA = ColorRGBA(*[1.0, 1.0, 1.0, 1.0]), - path_to_resource: Optional[str] = None): - """ - Create a Marker and add it to the MarkerArray - - :param name: Name of the Marker - :param marker_type: Type of the marker to create - :param marker_pose: Pose of the marker - :param marker_scales: individual scaling of the markers axes - :param color_rgba: Color of the marker as RGBA - :param path_to_resource: Path to the resource of a Bulletworld object - """ - - frame_id = marker_pose.header.frame_id - new_marker = Marker() - new_marker.id = self.current_id - new_marker.header.frame_id = frame_id - new_marker.ns = name - new_marker.header.stamp = rospy.Time.now() - new_marker.type = marker_type - new_marker.action = Marker.ADD - new_marker.pose = marker_pose.pose - new_marker.scale.x = marker_scales[0] - new_marker.scale.y = marker_scales[1] - new_marker.scale.z = marker_scales[2] - new_marker.color.a = color_rgba.a - new_marker.color.r = color_rgba.r - new_marker.color.g = color_rgba.g - new_marker.color.b = color_rgba.b - - if path_to_resource is not None: - new_marker.mesh_resource = 'file://' + path_to_resource - - self.marker_array.markers.append(new_marker) - self.marker_overview[name] = new_marker.id - self.current_id += 1 - - def _update_marker(self, marker_id: int, new_pose: Pose) -> bool: - """ - Update an existing marker to a new pose - - :param marker_id: id of the marker that should be updated - :param new_pose: Pose where the updated marker is set - - :return: True if update was successful, False otherwise - """ - - # Find the marker with the specified ID - for marker in self.marker_array.markers: - if marker.id == marker_id: - # Update successful - marker.pose = new_pose - self.log_message = f"Marker '{marker.ns}' updated" - self.marker_array_pub.publish(self.marker_array) - return True - - # Update was not successful - rospy.logwarn(f"Marker {marker_id} not found for update") - return False - - def remove_marker(self, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None): - """ - Remove a marker by object or name - - :param bw_object: Object which marker should be removed - :param name: Name of object that should be removed - """ - - if bw_object is not None: - bw_real = bw_object.resolve() - name = bw_real.name - - if name is None: - rospy.logerr('No name for object given, cannot remove marker') - return - - marker_id = self.marker_overview.pop(name) - - for marker in self.marker_array.markers: - if marker.id == marker_id: - marker.action = Marker.DELETE - - self.marker_array_pub.publish(self.marker_array) - self.marker_array.markers.pop(marker_id) - - rospy.loginfo(f"Removed Marker '{name}'") - - def clear_all_marker(self): - """ - Clear all existing markers - """ - for marker in self.marker_array.markers: - marker.action = Marker.DELETE - - self.marker_overview = {} - self.marker_array_pub.publish(self.marker_array) - - rospy.loginfo('Removed all markers') + def __init__(self): + raise DeprecationWarning("This function moved and can now be found in pycram.ros_utils.viz_marker_publisher") \ No newline at end of file diff --git a/src/pycram/ros_utils/__init__.py b/src/pycram/ros_utils/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/ros/force_torque_sensor.py b/src/pycram/ros_utils/force_torque_sensor.py similarity index 93% rename from src/pycram/ros/force_torque_sensor.py rename to src/pycram/ros_utils/force_torque_sensor.py index 5f52503f5..3d98e79cd 100644 --- a/src/pycram/ros/force_torque_sensor.py +++ b/src/pycram/ros_utils/force_torque_sensor.py @@ -2,11 +2,11 @@ import time import threading -import rospy - from geometry_msgs.msg import WrenchStamped from std_msgs.msg import Header from ..datastructures.world import World +from ..ros.data_types import Time +from ..ros.publisher import create_publisher class ForceTorqueSensor: @@ -34,7 +34,7 @@ def __init__(self, joint_name, fts_topic="/pycram/fts", interval=0.1): f" does not exist in robot object") self.world.enable_joint_force_torque_sensor(self.world.robot, self.fts_joint_idx) - self.fts_pub = rospy.Publisher(fts_topic, WrenchStamped, queue_size=10) + self.fts_pub = create_publisher(fts_topic, WrenchStamped, queue_size=10) self.interval = interval self.kill_event = threading.Event() @@ -53,7 +53,7 @@ def _publish(self) -> None: joint_ft = self.world.get_joint_reaction_force_torque(self.world.robot, self.fts_joint_idx) h = Header() h.seq = seq - h.stamp = rospy.Time.now() + h.stamp = Time().now() h.frame_id = self.joint_name wrench_msg = WrenchStamped() diff --git a/src/pycram/ros/joint_state_publisher.py b/src/pycram/ros_utils/joint_state_publisher.py similarity index 91% rename from src/pycram/ros/joint_state_publisher.py rename to src/pycram/ros_utils/joint_state_publisher.py index 08b78edf2..c747b0af0 100644 --- a/src/pycram/ros/joint_state_publisher.py +++ b/src/pycram/ros_utils/joint_state_publisher.py @@ -2,11 +2,11 @@ import threading import atexit -import rospy - from sensor_msgs.msg import JointState from std_msgs.msg import Header from ..datastructures.world import World +from ..ros.data_types import Time +from ..ros.publisher import create_publisher class JointStatePublisher: @@ -23,7 +23,7 @@ def __init__(self, joint_state_topic="/pycram/joint_state", interval=0.1): """ self.world = World.current_world - self.joint_state_pub = rospy.Publisher(joint_state_topic, JointState, queue_size=10) + self.joint_state_pub = create_publisher(joint_state_topic, JointState, queue_size=10) self.interval = interval self.kill_event = threading.Event() self.thread = threading.Thread(target=self._publish) @@ -43,7 +43,7 @@ def _publish(self) -> None: while not self.kill_event.is_set(): current_joint_states = [robot.get_joint_position(joint_name) for joint_name in joint_names] h = Header() - h.stamp = rospy.Time.now() + h.stamp = Time().now() h.seq = seq h.frame_id = "" joint_state_msg = JointState() diff --git a/src/pycram/ros/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py similarity index 85% rename from src/pycram/ros/robot_state_updater.py rename to src/pycram/ros_utils/robot_state_updater.py index 0f3ad0f4f..314d85535 100644 --- a/src/pycram/ros/robot_state_updater.py +++ b/src/pycram/ros_utils/robot_state_updater.py @@ -1,4 +1,3 @@ -import rospy import atexit import tf import time @@ -8,6 +7,8 @@ from ..datastructures.world import World from ..robot_descriptions import robot_description from ..datastructures.pose import Pose +from ..ros.data_types import Time, Duration +from ..ros.ros_tools import wait_for_message, create_timer class RobotStateUpdater: @@ -31,8 +32,8 @@ def __init__(self, tf_topic: str, joint_state_topic: str): self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - self.tf_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_tf) - self.joint_state_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_joint_state) + self.tf_timer = create_timer(Duration().from_sec(0.1), self._subscribe_tf) + self.joint_state_timer = create_timer(Duration().from_sec(0.1), self._subscribe_joint_state) atexit.register(self._stop_subscription) @@ -42,7 +43,7 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: :param msg: TransformStamped message published to the topic """ - trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, rospy.Time(0)) + trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, Time(0)) World.robot.set_pose(Pose(trans, rot)) def _subscribe_joint_state(self, msg: JointState) -> None: @@ -54,7 +55,7 @@ def _subscribe_joint_state(self, msg: JointState) -> None: :param msg: JointState message published to the topic. """ try: - msg = rospy.wait_for_message(self.joint_state_topic, JointState) + msg = wait_for_message(self.joint_state_topic, JointState) for name, position in zip(msg.name, msg.position): World.robot.set_joint_position(name, position) except AttributeError: diff --git a/src/pycram/ros/tf_broadcaster.py b/src/pycram/ros_utils/tf_broadcaster.py similarity index 85% rename from src/pycram/ros/tf_broadcaster.py rename to src/pycram/ros_utils/tf_broadcaster.py index 9f4661a43..2e6287e00 100644 --- a/src/pycram/ros/tf_broadcaster.py +++ b/src/pycram/ros_utils/tf_broadcaster.py @@ -1,18 +1,21 @@ import time -import rospy import threading import atexit from ..datastructures.pose import Pose from ..datastructures.world import World +from ..datastructures.enums import ExecutionType from tf2_msgs.msg import TFMessage +from ..ros.publisher import create_publisher +from ..ros.data_types import Time + class TFBroadcaster: """ Broadcaster that publishes TF frames for every object in the World. """ - def __init__(self, projection_namespace="simulated", odom_frame="odom", interval=0.1): + def __init__(self, projection_namespace=ExecutionType.SIMULATED, odom_frame="odom", interval=0.1): """ The broadcaster prefixes all published TF messages with a projection namespace to distinguish between the TF frames from the simulation and the one from the real robot. @@ -23,8 +26,8 @@ def __init__(self, projection_namespace="simulated", odom_frame="odom", interval """ self.world = World.current_world - self.tf_static_publisher = rospy.Publisher("/tf_static", TFMessage, queue_size=10) - self.tf_publisher = rospy.Publisher("/tf", TFMessage, queue_size=10) + self.tf_static_publisher = create_publisher("/tf_static", TFMessage, queue_size=10) + self.tf_publisher = create_publisher("/tf", TFMessage, queue_size=10) self.thread = threading.Thread(target=self._publish, daemon=True) self.kill_event = threading.Event() self.interval = interval @@ -52,11 +55,11 @@ def _update_objects(self) -> None: """ for obj in self.world.objects: pose = obj.get_pose() - pose.header.stamp = rospy.Time.now() + pose.header.stamp = Time.now() self._publish_pose(obj.tf_frame, pose) for link in obj.link_name_to_id.keys(): link_pose = obj.get_link_pose(link) - link_pose.header.stamp = rospy.Time.now() + link_pose.header.stamp = Time.now() self._publish_pose(obj.get_link_tf_frame(link), link_pose) def _update_static_odom(self) -> None: @@ -78,8 +81,8 @@ def _publish_pose(self, child_frame_id: str, pose: Pose, static=False) -> None: frame_id = pose.frame if frame_id != child_frame_id: tf_stamped = pose.to_transform(child_frame_id) - tf_stamped.frame = self.projection_namespace + "/" + tf_stamped.frame - tf_stamped.child_frame_id = self.projection_namespace + "/" + tf_stamped.child_frame_id + tf_stamped.frame = self.projection_namespace.name + "/" + tf_stamped.frame + tf_stamped.child_frame_id = self.projection_namespace.name + "/" + tf_stamped.child_frame_id tf2_msg = TFMessage() tf2_msg.transforms.append(tf_stamped) if static: diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py new file mode 100644 index 000000000..ceeb11910 --- /dev/null +++ b/src/pycram/ros_utils/viz_marker_publisher.py @@ -0,0 +1,327 @@ +import atexit +import threading +import time +from typing import List, Optional, Tuple + +import numpy as np +from geometry_msgs.msg import Vector3 +from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker, MarkerArray + +from ..datastructures.dataclasses import BoxVisualShape, CylinderVisualShape, MeshVisualShape, SphereVisualShape +from ..datastructures.pose import Pose, Transform +from ..designator import ObjectDesignatorDescription +from ..datastructures.world import World +from ..ros.data_types import Duration, Time +from ..ros.logging import loginfo, logwarn, logerr +from ..ros.publisher import create_publisher +from ..ros.ros_tools import sleep + + +class VizMarkerPublisher: + """ + Publishes an Array of visualization marker which represent the situation in the World + """ + + def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): + """ + The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the + World. This Array is published with a rate of interval. + + :param topic_name: The name of the topic to which the Visualization Marker should be published. + :param interval: The interval at which the visualization marker should be published, in seconds. + """ + self.topic_name = topic_name + self.interval = interval + + self.pub = create_publisher(self.topic_name, MarkerArray, queue_size=10) + + self.thread = threading.Thread(target=self._publish) + self.kill_event = threading.Event() + self.main_world = World.current_world if not World.current_world.is_prospection_world else World.current_world.world_sync.world + self.lock = self.main_world.object_lock + self.thread.start() + atexit.register(self._stop_publishing) + + def _publish(self) -> None: + """ + Constantly publishes the Marker Array. To the given topic name at a fixed rate. + """ + while not self.kill_event.is_set(): + self.lock.acquire() + marker_array = self._make_marker_array() + self.lock.release() + self.pub.publish(marker_array) + time.sleep(self.interval) + + def _make_marker_array(self) -> MarkerArray: + """ + Creates the Marker Array to be published. There is one Marker for link for each object in the Array, each Object + creates a name space in the visualization Marker. The type of Visualization Marker is decided by the collision + tag of the URDF. + + :return: An Array of Visualization Marker + """ + marker_array = MarkerArray() + for obj in self.main_world.objects: + if obj.name == "floor": + continue + for link in obj.link_name_to_id.keys(): + geom = obj.get_link_geometry(link) + if not geom: + continue + msg = Marker() + msg.header.frame_id = "map" + msg.ns = obj.name + msg.id = obj.link_name_to_id[link] + msg.type = Marker.MESH_RESOURCE + msg.action = Marker.ADD + link_pose = obj.get_link_transform(link) + if obj.get_link_origin(link) is not None: + link_origin = obj.get_link_origin_transform(link) + else: + link_origin = Transform() + link_pose_with_origin = link_pose * link_origin + msg.pose = link_pose_with_origin.to_pose().pose + + color = obj.get_link_color(link).get_rgba() + + msg.color = ColorRGBA(*color) + msg.lifetime = Duration(1) + + if isinstance(geom, MeshVisualShape): + msg.type = Marker.MESH_RESOURCE + msg.mesh_resource = "file://" + geom.file_name + msg.scale = Vector3(1, 1, 1) + msg.mesh_use_embedded_materials = True + elif isinstance(geom, CylinderVisualShape): + msg.type = Marker.CYLINDER + msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.length) + elif isinstance(geom, BoxVisualShape): + msg.type = Marker.CUBE + size = np.array(geom.size) * 2 + msg.scale = Vector3(size[0], size[1], size[2]) + elif isinstance(geom, SphereVisualShape): + msg.type = Marker.SPHERE + msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.radius * 2) + + marker_array.markers.append(msg) + return marker_array + + def _stop_publishing(self) -> None: + """ + Stops the publishing of the Visualization Marker update by setting the kill event and collecting the thread. + """ + self.kill_event.set() + self.thread.join() + + +class ManualMarkerPublisher: + """ + Class to manually add and remove marker of objects and poses. + """ + + def __init__(self, topic_name: str = '/pycram/manual_marker', interval: float = 0.1): + """ + The Publisher creates an Array of Visualization marker with a marker for a pose or object. + This Array is published with a rate of interval. + + :param topic_name: Name of the marker topic + :param interval: Interval at which the marker should be published + """ + self.start_time = None + self.marker_array_pub = create_publisher(topic_name, MarkerArray, queue_size=10) + + self.marker_array = MarkerArray() + self.marker_overview = {} + self.current_id = 0 + + self.interval = interval + self.log_message = None + + def publish(self, pose: Pose, color: Optional[List] = None, bw_object: Optional[ObjectDesignatorDescription] = None, + name: Optional[str] = None): + """ + Publish a pose or an object into the MarkerArray. + Priorities to add an object if possible + + :param pose: Pose of the marker + :param color: Color of the marker if no object is given + :param bw_object: Object to add as a marker + :param name: Name of the marker + """ + + if color is None: + color = [1, 0, 1, 1] + + self.start_time = time.time() + thread = threading.Thread(target=self._publish, args=(pose, bw_object, name, color)) + thread.start() + loginfo(self.log_message) + thread.join() + + def _publish(self, pose: Pose, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None, + color: Optional[List] = None): + """ + Publish the marker into the MarkerArray + """ + stop_thread = False + duration = 2 + + while not stop_thread: + if time.time() - self.start_time > duration: + stop_thread = True + if bw_object is None: + self._publish_pose(name=name, pose=pose, color=color) + else: + self._publish_object(name=name, pose=pose, bw_object=bw_object) + + sleep(self.interval) + + def _publish_pose(self, name: str, pose: Pose, color: Optional[List] = None): + """ + Publish a Pose as a marker + + :param name: Name of the marker + :param pose: Pose of the marker + :param color: Color of the marker + """ + + if name is None: + name = 'pose_marker' + + if name in self.marker_overview.keys(): + self._update_marker(self.marker_overview[name], new_pose=pose) + return + + color_rgba = ColorRGBA(*color) + self._make_marker_array(name=name, marker_type=Marker.ARROW, marker_pose=pose, + marker_scales=(0.05, 0.05, 0.05), color_rgba=color_rgba) + self.marker_array_pub.publish(self.marker_array) + self.log_message = f"Pose '{name}' published" + + def _publish_object(self, name: Optional[str], pose: Pose, bw_object: ObjectDesignatorDescription): + """ + Publish an Object as a marker + + :param name: Name of the marker + :param pose: Pose of the marker + :param bw_object: ObjectDesignatorDescription for the marker + """ + + bw_real = bw_object.resolve() + + if name is None: + name = bw_real.name + + if name in self.marker_overview.keys(): + self._update_marker(self.marker_overview[name], new_pose=pose) + return + + path = bw_real.world_object.root_link.geometry.file_name + + self._make_marker_array(name=name, marker_type=Marker.MESH_RESOURCE, marker_pose=pose, + path_to_resource=path) + + self.marker_array_pub.publish(self.marker_array) + self.log_message = f"Object '{name}' published" + + def _make_marker_array(self, name, marker_type: int, marker_pose: Pose, marker_scales: Tuple = (1.0, 1.0, 1.0), + color_rgba: ColorRGBA = ColorRGBA(*[1.0, 1.0, 1.0, 1.0]), + path_to_resource: Optional[str] = None): + """ + Create a Marker and add it to the MarkerArray + + :param name: Name of the Marker + :param marker_type: Type of the marker to create + :param marker_pose: Pose of the marker + :param marker_scales: individual scaling of the markers axes + :param color_rgba: Color of the marker as RGBA + :param path_to_resource: Path to the resource of a Bulletworld object + """ + + frame_id = marker_pose.header.frame_id + new_marker = Marker() + new_marker.id = self.current_id + new_marker.header.frame_id = frame_id + new_marker.ns = name + new_marker.header.stamp = Time.now() + new_marker.type = marker_type + new_marker.action = Marker.ADD + new_marker.pose = marker_pose.pose + new_marker.scale.x = marker_scales[0] + new_marker.scale.y = marker_scales[1] + new_marker.scale.z = marker_scales[2] + new_marker.color.a = color_rgba.a + new_marker.color.r = color_rgba.r + new_marker.color.g = color_rgba.g + new_marker.color.b = color_rgba.b + + if path_to_resource is not None: + new_marker.mesh_resource = 'file://' + path_to_resource + + self.marker_array.markers.append(new_marker) + self.marker_overview[name] = new_marker.id + self.current_id += 1 + + def _update_marker(self, marker_id: int, new_pose: Pose) -> bool: + """ + Update an existing marker to a new pose + + :param marker_id: id of the marker that should be updated + :param new_pose: Pose where the updated marker is set + + :return: True if update was successful, False otherwise + """ + + # Find the marker with the specified ID + for marker in self.marker_array.markers: + if marker.id == marker_id: + # Update successful + marker.pose = new_pose + self.log_message = f"Marker '{marker.ns}' updated" + self.marker_array_pub.publish(self.marker_array) + return True + + # Update was not successful + logwarn(f"Marker {marker_id} not found for update") + return False + + def remove_marker(self, bw_object: Optional[ObjectDesignatorDescription] = None, name: Optional[str] = None): + """ + Remove a marker by object or name + + :param bw_object: Object which marker should be removed + :param name: Name of object that should be removed + """ + + if bw_object is not None: + bw_real = bw_object.resolve() + name = bw_real.name + + if name is None: + logerr('No name for object given, cannot remove marker') + return + + marker_id = self.marker_overview.pop(name) + + for marker in self.marker_array.markers: + if marker.id == marker_id: + marker.action = Marker.DELETE + + self.marker_array_pub.publish(self.marker_array) + self.marker_array.markers.pop(marker_id) + + loginfo(f"Removed Marker '{name}'") + + def clear_all_marker(self): + """ + Clear all existing markers + """ + for marker in self.marker_array.markers: + marker.action = Marker.DELETE + + self.marker_overview = {} + self.marker_array_pub.publish(self.marker_array) + + loginfo('Removed all markers') diff --git a/src/pycram/tasktree.py b/src/pycram/tasktree.py index 448e1f718..06440829e 100644 --- a/src/pycram/tasktree.py +++ b/src/pycram/tasktree.py @@ -2,28 +2,22 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations - -from typing_extensions import TYPE_CHECKING - import datetime import inspect import logging from typing_extensions import List, Optional, Callable - import anytree import sqlalchemy.orm.session import tqdm - from .datastructures.world import World +from .helper import Singleton +from .orm.action_designator import Action from .orm.tasktree import TaskTreeNode as ORMTaskTreeNode from .orm.base import ProcessMetaData -from .plan_failures import PlanFailure +from .failures import PlanFailure from .datastructures.enums import TaskStatus from .datastructures.dataclasses import Color -if TYPE_CHECKING: - from .designators.performables import Action - class NoOperation: @@ -80,7 +74,7 @@ def __init__(self, action: Optional[Action] = NoOperation(), parent: Optional[Ta self.action = action self.status = TaskStatus.CREATED - self.start_time = None + self.start_time = datetime.datetime.now() self.end_time = None self.parent = parent self.reason: Optional[Exception] = reason @@ -117,7 +111,7 @@ def to_sql(self) -> ORMTaskTreeNode: else: reason = None - return ORMTaskTreeNode(self.start_time, self.end_time, self.status.name, reason) + return ORMTaskTreeNode(start_time=self.start_time, end_time=self.end_time, status=self.status, reason=reason) def insert(self, session: sqlalchemy.orm.session.Session, recursive: bool = True, parent: Optional[TaskTreeNode] = None, use_progress_bar: bool = True, @@ -186,7 +180,7 @@ def __enter__(self): self.suspended_tree = task_tree self.world_state = World.current_world.save_state() - self.simulated_root = TaskTreeNode() + self.simulated_root = TaskTree() task_tree = self.simulated_root World.current_world.add_text("Simulating...", [0, 0, 1.75], color=Color.from_rgb([0, 0, 0]), parent_object_id=1) @@ -202,21 +196,50 @@ def __exit__(self, exc_type, exc_val, exc_tb): World.current_world.remove_text() -task_tree: Optional[TaskTreeNode] = None -"""Current TaskTreeNode""" - - -def reset_tree() -> None: +class TaskTree(metaclass=Singleton): """ - Reset the current task tree to an empty root (NoOperation) node. + TaskTree represents the tree of functions that were called during a pycram plan. Consists of TaskTreeNodes. + Must be a singleton. """ - global task_tree - task_tree = TaskTreeNode() - task_tree.start_time = datetime.datetime.now() - task_tree.status = TaskStatus.RUNNING + def __init__(self): + """ + Create a new TaskTree with a root node. + """ + self.root = TaskTreeNode() + self.current_node = self.root -reset_tree() + def __len__(self): + """ + Get the number of nodes that are in this TaskTree. + + :return: The number of nodes. + """ + return len(self.root.children) + + def reset_tree(self): + """ + Reset the current task tree to an empty root (NoOperation) node. + """ + self.root = TaskTreeNode() + self.root.start_time = datetime.datetime.now() + self.root.status = TaskStatus.RUNNING + self.current_node = self.root + + def add_node(self, action: Optional[Action] = None) -> TaskTreeNode: + """ + Add a new node to the task tree and make it the current node. + + :param action: The action that is performed in this node. + :return: The new node. + """ + new_node = TaskTreeNode(action=action, parent=self.current_node) + self.current_node = new_node + return new_node + + +task_tree = TaskTree() +"""Current TaskTreeNode""" def with_tree(fun: Callable) -> Callable: @@ -227,7 +250,6 @@ def with_tree(fun: Callable) -> Callable: """ def handle_tree(*args, **kwargs): - # get the task tree global task_tree @@ -238,29 +260,31 @@ def handle_tree(*args, **kwargs): action = keyword_arguments.get("self", None) # create the task tree node - task_tree = TaskTreeNode(action, parent=task_tree) + task_tree.add_node(action) # Try to execute the task try: - task_tree.status = TaskStatus.CREATED - task_tree.start_time = datetime.datetime.now() + task_tree.current_node.status = TaskStatus.CREATED + task_tree.current_node.start_time = datetime.datetime.now() result = fun(*args, **kwargs) # if it succeeded set the flag - task_tree.status = TaskStatus.SUCCEEDED + task_tree.current_node.status = TaskStatus.SUCCEEDED # iff a PlanFailure occurs except PlanFailure as e: # log the error and set the flag - logging.exception("Task execution failed at %s. Reason %s" % (repr(task_tree), e)) - task_tree.reason = e - task_tree.status = TaskStatus.FAILED + logging.exception("Task execution failed at %s. Reason %s" % (repr(task_tree.current_node), e)) + task_tree.current_node.reason = e + task_tree.current_node.status = TaskStatus.FAILED raise e + finally: - # set and time and update current node pointer - task_tree.end_time = datetime.datetime.now() - task_tree = task_tree.parent + if task_tree.current_node.parent is not None: + task_tree.current_node.end_time = datetime.datetime.now() + task_tree.current_node = task_tree.current_node.parent + return result return handle_tree diff --git a/src/pycram/utils.py b/src/pycram/utils.py index 28b5109ac..30bd10538 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -7,14 +7,20 @@ GeneratorList -- implementation of generator list wrappers. """ from inspect import isgeneratorfunction -from typing_extensions import List, Tuple, Callable - import os +import math + +import numpy as np +from matplotlib import pyplot as plt +import matplotlib.colors as mcolors +from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING from .datastructures.pose import Pose -import math +from .local_transformer import LocalTransformer -from typing_extensions import Dict +if TYPE_CHECKING: + from .world_concepts.world_object import Object + from .robot_description import CameraDescription class bcolors: @@ -36,7 +42,7 @@ class bcolors: UNDERLINE = '\033[4m' -def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses: Tuple[Pose, Dict[str, float]]) -> None: +def _apply_ik(robot: 'Object', pose_and_joint_poses: Tuple[Pose, Dict[str, float]]) -> None: """ Apllies a list of joint poses calculated by an inverse kinematics solver to a robot @@ -46,7 +52,7 @@ def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses: """ pose, joint_states = pose_and_joint_poses robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) class GeneratorList: @@ -113,7 +119,7 @@ def axis_angle_to_quaternion(axis: List, angle: float) -> Tuple: z = normalized_axis[2] * math.sin(angle / 2) w = math.cos(angle / 2) - return (x, y, z, w) + return tuple((x, y, z, w)) class suppress_stdout_stderr(object): @@ -130,7 +136,7 @@ class suppress_stdout_stderr(object): def __init__(self): # Open a pair of null files - self.null_fds = [os.open(os.devnull, os.O_RDWR) for x in range(2)] + self.null_fds = [os.open(os.devnull, os.O_RDWR) for _ in range(2)] # Save the actual stdout (1) and stderr (2) file descriptors. self.save_fds = [os.dup(1), os.dup(2)] @@ -148,3 +154,257 @@ def __exit__(self, *_): # Close all file descriptors for fd in self.null_fds + self.save_fds: os.close(fd) + + +class RayTestUtils: + + def __init__(self, ray_test_batch: Callable, object_id_to_name: Dict = None): + """ + Initialize the ray test helper. + """ + self.local_transformer = LocalTransformer() + self.ray_test_batch = ray_test_batch + self.object_id_to_name = object_id_to_name + + def get_images_for_target(self, cam_pose: Pose, + camera_description: 'CameraDescription', + camera_frame: str, + size: int = 256, + camera_min_distance: float = 0.1, + camera_max_distance: int = 3, + plot: bool = False) -> List[np.ndarray]: + """ + Note: The returned color image is a repeated depth image in 3 channels. + """ + + # get the list of start positions of the rays. + rays_start_positions = self.get_camera_rays_start_positions(camera_description, camera_frame, cam_pose, size, + camera_min_distance).tolist() + + # get the list of end positions of the rays + rays_end_positions = self.get_camera_rays_end_positions(camera_description, camera_frame, cam_pose, size, + camera_max_distance).tolist() + + # apply the ray test + object_ids, distances = self.ray_test_batch(rays_start_positions, rays_end_positions, return_distance=True) + + # construct the images/masks + segmentation_mask = self.construct_segmentation_mask_from_ray_test_object_ids(object_ids, size) + depth_image = self.construct_depth_image_from_ray_test_distances(distances, size) + camera_min_distance + color_depth_image = self.construct_color_image_from_depth_image(depth_image) + + if plot: + self.plot_segmentation_mask(segmentation_mask) + self.plot_depth_image(depth_image) + + return [color_depth_image, depth_image, segmentation_mask] + + @staticmethod + def construct_segmentation_mask_from_ray_test_object_ids(object_ids: List[int], size: int) -> np.ndarray: + """ + Construct a segmentation mask from the object ids returned by the ray test. + + :param object_ids: The object ids. + :param size: The size of the grid. + :return: The segmentation mask. + """ + return np.array(object_ids).squeeze(axis=1).reshape(size, size) + + @staticmethod + def construct_depth_image_from_ray_test_distances(distances: List[float], size: int) -> np.ndarray: + """ + Construct a depth image from the distances returned by the ray test. + + :param distances: The distances. + :param size: The size of the grid. + :return: The depth image. + """ + return np.array(distances).reshape(size, size) + + @staticmethod + def construct_color_image_from_depth_image(depth_image: np.ndarray) -> np.ndarray: + """ + Construct a color image from the depth image. + + :param depth_image: The depth image. + :return: The color image. + """ + min_distance = np.min(depth_image) + max_distance = np.max(depth_image) + normalized_depth_image = (depth_image - min_distance) * 255 / (max_distance - min_distance) + return np.repeat(normalized_depth_image[:, :, np.newaxis], 3, axis=2).astype(np.uint8) + + def get_camera_rays_start_positions(self, camera_description: 'CameraDescription', camera_frame: str, + camera_pose: Pose, size: int, + camera_min_distance: float) -> np.ndarray: + + # get the start pose of the rays from the camera pose and minimum distance. + start_pose = self.get_camera_rays_start_pose(camera_description, camera_frame, camera_pose, camera_min_distance) + + # get the list of start positions of the rays. + return np.repeat(np.array([start_pose.position_as_list()]), size * size, axis=0) + + def get_camera_rays_start_pose(self, camera_description: 'CameraDescription', camera_frame: str, camera_pose: Pose, + camera_min_distance: float) -> Pose: + """ + Get the start position of the camera rays, which is the camera pose shifted by the minimum distance of the + camera. + + :param camera_description: The camera description. + :param camera_frame: The camera tf frame. + :param camera_pose: The camera pose. + :param camera_min_distance: The minimum distance from which the camera can see. + """ + camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame) + start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance + + np.array(camera_pose_in_camera_frame.position_as_list())) + start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), camera_frame) + return self.local_transformer.transform_pose(start_pose, "map") + + def get_camera_rays_end_positions(self, camera_description: 'CameraDescription', camera_frame: str, + camera_pose: Pose, size: int, camera_max_distance: float = 3.0) -> np.ndarray: + """ + Get the end positions of the camera rays. + + :param camera_description: The camera description. + :param camera_frame: The camera frame. + :param camera_pose: The camera pose. + :param size: The size of the grid. + :param camera_max_distance: The maximum distance of the camera. + :return: The end positions of the camera rays. + """ + rays_horizontal_angles, rays_vertical_angles = self.construct_grid_of_camera_rays_angles(camera_description, + size) + rays_end_positions = self.get_end_positions_of_rays_from_angles_and_distance(rays_vertical_angles, + rays_horizontal_angles, + camera_max_distance) + return self.transform_points_from_camera_frame_to_world_frame(camera_pose, camera_frame, rays_end_positions) + + @staticmethod + def transform_points_from_camera_frame_to_world_frame(camera_pose: Pose, camera_frame: str, + points: np.ndarray) -> np.ndarray: + """ + Transform points from the camera frame to the world frame. + + :param camera_pose: The camera pose. + :param camera_frame: The camera frame. + :param points: The points to transform. + :return: The transformed points. + """ + cam_to_world_transform = camera_pose.to_transform(camera_frame) + return cam_to_world_transform.apply_transform_to_array_of_points(points) + + @staticmethod + def get_end_positions_of_rays_from_angles_and_distance(vertical_angles: np.ndarray, horizontal_angles: np.ndarray, + distance: float) -> np.ndarray: + """ + Get the end positions of the rays from the angles and the distance. + + :param vertical_angles: The vertical angles of the rays. + :param horizontal_angles: The horizontal angles of the rays. + :param distance: The distance of the rays. + :return: The end positions of the rays. + """ + rays_end_positions_x = distance * np.cos(vertical_angles) * np.sin(horizontal_angles) + rays_end_positions_x = rays_end_positions_x.reshape(-1) + rays_end_positions_z = distance * np.cos(vertical_angles) * np.cos(horizontal_angles) + rays_end_positions_z = rays_end_positions_z.reshape(-1) + rays_end_positions_y = distance * np.sin(vertical_angles) + rays_end_positions_y = rays_end_positions_y.reshape(-1) + return np.stack((rays_end_positions_x, rays_end_positions_y, rays_end_positions_z), axis=1) + + @staticmethod + def construct_grid_of_camera_rays_angles(camera_description: 'CameraDescription', + size: int) -> Tuple[np.ndarray, np.ndarray]: + """ + Construct a 2D grid of camera rays angles. + + :param camera_description: The camera description. + :param size: The size of the grid. + :return: The 2D grid of the horizontal and the vertical angles of the camera rays. + """ + # get the camera fov angles + camera_horizontal_fov = camera_description.horizontal_angle + camera_vertical_fov = camera_description.vertical_angle + + # construct a 2d grid of rays angles + rays_horizontal_angles = np.linspace(-camera_horizontal_fov / 2, camera_horizontal_fov / 2, size) + rays_horizontal_angles = np.tile(rays_horizontal_angles, (size, 1)) + rays_vertical_angles = np.linspace(-camera_vertical_fov / 2, camera_vertical_fov / 2, size) + rays_vertical_angles = np.tile(rays_vertical_angles, (size, 1)).T + return rays_horizontal_angles, rays_vertical_angles + + @staticmethod + def plot_segmentation_mask(segmentation_mask, + object_id_to_name: Dict[int, str] = None): + """ + Plot the segmentation mask with different colors for each object. + + :param segmentation_mask: The segmentation mask. + :param object_id_to_name: The mapping from object id to object name. + """ + if object_id_to_name is None: + object_id_to_name = {uid: str(uid) for uid in np.unique(segmentation_mask)} + + # Create a custom color map + unique_ids = np.unique(segmentation_mask) + unique_ids = unique_ids[unique_ids != -1] # Exclude -1 values + + # Create a color map that assigns a unique color to each ID + colors = plt.cm.get_cmap('tab20', len(unique_ids)) # Use tab20 colormap for distinct colors + color_dict = {uid: colors(i) for i, uid in enumerate(unique_ids)} + + # Map each ID to its corresponding color + mask_shape = segmentation_mask.shape + segmentation_colored = np.zeros((mask_shape[0], mask_shape[1], 3)) + + for uid in unique_ids: + segmentation_colored[segmentation_mask == uid] = color_dict[uid][:3] # Ignore the alpha channel + + # Create a colormap for the color bar + cmap = mcolors.ListedColormap([color_dict[uid][:3] for uid in unique_ids]) + norm = mcolors.BoundaryNorm(boundaries=np.arange(len(unique_ids) + 1) - 0.5, ncolors=len(unique_ids)) + + # Plot the colored segmentation mask + fig, ax = plt.subplots() + _ = ax.imshow(segmentation_colored) + ax.axis('off') # Hide axes + ax.set_title('Segmentation Mask with Different Colors for Each Object') + + # Create color bar + cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), ax=ax, ticks=np.arange(len(unique_ids))) + cbar.ax.set_yticklabels( + [object_id_to_name[uid] for uid in unique_ids]) # Label the color bar with object IDs + cbar.set_label('Object Name') + + plt.show() + + @staticmethod + def plot_depth_image(depth_image): + # Plot the depth image + fig, ax = plt.subplots() + cax = ax.imshow(depth_image, cmap='viridis', vmin=0, vmax=np.max(depth_image)) + ax.axis('off') # Hide axes + ax.set_title('Depth Image') + + # Create color bar + cbar = fig.colorbar(cax, ax=ax) + cbar.set_label('Depth Value') + + plt.show() + + +def wxyz_to_xyzw(wxyz: List[float]) -> List[float]: + """ + Convert a quaternion from WXYZ to XYZW format. + """ + return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] + + +def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: + """ + Convert a quaternion from XYZW to WXYZ format. + + :param xyzw: The quaternion in XYZW format. + """ + return [xyzw[3], *xyzw[:3]] \ No newline at end of file diff --git a/src/pycram/validation/__init__.py b/src/pycram/validation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/validation/error_checkers.py b/src/pycram/validation/error_checkers.py new file mode 100644 index 000000000..f594b6bd6 --- /dev/null +++ b/src/pycram/validation/error_checkers.py @@ -0,0 +1,351 @@ +from abc import ABC, abstractmethod +from collections.abc import Iterable + +import numpy as np +from tf.transformations import quaternion_multiply, quaternion_inverse +from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable, TYPE_CHECKING, Tuple + +from ..datastructures.enums import JointType +if TYPE_CHECKING: + from ..datastructures.pose import Pose + + +class ErrorChecker(ABC): + """ + An abstract class that resembles an error checker. It has two main methods, one for calculating the error between + two values and another for checking if the error is acceptable. + """ + def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterable: Optional[bool] = False): + """ + Initialize the error checker. + + :param acceptable_error: The acceptable error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + self._acceptable_error: np.ndarray = np.array(acceptable_error) + self.tiled_acceptable_error: Optional[np.ndarray] = None + self.is_iterable = is_iterable + + def reset(self) -> None: + """ + Reset the error checker. + """ + self.tiled_acceptable_error = None + + @property + def acceptable_error(self) -> np.ndarray: + return self._acceptable_error + + @acceptable_error.setter + def acceptable_error(self, new_acceptable_error: Union[float, T_Iterable[float]]) -> None: + self._acceptable_error = np.array(new_acceptable_error) + + def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[float]] = None, + tile_to_match: Optional[Sized] = None,) -> None: + """ + Update the acceptable error with a new value, and tile it to match the length of the error if needed. + + :param new_acceptable_error: The new acceptable error. + :param tile_to_match: The iterable to match the length of the error with. + """ + if new_acceptable_error is not None: + self.acceptable_error = new_acceptable_error + if tile_to_match is not None and self.is_iterable: + self.update_tiled_acceptable_error(tile_to_match) + + def update_tiled_acceptable_error(self, tile_to_match: Sized) -> None: + """ + Tile the acceptable error to match the length of the error. + + :param tile_to_match: The object to match the length of the error. + :return: The tiled acceptable error. + """ + self.tiled_acceptable_error = np.tile(self.acceptable_error.flatten(), + len(tile_to_match) // self.acceptable_error.size) + + @abstractmethod + def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + pass + + def calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + if self.is_iterable: + return [self._calculate_error(v1, v2) for v1, v2 in zip(value_1, value_2)] + else: + return self._calculate_error(value_1, value_2) + + def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: + """ + Check if the error is acceptable. + + :param value_1: The first value. + :param value_2: The second value. + :return: Whether the error is acceptable. + """ + error = self.calculate_error(value_1, value_2) + if self.is_iterable: + error = np.array(error).flatten() + if self.tiled_acceptable_error is None or\ + len(error) != len(self.tiled_acceptable_error): + self.update_tiled_acceptable_error(error) + return np.all(error <= self.tiled_acceptable_error) + else: + return is_error_acceptable(error, self.acceptable_error) + + +class PoseErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Union[Tuple[float], T_Iterable[Tuple[float]]] = (1e-3, np.pi / 180), + is_iterable: Optional[bool] = False): + """ + Initialize the pose error checker. + + :param acceptable_error: The acceptable pose error (position error, orientation error). + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> List[float]: + """ + Calculate the error between two poses. + + :param value_1: The first pose. + :param value_2: The second pose. + """ + return calculate_pose_error(value_1, value_2) + + +class PositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the position error checker. + + :param acceptable_error: The acceptable position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two positions. + + :param value_1: The first position. + :param value_2: The second position. + :return: The error between the two positions. + """ + return calculate_position_error(value_1, value_2) + + +class OrientationErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + """ + Initialize the orientation error checker. + + :param acceptable_error: The acceptable orientation error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two quaternions. + + :param value_1: The first quaternion. + :param value_2: The second quaternion. + :return: The error between the two quaternions. + """ + return calculate_orientation_error(value_1, value_2) + + +class SingleValueErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the single value error checker. + + :param acceptable_error: The acceptable error between two values. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + return abs(value_1 - value_2) + + +class RevoluteJointPositionErrorChecker(SingleValueErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + """ + Initialize the revolute joint position error checker. + + :param acceptable_error: The acceptable revolute joint position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + +class PrismaticJointPositionErrorChecker(SingleValueErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the prismatic joint position error checker. + + :param acceptable_error: The acceptable prismatic joint position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + super().__init__(acceptable_error, is_iterable) + + +class IterableErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[T_Iterable[float]] = None): + """ + Initialize the iterable error checker. + + :param acceptable_error: The acceptable error between two values. + """ + super().__init__(acceptable_error, True) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + return abs(value_1 - value_2) + + +class MultiJointPositionErrorChecker(IterableErrorChecker): + + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): + """ + Initialize the multi-joint position error checker. + + :param joint_types: The types of the joints. + :param acceptable_error: The acceptable error between two joint positions. + """ + self.joint_types = joint_types + if acceptable_error is None: + acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] + super().__init__(acceptable_error) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + + :param value_1: The first joint position. + :param value_2: The second joint position. + :return: The error between the two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +def calculate_pose_error(pose_1: 'Pose', pose_2: 'Pose') -> List[float]: + """ + Calculate the error between two poses. + + :param pose_1: The first pose. + :param pose_2: The second pose. + :return: The error between the two poses. + """ + return [calculate_position_error(pose_1.position_as_list(), pose_2.position_as_list()), + calculate_orientation_error(pose_1.orientation_as_list(), pose_2.orientation_as_list())] + + +def calculate_position_error(position_1: List[float], position_2: List[float]) -> float: + """ + Calculate the error between two positions. + + :param position_1: The first position. + :param position_2: The second position. + :return: The error between the two positions. + """ + return np.linalg.norm(np.array(position_1) - np.array(position_2)) + + +def calculate_orientation_error(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculate the error between two quaternions. + + :param quat_1: The first quaternion. + :param quat_2: The second quaternion. + :return: The error between the two quaternions. + """ + return calculate_angle_between_quaternions(quat_1, quat_2) + + +def calculate_joint_position_error(joint_position_1: float, joint_position_2: float) -> float: + """ + Calculate the error between two joint positions. + + :param joint_position_1: The first joint position. + :param joint_position_2: The second joint position. + :return: The error between the two joint positions. + """ + return abs(joint_position_1 - joint_position_2) + + +def is_error_acceptable(error: Union[float, T_Iterable[float]], + acceptable_error: Union[float, T_Iterable[float]]) -> bool: + """ + Check if the error is acceptable. + + :param error: The error. + :param acceptable_error: The acceptable error. + :return: Whether the error is acceptable. + """ + if isinstance(error, Iterable): + return all([error_i <= acceptable_error_i for error_i, acceptable_error_i in zip(error, acceptable_error)]) + else: + return error <= acceptable_error + + +def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculates the angle between two quaternions. + + :param quat_1: The first quaternion. + :param quat_2: The second quaternion. + :return: A float value that represents the angle between the two quaternions. + """ + quat_diff = calculate_quaternion_difference(quat_1, quat_2) + quat_diff_angle = 2 * np.arctan2(np.linalg.norm(quat_diff[0:3]), quat_diff[3]) + if quat_diff_angle > np.pi: + quat_diff_angle = 2 * np.pi - quat_diff_angle + return quat_diff_angle + + +def calculate_quaternion_difference(quat_1: List[float], quat_2: List[float]) -> List[float]: + """ + Calculates the quaternion difference. + + :param quat_1: The quaternion of the object at the first time step. + :param quat_2: The quaternion of the object at the second time step. + :return: A list of float values that represent the quaternion difference. + """ + quat_diff = quaternion_multiply(quaternion_inverse(quat_1), quat_2) + return quat_diff diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py new file mode 100644 index 000000000..66756ae52 --- /dev/null +++ b/src/pycram/validation/goal_validator.py @@ -0,0 +1,550 @@ +from time import sleep, time + +import numpy as np +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING, Tuple + +from ..datastructures.enums import JointType +from .error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, SingleValueErrorChecker +from ..ros.logging import logerr, logwarn + +if TYPE_CHECKING: + from ..datastructures.world import World + from ..world_concepts.world_object import Object + from ..datastructures.pose import Pose + from ..description import ObjectDescription + + Joint = ObjectDescription.Joint + Link = ObjectDescription.Link + +OptionalArgCallable = Union[Callable[[], Any], Callable[[Any], Any]] + + +class GoalValidator: + """ + A class to validate the goal by tracking the goal achievement progress. + """ + + raise_error: Optional[bool] = False + """ + Whether to raise an error if the goal is not achieved. + """ + + def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalArgCallable, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the goal validator. + + :param error_checker: The error checker. + :param current_value_getter: The current value getter function which takes an optional input and returns the + current value. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be + used to check if this percentage is achieved instead of the complete goal. + """ + self.error_checker: ErrorChecker = error_checker + self.current_value_getter: Callable[[Optional[Any]], Any] = current_value_getter + self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved + self.goal_value: Optional[Any] = None + self.initial_error: Optional[np.ndarray] = None + self.current_value_getter_input: Optional[Any] = None + + def register_goal_and_wait_until_achieved(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None, + max_wait_time: Optional[float] = 1, + time_per_read: Optional[float] = 0.01) -> None: + """ + Register the goal value and wait until the target is reached. + + :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + self.wait_until_goal_is_achieved(max_wait_time, time_per_read) + + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, + time_per_read: Optional[float] = 0.01) -> None: + """ + Wait until the target is reached. + + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + if self.goal_value is None: + return # Skip if goal value is None + start_time = time() + current = self.current_value + while not self.goal_achieved: + sleep(time_per_read) + if time() - start_time > max_wait_time: + msg = f"Failed to achieve goal from initial error {self.initial_error} with" \ + f" goal {self.goal_value} within {max_wait_time}" \ + f" seconds, the current value is {current}, error is {self.current_error}, percentage" \ + f" of goal achieved is {self.percentage_of_goal_achieved}" + if self.raise_error: + logerr(msg) + raise TimeoutError(msg) + else: + logwarn(msg) + break + current = self.current_value + self.reset() + + def reset(self) -> None: + """ + Reset the goal validator. + """ + self.goal_value = None + self.initial_error = None + self.current_value_getter_input = None + self.error_checker.reset() + + @property + def _acceptable_error(self) -> np.ndarray: + """ + The acceptable error. + """ + if self.error_checker.is_iterable: + return self.tiled_acceptable_error + else: + return self.acceptable_error + + @property + def acceptable_error(self) -> np.ndarray: + """ + The acceptable error. + """ + return self.error_checker.acceptable_error + + @property + def tiled_acceptable_error(self) -> Optional[np.ndarray]: + """ + The tiled acceptable error. + """ + return self.error_checker.tiled_acceptable_error + + def register_goal(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None): + """ + Register the goal value. + + :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + if goal_value is None or (hasattr(goal_value, '__len__') and len(goal_value) == 0): + return # Skip if goal value is None or empty + self.goal_value = goal_value + self.current_value_getter_input = current_value_getter_input + self.update_initial_error(goal_value, initial_value=initial_value) + self.error_checker.update_acceptable_error(acceptable_error, self.initial_error) + + def update_initial_error(self, goal_value: Any, initial_value: Optional[Any] = None) -> None: + """ + Calculate the initial error. + + :param goal_value: The goal value. + :param initial_value: The initial value. + """ + if initial_value is None: + self.initial_error: np.ndarray = self.current_error + else: + self.initial_error: np.ndarray = self.calculate_error(goal_value, initial_value) + + @property + def current_value(self) -> Any: + """ + The current value of the monitored variable. + """ + if self.current_value_getter_input is not None: + return self.current_value_getter(self.current_value_getter_input) + else: + return self.current_value_getter() + + @property + def current_error(self) -> np.ndarray: + """ + The current error. + """ + return self.calculate_error(self.goal_value, self.current_value) + + def calculate_error(self, value_1: Any, value_2: Any) -> np.ndarray: + """ + Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error. + """ + return np.array(self.error_checker.calculate_error(value_1, value_2)).flatten() + + @property + def percentage_of_goal_achieved(self) -> float: + """ + The relative (relative to the acceptable error) achieved percentage of goal. + """ + percent_array = 1 - self.relative_current_error / self.relative_initial_error + percent_array_filtered = percent_array[self.relative_initial_error > self._acceptable_error] + if len(percent_array_filtered) == 0: + return 1 + else: + return np.mean(percent_array_filtered) + + @property + def actual_percentage_of_goal_achieved(self) -> float: + """ + The percentage of goal achieved. + """ + percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) + percent_array_filtered = percent_array[self.initial_error > self._acceptable_error] + if len(percent_array_filtered) == 0: + return 1 + else: + return np.mean(percent_array_filtered) + + @property + def relative_current_error(self) -> np.ndarray: + """ + The relative current error (relative to the acceptable error). + """ + return self.get_relative_error(self.current_error, threshold=0) + + @property + def relative_initial_error(self) -> np.ndarray: + """ + The relative initial error (relative to the acceptable error). + """ + return np.maximum(self.initial_error, 1e-3) + + def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: + """ + Get the relative error by comparing the error with the acceptable error and filtering out the errors that are + less than the threshold. + + :param error: The error. + :param threshold: The threshold. + :return: The relative error. + """ + return np.maximum(error - self._acceptable_error, threshold) + + @property + def goal_achieved(self) -> bool: + """ + Check if the goal is achieved. + """ + if self.acceptable_percentage_of_goal_achieved is None: + return self.is_current_error_acceptable + else: + return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved + + @property + def is_current_error_acceptable(self) -> bool: + """ + Check if the error is acceptable. + """ + return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) + + +class PoseGoalValidator(GoalValidator): + """ + A class to validate the pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_pose_getter: OptionalArgCallable = None, + acceptable_error: Union[Tuple[float], Iterable[Tuple[float]]] = (1e-3, np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the pose goal validator. + + :param current_pose_getter: The current pose getter function which takes an optional input and returns the + current pose. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(PoseErrorChecker(acceptable_error, is_iterable=is_iterable), current_pose_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPoseGoalValidator(PoseGoalValidator): + """ + A class to validate the multi-pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_poses_getter: OptionalArgCallable = None, + acceptable_error: Union[Tuple[float], Iterable[Tuple[float]]] = (1e-2, 5 * np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-pose goal validator. + + :param current_poses_getter: The current poses getter function which takes an optional input and returns the + current poses. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_poses_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class PositionGoalValidator(GoalValidator): + """ + A class to validate the position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the position goal validator. + + :param current_position_getter: The current position getter function which takes an optional input and + returns the current position. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of position vectors. + """ + super().__init__(PositionErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPositionGoalValidator(PositionGoalValidator): + """ + A class to validate the multi-position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-position goal validator. + + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_positions_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class OrientationGoalValidator(GoalValidator): + """ + A class to validate the orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientation_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the orientation goal validator. + + :param current_orientation_getter: The current orientation getter function which takes an optional input and + returns the current orientation. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of quaternions. + """ + super().__init__(OrientationErrorChecker(acceptable_error, is_iterable=is_iterable), current_orientation_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiOrientationGoalValidator(OrientationGoalValidator): + """ + A class to validate the multi-orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientations_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-orientation goal validator. + + :param current_orientations_getter: The current orientations getter function which takes an optional input and + returns the current orientations. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_orientations_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class JointPositionGoalValidator(GoalValidator): + """ + A class to validate the joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = None, + acceptable_revolute_joint_position_error: float = np.pi / 180, + acceptable_prismatic_joint_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8, + is_iterable: bool = False): + """ + Initialize the joint position goal validator. + + :param current_position_getter: The current position getter function which takes an optional input and returns + the current position. + :param acceptable_error: The acceptable error. + :param acceptable_revolute_joint_position_error: The acceptable orientation error. + :param acceptable_prismatic_joint_position_error: The acceptable position error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of joint positions. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_revolute_joint_position_error + self.acceptable_position_error = acceptable_prismatic_joint_position_error + + def register_goal(self, goal_value: Any, joint_type: JointType, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[float] = None): + """ + Register the goal value. + + :param goal_value: The goal value. + :param joint_type: The joint type (e.g. REVOLUTE, PRISMATIC). + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + if acceptable_error is None: + self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE\ + else self.acceptable_position_error + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +class MultiJointPositionGoalValidator(GoalValidator): + """ + A class to validate the multi-joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[Iterable[float]] = None, + acceptable_revolute_joint_position_error: float = np.pi / 180, + acceptable_prismatic_joint_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8): + """ + Initialize the multi-joint position goal validator. + + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_revolute_joint_position_error: The acceptable orientation error. + :param acceptable_prismatic_joint_position_error: The acceptable position error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, + acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_revolute_joint_position_error + self.acceptable_position_error = acceptable_prismatic_joint_position_error + + def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Iterable[float]] = None): + if acceptable_error is None: + self.error_checker.acceptable_error = [self.acceptable_orientation_error if jt == JointType.REVOLUTE + else self.acceptable_position_error for jt in joint_type] + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +def validate_object_pose(pose_setter_func): + """ + A decorator to validate the object pose. + + :param pose_setter_func: The function to set the pose of the object. + """ + + def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + + world.pose_goal_validator.register_goal(pose, obj) + + if not pose_setter_func(world, obj, pose): + world.pose_goal_validator.reset() + return False + + world.pose_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_multiple_object_poses(pose_setter_func): + """ + A decorator to validate multiple object poses. + + :param pose_setter_func: The function to set multiple poses of the objects. + """ + + def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']): + + world.multi_pose_goal_validator.register_goal(list(object_poses.values()), + list(object_poses.keys())) + + if not pose_setter_func(world, object_poses): + world.multi_pose_goal_validator.reset() + return False + + world.multi_pose_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_joint_position(position_setter_func): + """ + A decorator to validate the joint position. + + :param position_setter_func: The function to set the joint position. + """ + + def wrapper(world: 'World', joint: 'Joint', position: float): + + joint_type = joint.type + world.joint_position_goal_validator.register_goal(position, joint_type, joint) + + if not position_setter_func(world, joint, position): + world.joint_position_goal_validator.reset() + return False + + world.joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_multiple_joint_positions(position_setter_func): + """ + A decorator to validate the joint positions, this function does not validate the virtual joints, + as in multiverse the virtual joints take command velocities and not positions, so after their goals + are set, they are zeroed thus can't be validated. (They are actually validated by the robot pose in case + of virtual mobile base joints) + + :param position_setter_func: The function to set the joint positions. + """ + + def wrapper(world: 'World', joint_positions: Dict['Joint', float]): + joint_positions_to_validate = {joint: position for joint, position in joint_positions.items() + if not joint.is_virtual} + joint_types = [joint.type for joint in joint_positions_to_validate.keys()] + world.multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types, + list(joint_positions_to_validate.keys())) + if not position_setter_func(world, joint_positions): + world.multi_joint_position_goal_validator.reset() + return False + + world.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index aa41e3bdf..8de36bda1 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -2,7 +2,7 @@ import numpy as np from geometry_msgs.msg import Point -from typing_extensions import Union, List, Optional, TYPE_CHECKING +from typing_extensions import Union, List, Optional, TYPE_CHECKING, Self from ..datastructures.enums import JointType from ..datastructures.pose import Transform, Pose @@ -30,6 +30,44 @@ def __init__(self, self.child_to_constraint = child_to_constraint self._parent_to_child = None + def get_child_object_pose(self) -> Pose: + """ + :return: The pose of the child object. + """ + return self.child_link.object.pose + + def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: + """ + Get the pose of the child object given the parent pose. + + :param pose: The parent object pose. + :return: The pose of the child object. + """ + pose = self.parent_link.get_pose_given_object_pose(pose) + child_link_pose = self.get_child_link_target_pose_given_parent(pose) + return self.child_link.get_object_pose_given_link_pose(child_link_pose) + + def set_child_link_pose(self): + """ + Set the target pose of the child object to the current pose of the child object in the parent object frame. + """ + self.child_link.set_pose(self.get_child_link_target_pose()) + + def get_child_link_target_pose(self) -> Pose: + """ + :return: The target pose of the child object. (The pose of the child object in the parent object frame) + """ + return self.parent_to_child_transform.to_pose() + + def get_child_link_target_pose_given_parent(self, parent_pose: Pose) -> Pose: + """ + Get the target pose of the child object link given the parent link pose. + + :param parent_pose: The parent link pose. + :return: The target pose of the child object link. + """ + return (parent_pose.to_transform(self.parent_link.tf_frame) * self.parent_to_child_transform).to_pose() + @property def parent_to_child_transform(self) -> Union[Transform, None]: if self._parent_to_child is None: @@ -44,8 +82,6 @@ def parent_to_child_transform(self, transform: Transform) -> None: @property def parent_object_id(self) -> int: """ - Returns the id of the parent object of the constraint. - :return: The id of the parent object of the constraint """ return self.parent_link.object_id @@ -53,8 +89,6 @@ def parent_object_id(self) -> int: @property def child_object_id(self) -> int: """ - Returns the id of the child object of the constraint. - :return: The id of the child object of the constraint """ return self.child_link.object_id @@ -62,8 +96,6 @@ def child_object_id(self) -> int: @property def parent_link_id(self) -> int: """ - Returns the id of the parent link of the constraint. - :return: The id of the parent link of the constraint """ return self.parent_link.id @@ -71,8 +103,6 @@ def parent_link_id(self) -> int: @property def child_link_id(self) -> int: """ - Returns the id of the child link of the constraint. - :return: The id of the child link of the constraint """ return self.child_link.id @@ -80,8 +110,6 @@ def child_link_id(self) -> int: @property def position_wrt_parent_as_list(self) -> List[float]: """ - Returns the constraint frame pose with respect to the parent origin as a list. - :return: The constraint frame pose with respect to the parent origin as a list """ return self.pose_wrt_parent.position_as_list() @@ -89,8 +117,6 @@ def position_wrt_parent_as_list(self) -> List[float]: @property def orientation_wrt_parent_as_list(self) -> List[float]: """ - Returns the constraint frame orientation with respect to the parent origin as a list. - :return: The constraint frame orientation with respect to the parent origin as a list """ return self.pose_wrt_parent.orientation_as_list() @@ -98,8 +124,6 @@ def orientation_wrt_parent_as_list(self) -> List[float]: @property def pose_wrt_parent(self) -> Pose: """ - Returns the joint frame pose with respect to the parent origin. - :return: The joint frame pose with respect to the parent origin """ return self.parent_to_constraint.to_pose() @@ -107,8 +131,6 @@ def pose_wrt_parent(self) -> Pose: @property def position_wrt_child_as_list(self) -> List[float]: """ - Returns the constraint frame pose with respect to the child origin as a list. - :return: The constraint frame pose with respect to the child origin as a list """ return self.pose_wrt_child.position_as_list() @@ -116,8 +138,6 @@ def position_wrt_child_as_list(self) -> List[float]: @property def orientation_wrt_child_as_list(self) -> List[float]: """ - Returns the constraint frame orientation with respect to the child origin as a list. - :return: The constraint frame orientation with respect to the child origin as a list """ return self.pose_wrt_child.orientation_as_list() @@ -125,8 +145,6 @@ def orientation_wrt_child_as_list(self) -> List[float]: @property def pose_wrt_child(self) -> Pose: """ - Returns the joint frame pose with respect to the child origin. - :return: The joint frame pose with respect to the child origin """ return self.child_to_constraint.to_pose() @@ -151,8 +169,6 @@ def __init__(self, @property def axis_as_list(self) -> List[float]: """ - Returns the axis of this constraint as a list. - :return: The axis of this constraint as a list of xyz """ return [self.axis.x, self.axis.y, self.axis.z] @@ -162,9 +178,10 @@ class Attachment(AbstractConstraint): def __init__(self, parent_link: Link, child_link: Link, - bidirectional: Optional[bool] = False, + bidirectional: bool = False, parent_to_child_transform: Optional[Transform] = None, - constraint_id: Optional[int] = None): + constraint_id: Optional[int] = None, + is_inverse: bool = False): """ Creates an attachment between the parent object link and the child object link. This could be a bidirectional attachment, meaning that both objects will move when one moves. @@ -180,48 +197,61 @@ def __init__(self, self.id = constraint_id self.bidirectional: bool = bidirectional self._loose: bool = False + self.is_inverse: bool = is_inverse - if self.parent_to_child_transform is None: + if parent_to_child_transform is not None: + self.parent_to_child_transform = parent_to_child_transform + + elif self.parent_to_child_transform is None: self.update_transform() if self.id is None: self.add_fixed_constraint() + @property + def parent_object(self): + return self.parent_link.object + + @property + def child_object(self): + return self.child_link.object + def update_transform_and_constraint(self) -> None: """ - Updates the transform and constraint of this attachment. + Update the transform and constraint of this attachment. """ self.update_transform() self.update_constraint() def update_transform(self) -> None: """ - Updates the transform of this attachment by calculating the transform from the parent link to the child link. + Update the transform of this attachment by calculating the transform from the parent link to the child link. """ self.parent_to_child_transform = self.calculate_transform() def update_constraint(self) -> None: """ - Updates the constraint of this attachment by removing the old constraint if one exists and adding a new one. + Update the constraint of this attachment by removing the old constraint if one exists and adding a new one. """ self.remove_constraint_if_exists() self.add_fixed_constraint() def add_fixed_constraint(self) -> None: """ - Adds a fixed constraint between the parent link and the child link. + Add a fixed constraint between the parent link and the child link. """ - self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link) + self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, + self.parent_to_child_transform.invert()) def calculate_transform(self) -> Transform: """ - Calculates the transform from the parent link to the child link. + Calculate the transform from the parent link to the child link. """ return self.parent_link.get_transform_to_link(self.child_link) def remove_constraint_if_exists(self) -> None: """ - Removes the constraint between the parent and the child links if one exists. + Remove the constraint between the parent and the child links if one exists. """ if self.child_link in self.parent_link.constraint_ids: self.parent_link.remove_constraint_with_link(self.child_link) @@ -231,7 +261,7 @@ def get_inverse(self) -> 'Attachment': :return: A new Attachment object with the parent and child links swapped. """ attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, - constraint_id=self.id) + constraint_id=self.id, is_inverse=not self.is_inverse) attachment.loose = not self._loose return attachment @@ -245,22 +275,15 @@ def loose(self) -> bool: @loose.setter def loose(self, loose: bool) -> None: """ - Sets the loose property of this attachment. + Set the loose property of this attachment. :param loose: If true, then the child object will not move when parent moves. """ self._loose = loose and not self.bidirectional - @property - def is_reversed(self) -> bool: - """ - :return: True if the parent and child links are swapped. - """ - return self.loose - def __del__(self) -> None: """ - Removes the constraint between the parent and the child links if one exists when the attachment is deleted. + Remove the constraint between the parent and the child links if one exists when the attachment is deleted. """ self.remove_constraint_if_exists() @@ -272,10 +295,11 @@ def __eq__(self, other): return (self.parent_link.name == other.parent_link.name and self.child_link.name == other.child_link.name and self.bidirectional == other.bidirectional + and self.loose == other.loose and np.allclose(self.parent_to_child_transform.translation_as_list(), - other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-4) + other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-3) and np.allclose(self.parent_to_child_transform.rotation_as_list(), - other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-4)) + other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-3)) def __hash__(self): return hash((self.parent_link.name, self.child_link.name, self.bidirectional, self.parent_to_child_transform)) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 19b04e3fa..3c93d4272 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -2,23 +2,34 @@ import logging import os +from pathlib import Path import numpy as np -import rospy +from deprecated import deprecated from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint -from ..object_descriptors.urdf import ObjectDescription as URDFObject -from ..robot_descriptions import robot_description -from ..datastructures.world import WorldEntity, World -from ..world_concepts.constraints import Attachment from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape) + AxisAlignedBoundingBox, VisualShape, ClosestPointsList, + ContactPointsList) from ..datastructures.enums import ObjectType, JointType -from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose, Transform -from ..robot_description import RobotDescriptionManager +from ..datastructures.world import World +from ..datastructures.world_entity import WorldEntity +from ..description import ObjectDescription, LinkDescription, Joint +from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ + ObjectDescriptionUndefined +from ..local_transformer import LocalTransformer +from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription +from ..object_descriptors.urdf import ObjectDescription as URDF +from ..ros.logging import logwarn + +try: + from ..object_descriptors.mjcf import ObjectDescription as MJCF +except ImportError: + MJCF = None +from ..robot_description import RobotDescriptionManager, RobotDescription +from ..world_concepts.constraints import Attachment Link = ObjectDescription.Link @@ -28,18 +39,23 @@ class Object(WorldEntity): Represents a spawned Object in the World. """ - prospection_world_prefix: str = "prospection/" + tf_prospection_world_prefix: str = "prospection/" """ - The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source - file. + The prefix for the tf frame of objects in the prospection world. """ - def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Optional[Type[ObjectDescription]] = URDFObject, + extension_to_description_type: Dict[str, Type[ObjectDescription]] = {URDF.get_file_extension(): URDF} + """ + A dictionary that maps the file extension to the corresponding ObjectDescription type. + """ + + def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, + description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, - color: Optional[Color] = Color(), - ignore_cached_files: Optional[bool] = False): + color: Color = Color(), + ignore_cached_files: bool = False, + scale_mesh: Optional[float] = None): """ The constructor loads the description file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -48,37 +64,45 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param name: The name of the object :param obj_type: The type of the object as an ObjectType enum. - :param path: The path to the source file, if only a filename is provided then the resources directories will be searched. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched, it could be None in some cases when for example it is a generic object. :param description: The ObjectDescription of the object, this contains the joints and links of the object. :param pose: The pose at which the Object should be spawned - :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used. + :param world: The World in which the object should be spawned, if no world is specified the + :py:attr:`~World.current_world` will be used. :param color: The rgba_color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. + :param scale_mesh: The scale of the mesh. """ - super().__init__(-1, world) + super().__init__(-1, world if world is not None else World.current_world) + + pose = Pose() if pose is None else pose - if pose is None: - pose = Pose() - if name in [obj.name for obj in self.world.objects]: - rospy.logerr(f"An object with the name {name} already exists in the world.") - return None self.name: str = name + self.path: Optional[str] = path self.obj_type: ObjectType = obj_type self.color: Color = color - self.description = description() + self._resolve_description(path, description) self.cache_manager = self.world.cache_manager self.local_transformer = LocalTransformer() self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose - self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) + if path is not None: + self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, + self.description, self.name, + scale_mesh=scale_mesh) - self.description.update_description_from_file(self.path) + self.description.update_description_from_file(self.path) - self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") - + f"{self.name}") + if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: + self._update_world_robot_and_description() + + self.id = self._spawn_object_and_get_id() + + self.tf_frame = (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name self._init_joint_name_and_id_map() self._init_link_name_and_id_map() @@ -88,26 +112,178 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.attachments: Dict[Object, Attachment] = {} - if not self.world.is_prospection_world: - self._add_to_world_sync_obj_queue() + self.world.add_object(self) - self.world.objects.append(self) + def _resolve_description(self, path: Optional[str] = None, description: Optional[ObjectDescription] = None) -> None: + """ + Find the correct description type of the object and initialize it and set the description of this object to it. - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - rdm = RobotDescriptionManager() - rdm.load_description(self.name) - World.robot = self + :param path: The path to the source file. + :param description: The ObjectDescription of the object. + """ + if description is not None: + self.description = description + return + if path is None: + raise ObjectDescriptionUndefined(self.name) + extension = Path(path).suffix + if extension in self.extension_to_description_type: + self.description = self.extension_to_description_type[extension]() + elif extension in ObjectDescription.mesh_extensions: + self.description = self.world.conf.default_description_type() + else: + raise UnsupportedFileExtension(self.name, path) + + def set_mobile_robot_pose(self, pose: Pose) -> None: + """ + Set the goal for the mobile base joints of a mobile robot to reach a target pose. This is used for example when + the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). + + :param pose: The target pose. + """ + goal = self.get_mobile_base_joint_goal(pose) + self.set_multiple_joint_positions(goal) + + def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + """ + Get the goal for the mobile base joints of a mobile robot to reach a target pose. + + :param pose: The target pose. + :return: The goal for the mobile base joints. + """ + target_translation, target_angle = self.get_mobile_base_pose_difference(pose) + # Get the joints of the base link + mobile_base_joints = self.world.get_robot_mobile_base_joints() + return {mobile_base_joints.translation_x: target_translation.x, + mobile_base_joints.translation_y: target_translation.y, + mobile_base_joints.angular_z: target_angle} + + def get_mobile_base_pose_difference(self, pose: Pose) -> Tuple[Point, float]: + """ + Get the difference between the current and the target pose of the mobile base. + + :param pose: The target pose. + :return: The difference between the current and the target pose of the mobile base. + """ + return self.original_pose.get_position_diff(pose), self.original_pose.get_z_angle_difference(pose) + + @property + def joint_actuators(self) -> Optional[Dict[str, str]]: + """ + The joint actuators of the robot. + """ + if self.obj_type == ObjectType.ROBOT: + return self.robot_description.joint_actuators + return None + + @property + def has_actuators(self) -> bool: + """ + True if the object has actuators, otherwise False. + """ + return self.robot_description.has_actuators + + @property + def robot_description(self) -> RobotDescription: + """ + The current robot description. + """ + return self.world.robot_description + + def get_actuator_for_joint(self, joint: Joint) -> Optional[str]: + """ + Get the actuator name for a joint. + + :param joint: The joint object for which to get the actuator. + :return: The name of the actuator. + """ + return self.robot_description.get_actuator_for_joint(joint.name) + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of the object. + + :param links: The link objects of which to get the positions. + :return: The positions of the links. + """ + return self.world.get_multiple_link_positions(links) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of the object. + + :param links: The link objects of which to get the orientations. + :return: The orientations of the links. + """ + return self.world.get_multiple_link_orientations(links) + + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of the object. + + :param links: The link objects of which to get the poses. + :return: The poses of the links. + """ + return self.world.get_multiple_link_poses(links) + + def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: + """ + Get the poses of the attached objects. + + :return: The poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose() + for child_object, attachment in self.attachments.items() if not attachment.loose} + + def get_target_poses_of_attached_objects_given_parent(self, pose: Pose) -> Dict[Object, Pose]: + """ + Get the target poses of the attached objects of an object. Given the pose of the parent object. (i.e. the poses + to which the attached objects will move when the parent object is at the given pose) + + :param pose: The pose of the parent object. + :return: The target poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose_given_parent(pose) for child_object, attachment + in self.attachments.items() if not attachment.loose} + + @property + def name(self): + """ + The name of the object. + """ + return self._name + + @name.setter + def name(self, name: str): + """ + Set the name of the object. + """ + self._name = name + if name in [obj.name for obj in self.world.objects]: + raise ObjectAlreadyExists(self) @property def pose(self): + """ + The current pose of the object. + """ return self.get_pose() @pose.setter def pose(self, pose: Pose): + """ + Set the pose of the object. + """ self.set_pose(pose) - def _load_object_and_get_id(self, path: Optional[str] = None, - ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: + @property + def transform(self): + """ + The current transform of the object. + """ + return self.get_pose().to_transform(self.tf_frame) + + def _spawn_object_and_get_id(self) -> int: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -115,28 +291,18 @@ def _load_object_and_get_id(self, path: Optional[str] = None, and this URDf file will be loaded instead. When spawning a URDf file a new file will be created in the cache directory, if there exists none. This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. + to ROS packages instead there will be absolute file paths. - :param path: The path to the description file, if None then no file will be loaded, this is useful when the PyCRAM is not responsible for loading the file but another system is. - :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path of the file that was loaded. """ - if path is not None: - try: - path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) - except FileNotFoundError as e: - logging.error("Could not generate description from file.") - raise e + if isinstance(self.description, GenericObjectDescription): + return self.world.load_generic_object_and_get_id(self.description, pose=self._current_pose) + + path = self.path if self.world.conf.let_pycram_handle_spawning else self.name try: - simulator_object_path = path - if simulator_object_path is None: - # This is useful when the object is already loaded in the simulator so it would use its name instead of - # its path - simulator_object_path = self.name - obj_id = self.world.load_object_and_get_id(simulator_object_path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) - return obj_id, path + obj_id = self.world.load_object_and_get_id(path, self._current_pose, self.obj_type) + return obj_id except Exception as e: logging.error( @@ -145,9 +311,31 @@ def _load_object_and_get_id(self, path: Optional[str] = None, os.remove(path) raise e + def _update_world_robot_and_description(self): + """ + Initialize the robot description of the object, load the description from the RobotDescriptionManager and set + the robot as the current robot in the World. Also add the virtual mobile base joints to the robot. + """ + rdm = RobotDescriptionManager() + rdm.load_description(self.description.name) + World.robot = self + self._add_virtual_move_base_joints() + + def _add_virtual_move_base_joints(self): + """ + Add the virtual mobile base joints to the robot description. + """ + virtual_joints = self.robot_description.virtual_mobile_base_joints + if virtual_joints is None: + return + child_link = self.description.get_root() + axes = virtual_joints.get_axes() + for joint_name, joint_type in virtual_joints.get_types().items(): + self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name], is_virtual=True) + def _init_joint_name_and_id_map(self) -> None: """ - Creates a dictionary which maps the joint names to their unique ids and vice versa. + Create a dictionary which maps the joint names to their unique ids and vice versa. """ n_joints = len(self.joint_names) self.joint_name_to_id = dict(zip(self.joint_names, range(n_joints))) @@ -155,7 +343,7 @@ def _init_joint_name_and_id_map(self) -> None: def _init_link_name_and_id_map(self) -> None: """ - Creates a dictionary which maps the link names to their unique ids and vice versa. + Create a dictionary which maps the link names to their unique ids and vice versa. """ n_links = len(self.link_names) self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) @@ -164,7 +352,7 @@ def _init_link_name_and_id_map(self) -> None: def _init_links_and_update_transforms(self) -> None: """ - Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the + Initialize the link objects from the URDF file and creates a dictionary which maps the link names to the corresponding link objects. """ self.links = {} @@ -184,32 +372,54 @@ def _init_joints(self): """ self.joints = {} for joint_name, joint_id in self.joint_name_to_id.items(): - joint_description = self.description.get_joint_by_name(joint_name) - self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) + parsed_joint_description = self.description.get_joint_by_name(joint_name) + is_virtual = self.is_joint_virtual(joint_name) + self.joints[joint_name] = self.description.Joint(joint_id, parsed_joint_description, self, is_virtual) + + def is_joint_virtual(self, name: str): + """ + Check if a joint is virtual. + """ + return self.description.is_joint_virtual(name) + + @property + def virtual_joint_names(self): + """ + The names of the virtual joints. + """ + return self.description.virtual_joint_names - def _add_to_world_sync_obj_queue(self) -> None: + @property + def virtual_joints(self): """ - Adds this object to the objects queue of the WorldSync object of the World. + The virtual joints as a list. """ - self.world.world_sync.add_obj_queue.put(self) + return [joint for joint in self.joints.values() if joint.is_virtual] + + @property + def has_one_link(self) -> bool: + """ + True if the object has only one link, otherwise False. + """ + return len(self.links) == 1 @property def link_names(self) -> List[str]: """ - :return: The name of each link as a list. + The names of the links as a list. """ return self.world.get_object_link_names(self) @property def joint_names(self) -> List[str]: """ - :return: The name of each joint as a list. + The names of the joints as a list. """ return self.world.get_object_joint_names(self) def get_link(self, link_name: str) -> ObjectDescription.Link: """ - Returns the link object with the given name. + Return the link object with the given name. :param link_name: The name of the link. :return: The link object. @@ -218,7 +428,7 @@ def get_link(self, link_name: str) -> ObjectDescription.Link: def get_link_pose(self, link_name: str) -> Pose: """ - Returns the pose of the link with the given name. + Return the pose of the link with the given name. :param link_name: The name of the link. :return: The pose of the link. @@ -227,7 +437,7 @@ def get_link_pose(self, link_name: str) -> Pose: def get_link_position(self, link_name: str) -> Point: """ - Returns the position of the link with the given name. + Return the position of the link with the given name. :param link_name: The name of the link. :return: The position of the link. @@ -236,7 +446,7 @@ def get_link_position(self, link_name: str) -> Point: def get_link_position_as_list(self, link_name: str) -> List[float]: """ - Returns the position of the link with the given name. + Return the position of the link with the given name. :param link_name: The name of the link. :return: The position of the link. @@ -245,7 +455,7 @@ def get_link_position_as_list(self, link_name: str) -> List[float]: def get_link_orientation(self, link_name: str) -> Quaternion: """ - Returns the orientation of the link with the given name. + Return the orientation of the link with the given name. :param link_name: The name of the link. :return: The orientation of the link. @@ -254,7 +464,7 @@ def get_link_orientation(self, link_name: str) -> Quaternion: def get_link_orientation_as_list(self, link_name: str) -> List[float]: """ - Returns the orientation of the link with the given name. + Return the orientation of the link with the given name. :param link_name: The name of the link. :return: The orientation of the link. @@ -263,7 +473,7 @@ def get_link_orientation_as_list(self, link_name: str) -> List[float]: def get_link_tf_frame(self, link_name: str) -> str: """ - Returns the tf frame of the link with the given name. + Return the tf frame of the link with the given name. :param link_name: The name of the link. :return: The tf frame of the link. @@ -272,7 +482,7 @@ def get_link_tf_frame(self, link_name: str) -> str: def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of the link with the given name. + Return the axis aligned bounding box of the link with the given name. :param link_name: The name of the link. :return: The axis aligned bounding box of the link. @@ -281,7 +491,7 @@ def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBound def get_transform_between_links(self, from_link: str, to_link: str) -> Transform: """ - Returns the transform between two links. + Return the transform between two links. :param from_link: The name of the link from which the transform should be calculated. :param to_link: The name of the link to which the transform should be calculated. @@ -290,7 +500,7 @@ def get_transform_between_links(self, from_link: str, to_link: str) -> Transform def get_link_color(self, link_name: str) -> Color: """ - Returns the color of the link with the given name. + Return the color of the link with the given name. :param link_name: The name of the link. :return: The color of the link. @@ -299,7 +509,7 @@ def get_link_color(self, link_name: str) -> Color: def set_link_color(self, link_name: str, color: List[float]) -> None: """ - Sets the color of the link with the given name. + Set the color of the link with the given name. :param link_name: The name of the link. :param color: The new color of the link. @@ -308,7 +518,7 @@ def set_link_color(self, link_name: str, color: List[float]) -> None: def get_link_geometry(self, link_name: str) -> Union[VisualShape, None]: """ - Returns the geometry of the link with the given name. + Return the geometry of the link with the given name. :param link_name: The name of the link. :return: The geometry of the link. @@ -317,7 +527,7 @@ def get_link_geometry(self, link_name: str) -> Union[VisualShape, None]: def get_link_transform(self, link_name: str) -> Transform: """ - Returns the transform of the link with the given name. + Return the transform of the link with the given name. :param link_name: The name of the link. :return: The transform of the link. @@ -326,7 +536,7 @@ def get_link_transform(self, link_name: str) -> Transform: def get_link_origin(self, link_name: str) -> Pose: """ - Returns the origin of the link with the given name. + Return the origin of the link with the given name. :param link_name: The name of the link. :return: The origin of the link as a 'Pose'. @@ -335,7 +545,7 @@ def get_link_origin(self, link_name: str) -> Pose: def get_link_origin_transform(self, link_name: str) -> Transform: """ - Returns the origin transform of the link with the given name. + Return the origin transform of the link with the given name. :param link_name: The name of the link. :return: The origin transform of the link. @@ -358,16 +568,16 @@ def __repr__(self): def remove(self) -> None: """ - Removes this object from the World it currently resides in. + Remove this object from the World it currently resides in. For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to world remove object is done + is currently attached to. After this call world remove object to remove this Object from the simulation/world. """ self.world.remove_object(self) - def reset(self, remove_saved_states=True) -> None: + def reset(self, remove_saved_states=False) -> None: """ - Resets the Object to the state it was first spawned in. + Reset the Object to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and the object will be set to the position and orientation in which it was spawned. @@ -380,13 +590,23 @@ def reset(self, remove_saved_states=True) -> None: if remove_saved_states: self.remove_saved_states() + def has_type_environment(self) -> bool: + """ + Check if the object is of type environment. + + :return: True if the object is of type environment, False otherwise. + """ + return self.obj_type == ObjectType.ENVIRONMENT + def attach(self, child_object: Object, parent_link: Optional[str] = None, child_link: Optional[str] = None, - bidirectional: Optional[bool] = True) -> None: + bidirectional: bool = True, + coincide_the_objects: bool = False, + parent_to_child_transform: Optional[Transform] = None) -> None: """ - Attaches another object to this object. This is done by + Attach another object to this object. This is done by saving the transformation between the given link, if there is one, and the base pose of the other object. Additionally, the name of the link, to which the object is attached, will be saved. @@ -399,11 +619,15 @@ def attach(self, :param parent_link: The link name of this object. :param child_link: The link name of the other object. :param bidirectional: If the attachment should be a loose attachment. + :param coincide_the_objects: If True the object frames will be coincided. + :param parent_to_child_transform: The transform from the parent to the child object. """ parent_link = self.links[parent_link] if parent_link else self.root_link child_link = child_object.links[child_link] if child_link else child_object.root_link - attachment = Attachment(parent_link, child_link, bidirectional) + if coincide_the_objects and parent_to_child_transform is None: + parent_to_child_transform = Transform() + attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment child_object.attachments[self] = attachment.get_inverse() @@ -412,7 +636,7 @@ def attach(self, def detach(self, child_object: Object) -> None: """ - Detaches another object from this object. This is done by + Detache another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects and deleting the constraint of the simulator. Afterward the detachment event of the corresponding World will be fired. @@ -437,7 +661,7 @@ def update_attachment_with_object(self, child_object: Object): def get_position(self) -> Point: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -445,7 +669,7 @@ def get_position(self) -> Point: def get_orientation(self) -> Pose.orientation: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + Return the orientation of this object as a list of xyzw, representing a quaternion. :return: A list of xyzw """ @@ -453,7 +677,7 @@ def get_orientation(self) -> Pose.orientation: def get_position_as_list(self) -> List[float]: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -461,7 +685,7 @@ def get_position_as_list(self) -> List[float]: def get_base_position_as_list(self) -> List[float]: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -469,7 +693,7 @@ def get_base_position_as_list(self) -> List[float]: def get_orientation_as_list(self) -> List[float]: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + Return the orientation of this object as a list of xyzw, representing a quaternion. :return: A list of xyzw """ @@ -477,15 +701,17 @@ def get_orientation_as_list(self) -> List[float]: def get_pose(self) -> Pose: """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + Return the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. :return: The current pose of this object """ + if self.world.conf.update_poses_from_sim_on_get: + self.update_pose() return self._current_pose - def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: + def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True) -> None: """ - Sets the Pose of the object. + Set the Pose of the object. :param pose: New Pose for the object :param base: If True places the object base instead of origin at the specified position and orientation @@ -501,23 +727,24 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op self._set_attached_objects_poses() def reset_base_pose(self, pose: Pose): - self.world.reset_object_base_pose(self, pose) - self.update_pose() + if self.world.reset_object_base_pose(self, pose): + self.update_pose() def update_pose(self): """ - Updates the current pose of this object from the world, and updates the poses of all links. + Update the current pose of this object from the world, and updates the poses of all links. """ self._current_pose = self.world.get_object_pose(self) + # TODO: Probably not needed, need to test self._update_all_links_poses() self.update_link_transforms() def _update_all_links_poses(self): """ - Updates the poses of all links by getting them from the simulator. + Update the poses of all links by getting them from the simulator. """ for link in self.links.values(): - link._update_pose() + link.update_pose() def move_base_to_origin_pose(self) -> None: """ @@ -528,7 +755,7 @@ def move_base_to_origin_pose(self) -> None: def save_state(self, state_id) -> None: """ - Saves the state of this object by saving the state of all links and attachments. + Save the state of this object by saving the state of all links and attachments. :param state_id: The unique id of the state. """ @@ -538,7 +765,7 @@ def save_state(self, state_id) -> None: def save_links_states(self, state_id: int) -> None: """ - Saves the state of all links of this object. + Save the state of all links of this object. :param state_id: The unique id of the state. """ @@ -547,7 +774,7 @@ def save_links_states(self, state_id: int) -> None: def save_joints_states(self, state_id: int) -> None: """ - Saves the state of all joints of this object. + Save the state of all joints of this object. :param state_id: The unique id of the state. """ @@ -556,40 +783,107 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) + """ + The current state of this object as an ObjectState. + """ + return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), + self.joint_states.copy(), self.world.conf.get_pose_tolerance()) @current_state.setter def current_state(self, state: ObjectState) -> None: - if self.get_pose().dist(state.pose) != 0.0: + """ + Set the current state of this object to the given state. + """ + if self.current_state != state: self.set_pose(state.pose, base=False, set_attachments=False) - - self.set_attachments(state.attachments) - self.link_states = state.link_states - self.joint_states = state.joint_states + self.set_attachments(state.attachments) + self.link_states = state.link_states + self.joint_states = state.joint_states def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ - Sets the attachments of this object to the given attachments. + Set the attachments of this object to the given attachments. + + :param attachments: A dictionary with the object as key and the attachment as value. + """ + self.detach_objects_not_in_attachments(attachments) + self.attach_objects_in_attachments(attachments) + + def detach_objects_not_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: + """ + Detach objects that are not in the attachments list and are in the current attachments list. + + :param attachments: A dictionary with the object as key and the attachment as value. + """ + copy_of_attachments = self.attachments.copy() + for obj, attachment in copy_of_attachments.items(): + original_obj = obj + if self.world.is_prospection_world and len(attachments) > 0 \ + and not list(attachments.keys())[0].world.is_prospection_world: + obj = self.world.get_object_for_prospection_object(obj) + if obj not in attachments: + if attachment.is_inverse: + original_obj.detach(self) + else: + self.detach(original_obj) + + def attach_objects_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: + """ + Attach objects that are in the given attachments list but not in the current attachments list. :param attachments: A dictionary with the object as key and the attachment as value. """ for obj, attachment in attachments.items(): - if self.world.is_prospection_world and not obj.world.is_prospection_world: - # In case this object is in the prospection world and the other object is not, the attachment will no - # be set. - continue + is_prospection = self.world.is_prospection_world and not obj.world.is_prospection_world + if is_prospection: + obj = self.world.get_prospection_object_for_object(obj) if obj in self.attachments: if self.attachments[obj] != attachment: - self.detach(obj) + if attachment.is_inverse: + obj.detach(self) + else: + self.detach(obj) else: continue - self.attach(obj, attachment.parent_link.name, attachment.child_link.name, - attachment.bidirectional) + self.mimic_attachment_with_object(attachment, obj) + + def mimic_attachment_with_object(self, attachment: Attachment, child_object: Object) -> None: + """ + Mimic the given attachment for this and the given child objects. + + :param attachment: The attachment to mimic. + :param child_object: The child object. + """ + att_transform = self.get_attachment_transform_with_object(attachment, child_object) + if attachment.is_inverse: + child_object.attach(self, attachment.child_link.name, attachment.parent_link.name, + attachment.bidirectional, + parent_to_child_transform=att_transform.invert()) + else: + self.attach(child_object, attachment.parent_link.name, attachment.child_link.name, + attachment.bidirectional, parent_to_child_transform=att_transform) + + def get_attachment_transform_with_object(self, attachment: Attachment, child_object: Object) -> Transform: + """ + Return the attachment transform for the given parent and child objects, taking into account the prospection + world. + + :param attachment: The attachment. + :param child_object: The child object. + :return: The attachment transform. + """ + if self.world != child_object.world: + raise WorldMismatchErrorBetweenObjects(self, child_object) + att_transform = attachment.parent_to_child_transform.copy() + if self.world.is_prospection_world and not attachment.parent_object.world.is_prospection_world: + att_transform.frame = self.tf_prospection_world_prefix + att_transform.frame + att_transform.child_frame_id = self.tf_prospection_world_prefix + att_transform.child_frame_id + return att_transform @property def link_states(self) -> Dict[int, LinkState]: """ - Returns the current state of all links of this object. + The current state of all links of this object. :return: A dictionary with the link id as key and the current state of the link as value. """ @@ -598,7 +892,7 @@ def link_states(self) -> Dict[int, LinkState]: @link_states.setter def link_states(self, link_states: Dict[int, LinkState]) -> None: """ - Sets the current state of all links of this object. + Set the current state of all links of this object. :param link_states: A dictionary with the link id as key and the current state of the link as value. """ @@ -608,7 +902,7 @@ def link_states(self, link_states: Dict[int, LinkState]) -> None: @property def joint_states(self) -> Dict[int, JointState]: """ - Returns the current state of all joints of this object. + The current state of all joints of this object. :return: A dictionary with the joint id as key and the current state of the joint as value. """ @@ -617,16 +911,20 @@ def joint_states(self) -> Dict[int, JointState]: @joint_states.setter def joint_states(self, joint_states: Dict[int, JointState]) -> None: """ - Sets the current state of all joints of this object. + Set the current state of all joints of this object. :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. """ for joint in self.joints.values(): - joint.current_state = joint_states[joint.id] + if joint.name not in self.robot_virtual_move_base_joints_names(): + joint.current_state = joint_states[joint.id] + + def robot_virtual_move_base_joints_names(self): + return self.robot_description.virtual_mobile_base_joints.names def remove_saved_states(self) -> None: """ - Removes all saved states of this object. + Remove all saved states of this object. """ super().remove_saved_states() self.remove_links_saved_states() @@ -634,28 +932,30 @@ def remove_saved_states(self) -> None: def remove_links_saved_states(self) -> None: """ - Removes all saved states of the links of this object. + Remove all saved states of the links of this object. """ for link in self.links.values(): link.remove_saved_states() def remove_joints_saved_states(self) -> None: """ - Removes all saved states of the joints of this object. + Remove all saved states of the joints of this object. """ for joint in self.joints.values(): joint.remove_saved_states() def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ - Updates the positions of all attached objects. This is done + Update the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. + After this call _set_attached_objects method for all attached objects. - :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops + in the update. """ + if not self.world.conf.let_pycram_move_attached_objects: + return if already_moved_objects is None: already_moved_objects = [] @@ -671,13 +971,12 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec child.update_attachment_with_object(self) else: - link_to_object = attachment.parent_to_child_transform - child.set_pose(link_to_object.to_pose(), set_attachments=False) + child.set_pose(attachment.get_child_link_target_pose(), set_attachments=False) child._set_attached_objects_poses(already_moved_objects + [self]) def set_position(self, position: Union[Pose, Point, List], base=False) -> None: """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + Set this Object to the given position, if base is true, place the bottom of the Object at the position instead of the origin in the center of the Object. The given position can either be a Pose, in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. @@ -690,10 +989,13 @@ def set_position(self, position: Union[Pose, Point, List], base=False) -> None: pose.frame = position.frame elif isinstance(position, Point): target_position = position - elif isinstance(position, list): - target_position = position + elif isinstance(position, List): + if len(position) == 3: + target_position = Point(*position) + else: + raise ValueError("The given position has to be a list of 3 values.") else: - raise TypeError("The given position has to be a Pose, Point or a list of xyz.") + raise TypeError("The given position has to be a Pose, Point or an iterable of xyz values.") pose.position = target_position pose.orientation = self.get_orientation() @@ -701,7 +1003,7 @@ def set_position(self, position: Union[Pose, Point, List], base=False) -> None: def set_orientation(self, orientation: Union[Pose, Quaternion, List, Tuple, np.ndarray]) -> None: """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + Set the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. :param orientation: Target orientation given as a list of xyzw. @@ -724,7 +1026,7 @@ def set_orientation(self, orientation: Union[Pose, Quaternion, List, Tuple, np.n def get_joint_id(self, name: str) -> int: """ - Returns the unique id for a joint name. As used by the world/simulator. + Return the unique id for a joint name. As used by the world/simulator. :param name: The joint name :return: The unique id @@ -733,35 +1035,35 @@ def get_joint_id(self, name: str) -> int: def get_root_link_description(self) -> LinkDescription: """ - Returns the root link of the URDF of this object. + Return the root link of the URDF of this object. :return: The root link as defined in the URDF of this object. """ for link_description in self.description.links: - if link_description.name == self.root_link_name: + if link_description.name == self.description.get_root(): return link_description @property def root_link(self) -> ObjectDescription.Link: """ - Returns the root link of this object. + The root link of this object. :return: The root link of this object. """ return self.links[self.description.get_root()] @property - def root_link_name(self) -> str: + def tip_link(self) -> ObjectDescription.Link: """ - Returns the name of the root link of this object. + The tip link of this object. - :return: The name of the root link of this object. + :return: The tip link of this object. """ - return self.description.get_root() + return self.links[self.description.get_tip()] def get_root_link_id(self) -> int: """ - Returns the unique id of the root link of this object. + Return the unique id of the root link of this object. :return: The unique id of the root link of this object. """ @@ -769,7 +1071,7 @@ def get_root_link_id(self) -> int: def get_link_id(self, link_name: str) -> int: """ - Returns a unique id for a link name. + Return a unique id for a link name. :param link_name: The name of the link. :return: The unique id of the link. @@ -778,7 +1080,7 @@ def get_link_id(self, link_name: str) -> int: def get_link_by_id(self, link_id: int) -> ObjectDescription.Link: """ - Returns the link for a given unique link id + Return the link for a given unique link id :param link_id: The unique id of the link. :return: The link object. @@ -787,40 +1089,50 @@ def get_link_by_id(self, link_id: int) -> ObjectDescription.Link: def reset_all_joints_positions(self) -> None: """ - Sets the current position of all joints to 0. This is useful if the joints should be reset to their default + Set the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = list(self.joint_name_to_id.keys()) + joint_names = [joint.name for joint in self.joints.values()] + if len(joint_names) == 0: + return joint_positions = [0] * len(joint_names) - self.set_joint_positions(dict(zip(joint_names, joint_positions))) + self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_poses: dict) -> None: + def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ - Sets the current position of multiple joints at once, this method should be preferred when setting - multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. + Set the position of the given joint to the given joint pose and updates the poses of all attached objects. - :param joint_poses: + :param joint_name: The name of the joint + :param joint_position: The target pose for this joint """ - for joint_name, joint_position in joint_poses.items(): - self.joints[joint_name].position = joint_position - # self.update_pose() - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() + if self.world.reset_joint_position(self.joints[joint_name], joint_position): + self._update_on_joint_position_change() - def set_joint_position(self, joint_name: str, joint_position: float) -> None: + @deprecated("Use set_multiple_joint_positions instead") + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + self.set_multiple_joint_positions(joint_positions) + + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ - Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. + Set the current position of multiple joints at once, this method should be preferred when setting + multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. - :param joint_name: The name of the joint - :param joint_position: The target pose for this joint + :param joint_positions: A dictionary with the joint names as keys and the target positions as values. """ - self.joints[joint_name].position = joint_position + joint_positions = {self.joints[joint_name]: joint_position + for joint_name, joint_position in joint_positions.items()} + if self.world.set_multiple_joint_positions(joint_positions): + self._update_on_joint_position_change() + + def _update_on_joint_position_change(self): + self.update_pose() self._update_all_links_poses() self.update_link_transforms() self._set_attached_objects_poses() def get_joint_position(self, joint_name: str) -> float: """ + Return the current position of the given joint. + :param joint_name: The name of the joint :return: The current position of the given joint """ @@ -828,6 +1140,8 @@ def get_joint_position(self, joint_name: str) -> float: def get_joint_damping(self, joint_name: str) -> float: """ + Return the damping of the given joint (friction). + :param joint_name: The name of the joint :return: The damping of the given joint """ @@ -835,6 +1149,8 @@ def get_joint_damping(self, joint_name: str) -> float: def get_joint_upper_limit(self, joint_name: str) -> float: """ + Return the upper limit of the given joint. + :param joint_name: The name of the joint :return: The upper limit of the given joint """ @@ -842,6 +1158,8 @@ def get_joint_upper_limit(self, joint_name: str) -> float: def get_joint_lower_limit(self, joint_name: str) -> float: """ + Return the lower limit of the given joint. + :param joint_name: The name of the joint :return: The lower limit of the given joint """ @@ -849,6 +1167,8 @@ def get_joint_lower_limit(self, joint_name: str) -> float: def get_joint_axis(self, joint_name: str) -> Point: """ + Return the axis of the given joint. + :param joint_name: The name of the joint :return: The axis of the given joint """ @@ -856,6 +1176,8 @@ def get_joint_axis(self, joint_name: str) -> Point: def get_joint_type(self, joint_name: str) -> JointType: """ + Return the type of the given joint. + :param joint_name: The name of the joint :return: The type of the given joint """ @@ -863,6 +1185,8 @@ def get_joint_type(self, joint_name: str) -> JointType: def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: """ + Return the lower and upper limits of the given joint. + :param joint_name: The name of the joint :return: The lower and upper limits of the given joint """ @@ -870,6 +1194,8 @@ def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: """ + Return the child link of the given joint. + :param joint_name: The name of the joint :return: The child link of the given joint """ @@ -877,6 +1203,8 @@ def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: def get_joint_parent_link(self, joint_name: str) -> ObjectDescription.Link: """ + Return the parent link of the given joint. + :param joint_name: The name of the joint :return: The parent link of the given joint """ @@ -884,7 +1212,7 @@ def get_joint_parent_link(self, joint_name: str) -> ObjectDescription.Link: def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: """ - Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. + Traverse the chain from 'link' to the URDF origin and return the first joint that is of type 'joint_type'. :param link_name: AbstractLink name above which the joint should be found :param joint_type: Joint type that should be searched for @@ -898,35 +1226,46 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: container_joint = element break if not container_joint: - rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") + logwarn(f"No joint of type {joint_type} found above link {link_name}") return container_joint + def get_multiple_joint_positions(self, joint_names: List[str]) -> Dict[str, float]: + """ + Return the positions of multiple joints at once. + + :param joint_names: A list of joint names. + :return: A dictionary with the joint names as keys and the joint positions as values. + """ + return self.world.get_multiple_joint_positions([self.joints[joint_name] for joint_name in joint_names]) + def get_positions_of_all_joints(self) -> Dict[str, float]: """ - Returns the positions of all joints of the object as a dictionary of joint names and joint positions. + Return the positions of all joints of the object as a dictionary of joint names and joint positions. :return: A dictionary with all joints positions'. """ return {j.name: j.position for j in self.joints.values()} - def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: + def update_link_transforms(self, transform_time: Optional[Time] = None) -> None: """ - Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + Update the transforms of all links of this object using time 'transform_time' or the current ros time. + + :param transform_time: The time to use for the transform update. """ for link in self.links.values(): link.update_transform(transform_time) - def contact_points(self) -> List: + def contact_points(self) -> ContactPointsList: """ - Returns a list of contact points of this Object with other Objects. + Return a list of contact points of this Object with other Objects. :return: A list of all contact points with other objects """ return self.world.get_object_contact_points(self) - def contact_points_simulated(self) -> List: + def contact_points_simulated(self) -> ContactPointsList: """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + Return a list of all contact points between this Object and other Objects after stepping the simulation once. :return: A list of contact points between this Object and other Objects """ @@ -936,9 +1275,28 @@ def contact_points_simulated(self) -> List: self.world.restore_state(state_id) return contact_points + def closest_points(self, max_distance: float) -> ClosestPointsList: + """ + Return a list of closest points between this Object and other Objects. + + :param max_distance: The maximum distance between the closest points + :return: A list of closest points between this Object and other Objects + """ + return self.world.get_object_closest_points(self, max_distance) + + def closest_points_with_obj(self, other_object: Object, max_distance: float) -> ClosestPointsList: + """ + Return a list of closest points between this Object and another Object. + + :param other_object: The other object + :param max_distance: The maximum distance between the closest points + :return: A list of closest points between this Object and the other Object + """ + return self.world.get_closest_points_between_objects(self, other_object, max_distance) + def set_color(self, rgba_color: Color) -> None: """ - Changes the color of this object, the color has to be given as a list + Change the color of this object, the color has to be given as a list of RGBA values. :param rgba_color: The color as Color object with RGBA values between 0 and 1 @@ -953,7 +1311,7 @@ def set_color(self, rgba_color: Color) -> None: def get_color(self) -> Union[Color, Dict[str, Color]]: """ - This method returns the rgba_color of this object. The return is either: + Return the rgba_color of this object. The return is either: 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) @@ -961,7 +1319,8 @@ def get_color(self) -> Union[Color, Dict[str, Color]]: Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which the object is spawned. - :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and the rgba_color as value. + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. """ link_to_color_dict = self.links_colors @@ -979,12 +1338,16 @@ def links_colors(self) -> Dict[str, Color]: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ + Return the axis aligned bounding box of this object. + :return: The axis aligned bounding box of this object. """ return self.world.get_object_axis_aligned_bounding_box(self) def get_base_origin(self) -> Pose: """ + Return the origin of the base/bottom of this object. + :return: the origin of the base/bottom of this object. """ aabb = self.get_axis_aligned_bounding_box() @@ -995,40 +1358,43 @@ def get_base_origin(self) -> Pose: def get_joint_by_id(self, joint_id: int) -> Joint: """ - Returns the joint object with the given id. + Return the joint object with the given id. :param joint_id: The unique id of the joint. :return: The joint object. """ return dict([(joint.id, joint) for joint in self.joints.values()])[joint_id] + def get_link_for_attached_objects(self) -> Dict[Object, ObjectDescription.Link]: + """ + Return a dictionary which maps attached object to the link of this object to which the given object is attached. + + :return: The link of this object to which the given object is attached. + """ + return {obj: attachment.parent_link for obj, attachment in self.attachments.items()} + def copy_to_prospection(self) -> Object: """ - Copies this object to the prospection world. + Copy this object to the prospection world. :return: The copied object in the prospection world. """ - obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), - self.world.prospection_world, self.color) - obj.current_state = self.current_state - return obj + return self.copy_to_world(self.world.prospection_world) - def __copy__(self) -> Object: + def copy_to_world(self, world: World) -> Object: """ - Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. + Copy this object to the given world. - :return: A copy of this object. + :param world: The world to which the object should be copied. + :return: The copied object in the given world. """ - obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), - self.world.prospection_world, self.color) - obj.current_state = self.current_state + obj = Object(self.name, self.obj_type, self.path, self.description, self.get_pose(), + world, self.color) return obj def __eq__(self, other): - if not isinstance(other, Object): - return False - return (self.id == other.id and self.world == other.world and self.name == other.name - and self.obj_type == other.obj_type) + return (isinstance(other, Object) and self.id == other.id and self.name == other.name + and self.world == other.world) def __hash__(self): - return hash((self.name, self.obj_type, self.id, self.world.id)) + return hash((self.id, self.name, self.world)) diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index ed7079d12..8fd0501b9 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -1,13 +1,14 @@ -import itertools -from typing_extensions import List, Tuple, Optional, Union, Dict - import numpy as np +from typing_extensions import List, Tuple, Optional, Union, Dict -from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp +from .datastructures.dataclasses import ContactPointsList from .datastructures.pose import Pose, Transform +from .datastructures.world import World, UseProspectionWorld +from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .robot_description import RobotDescription +from .utils import RayTestUtils from .world_concepts.world_object import Object -from .datastructures.world import World, UseProspectionWorld +from .config import world_conf as conf def stable(obj: Object) -> bool: @@ -50,41 +51,44 @@ def contact( with UseProspectionWorld(): prospection_obj1 = World.current_world.get_prospection_object_for_object(object1) prospection_obj2 = World.current_world.get_prospection_object_for_object(object2) - World.current_world.perform_collision_detection() - con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) - + con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1, + prospection_obj2) + objects_are_in_contact = len(con_points) > 0 if return_links: - contact_links = [] - for point in con_points: - contact_links.append((prospection_obj1.get_link_by_id(point[3]), - prospection_obj2.get_link_by_id(point[4]))) - return con_points != (), contact_links - + contact_links = [(point.link_a, point.link_b) for point in con_points] + return objects_are_in_contact, contact_links else: - return con_points != () + return objects_are_in_contact def get_visible_objects( camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: + front_facing_axis: Optional[List[float]] = None, + plot_segmentation_mask: bool = False) -> Tuple[np.ndarray, Pose]: """ - Returns a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. + Return a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. :param camera_pose: The pose of the camera in world coordinate frame. :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param plot_segmentation_mask: If the segmentation mask should be plotted :return: A segmentation mask of the objects that are visible and the pose of the point at exactly 2 meters in front of the camera in the direction of the front facing axis with respect to the world coordinate frame. """ - front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis + if front_facing_axis is None: + front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis - world_to_cam = camera_pose.to_transform("camera") + camera_frame = RobotDescription.current_robot_description.get_camera_frame() + world_to_cam = camera_pose.to_transform(camera_frame) - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], camera_frame, "point") target_point = (world_to_cam * cam_to_point).to_pose() seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] + if plot_segmentation_mask: + RayTestUtils.plot_segmentation_mask(seg_mask) + return seg_mask, target_point @@ -92,7 +96,8 @@ def visible( obj: Object, camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - threshold: float = 0.8) -> bool: + threshold: float = 0.8, + plot_segmentation_mask: bool = False) -> bool: """ Checks if an object is visible from a given position. This will be achieved by rendering the object alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the @@ -102,6 +107,7 @@ def visible( :param camera_pose: The pose of the camera in map frame :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz :param threshold: The minimum percentage of the object that needs to be visible for this method to return true. + :param plot_segmentation_mask: If the segmentation mask should be plotted. :return: True if the object is visible from the camera_position False if not """ with UseProspectionWorld(): @@ -116,9 +122,9 @@ def visible( else: obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1]), set_attachments=False) - seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) - flat_list = list(itertools.chain.from_iterable(seg_mask)) - max_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis, plot_segmentation_mask) + max_pixel = np.array(seg_mask == prospection_obj.id).sum() + World.current_world.restore_state(state_id) if max_pixel == 0: @@ -126,8 +132,7 @@ def visible( return False seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] - flat_list = list(itertools.chain.from_iterable(seg_mask)) - real_pixel = sum(list(map(lambda x: 1 if x == prospection_obj.id else 0, flat_list))) + real_pixel = np.array(seg_mask == prospection_obj.id).sum() return real_pixel / max_pixel > threshold > 0 @@ -135,7 +140,8 @@ def visible( def occluding( obj: Object, camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> List[Object]: + front_facing_axis: Optional[List[float]] = None, + plot_segmentation_mask: bool = False) -> List[Object]: """ Lists all objects which are occluding the given object. This works similar to 'visible'. First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. @@ -145,6 +151,7 @@ def occluding( :param obj: The object for which occlusion should be checked :param camera_pose: The pose of the camera in world coordinate frame :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param plot_segmentation_mask: If the segmentation mask should be plotted :return: A list of occluding objects """ @@ -158,7 +165,7 @@ def occluding( else: other_obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis, plot_segmentation_mask) # All indices where the object that could be occluded is in the image # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension @@ -226,17 +233,15 @@ def blocking( :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose or object is not reachable. """ - prospection_robot = World.current_world.get_prospection_object_for_object(robot) with UseProspectionWorld(): + prospection_robot = World.current_world.get_prospection_object_for_object(robot) if grasp: try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) else: try_to_reach(pose_or_object, prospection_robot, gripper_name) - block = [] - for obj in World.current_world.objects: - if contact(prospection_robot, obj): - block.append(World.current_world.get_object_for_prospection_object(obj)) + block = [World.current_world.get_object_for_prospection_object(obj) for obj in World.current_world.objects + if contact(prospection_robot, obj)] return block @@ -259,7 +264,7 @@ def link_pose_for_joint_config( joint_config: Dict[str, float], link_name: str) -> Pose: """ - Returns the pose a link would be in if the given joint configuration would be applied to the object. + Get the pose a link would be in if the given joint configuration would be applied to the object. This is done by using the respective object in the prospection world and applying the joint configuration to this one. After applying the joint configuration the link position is taken from there. diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 77d6c1958..90851e466 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -5,18 +5,20 @@ import time import numpy as np -import pybullet as p -import rosgraph -import rospy +import pycram_bullet as p from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict +from typing_extensions import List, Optional, Dict, Any +from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ + ClosestPoint, LateralFriction, ContactPoint, ContactPointsList, ClosestPointsList from ..datastructures.enums import ObjectType, WorldMode, JointType from ..datastructures.pose import Pose -from ..object_descriptors.urdf import ObjectDescription from ..datastructures.world import World +from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription +from ..object_descriptors.urdf import ObjectDescription +from ..validation.goal_validator import (validate_multiple_joint_positions, validate_joint_position, + validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint -from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape from ..world_concepts.world_object import Object Link = ObjectDescription.Link @@ -31,13 +33,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - extension: str = ObjectDescription.get_file_extension() - - # Check is for sphinx autoAPI to be able to work in a CI workflow - if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): - rospy.init_node('pycram') - - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, sim_frequency=240): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -46,7 +42,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_frequency=sim_frequency) + super().__init__(mode=mode, is_prospection_world=is_prospection_world) # This disables file caching from PyBullet, since this would also cache # files that can not be loaded @@ -60,7 +56,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, + _ = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) def _init_world(self, mode: WorldMode): @@ -68,7 +64,30 @@ def _init_world(self, mode: WorldMode): self._gui_thread.start() time.sleep(0.1) - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_generic_object_and_get_id(self, description: GenericObjectDescription, + pose: Optional[Pose] = None) -> int: + """ + Creates a visual and collision box in the simulation. + """ + # Create visual shape + vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=description.shape_data, + rgbaColor=description.color.get_rgba(), physicsClientId=self.id) + + # Create collision shape + col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=description.shape_data, physicsClientId=self.id) + + # Create MultiBody with both visual and collision shapes + obj_id = p.createMultiBody(baseMass=1.0, baseCollisionShapeIndex=col_shape, baseVisualShapeIndex=vis_shape, + basePosition=description.origin.position_as_list(), + baseOrientation=description.origin.orientation_as_list(), physicsClientId=self.id) + + if pose is not None: + self._set_object_pose_by_id(obj_id, pose) + # Assuming you have a list to keep track of created objects + return obj_id + + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: if pose is None: pose = Pose() return self._load_object_and_get_id(path, pose) @@ -80,11 +99,21 @@ def _load_object_and_get_id(self, path: str, pose: Pose) -> int: basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) - def remove_object_from_simulator(self, obj: Object) -> None: - p.removeBody(obj.id, self.id) + def _remove_visual_object(self, obj_id: int) -> bool: + self._remove_body(obj_id) + return True + + def remove_object_from_simulator(self, obj: Object) -> bool: + self._remove_body(obj.id) + return True + + def _remove_body(self, body_id: int) -> Any: + """ + Remove a body from PyBullet using the body id. - def remove_object_by_id(self, obj_id: int) -> None: - p.removeBody(obj_id, self.id) + :param body_id: The id of the body. + """ + return p.removeBody(body_id, self.id) def add_constraint(self, constraint: Constraint) -> int: @@ -111,6 +140,21 @@ def get_object_joint_names(self, obj: Object) -> List[str]: return [p.getJointInfo(obj.id, i, physicsClientId=self.id)[1].decode('utf-8') for i in range(self.get_object_number_of_joints(obj))] + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return {link.name: self.get_link_pose(link) for link in links} + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_position(link) for link in links} + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_orientation(link) for link in links} + + def get_link_position(self, link: Link) -> List[float]: + return self.get_link_pose(link).position_as_list() + + def get_link_orientation(self, link: Link) -> List[float]: + return self.get_link_pose(link).orientation_as_list() + def get_link_pose(self, link: ObjectDescription.Link) -> Pose: bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id) return Pose(*bullet_link_state[4:6]) @@ -128,29 +172,94 @@ def get_object_number_of_links(self, obj: Object) -> int: def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ - For a more detailed explanation of the - returned list please look at: - `PyBullet Doc `_ + Get the contact points of the object with akk other objects in the world. The contact points are returned as a + ContactPointsList object. + + :param obj: The object for which the contact points should be returned. + :return: The contact points of the object with all other objects in the world. """ self.perform_collision_detection() - return p.getContactPoints(obj.id, physicsClientId=self.id) + points_list = p.getContactPoints(obj.id, physicsClientId=self.id) + return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> ContactPointsList: self.perform_collision_detection() - return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.id) + points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id) + return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) - def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: str) -> None: + def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> ClosestPointsList: + points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) + return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) + + def parse_points_list_to_args(self, point: List) -> Dict: + """ + Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the + ContactPoint class. + + :param point: The list of points. + """ + return {"link_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]), + "link_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]), + "position_on_object_a": point[5], + "position_on_object_b": point[6], + "normal_on_b": point[7], + "distance": point[8], + "normal_force": point[9], + "lateral_friction_1": LateralFriction(point[10], point[11]), + "lateral_friction_2": LateralFriction(point[12], point[13])} + + @validate_multiple_joint_positions + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + for joint, joint_position in joint_positions.items(): + self.reset_joint_position(joint, joint_position) + return True + + @validate_joint_position + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) + return True - def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + return {joint.name: self.get_joint_position(joint) for joint in joints} + + @validate_multiple_object_poses + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: + for obj, pose in objects.items(): + self.reset_object_base_pose(obj, pose) + return True + + @validate_object_pose + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: + return self._set_object_pose_by_id(obj.id, pose) + + def _set_object_pose_by_id(self, obj_id: int, pose: Pose) -> bool: + p.resetBasePositionAndOrientation(obj_id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) + return True def step(self): p.stepSimulation(physicsClientId=self.id) + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return {obj.name: self.get_object_pose(obj) for obj in objects} + + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).position_as_list() for obj in objects} + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).orientation_as_list() for obj in objects} + + def get_object_position(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).position_as_list() + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).orientation_as_list() + def get_object_pose(self, obj: Object) -> Pose: return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.id)) @@ -193,7 +302,7 @@ def join_gui_thread_if_exists(self): if self._gui_thread: self._gui_thread.join() - def save_physics_simulator_state(self) -> int: + def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: return p.saveState(physicsClientId=self.id) def restore_physics_simulator_state(self, state_id): @@ -202,14 +311,15 @@ def restore_physics_simulator_state(self, state_id): def remove_physics_simulator_state(self, state_id: int): p.removeState(state_id, physicsClientId=self.id) - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: + def _add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> int: """ Creates a Visual object which represents the coordinate frame at the given position and orientation. There can be an unlimited amount of vis axis objects. :param pose: The pose at which the axis should be spawned :param length: Optional parameter to configure the length of the axes + :return: The id of the spawned object """ pose_in_map = self.local_transformer.transform_pose(pose, "map") @@ -231,9 +341,11 @@ def add_vis_axis(self, pose: Pose, link_joint_axis=[Point(1, 0, 0), Point(0, 1, 0), Point(0, 0, 1)], link_collision_shape_indices=[-1, -1, -1]) - self.vis_axis.append(self.create_multi_body(multibody)) + body_id = self._create_multi_body(multibody) + self.vis_axis.append(body_id) + return body_id - def remove_vis_axis(self) -> None: + def _remove_vis_axis(self) -> None: """ Removes all spawned vis axis objects that are currently in this BulletWorld. """ @@ -250,13 +362,13 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L return p.rayTestBatch(from_positions, to_positions, numThreads=num_threads, physicsClientId=self.id) - def create_visual_shape(self, visual_shape: VisualShape) -> int: + def _create_visual_shape(self, visual_shape: VisualShape) -> int: return p.createVisualShape(visual_shape.visual_geometry_type.value, rgbaColor=visual_shape.rgba_color.get_rgba(), visualFramePosition=visual_shape.visual_frame_position, physicsClientId=self.id, **visual_shape.shape_data()) - def create_multi_body(self, multi_body: MultiBody) -> int: + def _create_multi_body(self, multi_body: MultiBody) -> int: return p.createMultiBody(baseVisualShapeIndex=-multi_body.base_visual_shape_index, linkVisualShapeIndices=multi_body.link_visual_shape_indices, basePosition=multi_body.base_pose.position_as_list(), @@ -289,9 +401,9 @@ def get_images_for_target(self, return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=self.id))[2:5] - def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, - size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, - parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: + def _add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, + size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: args = {} if orientation: args["textOrientation"] = orientation @@ -305,7 +417,7 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ args["parentLinkIndex"] = parent_link_id return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.id, **args) - def remove_text(self, text_id: Optional[int] = None) -> None: + def _remove_text(self, text_id: Optional[int] = None) -> None: if text_id is not None: p.removeUserDebugItem(text_id, physicsClientId=self.id) else: @@ -395,7 +507,7 @@ def run(self): width, height, dist = (p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], p.getDebugVisualizerCamera()[10]) - #print("width: ", width, "height: ", height, "dist: ", dist) + # print("width: ", width, "height: ", height, "dist: ", dist) camera_target_position = p.getDebugVisualizerCamera(self.world.id)[11] # Get vectors used for movement on x,y,z Vector @@ -550,5 +662,6 @@ def run(self): cameraTargetPosition=camera_target_position, physicsClientId=self.world.id) if visible == 0: camera_target_position = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1], physicsClientId=self.world.id) + p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1], + physicsClientId=self.world.id) time.sleep(1. / 80.) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py new file mode 100644 index 000000000..b4b65380e --- /dev/null +++ b/src/pycram/worlds/multiverse.py @@ -0,0 +1,658 @@ +import logging +from time import sleep + +import numpy as np +from tf.transformations import quaternion_matrix +from typing_extensions import List, Dict, Optional, Union, Tuple + +from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI +from ..config.multiverse_conf import MultiverseConfig +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint +from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ + MultiverseJointCMD +from ..datastructures.pose import Pose +from ..datastructures.world import World +from ..description import Link, Joint, ObjectDescription +from ..object_descriptors.mjcf import ObjectDescription as MJCF +from ..robot_description import RobotDescription +from ..ros.logging import logwarn, logerr +from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz +from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ + validate_joint_position, validate_multiple_object_poses +from ..world_concepts.constraints import Constraint +from ..world_concepts.world_object import Object + + +class Multiverse(World): + """ + This class implements an interface between Multiverse and PyCRAM. + """ + + conf: MultiverseConfig = MultiverseConfig + """ + The Multiverse configuration. + """ + + supported_joint_types = (JointType.REVOLUTE, JointType.CONTINUOUS, JointType.PRISMATIC) + """ + A Tuple for the supported pycram joint types in Multiverse. + """ + + added_multiverse_resources: bool = False + """ + A flag to check if the multiverse resources have been added. + """ + + simulation: Optional[str] = None + """ + The simulation name to be used in the Multiverse world (this is the name defined in + the multiverse configuration file). + """ + + Object.extension_to_description_type[MJCF.get_file_extension()] = MJCF + """ + Add the MJCF description extension to the extension to description type mapping for the objects. + """ + + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, + is_prospection: Optional[bool] = False, + simulation_name: str = "pycram_test", + clear_cache: bool = False): + """ + Initialize the Multiverse Socket and the PyCram World. + + :param mode: The mode of the world (DIRECT or GUI). + :param is_prospection: Whether the world is prospection or not. + :param simulation_name: The name of the simulation. + :param clear_cache: Whether to clear the cache or not. + """ + + self.latest_save_id: Optional[int] = None + self.saved_simulator_states: Dict = {} + self._make_sure_multiverse_resources_are_added(clear_cache=clear_cache) + + if Multiverse.simulation is None: + if simulation_name is None: + logging.error("Simulation name not provided") + raise ValueError("Simulation name not provided") + Multiverse.simulation = simulation_name + + self.simulation = (self.conf.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) + self._init_clients(is_prospection=is_prospection) + + World.__init__(self, mode, is_prospection) + + self._init_constraint_and_object_id_name_map_collections() + + self.ray_test_utils = RayTestUtils(self.ray_test_batch, self.object_id_to_name) + + if not self.is_prospection_world: + self._spawn_floor() + + if self.conf.use_static_mode: + self.api_requester.pause_simulation() + + def _init_clients(self, is_prospection: bool = False): + """ + Initialize the Multiverse clients that will be used to communicate with the Multiverse server. + Each client is responsible for a specific task, e.g. reading data from the server, writing data to the serve, + calling the API, or controlling the robot joints. + + :param is_prospection: Whether the world is prospection or not. + """ + self.reader: MultiverseReader = self.client_manager.create_reader( + is_prospection_world=is_prospection) + self.writer: MultiverseWriter = self.client_manager.create_writer( + self.simulation, + is_prospection_world=is_prospection) + self.api_requester: MultiverseAPI = self.client_manager.create_api_requester( + self.simulation, + is_prospection_world=is_prospection) + if self.conf.use_controller: + self.joint_controller: MultiverseController = self.client_manager.create_controller( + is_prospection_world=is_prospection) + + def _init_constraint_and_object_id_name_map_collections(self): + self.last_object_id: int = -1 + self.last_constraint_id: int = -1 + self.constraints: Dict[int, Constraint] = {} + self.object_name_to_id: Dict[str, int] = {} + self.object_id_to_name: Dict[int, str] = {} + + def _init_world(self, mode: WorldMode): + pass + + def _make_sure_multiverse_resources_are_added(self, clear_cache: bool = False): + """ + Add the multiverse resources to the pycram world resources, and change the data directory and cache manager. + + :param clear_cache: Whether to clear the cache or not. + """ + if not self.added_multiverse_resources: + if clear_cache: + World.cache_manager.clear_cache() + World.add_resource_path(self.conf.resources_path, prepend=True) + World.change_cache_dir_path(self.conf.resources_path) + self.added_multiverse_resources = True + + def remove_multiverse_resources(self): + """ + Remove the multiverse resources from the pycram world resources. + """ + if self.added_multiverse_resources: + World.remove_resource_path(self.conf.resources_path) + World.change_cache_dir_path(self.conf.cache_dir) + self.added_multiverse_resources = False + + def _spawn_floor(self): + """ + Spawn the plane in the simulator. + """ + self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", + world=self) + + def get_images_for_target(self, target_pose: Pose, + cam_pose: Pose, + size: int = 256, + camera_min_distance: float = 0.1, + camera_max_distance: int = 3, + plot: bool = False) -> List[np.ndarray]: + """ + Uses ray test to get the images for the target object. (target_pose is currently not used) + """ + camera_description = RobotDescription.current_robot_description.get_default_camera() + camera_frame = RobotDescription.current_robot_description.get_camera_frame() + return self.ray_test_utils.get_images_for_target(cam_pose, camera_description, camera_frame, + size, camera_min_distance, camera_max_distance, plot) + + @staticmethod + def get_joint_position_name(joint: Joint) -> MultiverseJointPosition: + """ + Get the attribute name of the joint position in the Multiverse from the pycram joint type. + + :param joint: The joint. + """ + return MultiverseJointPosition.from_pycram_joint_type(joint.type) + + def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: + """ + Spawn the robot in the simulator. + + :param name: The name of the robot. + :param pose: The pose of the robot. + """ + actuator_joint_commands = { + actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] + for joint_name, actuator_name in self.robot_joint_actuators.items() + } + self.joint_controller.init_controller(actuator_joint_commands) + self.writer.spawn_robot_with_actuators(name, pose.position_as_list(), + xyzw_to_wxyz(pose.orientation_as_list()), + actuator_joint_commands) + + def load_object_and_get_id(self, name: Optional[str] = None, + pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: + """ + Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as + the name of the object in the description file. + + :param name: The name of the object to be loaded. + :param pose: The pose of the object. + :param obj_type: The type of the object. + """ + if pose is None: + pose = Pose() + + # Do not spawn objects with type environment as they should be already present in the simulator through the + # multiverse description file (.muv file). + if not obj_type == ObjectType.ENVIRONMENT: + self.spawn_object(name, obj_type, pose) + + return self._update_object_id_name_maps_and_get_latest_id(name) + + def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: + """ + Spawn the object in the simulator. + + :param name: The name of the object. + :param object_type: The type of the object. + :param pose: The pose of the object. + """ + if object_type == ObjectType.ROBOT and self.conf.use_controller: + self.spawn_robot_with_controller(name, pose) + else: + self._set_body_pose(name, pose) + + def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: + """ + Update the object id name maps and return the latest object id. + + :param name: The name of the object. + :return: The latest object id. + """ + self.last_object_id += 1 + self.object_name_to_id[name] = self.last_object_id + self.object_id_to_name[self.last_object_id] = name + return self.last_object_id + + def get_object_joint_names(self, obj: Object) -> List[str]: + return [joint.name for joint in obj.description.joints if joint.type in self.supported_joint_types] + + def get_object_link_names(self, obj: Object) -> List[str]: + return [link.name for link in obj.description.links] + + def get_link_position(self, link: Link) -> List[float]: + return self.reader.get_body_position(link.name) + + def get_link_orientation(self, link: Link) -> List[float]: + return self.reader.get_body_orientation(link.name) + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([link.name for link in links]) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([link.name for link in links]) + + @validate_joint_position + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + if self.conf.use_controller and self.joint_has_actuator(joint): + self._reset_joint_position_using_controller(joint, joint_position) + else: + self._set_multiple_joint_positions_without_controller({joint: joint_position}) + return True + + def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: + """ + Reset the position of a joint in the simulator using the controller. + + :param joint: The joint. + :param joint_position: The position of the joint. + :return: True if the joint position is reset successfully. + """ + self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), + self.get_joint_cmd_name(joint.type), + [joint_position]) + return True + + @validate_multiple_joint_positions + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints in the simulator. Also check if the joint is controlled by an actuator + and use the controller to set the joint position if the joint is controlled. + + :param joint_positions: The dictionary of joints and positions. + :return: True if the joint positions are set successfully (this means that the joint positions are set without + errors, but not necessarily that the joint positions are set to the specified values). + """ + + if self.conf.use_controller: + controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) + if len(controlled_joints) > 0: + controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints} + self._set_multiple_joint_positions_using_controller(controlled_joint_positions) + joint_positions = {joint: joint_positions[joint] for joint in joint_positions.keys() + if joint not in controlled_joints} + if len(joint_positions) > 0: + self._set_multiple_joint_positions_without_controller(joint_positions) + + return True + + def get_controlled_joints(self, joints: Optional[List[Joint]] = None) -> List[Joint]: + """ + Get the joints that are controlled by an actuator from the list of joints. + + :param joints: The list of joints to check. + :return: The list of controlled joints. + """ + joints = self.robot.joints if joints is None else joints + return [joint for joint in joints if self.joint_has_actuator(joint)] + + def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> None: + """ + Set the positions of multiple joints in the simulator without using the controller. + + :param joint_positions: The dictionary of joints and positions. + """ + joints_data = {joint.name: {self.get_joint_position_name(joint): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(joints_data) + + def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints in the simulator using the controller. + + :param joint_positions: The dictionary of joints and positions. + """ + controlled_joints_data = {self.get_actuator_for_joint(joint): + {self.get_joint_cmd_name(joint.type): [position]} + for joint, position in joint_positions.items()} + self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) + return True + + def get_joint_position(self, joint: Joint) -> Optional[float]: + joint_position_name = self.get_joint_position_name(joint) + data = self.reader.get_body_data(joint.name, [joint_position_name]) + if data is not None: + return data[joint_position_name.value][0] + + def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: + joint_names = [joint.name for joint in joints] + data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] + for joint in joints}) + if data is not None: + return {name: list(value.values())[0][0] for name, value in data.items()} + + @staticmethod + def get_joint_cmd_name(joint_type: JointType) -> MultiverseJointCMD: + """ + Get the attribute name of the joint command in the Multiverse from the pycram joint type. + + :param joint_type: The pycram joint type. + """ + return MultiverseJointCMD.from_pycram_joint_type(joint_type) + + def get_link_pose(self, link: Link) -> Optional[Pose]: + return self._get_body_pose(link.name) + + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([link.name for link in links]) + + def get_object_pose(self, obj: Object) -> Pose: + if obj.has_type_environment(): + return Pose() + return self._get_body_pose(obj.name) + + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + """ + Set the poses of multiple objects in the simulator. If the object is of type environment, the pose will be + the default pose. + + :param objects: The list of objects. + :return: The dictionary of object names and poses. + """ + non_env_objects = [obj for obj in objects if not obj.has_type_environment()] + all_poses = self._get_multiple_body_poses([obj.name for obj in non_env_objects]) + all_poses.update({obj.name: Pose() for obj in objects if obj.has_type_environment()}) + return all_poses + + @validate_object_pose + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: + if obj.has_type_environment(): + return False + + if (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): + obj.set_mobile_robot_pose(pose) + else: + self._set_body_pose(obj.name, pose) + + return True + + @validate_multiple_object_poses + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + """ + Reset the poses of multiple objects in the simulator. + + :param objects: The dictionary of objects and poses. + """ + for obj in objects.keys(): + if (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): + obj.set_mobile_robot_pose(objects[obj]) + objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, + ObjectType.ROBOT]} + self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) + + def _set_body_pose(self, body_name: str, pose: Pose) -> None: + """ + Reset the pose of a body (object, link, or joint) in the simulator. + + :param body_name: The name of the body. + :param pose: The pose of the body. + """ + self._set_multiple_body_poses({body_name: pose}) + + def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: + """ + Reset the poses of multiple bodies in the simulator. + + :param body_poses: The dictionary of body names and poses. + """ + self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), + MultiverseBodyProperty.ORIENTATION: + xyzw_to_wxyz(pose.orientation_as_list()), + MultiverseBodyProperty.RELATIVE_VELOCITY: [0.0] * 6} + for name, pose in body_poses.items()}) + + def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: + """ + Get the pose of a body in the simulator. + + :param body_name: The name of the body. + :param wait: Whether to wait until the pose is received. + :return: The pose of the body. + """ + data = self.reader.get_body_pose(body_name, wait) + return Pose(data[MultiverseBodyProperty.POSITION.value], + wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) + + def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: + """ + Get the poses of multiple bodies in the simulator. + + :param body_names: The list of body names. + """ + return self.reader.get_multiple_body_poses(body_names) + + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([obj.name for obj in objects]) + + def get_object_position(self, obj: Object) -> List[float]: + return self.reader.get_body_position(obj.name) + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([obj.name for obj in objects]) + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.reader.get_body_orientation(obj.name) + + def multiverse_reset_world(self): + """ + Reset the world using the Multiverse API. + """ + self.writer.reset_world() + + def disconnect_from_physics_server(self) -> None: + MultiverseClientManager.stop_all_clients() + + def join_threads(self) -> None: + self.reader.stop_thread = True + self.reader.join() + + def _remove_visual_object(self, obj_id: int) -> bool: + logwarn("Currently multiverse does not create visual objects") + return False + + def remove_object_from_simulator(self, obj: Object) -> bool: + if obj.obj_type != ObjectType.ENVIRONMENT: + self.writer.remove_body(obj.name) + return True + logwarn("Cannot remove environment objects") + return False + + def add_constraint(self, constraint: Constraint) -> int: + + if constraint.type != JointType.FIXED: + logging.error("Only fixed constraints are supported in Multiverse") + raise ValueError + + if not self.conf.let_pycram_move_attached_objects: + self.api_requester.attach(constraint) + + return self._update_constraint_collection_and_get_latest_id(constraint) + + def _update_constraint_collection_and_get_latest_id(self, constraint: Constraint) -> int: + """ + Update the constraint collection and return the latest constraint id. + + :param constraint: The constraint to be added. + :return: The latest constraint id. + """ + self.last_constraint_id += 1 + self.constraints[self.last_constraint_id] = constraint + return self.last_constraint_id + + def remove_constraint(self, constraint_id) -> None: + constraint = self.constraints.pop(constraint_id) + self.api_requester.detach(constraint) + + def perform_collision_detection(self) -> None: + ... + + def get_object_contact_points(self, obj: Object) -> ContactPointsList: + """ + Note: Currently Multiverse only gets one contact point per contact objects. + """ + multiverse_contact_points = self.api_requester.get_contact_points(obj) + contact_points = ContactPointsList([]) + body_link = None + for point in multiverse_contact_points: + if point.body_name == "world": + point.body_name = "floor" + body_object = self.get_object_by_name(point.body_name) + if body_object is None: + for obj in self.objects: + for link in obj.links.values(): + if link.name == point.body_name: + body_link = link + break + else: + body_link = body_object.root_link + if body_link is None: + logging.error(f"Body link not found: {point.body_name}") + raise ValueError(f"Body link not found: {point.body_name}") + contact_points.append(ContactPoint(obj.root_link, body_link)) + contact_points[-1].force_x_in_world_frame = point.contact_force[0] + contact_points[-1].force_y_in_world_frame = point.contact_force[1] + contact_points[-1].force_z_in_world_frame = point.contact_force[2] + contact_points[-1].normal_on_b = point.contact_force[2] + contact_points[-1].normal_force = point.contact_force[2] + return contact_points + + @staticmethod + def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: List[float]) -> float: + """ + Get the normal force on an object from the contact force exerted by another object that is expressed in the + world frame. Thus transforming the contact force to the object frame is necessary. + + :param obj: The object. + :param contact_force: The contact force. + :return: The normal force on the object. + """ + obj_quat = obj.get_orientation_as_list() + obj_rot_matrix = quaternion_matrix(obj_quat)[:3, :3] + # invert the rotation matrix to get the transformation from world to object frame + obj_rot_matrix = np.linalg.inv(obj_rot_matrix) + contact_force_array = obj_rot_matrix @ np.array(contact_force).reshape(3, 1) + return contact_force_array.flatten().tolist()[2] + + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: + obj1_contact_points = self.get_object_contact_points(obj1) + return obj1_contact_points.get_points_of_object(obj2) + + def ray_test(self, from_position: List[float], to_position: List[float]) -> Optional[int]: + ray_test_result = self.ray_test_batch([from_position], [to_position])[0] + return ray_test_result[0] if ray_test_result[0] != -1 else None + + def ray_test_batch(self, from_positions: List[List[float]], + to_positions: List[List[float]], + num_threads: int = 1, + return_distance: bool = False) -> Union[List, Tuple[List, List[float]]]: + """ + Note: Currently, num_threads is not used in Multiverse. + """ + ray_results = self.api_requester.get_objects_intersected_with_rays(from_positions, to_positions) + results = [] + distances = [] + for ray_result in ray_results: + results.append([]) + if ray_result.intersected(): + body_name = ray_result.body_name + if body_name == "world": + results[-1].append(0) # The floor id, which is always 0 since the floor is spawned first. + elif body_name in self.object_name_to_id.keys(): + results[-1].append(self.object_name_to_id[body_name]) + else: + for obj in self.objects: + if body_name in obj.links.keys(): + results[-1].append(obj.id) + break + else: + results[-1].append(-1) + if return_distance: + distances.append(ray_result.distance) + if return_distance: + return results, distances + else: + return results + + def step(self): + """ + Perform a simulation step in the simulator, this is useful when use_static_mode is True. + """ + if self.conf.use_static_mode: + self.api_requester.unpause_simulation() + sleep(self.simulation_time_step) + self.api_requester.pause_simulation() + + def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int: + if state_id is None: + self.latest_save_id = 0 if self.latest_save_id is None else self.latest_save_id + int(not use_same_id) + state_id = self.latest_save_id + save_name = f"save_{state_id}" + self.saved_simulator_states[state_id] = self.api_requester.save(save_name) + return state_id + + def remove_physics_simulator_state(self, state_id: int) -> None: + self.saved_simulator_states.pop(state_id) + + def restore_physics_simulator_state(self, state_id: int) -> None: + self.api_requester.load(self.saved_simulator_states[state_id]) + + def set_link_color(self, link: Link, rgba_color: Color): + logwarn("set_link_color is not implemented in Multiverse") + + def get_link_color(self, link: Link) -> Color: + logwarn("get_link_color is not implemented in Multiverse") + return Color() + + def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: + logwarn("get_colors_of_object_links is not implemented in Multiverse") + return {} + + def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: + logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse") + raise NotImplementedError + + def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: + logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse") + raise NotImplementedError + + def set_realtime(self, real_time: bool) -> None: + logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the" + "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") + + def set_gravity(self, gravity_vector: List[float]) -> None: + logwarn("set_gravity is not implemented in Multiverse") + + def check_object_exists(self, obj: Object) -> bool: + """ + Check if the object exists in the Multiverse world. + + :param obj: The object. + :return: True if the object exists, False otherwise. + """ + return self.api_requester.check_object_exists(obj) diff --git a/src/pycram/worlds/multiverse_communication/__init__.py b/src/pycram/worlds/multiverse_communication/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py new file mode 100644 index 000000000..a1b768172 --- /dev/null +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -0,0 +1,97 @@ +from typing_extensions import Optional, Type, Union, Dict + +from ...worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ + MultiverseReader, MultiverseController + +from ...config.multiverse_conf import MultiverseConfig as Conf + + +class MultiverseClientManager: + BASE_PORT: int = Conf.BASE_CLIENT_PORT + """ + The base port of the Multiverse client. + """ + clients: Optional[Dict[str, MultiverseClient]] = {} + """ + The list of Multiverse clients. + """ + last_used_port: int = BASE_PORT + + def __init__(self, simulation_wait_time_factor: Optional[float] = 1.0): + """ + Initialize the Multiverse client manager. + + :param simulation_wait_time_factor: The simulation wait time factor. + """ + self.simulation_wait_time_factor = simulation_wait_time_factor + + def create_reader(self, is_prospection_world: Optional[bool] = False) -> MultiverseReader: + """ + Create a Multiverse reader client. + + :param is_prospection_world: Whether the reader is connected to the prospection world. + """ + return self.create_client(MultiverseReader, "reader", is_prospection_world) + + def create_writer(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseWriter: + """ + Create a Multiverse writer client. + + :param simulation: The name of the simulation that the writer is connected to + (usually the name defined in the .muv file). + :param is_prospection_world: Whether the writer is connected to the prospection world. + """ + return self.create_client(MultiverseWriter, "writer", is_prospection_world, + simulation=simulation) + + def create_controller(self, is_prospection_world: Optional[bool] = False) -> MultiverseController: + """ + Create a Multiverse controller client. + + :param is_prospection_world: Whether the controller is connected to the prospection world. + """ + return self.create_client(MultiverseController, "controller", is_prospection_world) + + def create_api_requester(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseAPI: + """ + Create a Multiverse API client. + + :param simulation: The name of the simulation that the API is connected to + (usually the name defined in the .muv file). + :param is_prospection_world: Whether the API is connected to the prospection world. + """ + return self.create_client(MultiverseAPI, "api_requester", is_prospection_world, simulation=simulation) + + def create_client(self, + client_type: Type[MultiverseClient], + name: Optional[str] = None, + is_prospection_world: Optional[bool] = False, + **kwargs) -> Union[MultiverseClient, MultiverseAPI, + MultiverseReader, MultiverseWriter, MultiverseController]: + """ + Create a Multiverse client. + + :param client_type: The type of the client to create. + :param name: The name of the client. + :param is_prospection_world: Whether the client is connected to the prospection world. + :param kwargs: Any other keyword arguments that should be passed to the client constructor. + """ + MultiverseClientManager.last_used_port += 1 + name = (name or client_type.__name__) + f"_{self.last_used_port}" + client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, + simulation_wait_time_factor=self.simulation_wait_time_factor, **kwargs) + self.clients[name] = client + return client + + @classmethod + def stop_all_clients(cls): + """ + Stop all clients. + """ + for client in cls.clients: + if isinstance(client, MultiverseReader): + client.stop_thread = True + client.join() + elif isinstance(client, MultiverseClient): + client.stop() + cls.clients = {} diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py new file mode 100644 index 000000000..b10959a90 --- /dev/null +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -0,0 +1,832 @@ +import datetime +import logging +import os +import threading +from time import time, sleep + +from typing_extensions import List, Dict, Tuple, Optional, Callable, Union + +from .socket import MultiverseSocket, MultiverseMetaData +from ...config.multiverse_conf import MultiverseConfig as Conf +from ...datastructures.dataclasses import RayResult, MultiverseContactPoint +from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, + MultiverseProperty as Property) +from ...datastructures.pose import Pose +from ...ros.logging import logwarn +from ...utils import wxyz_to_xyzw +from ...world_concepts.constraints import Constraint +from ...world_concepts.world_object import Object, Link + + +class MultiverseClient(MultiverseSocket): + + def __init__(self, name: str, port: int, is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): + """ + Initialize the Multiverse client, which connects to the Multiverse server. + + :param name: The name of the client. + :param port: The port of the client. + :param is_prospection_world: Whether the client is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor (default is 1.0), which can be used to + increase or decrease the wait time for the simulation. + """ + meta_data = MultiverseMetaData() + meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "") + name + meta_data.world_name = ((Conf.prospection_world_prefix if is_prospection_world else "") + + meta_data.world_name) + self.is_prospection_world = is_prospection_world + super().__init__(port=str(port), meta_data=meta_data) + self.simulation_wait_time_factor = simulation_wait_time_factor + self.run() + + +class MultiverseReader(MultiverseClient): + MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = Conf.READER_MAX_WAIT_TIME_FOR_DATA + """ + The maximum wait time for the data in seconds. + """ + + def __init__(self, name: str, port: int, is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): + """ + Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. + This class provides methods to get data (e.g., position, orientation) from the Multiverse server. + + :param port: The port of the Multiverse reader client. + :param is_prospection_world: Whether the reader is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor. + """ + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) + + self.request_meta_data["receive"][""] = [""] + + self.data_lock = threading.Lock() + self.thread = threading.Thread(target=self.receive_all_data_from_server) + self.stop_thread = False + + self.thread.start() + + def get_body_pose(self, name: str, wait: bool = False) -> Optional[Dict[str, List[float]]]: + """ + Get the body pose from the multiverse server. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The position and orientation of the body. + """ + return self.get_body_data(name, [BodyProperty.POSITION, BodyProperty.ORIENTATION], wait=wait) + + def get_multiple_body_poses(self, body_names: List[str], wait: bool = False) -> Optional[Dict[str, Pose]]: + """ + Get the body poses from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The positions and orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_data(body_names, + {name: [BodyProperty.POSITION, BodyProperty.ORIENTATION] + for name in body_names + }, + wait=wait) + if data is not None: + return {name: Pose(data[name][BodyProperty.POSITION.value], + wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) + for name in body_names} + + def get_body_position(self, name: str, wait: bool = False) -> Optional[List[float]]: + """ + Get the body position from the multiverse server. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The position of the body. + """ + return self.get_body_property(name, BodyProperty.POSITION, wait=wait) + + def get_multiple_body_positions(self, body_names: List[str], + wait: bool = False) -> Optional[Dict[str, List[float]]]: + """ + Get the body positions from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The positions of the bodies as a dictionary. + """ + return self.get_multiple_body_properties(body_names, [BodyProperty.POSITION], wait=wait) + + def get_body_orientation(self, name: str, wait: bool = False) -> Optional[List[float]]: + """ + Get the body orientation from the multiverse server. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The orientation of the body. + """ + return self.get_body_property(name, BodyProperty.ORIENTATION, wait=wait) + + def get_multiple_body_orientations(self, body_names: List[str], + wait: bool = False) -> Optional[Dict[str, List[float]]]: + """ + Get the body orientations from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_properties(body_names, [BodyProperty.ORIENTATION], wait=wait) + if data is not None: + return {name: wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} + + def get_body_property(self, name: str, property_: Property, wait: bool = False) -> Optional[List[float]]: + """ + Get the body property from the multiverse server. + + :param name: The name of the body. + :param property_: The property of the body as a Property. + :param wait: Whether to wait for the data. + :return: The property of the body. + """ + data = self.get_body_data(name, [property_], wait=wait) + if data is not None: + return data[property_.value] + + def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], + wait: bool = False) -> Optional[Dict[str, Dict[str, List[float]]]]: + """ + Get the body properties from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param wait: Whether to wait for the data. + :return: The properties of the bodies as a dictionary. + """ + return self.get_multiple_body_data(body_names, {name: properties for name in body_names}, wait=wait) + + def get_body_data(self, name: str, + properties: Optional[List[Property]] = None, + wait: bool = False) -> Optional[Dict]: + """ + Get the body data from the multiverse server. + + :param name: The name of the body. + :param properties: The properties of the body. + :param wait: Whether to wait for the data. + :return: The body data as a dictionary. + """ + if wait: + return self.wait_for_body_data(name, properties) + + data = self.get_received_data() + if self.check_for_body_data(name, data, properties): + return data[name] + + def get_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[Property]]] = None, + wait: bool = False) -> Optional[Dict]: + """ + Get the body data from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param wait: Whether to wait for the data. + :return: The body data as a dictionary. + """ + + if wait: + return self.wait_for_multiple_body_data(body_names, properties) + + data = self.get_received_data() + if self.check_multiple_body_data(body_names, data, properties): + return {name: data[name] for name in body_names} + + def wait_for_body_data(self, name: str, properties: Optional[List[Property]] = None) -> Dict: + """ + Wait for the body data from the multiverse server. + + :param name: The name of the body. + :param properties: The properties of the body. + :return: The body data as a dictionary. + """ + return self._wait_for_body_data_template(name, self.check_for_body_data, properties)[name] + + def wait_for_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[Property]]] = None) -> Dict: + """ + Wait for the body data from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :return: The body data as a dictionary. + """ + return self._wait_for_body_data_template(body_names, self.check_multiple_body_data, properties) + + def _wait_for_body_data_template(self, body_names: Union[str, List[str]], + check_func: Callable[[Union[str, List[str]], Dict, Union[Dict, List]], bool], + properties: Optional[Union[Dict, List]] = None) -> Dict: + """ + Wait for the body data from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param check_func: The function to check if the data is received. + :return: The body data as a dictionary. + """ + start = time() + data_received_flag = False + while time() - start < self.MAX_WAIT_TIME_FOR_DATA.total_seconds(): + received_data = self.get_received_data() + data_received_flag = check_func(body_names, received_data, properties) + if data_received_flag: + return received_data + if not data_received_flag: + properties_str = "Data" if properties is None else f"Properties {properties}" + msg = f"{properties_str} for {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" + logging.error(msg) + raise ValueError(msg) + + def check_multiple_body_data(self, body_names: List[str], data: Dict, + properties: Optional[Dict[str, List[Property]]] = None) -> bool: + """ + Check if the body data is received from the multiverse server for multiple bodies. + + :param body_names: The names of the bodies. + :param data: The data received from the multiverse server. + :param properties: The properties of the bodies. + :return: Whether the body data is received. + """ + if properties is None: + return all([self.check_for_body_data(name, data) for name in body_names]) + else: + return all([self.check_for_body_data(name, data, properties[name]) for name in body_names]) + + @staticmethod + def check_for_body_data(name: str, data: Dict, properties: Optional[List[Property]] = None) -> bool: + """ + Check if the body data is received from the multiverse server. + + :param name: The name of the body. + :param data: The data received from the multiverse server. + :param properties: The properties of the body. + :return: Whether the body data is received. + """ + if properties is None: + return name in data + else: + return name in data and all([prop.value in data[name] and None not in data[name][prop.value] + for prop in properties]) + + def get_received_data(self): + """ + Get the latest received data from the multiverse server. + """ + self.data_lock.acquire() + data = self.response_meta_data["receive"] + self.data_lock.release() + return data + + def receive_all_data_from_server(self): + """ + Get all data from the multiverse server. + """ + while not self.stop_thread: + self.request_meta_data["receive"][""] = [""] + self.data_lock.acquire() + self.send_and_receive_meta_data() + self.data_lock.release() + sleep(0.01) + self.stop() + + def join(self): + self.thread.join() + + +class MultiverseWriter(MultiverseClient): + + def __init__(self, name: str, port: int, simulation: Optional[str] = None, + is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): + """ + Initialize the Multiverse writer, which writes the data to the Multiverse server. + This class provides methods to send data (e.g., position, orientation) to the Multiverse server. + + :param port: The port of the Multiverse writer client. + :param simulation: The name of the simulation that the writer is connected to + (usually the name defined in the .muv file). + :param is_prospection_world: Whether the writer is connected to the prospection world. + :param simulation_wait_time_factor: The wait time factor for the simulation (default is 1.0), which can be used + to increase or decrease the wait time for the simulation. + """ + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) + self.simulation = simulation + + def spawn_robot_with_actuators(self, robot_name: str, position: List[float], orientation: List[float], + actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None: + """ + Spawn the robot with controlled actuators in the simulation. + + :param robot_name: The name of the robot. + :param position: The position of the robot. + :param orientation: The orientation of the robot. + :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. + """ + send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value, + BodyProperty.RELATIVE_VELOCITY.value]} + relative_velocity = [0.0] * 6 + data = [self.sim_time, *position, *orientation, *relative_velocity] + self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands) + + def _reset_request_meta_data(self, set_simulation_name: bool = True): + """ + Reset the request metadata. + + :param set_simulation_name: Whether to set the simulation name to the value of self.simulation_name. + """ + self.request_meta_data = { + "meta_data": self._meta_data.__dict__.copy(), + "send": {}, + "receive": {}, + } + if self.simulation is not None and set_simulation_name: + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation + + def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]) -> None: + """ + Set the body pose in the simulation. + + :param body_name: The name of the body. + :param position: The position of the body. + :param orientation: The orientation of the body. + """ + self.send_body_data_to_server(body_name, + {BodyProperty.POSITION: position, + BodyProperty.ORIENTATION: orientation, + BodyProperty.RELATIVE_VELOCITY: [0.0] * 6}) + + def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: + """ + Set the body poses in the simulation for multiple bodies. + + :param body_data: The data to be sent for multiple bodies. + """ + self.send_multiple_body_data_to_server(body_data) + + def set_body_position(self, body_name: str, position: List[float]) -> None: + """ + Set the body position in the simulation. + + :param body_name: The name of the body. + :param position: The position of the body. + """ + self.set_body_property(body_name, BodyProperty.POSITION, position) + + def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: + """ + Set the body orientation in the simulation. + + :param body_name: The name of the body. + :param orientation: The orientation of the body. + """ + self.set_body_property(body_name, BodyProperty.ORIENTATION, orientation) + + def set_body_property(self, body_name: str, property_: Property, value: List[float]) -> None: + """ + Set the body property in the simulation. + + :param body_name: The name of the body. + :param property_: The property of the body. + :param value: The value of the property. + """ + self.send_body_data_to_server(body_name, {property_: value}) + + def remove_body(self, body_name: str) -> None: + """ + Remove the body from the simulation. + + :param body_name: The name of the body. + """ + self.send_data_to_server([self.sim_time], + send_meta_data={body_name: []}, + receive_meta_data={body_name: []}) + + def reset_world(self) -> None: + """ + Reset the world in the simulation. + """ + self.send_data_to_server([0], set_simulation_name=False) + + def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, List[float]]) -> Dict: + """ + Send data to the multiverse server. + + :param body_name: The name of the body. + :param body_data: The data to be sent. + :return: The response from the server. + """ + send_meta_data = {body_name: list(map(str, body_data.keys()))} + flattened_data = [value for data in body_data.values() for value in data] + return self.send_data_to_server([self.sim_time, *flattened_data], send_meta_data=send_meta_data) + + def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: + """ + Send data to the multiverse server for multiple bodies. + + :param body_data: The data to be sent for multiple bodies. + :return: The response from the server. + """ + send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} + response_meta_data = self.send_meta_data_and_get_response(send_meta_data) + body_names = list(response_meta_data["send"].keys()) + flattened_data = [value for body_name in body_names for data in body_data[body_name].values() + for value in data] + self.send_data = [self.sim_time, *flattened_data] + self.send_and_receive_data() + return self.response_meta_data + + def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: + """ + Send metadata to the multiverse server and get the response. + + :param send_meta_data: The metadata to be sent. + :return: The response from the server. + """ + self._reset_request_meta_data() + self.request_meta_data["send"] = send_meta_data + self.send_and_receive_meta_data() + return self.response_meta_data + + def send_data_to_server(self, data: List, + send_meta_data: Optional[Dict] = None, + receive_meta_data: Optional[Dict] = None, + set_simulation_name: bool = True) -> Dict: + """ + Send data to the multiverse server. + + :param data: The data to be sent. + :param send_meta_data: The metadata to be sent. + :param receive_meta_data: The metadata to be received. + :param set_simulation_name: Whether to set the simulation name to the value of self.simulation. + :return: The response from the server. + """ + self._reset_request_meta_data(set_simulation_name=set_simulation_name) + if send_meta_data: + self.request_meta_data["send"] = send_meta_data + if receive_meta_data: + self.request_meta_data["receive"] = receive_meta_data + self.send_and_receive_meta_data() + self.send_data = data + self.send_and_receive_data() + return self.response_meta_data + + +class MultiverseController(MultiverseWriter): + + def __init__(self, name: str, port: int, is_prospection_world: bool = False, **kwargs): + """ + Initialize the Multiverse controller, which controls the robot in the simulation. + This class provides methods to send controller data to the Multiverse server. + + :param port: The port of the Multiverse controller client. + :param is_prospection_world: Whether the controller is connected to the prospection world. + """ + super().__init__(name, port, is_prospection_world=is_prospection_world) + + def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None: + """ + Initialize the controller by sending the controller data to the multiverse server. + + :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. + """ + self.send_data_to_server([self.sim_time] + [0.0] * len(actuator_joint_commands), + send_meta_data=actuator_joint_commands) + + +class MultiverseAPI(MultiverseClient): + API_REQUEST_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=200) + """ + The wait time for the API request in seconds. + """ + APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH] + + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0): + """ + Initialize the Multiverse API, which sends API requests to the Multiverse server. + This class provides methods like attach and detach objects, get contact points, and other API requests. + + :param port: The port of the Multiverse API client. + :param simulation: The name of the simulation that the API is connected to + (usually the name defined in the .muv file). + :param is_prospection_world: Whether the API is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor, which can be used to increase or decrease + the wait time for the simulation. + """ + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) + self.simulation = simulation + self.wait: bool = False # Whether to wait after sending the API request. + + def save(self, save_name: str, save_directory: Optional[str] = None) -> str: + """ + Save the current state of the simulation. + + :param save_name: The name of the save. + :param save_directory: The path to save the simulation, can be relative or absolute. If the path is relative, + it will be saved in the saved folder in multiverse. + :return: The save path. + """ + response = self._request_single_api_callback(API.SAVE, self.get_save_path(save_name, save_directory)) + return response[0] + + def load(self, save_name: str, save_directory: Optional[str] = None) -> None: + """ + Load the saved state of the simulation. + + :param save_name: The name of the save. + :param save_directory: The path to load the simulation, can be relative or absolute. If the path is relative, + it will be loaded from the saved folder in multiverse. + """ + self._request_single_api_callback(API.LOAD, self.get_save_path(save_name, save_directory)) + + @staticmethod + def get_save_path(save_name: str, save_directory: Optional[str] = None) -> str: + """ + Get the save path. + + :param save_name: The save name. + :param save_directory: The save directory. + :return: The save path. + """ + return save_name if save_directory is None else os.path.join(save_directory, save_name) + + def attach(self, constraint: Constraint) -> None: + """ + Request to attach the child link to the parent link. + + :param constraint: The constraint. + """ + self.wait = True + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + attachment_pose = self._get_attachment_pose_as_string(constraint) + self._attach(child_link_name, parent_link_name, attachment_pose) + + def _attach(self, child_link_name: str, parent_link_name: str, attachment_pose: str) -> None: + """ + Attach the child link to the parent link. + + :param child_link_name: The name of the child link. + :param parent_link_name: The name of the parent link. + :param attachment_pose: The attachment pose. + """ + self._request_single_api_callback(API.ATTACH, child_link_name, parent_link_name, + attachment_pose) + + def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: + """ + Get the link names of the constraint. + + :param constraint: The constraint. + :return: The link names of the constraint. + """ + return self.get_parent_link_name(constraint), self.get_constraint_child_link_name(constraint) + + def get_parent_link_name(self, constraint: Constraint) -> str: + """ + Get the parent link name of the constraint. + + :param constraint: The constraint. + :return: The parent link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.parent_link) + + def get_constraint_child_link_name(self, constraint: Constraint) -> str: + """ + Get the child link name of the constraint. + + :param constraint: The constraint. + :return: The child link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.child_link) + + @staticmethod + def get_link_name_for_constraint(link: Link) -> str: + """ + Get the link name from link object, if the link belongs to a one link object, return the object name. + + :param link: The link. + :return: The link name. + """ + return link.name if not link.is_only_link else link.object.name + + def detach(self, constraint: Constraint) -> None: + """ + Request to detach the child link from the parent link. + + :param constraint: The constraint. + """ + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + self._detach(child_link_name, parent_link_name) + + def _detach(self, child_link_name: str, parent_link_name: str) -> None: + """ + Detach the child link from the parent link. + + :param child_link_name: The name of the child link. + :param parent_link_name: The name of the parent link. + """ + self._request_single_api_callback(API.DETACH, child_link_name, parent_link_name) + + def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: + """ + Get the attachment pose as a string. + + :param constraint: The constraint. + :return: The attachment pose as a string. + """ + pose = constraint.parent_to_child_transform.to_pose() + return self._pose_to_string(pose) + + @staticmethod + def _pose_to_string(pose: Pose) -> str: + """ + Convert the pose to a string. + + :param pose: The pose. + :return: The pose as a string. + """ + return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ + f"{pose.orientation.y} {pose.orientation.z}" + + def check_object_exists(self, obj: Object) -> bool: + """ + Check if the object exists in the simulation. + + :param obj: The object. + :return: Whether the object exists in the simulation. + """ + return self._request_single_api_callback(API.EXIST, obj.name)[0] == 'yes' + + def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: + """ + Request the contact points of an object, this includes the object names and the contact forces and torques. + + :param obj: The object. + :return: The contact points of the object as a list of MultiverseContactPoint. + """ + api_response_data = self._get_contact_points(obj.name) + body_names = api_response_data[API.GET_CONTACT_BODIES] + contact_efforts = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT]) + return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) + for i in range(len(body_names))] + + def get_objects_intersected_with_rays(self, from_positions: List[List[float]], + to_positions: List[List[float]]) -> List[RayResult]: + """ + Get the rays intersections with the objects from the from_positions to the to_positions. + + :param from_positions: The starting positions of the rays. + :param to_positions: The ending positions of the rays. + :return: The rays intersections with the objects as a list of RayResult. + """ + get_rays_response = self._get_rays(from_positions, to_positions) + return self._parse_get_rays_response(get_rays_response) + + def _get_rays(self, from_positions: List[List[float]], + to_positions: List[List[float]]) -> List[str]: + """ + Get the rays intersections with the objects from the from_positions to the to_positions. + + :param from_positions: The starting positions of the rays. + :param to_positions: The ending positions of the rays. + :return: The rays intersections with the objects as a dictionary. + """ + from_positions = self.list_of_positions_to_string(from_positions) + to_positions = self.list_of_positions_to_string(to_positions) + return self._request_single_api_callback(API.GET_RAYS, from_positions, to_positions) + + @staticmethod + def _parse_get_rays_response(response: List[str]) -> List[RayResult]: + """ + Parse the response of the get rays API. + + :param response: The response of the get rays API as a list of strings. + :return: The rays as a list of lists of floats. + """ + get_rays_results = [] + for ray_response in response: + if ray_response == "None": + get_rays_results.append(RayResult("", -1)) + else: + result = ray_response.split() + result[1] = float(result[1]) + get_rays_results.append(RayResult(*result)) + return get_rays_results + + @staticmethod + def list_of_positions_to_string(positions: List[List[float]]) -> str: + """ + Convert the list of positions to a string. + + :param positions: The list of positions. + :return: The list of positions as a string. + """ + return " ".join([f"{position[0]} {position[1]} {position[2]}" for position in positions]) + + @staticmethod + def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: + """ + Parse the contact effort of an object. + + :param contact_effort: The contact effort of the object as a list of strings. + :return: The contact effort of the object as a list of floats. + """ + contact_effort = contact_effort[0].split() + if 'failed' in contact_effort: + logwarn("Failed to get contact effort") + return [0.0] * 6 + return list(map(float, contact_effort)) + + def _get_contact_points(self, object_name) -> Dict[API, List]: + """ + Request the contact points of an object. + + :param object_name: The name of the object. + :return: The contact points api response as a dictionary. + """ + return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], + API.GET_CONSTRAINT_EFFORT: [object_name] + }) + + def pause_simulation(self) -> None: + """ + Pause the simulation. + """ + self._request_single_api_callback(API.PAUSE) + + def unpause_simulation(self) -> None: + """ + Unpause the simulation. + """ + self._request_single_api_callback(API.UNPAUSE) + + def _request_single_api_callback(self, api_name: API, *params) -> List[str]: + """ + Request a single API callback from the server. + + :param api_data: The API data to request the callback. + :return: The API response as a list of strings. + """ + response = self._request_apis_callbacks({api_name: list(params)}) + return response[api_name] + + def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[str]]: + """ + Request the API callbacks from the server. + + :param api_data: The API data to add to the request metadata. + :return: The API response as a list of strings. + """ + self._reset_api_callback() + for api_name, params in api_data.items(): + self._add_api_request(api_name.value, *params) + self._send_api_request() + responses = self._get_all_apis_responses() + if self.wait: + sleep(self.API_REQUEST_WAIT_TIME.total_seconds() * self.simulation_wait_time_factor) + self.wait = False + return responses + + def _get_all_apis_responses(self) -> Dict[API, List[str]]: + """ + Get all the API responses from the server. + + :return: The API responses as a list of APIData. + """ + list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] + return {API[api_name.upper()]: response for api_response in list_of_api_responses + for api_name, response in api_response.items()} + + def _add_api_request(self, api_name: str, *params): + """ + Add an API request to the request metadata. + + :param api_name: The name of the API. + :param params: The parameters of the API. + """ + self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) + + def _send_api_request(self): + """ + Send the API request to the server. + """ + if "api_callbacks" not in self.request_meta_data: + logging.error("No API request to send") + raise ValueError + self.send_and_receive_meta_data() + self.request_meta_data.pop("api_callbacks") + + def _reset_api_callback(self): + """ + Initialize the API callback in the request metadata. + """ + self.request_meta_data["api_callbacks"] = {self.simulation: []} diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py new file mode 100644 index 000000000..863a45d8a --- /dev/null +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 + +"""Multiverse Client base class.""" + +from multiverse_client_pybind import MultiverseClientPybind # noqa +from typing_extensions import Optional, List, Dict, Callable, TypeVar + +from ...datastructures.dataclasses import MultiverseMetaData +from ...config.multiverse_conf import MultiverseConfig as Conf +from ...ros.logging import loginfo, logwarn + +T = TypeVar("T") + + +class MultiverseSocket: + + def __init__( + self, + port: str, + host: str = Conf.HOST, + meta_data: MultiverseMetaData = MultiverseMetaData(), + ) -> None: + """ + Initialize the MultiverseSocket, connect to the Multiverse Server and start the communication. + + :param port: The port of the client. + :param host: The host of the client. + :param meta_data: The metadata for the Multiverse Client as MultiverseMetaData. + """ + if not isinstance(port, str) or port == "": + raise ValueError(f"Must specify client port for {self.__class__.__name__}") + self._send_data = None + self.port = port + self.host = host + self._meta_data = meta_data + self.client_name = self._meta_data.simulation_name + self._multiverse_socket = MultiverseClientPybind( + f"{Conf.SERVER_HOST}:{Conf.SERVER_PORT}" + ) + self.request_meta_data = { + "meta_data": self._meta_data.__dict__, + "send": {}, + "receive": {}, + } + self._api_callbacks: Optional[Dict] = None + + self._start_time = 0.0 + + def run(self) -> None: + """Run the client.""" + self.log_info("Start") + self._run() + + def _run(self) -> None: + """Run the client, should call the _connect_and_start() method. It's left to the user to implement this method + in threaded or non-threaded fashion. + """ + self._connect_and_start() + + def stop(self) -> None: + """Stop the client.""" + self._disconnect() + + @property + def request_meta_data(self) -> Dict: + """The request_meta_data which is sent to the server. + """ + return self._request_meta_data + + @request_meta_data.setter + def request_meta_data(self, request_meta_data: Dict) -> None: + """Set the request_meta_data, make sure to clear the `send` and `receive` field before setting the request + """ + self._request_meta_data = request_meta_data + self._multiverse_socket.set_request_meta_data(self._request_meta_data) + + @property + def response_meta_data(self) -> Dict: + """Get the response_meta_data. + + :return: The response_meta_data as a dictionary. + """ + response_meta_data = self._multiverse_socket.get_response_meta_data() + assert isinstance(response_meta_data, dict) + if response_meta_data == {}: + message = f"[Client {self.port}] Receive empty response meta data." + self.log_warn(message) + return response_meta_data + + def send_and_receive_meta_data(self): + """ + Send and receive the metadata, this should be called before sending and receiving data. + """ + self._communicate(True) + + def send_and_receive_data(self): + """ + Send and receive the data, this should be called after sending and receiving the metadata. + """ + self._communicate(False) + + @property + def send_data(self) -> List[float]: + """Get the send_data.""" + return self._send_data + + @send_data.setter + def send_data(self, send_data: List[float]) -> None: + """Set the send_data, the first element should be the current simulation time, + the rest should be the data to send with the following order: + double -> uint8_t -> uint16_t + + :param send_data: The data to send. + """ + assert isinstance(send_data, list) + self._send_data = send_data + self._multiverse_socket.set_send_data(self._send_data) + + @property + def receive_data(self) -> List[float]: + """Get the receive_data, the first element should be the current simulation time, + the rest should be the received data with the following order: + double -> uint8_t -> uint16_t + + :return: The received data. + """ + receive_data = self._multiverse_socket.get_receive_data() + assert isinstance(receive_data, list) + return receive_data + + @property + def api_callbacks(self) -> Dict[str, Callable[[List[str]], List[str]]]: + """Get the api_callbacks. + + :return: The api_callbacks as a dictionary of function names and their respective callbacks. + """ + return self._api_callbacks + + @api_callbacks.setter + def api_callbacks(self, api_callbacks: Dict[str, Callable[[List[str]], List[str]]]) -> None: + """Set the api_callbacks. + + :param api_callbacks: The api_callbacks as a dictionary of function names and their respective callbacks. + """ + self._multiverse_socket.set_api_callbacks(api_callbacks) + self._api_callbacks = api_callbacks + + def _bind_request_meta_data(self, request_meta_data: T) -> T: + """Bind the request_meta_data before sending it to the server. + + :param request_meta_data: The request_meta_data to bind. + :return: The bound request_meta_data. + """ + pass + + def _bind_response_meta_data(self, response_meta_data: T) -> T: + """Bind the response_meta_data after receiving it from the server. + + :param response_meta_data: The response_meta_data to bind. + :return: The bound response_meta_data. + """ + pass + + def _bind_send_data(self, send_data: T) -> T: + """Bind the send_data before sending it to the server. + + :param send_data: The send_data to bind. + :return: The bound send_data. + """ + pass + + def _bind_receive_data(self, receive_data: T) -> T: + """Bind the receive_data after receiving it from the server. + + :param receive_data: The receive_data to bind. + :return: The bound receive_data. + """ + pass + + def _connect_and_start(self) -> None: + """Connect to the server and start the client. + """ + self._multiverse_socket.connect(self.host, self.port) + self._multiverse_socket.start() + self._start_time = self._multiverse_socket.get_time_now() + + def _disconnect(self) -> None: + """Disconnect from the server. + """ + self._multiverse_socket.disconnect() + + def _communicate(self, resend_request_meta_data: bool = False) -> bool: + """Communicate with the server. + + :param resend_request_meta_data: Resend the request metadata. + :return: True if the communication was successful, False otherwise. + """ + return self._multiverse_socket.communicate(resend_request_meta_data) + + def _restart(self) -> None: + """Restart the client. + """ + self._disconnect() + self._connect_and_start() + + def log_info(self, message: str) -> None: + """Log information. + + :param message: The message to log. + """ + loginfo(self._message_template(message)) + + def log_warn(self, message: str) -> None: + """Warn the user. + + :param message: The message to warn about. + """ + logwarn(self._message_template(message)) + + def _message_template(self, message: str) -> str: + return (f"[{self.__class__.__name__}:{self.port}]: {message} : sim time {self.sim_time}," + f" world time {self.world_time}") + + @property + def world_time(self) -> float: + """Get the world time from the server. + + :return: The world time. + """ + return self._multiverse_socket.get_world_time() + + @property + def sim_time(self) -> float: + """Get the current simulation time. + + :return: The current simulation time. + """ + return self._multiverse_socket.get_time_now() - self._start_time diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 54a48922e..4bb0a27b9 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -2,6 +2,7 @@ import unittest import pycram.tasktree +from pycram.datastructures.world import UseProspectionWorld from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose @@ -9,7 +10,7 @@ from pycram.process_module import ProcessModule from pycram.datastructures.enums import ObjectType, WorldMode from pycram.object_descriptors.urdf import ObjectDescription -from pycram.ros.viz_marker_publisher import VizMarkerPublisher +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI @@ -29,22 +30,26 @@ def setUpClass(cls): RobotDescription.current_robot_description.name + cls.extension) cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False cls.viz_marker_publisher = VizMarkerPublisher() OntologyManager(SOMA_ONTOLOGY_IRI) def setUp(self): - self.world.reset_world() + self.world.reset_world(remove_saved_states=True) + with UseProspectionWorld(): + pass # DO NOT WRITE TESTS HERE!!! # Test related to the BulletWorld should be written in test_bullet_world.py # Tests in here would not be properly executed in the CI def tearDown(self): - pycram.tasktree.reset_tree() + pycram.tasktree.task_tree.reset_tree() time.sleep(0.05) - self.world.reset_world() + self.world.reset_world(remove_saved_states=True) + with UseProspectionWorld(): + pass @classmethod def tearDownClass(cls): @@ -67,7 +72,7 @@ def setUpClass(cls): RobotDescription.current_robot_description.name + cls.extension) cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False cls.viz_marker_publisher = VizMarkerPublisher() diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 9ac352828..df060aa22 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -17,16 +17,14 @@ class TestActionDesignatorGrounding(BulletWorldTestCase): def test_move_torso(self): description = action_designator.MoveTorsoAction([0.3]) - # SOMA ontology seems not provide a corresponding concept yet for MoveTorso - #self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().position, 0.3) with simulated_robot: description.resolve().perform() - self.assertEqual(self.world.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint), 0.3) + self.assertEqual(self.world.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint), + 0.3) def test_set_gripper(self): description = action_designator.SetGripperAction([Arms.LEFT], [GripperState.OPEN, GripperState.CLOSE]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().gripper, Arms.LEFT) self.assertEqual(description.ground().motion, GripperState.OPEN) self.assertEqual(len(list(iter(description))), 2) @@ -38,21 +36,18 @@ def test_set_gripper(self): def test_release(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.ReleaseAction([Arms.LEFT], object_description) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().gripper, Arms.LEFT) self.assertEqual(description.ground().object_designator.name, "milk") def test_grip(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.GripAction([Arms.LEFT], object_description, [0.5]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().gripper, Arms.LEFT) self.assertEqual(description.ground().object_designator.name, "milk") def test_park_arms(self): description = action_designator.ParkArmsAction([Arms.BOTH]) self.assertEqual(description.ground().arm, Arms.BOTH) - self.assertTrue(description.ontology_concept_holders) with simulated_robot: description.resolve().perform() for joint, pose in RobotDescription.current_robot_description.get_static_joint_chain("right", "park").items(): @@ -66,13 +61,11 @@ def test_navigate(self): with simulated_robot: description.resolve().perform() self.assertEqual(description.ground().target_location, Pose([0.3, 0, 0], [0, 0, 0, 1])) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(self.robot.get_pose(), Pose([0.3, 0, 0])) def test_pick_up(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PickUpAction(object_description, [Arms.LEFT], [Grasp.FRONT]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() @@ -83,7 +76,6 @@ def test_pick_up(self): def test_place(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() @@ -94,7 +86,6 @@ def test_place(self): def test_look_at(self): description = action_designator.LookAtAction([Pose([1, 0, 1])]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().target, Pose([1, 0, 1])) with simulated_robot: description.resolve().perform() @@ -105,7 +96,6 @@ def test_detect(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.DetectAction(object_description) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: detected_object = description.resolve().perform() @@ -118,14 +108,12 @@ def test_detect(self): def test_open(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.OpenAction(object_description, [Arms.LEFT]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().object_designator.name, "milk") @unittest.skip def test_close(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.CloseAction(object_description, [Arms.LEFT]) - self.assertTrue(description.ontology_concept_holders) self.assertEqual(description.ground().object_designator.name, "milk") def test_transport(self): @@ -134,7 +122,6 @@ def test_transport(self): [Arms.LEFT], [Pose([-1.35, 0.78, 0.95], [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]) - self.assertTrue(description.ontology_concept_holders) with simulated_robot: action_designator.MoveTorsoAction([0.2]).resolve().perform() description.resolve().perform() @@ -148,7 +135,6 @@ def test_grasping(self): self.robot.set_pose(Pose([-2.14, 1.06, 0])) milk_desig = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.GraspingAction([Arms.RIGHT], milk_desig) - self.assertTrue(description.ontology_concept_holders) with simulated_robot: description.resolve().perform() dist = np.linalg.norm( @@ -161,7 +147,3 @@ def test_facing(self): FaceAtPerformable(self.milk.pose).perform() milk_in_robot_frame = LocalTransformer().transform_to_object_frame(self.milk.pose, self.robot) self.assertAlmostEqual(milk_in_robot_frame.position.y, 0.) - - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_attachment.py b/test/test_attachment.py index d521d752a..bf8487942 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -20,6 +20,20 @@ def test_detach(self): self.assertTrue(self.robot not in self.milk.attachments) self.assertTrue(self.milk not in self.robot.attachments) + def test_detach_sync_in_prospection_world(self): + self.milk.attach(self.robot) + with UseProspectionWorld(): + pass + self.milk.detach(self.robot) + with UseProspectionWorld(): + pass + self.assertTrue(self.milk not in self.robot.attachments) + self.assertTrue(self.robot not in self.milk.attachments) + prospection_milk = self.world.get_prospection_object_for_object(self.milk) + prospection_robot = self.world.get_prospection_object_for_object(self.robot) + self.assertTrue(prospection_milk not in prospection_robot.attachments) + self.assertTrue(prospection_robot not in prospection_milk.attachments) + def test_attachment_behavior(self): self.robot.attach(self.milk) @@ -52,27 +66,28 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): time.sleep(0.05) milk_2.attach(cereal_2) time.sleep(0.05) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - # self.assertTrue(cereal_2 not in prospection_milk.attachments) - prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) - # self.assertTrue(prospection_cereal in prospection_milk.attachments) - self.assertTrue(prospection_milk.attachments == {}) - - # Assert that when prospection object is moved, the real object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + # self.assertTrue(cereal_2 not in prospection_milk.attachments) + prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) + # self.assertTrue(prospection_cereal in prospection_milk.attachments) + self.assertTrue(prospection_cereal in prospection_milk.attachments.keys()) + + # Assert that when prospection object is moved, the real object is not moved prospection_milk_pos = prospection_milk.get_position() cereal_pos = cereal_2.get_position() - prospection_cereal_pos = prospection_cereal.get_position() + estimated_prospection_cereal_pos = prospection_cereal.get_position() + estimated_prospection_cereal_pos.x += 1 # Move prospection milk object prospection_milk_pos.x += 1 prospection_milk.set_position(prospection_milk_pos) - # Prospection object should not move + # Prospection cereal should move since it is attached to prospection milk new_prospection_cereal_pose = prospection_cereal.get_position() - self.assertTrue(new_prospection_cereal_pose == prospection_cereal_pos) + self.assertAlmostEqual(new_prospection_cereal_pose.x, estimated_prospection_cereal_pos.x, delta=0.01) - # Real cereal object should not move + # Also Real cereal object should not move since it is not affected by prospection milk new_cereal_pos = cereal_2.get_position() assumed_cereal_pos = cereal_pos self.assertTrue(new_cereal_pos == assumed_cereal_pos) @@ -80,22 +95,6 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): self.world.remove_object(milk_2) self.world.remove_object(cereal_2) - def test_no_attachment_in_prospection_world(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) - - milk_2.attach(cereal_2) - - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) - - self.assertTrue(prospection_milk.attachments == {}) - self.assertTrue(prospection_cereal.attachments == {}) - - self.world.remove_object(milk_2) - self.world.remove_object(cereal_2) - def test_attaching_to_robot_and_moving(self): self.robot.attach(self.milk) milk_pos = self.milk.get_position() @@ -106,5 +105,3 @@ def test_attaching_to_robot_and_moving(self): new_milk_pos = self.milk.get_position() self.assertEqual(new_milk_pos.x, milk_pos.x + 1) - - diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index ec398df7a..565e98342 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -11,7 +11,7 @@ from pycram.object_descriptors.urdf import ObjectDescription from pycram.datastructures.dataclasses import Color from pycram.world_concepts.world_object import Object -from pycram.datastructures.world import UseProspectionWorld +from pycram.datastructures.world import UseProspectionWorld, World fix_missing_inertial = ObjectDescription.fix_missing_inertial @@ -53,8 +53,7 @@ def test_remove_object(self): self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", - ObjectDescription, pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) def test_remove_robot(self): robot_id = self.robot.id @@ -65,7 +64,7 @@ def test_remove_robot(self): RobotDescription.current_robot_description.name + self.extension) def test_get_joint_position(self): - self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) + self.assertAlmostEqual(self.robot.get_joint_position("head_pan_joint"), 0.0, delta=0.01) def test_get_object_contact_points(self): self.assertEqual(len(self.robot.contact_points()), 0) @@ -136,51 +135,34 @@ def test_equal_world_states(self): time.sleep(2.5) self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1])) self.assertFalse(self.world.world_sync.check_for_equal()) - self.world.prospection_world.object_states = self.world.current_state.object_states - time.sleep(0.05) - self.assertTrue(self.world.world_sync.check_for_equal()) + with UseProspectionWorld(): + self.assertTrue(self.world.world_sync.check_for_equal()) def test_add_resource_path(self): self.world.add_resource_path("test") - self.assertTrue("test" in self.world.data_directory) + self.assertTrue("test" in self.world.get_data_directories()) def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - time.sleep(0.05) try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) - time.sleep(0.1) self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) - except ValueError as e: - self.assertTrue(True) - - def test_no_object_found_for_given_prospection_object(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - time.sleep(0.05) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) - try: - self.world.remove_object(milk_2) - self.world.get_object_for_prospection_object(prospection_milk) - time.sleep(0.1) - self.assertFalse(True) - except ValueError as e: + except KeyError as e: self.assertTrue(True) - time.sleep(0.05) def test_real_object_position_does_not_change_with_prospection_object(self): milk_2_pos = [1.3, 1, 0.9] milk_2 = Object("milk_3", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_milk_pos = prospection_milk.get_position() - self.assertTrue(prospection_milk_pos == milk_2_pos) # Assert that when prospection object is moved, the real object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + prospection_milk_pos = prospection_milk.get_position() + self.assertTrue(prospection_milk_pos == milk_2_pos) prospection_milk_pos.x += 1 prospection_milk.set_position(prospection_milk_pos) self.assertTrue(prospection_milk.get_position() != milk_2.get_position()) @@ -191,32 +173,32 @@ def test_prospection_object_position_does_not_change_with_real_object(self): milk_2 = Object("milk_4", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_milk_pos = prospection_milk.get_position() - self.assertTrue(prospection_milk_pos == milk_2_pos) # Assert that when real object is moved, the prospection object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + prospection_milk_pos = prospection_milk.get_position() + self.assertTrue(prospection_milk_pos == milk_2_pos) milk_2_pos.x += 1 milk_2.set_position(milk_2_pos) self.assertTrue(prospection_milk.get_position() != milk_2.get_position()) self.world.remove_object(milk_2) def test_add_vis_axis(self): - self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame())) + self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link())) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) def test_add_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id = self.world.add_text("test", link.position_as_list, link.orientation_as_list, 1, Color(1, 0, 0, 1), 3, link.object_id, link.id) if self.world.mode == WorldMode.GUI: time.sleep(4) def test_remove_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, @@ -229,7 +211,7 @@ def test_remove_text(self): time.sleep(3) def test_remove_all_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 3fafe27d4..8d8c1061b 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -20,28 +20,35 @@ def test_visible(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) - camera_frame = RobotDescription.current_robot_description.get_camera_frame() - self.world.add_vis_axis(self.robot.get_link_pose(camera_frame)) - self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(camera_frame), + camera_link = RobotDescription.current_robot_description.get_camera_link() + self.world.add_vis_axis(self.robot.get_link_pose(camera_link)) + self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(camera_link), RobotDescription.current_robot_description.get_default_camera().front_facing_axis)) def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) - self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()), + self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose( + RobotDescription.current_robot_description.get_camera_link()), RobotDescription.current_robot_description.get_default_camera().front_facing_axis) != []) def test_reachable(self): self.robot.set_pose(Pose()) time.sleep(1) - self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame())) - self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame())) + self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains[ + "right"].get_tool_frame())) + self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains[ + "right"].get_tool_frame())) def test_blocking(self): self.milk.set_pose(Pose([0.5, -0.7, 1])) self.robot.set_pose(Pose()) time.sleep(2) - self.assertTrue(btr.blocking(Pose([0.5, -0.7, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame()) != []) + blocking = btr.blocking(Pose([0.5, -0.7, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame()) + self.assertTrue(blocking != []) def test_supporting(self): self.milk.set_pose(Pose([1.3, 0, 0.9])) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index bad803f22..9f208d90f 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,20 +1,18 @@ +import os from pathlib import Path from bullet_world_testcase import BulletWorldTestCase -from pycram.datastructures.enums import ObjectType -from pycram.world_concepts.world_object import Object -import pathlib +from pycram.object_descriptors.urdf import ObjectDescription as URDFObject +from pycram.config import world_conf as conf class TestCacheManager(BulletWorldTestCase): def test_generate_description_and_write_to_cache(self): cache_manager = self.world.cache_manager - file_path = pathlib.Path(__file__).parent.resolve() - path = str(file_path) + "/../resources/apartment.urdf" + path = os.path.join(self.world.conf.resources_path, "objects/apartment.urdf") extension = Path(path).suffix - cache_path = self.world.cache_dir + "apartment.urdf" - apartment = Object("apartment", ObjectType.ENVIRONMENT, path) - cache_manager.generate_description_and_write_to_cache(path, apartment.name, extension, cache_path, - apartment.description) - self.assertTrue(cache_manager.is_cached(path, apartment.description)) + cache_path = os.path.join(self.world.conf.cache_dir, "apartment.urdf") + apartment = URDFObject(path) + apartment.generate_description_from_file(path, "apartment", extension, cache_path) + self.assertTrue(cache_manager.is_cached(path, apartment)) diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 3258353e1..504bbe1a5 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,3 +1,5 @@ +import unittest + import numpy as np from random_events.variable import Continuous # import plotly.graph_objects as go @@ -5,11 +7,12 @@ from random_events.interval import * from bullet_world_testcase import BulletWorldTestCase -from pycram.costmaps import OccupancyCostmap +from pycram.costmaps import OccupancyCostmap, AlgebraicSemanticCostmap from pycram.datastructures.pose import Pose +import plotly.graph_objects as go -class TestCostmapsCase(BulletWorldTestCase): +class CostmapTestCase(BulletWorldTestCase): def test_raytest_bug(self): for i in range(30): @@ -55,3 +58,35 @@ def test_visualize(self): o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, origin=Pose([0, 0, 0], [0, 0, 0, 1])) o.visualize() + + +class SemanticCostmapTestCase(BulletWorldTestCase): + + def test_generate_map(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.left() + costmap.valid_area &= costmap.top() + costmap.valid_area &= costmap.border(0.2) + self.assertEqual(len(costmap.valid_area.simple_sets), 2) + + def test_as_distribution(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.right() & costmap.bottom() & costmap.border(0.2) + model = costmap.as_distribution() + self.assertEqual(len(model.nodes), 7) + # fig = go.Figure(model.plot(), model.plotly_layout()) + # fig.show() + # supp = model.support + # fig = go.Figure(supp.plot(), supp.plotly_layout()) + # fig.show() + + def test_iterate(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.left() & costmap.top() & costmap.border(0.2) + for sample in iter(costmap): + self.assertIsInstance(sample, Pose) + self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) + + +class OntologySemanticLocationTestCase(unittest.TestCase): + ... \ No newline at end of file diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 5f015d831..7392dbac2 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -2,7 +2,7 @@ import unittest import sqlalchemy import sqlalchemy.orm -import pycram.plan_failures +import pycram.failures from pycram.world_concepts.world_object import Object from pycram.datastructures.world import World from pycram.designators import action_designator @@ -24,7 +24,7 @@ pycrorm_uri = "mysql+pymysql://" + pycrorm_uri -@unittest.skipIf(pycrorm_uri is None, "pycrorm database is not available.") +@unittest.skip class DatabaseResolverTestCase(unittest.TestCase,): world: World milk: Object @@ -37,7 +37,8 @@ def setUpClass(cls) -> None: global pycrorm_uri cls.world = BulletWorld(WorldMode.DIRECT) cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(robot_description.name, ObjectType.ROBOT, RobotDescription.current_robot_description.name + ".urdf") + cls.robot = Object(RobotDescription.current_robot_description.name, + ObjectType.ROBOT, RobotDescription.current_robot_description.name + ".urdf") ProcessModule.execution_delay = False cls.engine = sqlalchemy.create_engine(pycrorm_uri) diff --git a/test/test_description.py b/test/test_description.py index a0d12b3b2..e324384ac 100644 --- a/test/test_description.py +++ b/test/test_description.py @@ -1,3 +1,4 @@ +import os.path import pathlib from bullet_world_testcase import BulletWorldTestCase @@ -22,10 +23,16 @@ def test_joint_child_link(self): def test_generate_description_from_mesh(self): file_path = pathlib.Path(__file__).parent.resolve() - self.assertTrue(self.milk.description.generate_description_from_file(str(file_path) + "/../resources/cached/milk.stl", - "milk", ".stl")) + cache_path = self.world.cache_manager.cache_dir + cache_path = os.path.join(cache_path, f"{self.milk.description.name}.urdf") + self.milk.description.generate_from_mesh_file(str(file_path) + "/../resources/milk.stl", "milk", cache_path) + self.assertTrue(self.world.cache_manager.is_cached(f"{self.milk.name}", self.milk.description)) def test_generate_description_from_description_file(self): file_path = pathlib.Path(__file__).parent.resolve() - self.assertTrue(self.milk.description.generate_description_from_file(str(file_path) + "/../resources/cached/milk.urdf", - "milk", ".urdf")) + file_extension = self.robot.description.get_file_extension() + pr2_path = str(file_path) + f"/../resources/robots/{self.robot.description.name}{file_extension}" + cache_path = self.world.cache_manager.cache_dir + cache_path = os.path.join(cache_path, f"{self.robot.description.name}.urdf") + self.robot.description.generate_from_description_file(pr2_path, cache_path) + self.assertTrue(self.world.cache_manager.is_cached(self.robot.name, self.robot.description)) diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py new file mode 100644 index 000000000..63bf06416 --- /dev/null +++ b/test/test_error_checkers.py @@ -0,0 +1,131 @@ +from unittest import TestCase + +import numpy as np +from tf.transformations import quaternion_from_euler + +from pycram.datastructures.enums import JointType +from pycram.validation.error_checkers import calculate_angle_between_quaternions, \ + PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ + PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker + +from pycram.datastructures.pose import Pose + + +class TestErrorCheckers(TestCase): + @classmethod + def setUpClass(cls): + pass + + @classmethod + def tearDownClass(cls): + pass + + def tearDown(self): + pass + + def test_calculate_quaternion_error(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, 0.0) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + + def test_pose_error_checker(self): + pose_1 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + pose_2 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + error_checker = PoseErrorChecker() + error = error_checker.calculate_error(pose_1, pose_2) + self.assertEqual(error, [0.0, 0.0]) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + pose_2 = Pose([0, 1, np.sqrt(3)], quat) + error = error_checker.calculate_error(pose_1, pose_2) + self.assertAlmostEqual(error[0], 2, places=5) + self.assertEqual(error[1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi/360) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi / 179) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + + def test_position_error_checker(self): + position_1 = [0.0, 0.0, 0.0] + position_2 = [0.0, 0.0, 0.0] + error_checker = PositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = [1.0, 1.0, 1.0] + error = error_checker.calculate_error(position_1, position_2) + self.assertAlmostEqual(error, np.sqrt(3), places=5) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_orientation_error_checker(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error_checker = OrientationErrorChecker() + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(quat_1, quat_2)) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(quat_1, quat_2)) + + def test_revolute_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = RevoluteJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = np.pi/2 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_prismatic_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = PrismaticJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = 1.0 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 1.0) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_list_of_poses_error_checker(self): + poses_1 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + poses_2 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + error_checker = PoseErrorChecker(is_iterable=True) + error = error_checker.calculate_error(poses_1, poses_2) + self.assertEqual(error, [[0.0, 0.0], [0.0, 0.0]]) + self.assertTrue(error_checker.is_error_acceptable(poses_1, poses_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + poses_2 = [Pose([0, 1, np.sqrt(3)], quat), + Pose([0, 1, np.sqrt(3)], quat)] + error = error_checker.calculate_error(poses_1, poses_2) + self.assertAlmostEqual(error[0][0], 2, places=5) + self.assertEqual(error[0][1], np.pi/2) + self.assertAlmostEqual(error[1][0], 2, places=5) + self.assertEqual(error[1][1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(poses_1, poses_2)) + + def test_multi_joint_error_checker(self): + positions_1 = [0.0, 0.0] + positions_2 = [np.pi/2, 0.1] + joint_types = [JointType.REVOLUTE, JointType.PRISMATIC] + error_checker = MultiJointPositionErrorChecker(joint_types) + error = error_checker.calculate_error(positions_1, positions_2) + self.assertEqual(error, [np.pi/2, 0.1]) + self.assertFalse(error_checker.is_error_acceptable(positions_1, positions_2)) + positions_2 = [np.pi/180, 0.0001] + self.assertTrue(error_checker.is_error_acceptable(positions_1, positions_2)) diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index b28420f96..190a48922 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -7,7 +7,7 @@ from pycram.designators.action_designator import ParkArmsAction from pycram.datastructures.enums import ObjectType, Arms, WorldMode from pycram.failure_handling import Retry -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot from pycram.robot_description import RobotDescription from pycram.object_descriptors.urdf import ObjectDescription @@ -33,8 +33,8 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, RobotDescription.current_robot_description.name + extension, - ObjectDescription) + cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + RobotDescription.current_robot_description.name + extension) ProcessModule.execution_delay = True def setUp(self): diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py new file mode 100644 index 000000000..9b79cc114 --- /dev/null +++ b/test/test_goal_validator.py @@ -0,0 +1,321 @@ +import numpy as np +from tf.transformations import quaternion_from_euler +from typing_extensions import Optional, List + +from bullet_world_testcase import BulletWorldTestCase +from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Pose +from pycram.robot_description import RobotDescription +from pycram.validation.error_checkers import PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ + MultiJointPositionErrorChecker +from pycram.validation.goal_validator import GoalValidator, PoseGoalValidator, \ + PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ + MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator + + +class TestGoalValidator(BulletWorldTestCase): + + def test_single_pose_goal(self): + pose_goal_validators = PoseGoalValidator(self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def test_single_pose_goal_generic(self): + pose_goal_validators = GoalValidator(PoseErrorChecker(), self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def validate_pose_goal(self, goal_validator): + milk_goal_pose = Pose([1.3, 1.5, 0.9]) + goal_validator.register_goal(milk_goal_pose) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0.5, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) + self.milk.set_pose(milk_goal_pose) + self.assertEqual(self.milk.get_pose(), milk_goal_pose) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) + + def test_single_position_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + + def test_single_position_goal(self): + goal_validator = PositionGoalValidator(self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + + def validate_position_goal(self, goal_validator): + cereal_goal_position = [1.3, 1.5, 0.95] + goal_validator.register_goal(cereal_goal_position) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, 0.8) + self.cereal.set_position(cereal_goal_position) + self.assertEqual(self.cereal.get_position_as_list(), cereal_goal_position) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.current_error, 0) + + def test_single_orientation_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + + def test_single_orientation_goal(self): + goal_validator = OrientationGoalValidator(self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + + def validate_orientation_goal(self, goal_validator): + cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) + goal_validator.register_goal(cereal_goal_orientation) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, [np.pi / 2]) + self.cereal.set_orientation(cereal_goal_orientation) + for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): + self.assertAlmostEqual(v1, v2, places=5) + self.assertTrue(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, 1, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + + def test_single_revolute_joint_position_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator) + + def test_single_revolute_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator, JointType.REVOLUTE) + + def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): + goal_joint_position = -np.pi / 4 + joint_name = 'l_shoulder_lift_joint' + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, joint_name) + else: + goal_validator.register_goal(goal_joint_position, joint_name) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position * percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) + + def test_single_prismatic_joint_position_goal_generic(self): + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator) + + def test_single_prismatic_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator, JointType.PRISMATIC) + + def validate_prismatic_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): + goal_joint_position = 0.2 + torso = RobotDescription.current_robot_description.torso_joint + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, torso) + else: + goal_validator.register_goal(goal_joint_position, torso) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position(torso, goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position * percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) + + def test_multi_joint_goal_generic(self): + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_multi_joint_goal(goal_validator) + + def test_multi_joint_goal(self): + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_multi_joint_goal(goal_validator, joint_types) + + def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[JointType]] = None): + goal_joint_positions = np.array([0.2, -np.pi / 4]) + joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_positions = goal_joint_positions * percent + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) + self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], + atol=0.001)) + self.assertTrue( + np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(0.2) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(-np.pi / 4) * (1 - percent), places=5) + + def test_list_of_poses_goal_generic(self): + goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), + lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + + def test_list_of_poses_goal(self): + goal_validator = MultiPoseGoalValidator(lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + + def validate_list_of_poses_goal(self, goal_validator): + position_goal = [0.0, 1.0, 0.0] + orientation_goal = np.array([0, 0, np.pi / 2]) + poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), + Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] + goal_validator.register_goal(poses_goal) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertTrue( + np.allclose(goal_validator.current_error, np.array([1.0, np.pi / 2, 1.0, np.pi / 2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + current_pose_goal = Pose([0.0, 1.0 * percent, 0.0], + quaternion_from_euler(*current_orientation_goal.tolist())) + self.robot.set_pose(current_pose_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_pose_goal.position_as_list(), + atol=0.001)) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), current_pose_goal.orientation_as_list(), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[2], (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[3], np.pi * (1 - percent) / 2, places=5) + + def test_list_of_positions_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), + lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + + def test_list_of_positions_goal(self): + goal_validator = MultiPositionGoalValidator(lambda: [self.robot.get_position_as_list(), + self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + + def validate_list_of_positions_goal(self, goal_validator): + position_goal = [0.0, 1.0, 0.0] + positions_goal = [position_goal, position_goal] + goal_validator.register_goal(positions_goal) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) + + for percent in [0.5, 1]: + current_position_goal = [0.0, 1.0 * percent, 0.0] + self.robot.set_position(current_position_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_position_goal, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 1 - percent, places=5) + + def test_list_of_orientations_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), + lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + + def test_list_of_orientations_goal(self): + goal_validator = MultiOrientationGoalValidator(lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + + def validate_list_of_orientations_goal(self, goal_validator): + orientation_goal = np.array([0, 0, np.pi / 2]) + orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), + quaternion_from_euler(*orientation_goal.tolist())] + goal_validator.register_goal(orientations_goals) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi / 2, np.pi / 2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + self.robot.set_orientation(quaternion_from_euler(*current_orientation_goal.tolist())) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), + quaternion_from_euler(*current_orientation_goal.tolist()), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + + def test_list_of_revolute_joint_positions_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator) + + def test_list_of_revolute_joint_positions_goal(self): + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator, [JointType.REVOLUTE, JointType.REVOLUTE]) + + def validate_list_of_revolute_joint_positions_goal(self, goal_validator, + joint_types: Optional[List[JointType]] = None): + goal_joint_position = -np.pi / 4 + goal_joint_positions = np.array([goal_joint_position, goal_joint_position]) + joint_names = ['l_shoulder_lift_joint', 'r_shoulder_lift_joint'] + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, + np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_position = goal_joint_positions * percent + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_position))) + self.assertTrue(np.allclose(list(self.robot.get_multiple_joint_positions(joint_names).values()), + current_joint_position, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(goal_joint_position) * (1 - percent), + places=5) diff --git a/test/test_language.py b/test/test_language.py index c28615765..5ab886cbe 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -4,8 +4,9 @@ from pycram.designators.action_designator import * from pycram.designators.object_designator import BelieveObject from pycram.datastructures.enums import ObjectType, State +from pycram.failure_handling import RetryMonitor from pycram.fluent import Fluent -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure, NotALanguageExpression from pycram.datastructures.pose import Pose from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Code from pycram.process_module import simulated_robot @@ -115,6 +116,80 @@ def monitor_func(): self.assertRaises(AttributeError, lambda: Monitor(monitor_func) >> Monitor(monitor_func)) + def test_retry_monitor_construction(self): + act = ParkArmsAction([Arms.BOTH]) + act2 = MoveTorsoAction([0.3]) + + def monitor_func(): + time.sleep(1) + return True + + def recovery1(): + return + + recover1 = Code(lambda: recovery1()) + recovery = {NotALanguageExpression: recover1} + + subplan = act + act2 >> Monitor(monitor_func) + plan = RetryMonitor(subplan, max_tries=6, recovery=recovery) + self.assertEqual(len(plan.recovery), 1) + self.assertIsInstance(plan.designator_description, Monitor) + + def test_retry_monitor_tries(self): + act = ParkArmsAction([Arms.BOTH]) + act2 = MoveTorsoAction([0.3]) + tries_counter = 0 + + def monitor_func(): + nonlocal tries_counter + tries_counter += 1 + return True + + subplan = act + act2 >> Monitor(monitor_func) + plan = RetryMonitor(subplan, max_tries=6) + try: + plan.perform() + except PlanFailure as e: + pass + self.assertEqual(tries_counter, 6) + + def test_retry_monitor_recovery(self): + recovery1_counter = 0 + recovery2_counter = 0 + + def monitor_func(): + if not hasattr(monitor_func, 'tries_counter'): + monitor_func.tries_counter = 0 + if monitor_func.tries_counter % 2: + monitor_func.tries_counter += 1 + return NotALanguageExpression + monitor_func.tries_counter += 1 + return PlanFailure + + def recovery1(): + nonlocal recovery1_counter + recovery1_counter += 1 + + def recovery2(): + nonlocal recovery2_counter + recovery2_counter += 1 + + recover1 = Code(lambda: recovery1()) + recover2 = Code(lambda: recovery2()) + recovery = {NotALanguageExpression: recover1, + PlanFailure: recover2} + + act = ParkArmsAction([Arms.BOTH]) + act2 = MoveTorsoAction([0.3]) + subplan = act + act2 >> Monitor(monitor_func) + plan = RetryMonitor(subplan, max_tries=6, recovery=recovery) + try: + plan.perform() + except PlanFailure as e: + pass + self.assertEqual(recovery1_counter, 2) + self.assertEqual(recovery2_counter, 3) + def test_repeat_construction(self): act = ParkArmsAction([Arms.BOTH]) act2 = MoveTorsoAction([0.3]) @@ -196,7 +271,7 @@ def raise_except(): plan = act + code with simulated_robot: - state = plan.perform() + state, _ = plan.perform() self.assertIsInstance(plan.exceptions[plan], PlanFailure) self.assertEqual(len(plan.exceptions.keys()), 1) self.assertEqual(state, State.FAILED) @@ -209,7 +284,7 @@ def raise_except(): plan = act - code with simulated_robot: - state = plan.perform() + state, _ = plan.perform() self.assertIsInstance(plan.exceptions[plan], list) self.assertIsInstance(plan.exceptions[plan][0], PlanFailure) self.assertEqual(len(plan.exceptions.keys()), 1) @@ -223,7 +298,7 @@ def raise_except(): plan = act | code with simulated_robot: - state = plan.perform() + state, _ = plan.perform() self.assertIsInstance(plan.exceptions[plan], list) self.assertIsInstance(plan.exceptions[plan][0], PlanFailure) self.assertEqual(len(plan.exceptions.keys()), 1) @@ -237,7 +312,7 @@ def raise_except(): plan = act ^ code with simulated_robot: - state = plan.perform() + state, _ = plan.perform() self.assertIsInstance(plan.exceptions[plan], list) self.assertIsInstance(plan.exceptions[plan][0], PlanFailure) self.assertEqual(len(plan.exceptions.keys()), 1) diff --git a/test/test_mjcf.py b/test/test_mjcf.py new file mode 100644 index 000000000..edfbb842b --- /dev/null +++ b/test/test_mjcf.py @@ -0,0 +1,40 @@ +from unittest import TestCase, skipIf +from dm_control import mjcf +try: + from pycram.object_descriptors.mjcf import ObjectDescription as MJCFObjDesc +except ImportError: + MJCFObjDesc = None + + +@skipIf(MJCFObjDesc is None, "Multiverse not found.") +class TestMjcf(TestCase): + model: MJCFObjDesc + + @classmethod + def setUpClass(cls): + # Example usage + model = mjcf.RootElement("test") + + model.default.dclass = 'default' + + # Define a simple model with bodies and joints + body1 = model.worldbody.add('body', name='body1') + body2 = body1.add('body', name='body2') + joint1 = body2.add('joint', name='joint1', type='hinge') + + body3 = body2.add('body', name='body3') + joint2 = body3.add('joint', name='joint2', type='slide') + + cls.model = MJCFObjDesc() + cls.model.update_description_from_string(model.to_xml_string()) + + def test_child_map(self): + self.assertEqual(self.model.child_map, {'body1': [('joint1', 'body2')], 'body2': [('joint2', 'body3')]}) + + def test_parent_map(self): + self.assertEqual(self.model.parent_map, {'body2': ('joint1', 'body1'), 'body3': ('joint2', 'body2')}) + + def test_get_chain(self): + self.assertEqual(self.model.get_chain('body1', 'body3'), + ['body1', 'joint1', 'body2', 'joint2', 'body3']) + diff --git a/test/test_move_and_pick_up.py b/test/test_move_and_pick_up.py index 013a8708d..2c4268950 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_move_and_pick_up.py @@ -9,7 +9,7 @@ from pycram.designators.action_designator import MoveTorsoActionPerformable from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPickUp, GaussianCostmapModel) -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.process_module import simulated_robot diff --git a/test/test_multiverse.py b/test/test_multiverse.py new file mode 100644 index 000000000..3164de50a --- /dev/null +++ b/test/test_multiverse.py @@ -0,0 +1,410 @@ +#!/usr/bin/env python3 +import os +import unittest + +import numpy as np +import psutil +from tf.transformations import quaternion_from_euler, quaternion_multiply +from typing_extensions import Optional, List + +from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint +from pycram.datastructures.enums import ObjectType, Arms, JointType +from pycram.datastructures.pose import Pose +from pycram.robot_description import RobotDescriptionManager +from pycram.world_concepts.world_object import Object +from pycram.validation.error_checkers import calculate_angle_between_quaternions +from pycram.helper import get_robot_mjcf_path, parse_mjcf_actuators + +multiverse_installed = True +try: + from pycram.worlds.multiverse import Multiverse +except ImportError: + multiverse_installed = False + +processes = psutil.process_iter() +process_names = [p.name() for p in processes] +multiverse_running = True +mujoco_running = True +if 'multiverse_server' not in process_names: + multiverse_running = False +if 'mujoco' not in process_names: + mujoco_running = False + + +@unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") +@unittest.skipIf(not multiverse_running, "Multiverse server is not running.") +@unittest.skipIf(not mujoco_running, "Mujoco is not running.") +class MultiversePyCRAMTestCase(unittest.TestCase): + if multiverse_installed: + multiverse: Multiverse + big_bowl: Optional[Object] = None + + @classmethod + def setUpClass(cls): + if not multiverse_installed: + return + cls.multiverse = Multiverse() + + @classmethod + def tearDownClass(cls): + cls.multiverse.exit(remove_saved_states=True) + cls.multiverse.remove_multiverse_resources() + + def tearDown(self): + self.multiverse.remove_all_objects() + + def test_spawn_xml_object(self): + bread = Object("bread_1", ObjectType.GENERIC_OBJECT, "bread_1.xml", pose=Pose([1, 1, 0.1])) + self.assert_poses_are_equal(bread.get_pose(), Pose([1, 1, 0.1])) + + def test_spawn_mesh_object(self): + milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) + self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) + self.multiverse.simulate(0.2) + contact_points = milk.contact_points() + self.assertTrue(len(contact_points) > 0) + + def test_parse_mjcf_actuators(self): + mjcf_file = get_robot_mjcf_path("pal_robotics", "tiago_dual") + self.assertTrue(os.path.exists(mjcf_file)) + joint_actuators = parse_mjcf_actuators(mjcf_file) + self.assertIsInstance(joint_actuators, dict) + self.assertTrue(len(joint_actuators) > 0) + self.assertTrue("arm_left_1_joint" in joint_actuators) + self.assertTrue("arm_right_1_joint" in joint_actuators) + self.assertTrue(joint_actuators["arm_right_1_joint"] == "arm_right_1_actuator") + + def test_get_actuator_for_joint(self): + robot = self.spawn_robot() + joint_name = "arm_right_1_joint" + actuator_name = robot.get_actuator_for_joint(robot.joints[joint_name]) + self.assertEqual(actuator_name, "arm_right_1_actuator") + + def test_get_images_for_target(self): + robot = self.spawn_robot(robot_name='pr2') + camera_description = self.multiverse.robot_description.get_default_camera() + camera_link_name = camera_description.link_name + camera_pose = robot.get_link_pose(camera_link_name) + camera_frame = self.multiverse.robot_description.get_camera_frame() + camera_front_facing_axis = camera_description.front_facing_axis + milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 + orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() + milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) + self.assertIsInstance(depth, np.ndarray) + self.assertIsInstance(segmentation_mask, np.ndarray) + self.assertTrue(depth.shape == (256, 256)) + self.assertTrue(segmentation_mask.shape == (256, 256)) + self.assertTrue(milk.id in np.unique(segmentation_mask).flatten().tolist()) + avg_depth_of_milk = np.mean(depth[segmentation_mask == milk.id]) + self.assertAlmostEqual(avg_depth_of_milk, 0.5, delta=0.1) + + def test_reset_world(self): + set_position = [1, 1, 0.1] + milk = self.spawn_milk(set_position) + milk.set_position(set_position) + milk_position = milk.get_position_as_list() + self.assert_list_is_equal(milk_position[:2], set_position[:2], delta=self.multiverse.conf.position_tolerance) + self.multiverse.reset_world() + milk_pose = milk.get_pose() + self.assert_list_is_equal(milk_pose.position_as_list()[:2], + milk.original_pose.position_as_list()[:2], + delta=self.multiverse.conf.position_tolerance) + self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) + + def test_spawn_robot_with_actuators_directly_from_multiverse(self): + if self.multiverse.conf.use_controller: + robot_name = "tiago_dual" + rdm = RobotDescriptionManager() + rdm.load_description(robot_name) + self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001])) + + def test_spawn_object(self): + milk = self.spawn_milk([1, 1, 0.1]) + self.assertIsInstance(milk, Object) + milk_pose = milk.get_pose() + self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 1], + delta=self.multiverse.conf.position_tolerance) + self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) + + def test_remove_object(self): + milk = self.spawn_milk([1, 1, 0.1]) + milk.remove() + self.assertTrue(milk not in self.multiverse.objects) + self.assertFalse(self.multiverse.check_object_exists(milk)) + + def test_check_object_exists(self): + milk = self.spawn_milk([1, 1, 0.1]) + self.assertTrue(self.multiverse.check_object_exists(milk)) + + def test_set_position(self): + milk = self.spawn_milk([1, 1, 0.1]) + original_milk_position = milk.get_position_as_list() + original_milk_position[0] += 1 + milk.set_position(original_milk_position) + milk_position = milk.get_position_as_list() + self.assert_list_is_equal(milk_position[:2], original_milk_position[:2], + delta=self.multiverse.conf.position_tolerance) + + def test_update_position(self): + milk = self.spawn_milk([1, 1, 0.1]) + milk.update_pose() + milk_position = milk.get_position_as_list() + self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.conf.position_tolerance) + + def test_set_joint_position(self): + if self.multiverse.robot is None: + robot = self.spawn_robot() + else: + robot = self.multiverse.robot + step = 0.2 + for joint in ['torso_lift_joint']: + joint_type = robot.joints[joint].type + original_joint_position = robot.get_joint_position(joint) + robot.set_joint_position(joint, original_joint_position + step) + joint_position = robot.get_joint_position(joint) + if not self.multiverse.conf.use_controller: + delta = self.multiverse.conf.prismatic_joint_position_tolerance if joint_type == JointType.PRISMATIC \ + else self.multiverse.conf.revolute_joint_position_tolerance + else: + delta = 0.18 + self.assertAlmostEqual(joint_position, original_joint_position + step, delta=delta) + + def test_spawn_robot(self): + if self.multiverse.robot is not None: + robot = self.multiverse.robot + else: + robot = self.spawn_robot(robot_name="pr2") + self.assertIsInstance(robot, Object) + self.assertTrue(robot in self.multiverse.objects) + self.assertTrue(self.multiverse.robot.name == robot.name) + + def test_destroy_robot(self): + if self.multiverse.robot is None: + self.spawn_robot() + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + self.multiverse.robot.remove() + self.assertTrue(self.multiverse.robot not in self.multiverse.objects) + + def test_respawn_robot(self): + self.spawn_robot() + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + self.multiverse.robot.remove() + self.assertTrue(self.multiverse.robot not in self.multiverse.objects) + self.spawn_robot() + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + + def test_set_robot_position(self): + step = -1 + for i in range(3): + self.spawn_robot() + new_position = [-3 + step * i, -3 + step * i, 0.001] + self.multiverse.robot.set_position(new_position) + robot_position = self.multiverse.robot.get_position_as_list() + self.assert_list_is_equal(robot_position[:2], new_position[:2], + delta=self.multiverse.conf.position_tolerance) + self.tearDown() + + def test_set_robot_orientation(self): + self.spawn_robot() + for i in range(3): + current_quaternion = self.multiverse.robot.get_orientation_as_list() + # rotate by 45 degrees without using euler angles + rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) + new_quaternion = quaternion_multiply(current_quaternion, rotation_quaternion) + self.multiverse.robot.set_orientation(new_quaternion) + robot_orientation = self.multiverse.robot.get_orientation_as_list() + quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) + self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.conf.orientation_tolerance) + + def test_set_robot_pose(self): + self.spawn_robot(orientation=quaternion_from_euler(0, 0, np.pi / 4)) + position_step = -1 + angle_step = np.pi / 4 + num_steps = 10 + self.step_robot_pose(self.multiverse.robot, position_step, angle_step, num_steps) + position_step = 1 + angle_step = -np.pi / 4 + self.step_robot_pose(self.multiverse.robot, position_step, angle_step, num_steps) + + def step_robot_pose(self, robot, position_step, angle_step, num_steps): + original_position = robot.get_position_as_list() + original_orientation = robot.get_orientation_as_list() + for i in range(num_steps): + new_position = [original_position[0] + position_step * (i + 1), + original_position[1] + position_step * (i + 1), original_position[2]] + rotation_quaternion = quaternion_from_euler(0, 0, angle_step * (i + 1)) + new_quaternion = quaternion_multiply(original_orientation, rotation_quaternion) + new_pose = Pose(new_position, new_quaternion) + self.multiverse.robot.set_pose(new_pose) + robot_pose = self.multiverse.robot.get_pose() + self.assert_poses_are_equal(new_pose, robot_pose, + position_delta=self.multiverse.conf.position_tolerance, + orientation_delta=self.multiverse.conf.orientation_tolerance) + + def test_get_environment_pose(self): + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + pose = apartment.get_pose() + self.assertIsInstance(pose, Pose) + + def test_attach_object(self): + for _ in range(3): + milk = self.spawn_milk([1, 0.1, 0.1]) + cup = self.spawn_cup([1, 1.1, 0.1]) + milk.attach(cup) + self.assertTrue(cup in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 1 + cup_position = cup.get_position_as_list() + estimated_cup_position = cup_position.copy() + estimated_cup_position[0] += 1 + milk.set_position(milk_position) + new_cup_position = cup.get_position_as_list() + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], + self.multiverse.conf.position_tolerance) + self.tearDown() + + def test_detach_object(self): + for i in range(2): + milk = self.spawn_milk([1, 0, 0.1]) + cup = self.spawn_cup([1, 1, 0.1]) + milk.attach(cup) + self.assertTrue(cup in milk.attachments) + milk.detach(cup) + self.assertTrue(cup not in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 1 + cup_position = cup.get_position_as_list() + estimated_cup_position = cup_position.copy() + milk.set_position(milk_position) + new_milk_position = milk.get_position_as_list() + new_cup_position = cup.get_position_as_list() + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], + self.multiverse.conf.position_tolerance) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], + self.multiverse.conf.position_tolerance) + self.tearDown() + + def test_attach_with_robot(self): + milk = self.spawn_milk([-1, -1, 0.1]) + robot = self.spawn_robot() + ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) + # Get position of milk relative to robot end effector + robot.attach(milk, ee_link.name, coincide_the_objects=False) + self.assertTrue(robot in milk.attachments) + milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) + robot_position = 1.57 + robot.set_joint_position("arm_right_2_joint", robot_position) + milk_pose = milk.root_link.get_pose_wrt_link(ee_link) + self.assert_poses_are_equal(milk_initial_pose, milk_pose) + + def test_get_object_contact_points(self): + for i in range(10): + milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) + contact_points = self.multiverse.get_object_contact_points(milk) + self.assertIsInstance(contact_points, ContactPointsList) + self.assertEqual(len(contact_points), 1) + self.assertIsInstance(contact_points[0], ContactPoint) + self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) + cup = self.spawn_cup([1, 1, 0.15]) + # This is needed because the cup is spawned in the air, so it needs to fall + # to get in contact with the milk + self.multiverse.simulate(0.3) + contact_points = self.multiverse.get_object_contact_points(cup) + self.assertIsInstance(contact_points, ContactPointsList) + self.assertEqual(len(contact_points), 1) + self.assertIsInstance(contact_points[0], ContactPoint) + self.assertTrue(contact_points[0].link_b.object, milk) + self.tearDown() + + def test_get_contact_points_between_two_objects(self): + for i in range(3): + milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) + cup = self.spawn_cup([1, 1, 0.15]) + # This is needed because the cup is spawned in the air so it needs to fall + # to get in contact with the milk + self.multiverse.simulate(0.3) + contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) + self.assertIsInstance(contact_points, ContactPointsList) + self.assertEqual(len(contact_points), 1) + self.assertIsInstance(contact_points[0], ContactPoint) + self.assertTrue(contact_points[0].link_a.object, milk) + self.assertTrue(contact_points[0].link_b.object, cup) + self.tearDown() + + def test_get_one_ray(self): + milk = self.spawn_milk([1, 1, 0.1]) + intersected_object = self.multiverse.ray_test([1, 2, 0.1], [1, 1.5, 0.1]) + self.assertTrue(intersected_object is None) + intersected_object = self.multiverse.ray_test([1, 2, 0.1], [1, 1, 0.1]) + self.assertTrue(intersected_object == milk.id) + + def test_get_rays(self): + milk = self.spawn_milk([1, 1, 0.1]) + intersected_objects = self.multiverse.ray_test_batch([[1, 2, 0.1], [1, 2, 0.1]], + [[1, 1.5, 0.1], [1, 1, 0.1]]) + self.assertTrue(intersected_objects[0][0] == -1) + self.assertTrue(intersected_objects[1][0] == milk.id) + + @staticmethod + def spawn_big_bowl() -> Object: + big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", + pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) + return big_bowl + + @staticmethod + def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") -> Object: + if orientation is None: + orientation = [0, 0, 0, 1] + milk = Object("milk_box", ObjectType.MILK, "milk_box.xml", + pose=Pose(position, orientation, frame=frame)) + return milk + + def spawn_robot(self, position: Optional[List[float]] = None, + orientation: Optional[List[float]] = None, + robot_name: Optional[str] = 'tiago_dual', + replace: Optional[bool] = True) -> Object: + if position is None: + position = [-2, -2, 0.001] + if orientation is None: + orientation = [0, 0, 0, 1] + if self.multiverse.robot is None or replace: + if self.multiverse.robot is not None: + self.multiverse.robot.remove() + robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}.urdf", + pose=Pose(position, [0, 0, 0, 1])) + else: + robot = self.multiverse.robot + robot.set_position(position) + return robot + + @staticmethod + def spawn_cup(position: List) -> Object: + cup = Object("cup", ObjectType.GENERIC_OBJECT, "Cup.obj", + pose=Pose(position, [0, 0, 0, 1])) + return cup + + def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, + position_delta: Optional[float] = None, orientation_delta: Optional[float] = None): + if position_delta is None: + position_delta = self.multiverse.conf.position_tolerance + if orientation_delta is None: + orientation_delta = self.multiverse.conf.orientation_tolerance + self.assert_position_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) + self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), + delta=orientation_delta) + + def assert_position_is_equal(self, position1: List[float], position2: List[float], delta: Optional[float] = None): + if delta is None: + delta = self.multiverse.conf.position_tolerance + self.assert_list_is_equal(position1, position2, delta=delta) + + def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], + delta: Optional[float] = None): + if delta is None: + delta = self.multiverse.conf.orientation_tolerance + self.assertAlmostEqual(calculate_angle_between_quaternions(orientation1, orientation2), 0, delta=delta) + + def assert_list_is_equal(self, list1: List, list2: List, delta: float): + for i in range(len(list1)): + self.assertAlmostEqual(list1[i], list2[i], delta=delta) diff --git a/test/test_object.py b/test/test_object.py index 74c22f7b8..bede0300b 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -5,15 +5,18 @@ from pycram.datastructures.enums import JointType, ObjectType from pycram.datastructures.pose import Pose from pycram.datastructures.dataclasses import Color +from pycram.failures import UnsupportedFileExtension from pycram.world_concepts.world_object import Object +from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription from geometry_msgs.msg import Point, Quaternion import pathlib + class TestObject(BulletWorldTestCase): def test_wrong_object_description_path(self): - with self.assertRaises(FileNotFoundError): + with self.assertRaises(UnsupportedFileExtension): milk = Object("milk_not_found", ObjectType.MILK, "wrong_path.sk") def test_malformed_object_description(self): @@ -160,3 +163,12 @@ def test_object_equal(self): self.assertEqual(self.milk, self.milk) self.assertNotEqual(self.milk, self.cereal) self.assertNotEqual(self.milk, self.world) + + +class GenericObjectTestCase(BulletWorldTestCase): + + def test_init_generic_object(self): + gen_obj_desc = GenericObjectDescription("robokudo_object", [0,0,0], [0.1, 0.1, 0.1]) + obj = Object("robokudo_object", ObjectType.MILK, None, gen_obj_desc) + pose = obj.get_pose() + self.assertTrue(isinstance(pose, Pose)) diff --git a/test/test_ontology.py b/test/test_ontology.py index 130b67568..f4754d4b2 100644 --- a/test/test_ontology.py +++ b/test/test_ontology.py @@ -1,5 +1,6 @@ from __future__ import annotations +import os.path import unittest import logging from pathlib import Path @@ -8,16 +9,29 @@ from pycram.designator import ObjectDesignatorDescription import rospy + +# Owlready2 try: from owlready2 import * except ImportError: owlready2 = None rospy.logwarn("Could not import owlready2, Ontology unit-tests could not run!") -from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI -from pycram.ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder +# Java runtime, required by Owlready2 reasoning +java_runtime_installed = owlready2 is not None +if owlready2: + try: + subprocess.run(["java", "--version"], check=True) + except (FileNotFoundError, subprocess.CalledProcessError): + java_runtime_installed = False + rospy.logwarn("Java runtime is not installed, Ontology reasoning unit-test could not run!") +from pycram.ontology.ontology import OntologyManager, SOMA_HOME_ONTOLOGY_IRI, SOMA_ONTOLOGY_IRI +from pycram.ontology.ontology_common import (OntologyConceptHolderStore, OntologyConceptHolder, + ONTOLOGY_SQL_BACKEND_FILE_EXTENSION, ONTOLOGY_OWL_FILE_EXTENSION, + ONTOLOGY_SQL_IN_MEMORY_BACKEND) +DEFAULT_LOCAL_ONTOLOGY_IRI = "default.owl" class TestOntologyManager(unittest.TestCase): ontology_manager: OntologyManager main_ontology: Optional[owlready2.Ontology] @@ -26,48 +40,93 @@ class TestOntologyManager(unittest.TestCase): @classmethod def setUpClass(cls): - cls.ontology_manager = OntologyManager(SOMA_ONTOLOGY_IRI) + # Try loading from remote `SOMA_ONTOLOGY_IRI`, which will fail given no internet access + cls.ontology_manager = OntologyManager(main_ontology_iri=SOMA_ONTOLOGY_IRI, + main_sql_backend_filename=os.path.join(Path.home(), + f"{Path(SOMA_ONTOLOGY_IRI).stem}{ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}")) if cls.ontology_manager.initialized(): - cls.main_ontology = cls.ontology_manager.main_ontology cls.soma = cls.ontology_manager.soma cls.dul = cls.ontology_manager.dul else: - cls.main_ontology = None + # Else, load from `DEFAULT_LOCAL_ONTOLOGY_IRI` cls.soma = None cls.dul = None + cls.ontology_manager.main_ontology_iri = DEFAULT_LOCAL_ONTOLOGY_IRI + cls.ontology_manager.main_ontology_sql_backend = ONTOLOGY_SQL_IN_MEMORY_BACKEND + cls.ontology_manager.create_main_ontology_world() + cls.ontology_manager.create_main_ontology() + cls.main_ontology = cls.ontology_manager.main_ontology @classmethod def tearDownClass(cls): - if cls.ontology_manager.initialized(): - save_dir = Path(f"{Path.home()}/ontologies") - save_dir.mkdir(parents=True, exist_ok=True) - cls.ontology_manager.save(f"{save_dir}/{Path(cls.ontology_manager.main_ontology_iri).stem}.owl") + save_dir = cls.ontology_manager.get_main_ontology_dir() + owl_filepath = f"{save_dir}/{Path(cls.ontology_manager.main_ontology_iri).stem}{ONTOLOGY_OWL_FILE_EXTENSION}" + os.remove(owl_filepath) + cls.remove_sql_file(cls.ontology_manager.main_ontology_sql_backend) + + @classmethod + def remove_sql_file(cls, sql_filepath: str): + if os.path.exists(sql_filepath): + os.remove(sql_filepath) + sql_journal_filepath = f"{sql_filepath}-journal" + if os.path.exists(sql_journal_filepath): + os.remove(sql_journal_filepath) def test_ontology_manager(self): - if self.ontology_manager.initialized(): - self.assertIs(self.ontology_manager, OntologyManager()) + self.assertIs(self.ontology_manager, OntologyManager()) + if owlready2: + self.assertTrue(self.ontology_manager.initialized()) + + @unittest.skipUnless(owlready2, 'Owlready2 is required') + def test_ontology_world(self): + # Main ontology world as the global default world + main_world = self.ontology_manager.main_ontology_world + self.assertIsNotNone(main_world) + self.assertTrue(main_world is owlready2.default_world) + + # Extra world with memory backend + extra_memory_world = self.ontology_manager.create_ontology_world(use_global_default_world=False) + self.assertIsNotNone(extra_memory_world) + self.assertTrue(extra_memory_world != owlready2.default_world) + + # Extra world with SQL backend from a non-existing SQL file + extra_world_sql_filename = f"{self.ontology_manager.get_main_ontology_dir()}/extra_world{ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}" + extra_sql_world = self.ontology_manager.create_ontology_world(use_global_default_world=False, + sql_backend_filename=extra_world_sql_filename) + self.assertIsNotNone(extra_sql_world) + # Save it at [extra_world_sql_filename] + extra_sql_world.save() + self.assertTrue(os.path.isfile(extra_world_sql_filename)) + # Extra world with SQL backend from an existing SQL file + extra_sql_world_2 = self.ontology_manager.create_ontology_world(use_global_default_world=False, + sql_backend_filename=extra_world_sql_filename) + self.assertIsNotNone(extra_sql_world_2) + + # Remove SQL file finally + self.remove_sql_file(extra_world_sql_filename) + + @unittest.skipUnless(owlready2, 'Owlready2 is required') def test_ontology_concept_holder(self): - if not self.ontology_manager.initialized(): - return dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class('DynamicOntologyConcept') - dynamic_ontology_concept_holder = OntologyConceptHolder(dynamic_ontology_concept_class(name='dynamic_ontology_concept1', - namespace=self.main_ontology)) + dynamic_ontology_concept_holder = OntologyConceptHolder( + dynamic_ontology_concept_class(name='dynamic_ontology_concept1', + namespace=self.main_ontology)) self.assertTrue(owlready2.isinstance_python(dynamic_ontology_concept_holder.ontology_concept, owlready2.Thing)) + @unittest.skipUnless(owlready2, 'Owlready2 is required') def test_loaded_ontologies(self): - if not self.ontology_manager.initialized(): - return self.assertIsNotNone(self.main_ontology) self.assertTrue(self.main_ontology.loaded) - self.assertIsNotNone(self.soma) - self.assertTrue(self.soma.loaded) - self.assertIsNotNone(self.dul) - self.assertTrue(self.dul.loaded) + if self.ontology_manager.main_ontology_iri is SOMA_ONTOLOGY_IRI or \ + self.ontology_manager.main_ontology_iri is SOMA_HOME_ONTOLOGY_IRI: + self.assertIsNotNone(self.soma) + self.assertTrue(self.soma.loaded) + self.assertIsNotNone(self.dul) + self.assertTrue(self.dul.loaded) + @unittest.skipUnless(owlready2, 'Owlready2 is required') def test_ontology_concept_class_dynamic_creation(self): - if not self.ontology_manager.initialized(): - return dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class('DynamicOntologyConcept') self.assertIsNotNone(dynamic_ontology_concept_class) self.assertEqual(dynamic_ontology_concept_class.namespace, self.main_ontology) @@ -77,14 +136,13 @@ def test_ontology_concept_class_dynamic_creation(self): namespace=self.main_ontology) self.assertTrue(owlready2.isinstance_python(dynamic_ontology_concept, owlready2.Thing)) + @unittest.skipUnless(owlready2, 'Owlready2 is required') def test_ontology_triple_classes_dynamic_creation(self): - if not self.ontology_manager.initialized(): - return # Test dynamic triple classes creation without inheritance from existing parent ontology classes - self.ontology_manager.create_ontology_triple_classes(subject_class_name="OntologySubject", - object_class_name="OntologyObject", - predicate_name="predicate", - inverse_predicate_name="inverse_predicate") + self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name="OntologySubject", + object_class_name="OntologyObject", + predicate_class_name="predicate", + inverse_predicate_class_name="inverse_predicate")) subject_class = self.main_ontology.OntologySubject self.assertIsNotNone(subject_class) @@ -99,14 +157,19 @@ def test_ontology_triple_classes_dynamic_creation(self): # Test dynamic triple classes creation as inheriting from existing parent ontology classes PLACEABLE_ON_PREDICATE_NAME = "placeable_on" HOLD_OBJ_PREDICATE_NAME = "hold_obj" - self.ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=self.soma.Container, - subject_class_name="OntologyPlaceHolderObject", - ontology_object_parent_class=self.dul.PhysicalObject, - object_class_name="OntologyHandheldObject", - predicate_name=PLACEABLE_ON_PREDICATE_NAME, - inverse_predicate_name=HOLD_OBJ_PREDICATE_NAME, - ontology_property_parent_class=self.soma.affordsBearer, - ontology_inverse_property_parent_class=self.soma.isBearerAffordedBy) + self.assertTrue( + self.ontology_manager.create_ontology_triple_classes( + ontology_subject_parent_class=self.soma.Container if self.soma else None, + subject_class_name="OntologyPlaceHolderObject", + ontology_object_parent_class=self.dul.PhysicalObject + if self.dul else None, + object_class_name="OntologyHandheldObject", + predicate_class_name=PLACEABLE_ON_PREDICATE_NAME, + inverse_predicate_class_name=HOLD_OBJ_PREDICATE_NAME, + ontology_property_parent_class=self.soma.affordsBearer + if self.soma else None, + ontology_inverse_property_parent_class=self.soma.isBearerAffordedBy + if self.soma else None)) def create_ontology_handheld_object_designator(object_name: str, ontology_parent_class: Type[owlready2.Thing]): return self.ontology_manager.create_ontology_linked_designator(object_name=object_name, @@ -139,9 +202,8 @@ def create_ontology_handheld_object_designator(object_name: str, ontology_parent self.assertTrue(len(egg_tray_holdables) == 1) self.assertEqual(egg_tray_holdables[0], ["egg"]) + @unittest.skipUnless(owlready2, 'Owlready2 is required') def test_ontology_class_destruction(self): - if not self.ontology_manager.initialized(): - return concept_class_name = 'DynamicOntologyConcept' dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class(concept_class_name) OntologyConceptHolder(dynamic_ontology_concept_class(name='dynamic_ontology_concept3', @@ -151,6 +213,100 @@ def test_ontology_class_destruction(self): self.assertIsNone(self.ontology_manager.get_ontology_class(concept_class_name)) self.assertFalse(OntologyConceptHolderStore().get_ontology_concepts_by_class(dynamic_ontology_concept_class)) + @unittest.skipUnless(owlready2, 'Owlready2 is required') + @unittest.skipUnless(java_runtime_installed, 'Java runtime is required') + def test_ontology_reasoning(self): + REASONING_TEST_ONTOLOGY_IRI = f"reasoning_test{ONTOLOGY_OWL_FILE_EXTENSION}" + ENTITY_CONCEPT_NAME = "Entity" + CAN_TRANSPORT_PREDICATE_NAME = "can_transport" + TRANSPORTABLE_BY_PREDICATE_NAME = "transportable_by" + CORESIDE_PREDICATE_NAME = "coreside" + + # Create a test world (with memory SQL backend) for reasoning + reasoning_world = self.ontology_manager.create_ontology_world() + reasoning_ontology = reasoning_world.get_ontology(REASONING_TEST_ONTOLOGY_IRI) + + # Create Entity class & types of relations among its instances + self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name=ENTITY_CONCEPT_NAME, + object_class_name=ENTITY_CONCEPT_NAME, + predicate_class_name=CAN_TRANSPORT_PREDICATE_NAME, + inverse_predicate_class_name=TRANSPORTABLE_BY_PREDICATE_NAME, + ontology_property_parent_class=owlready2.ObjectProperty, + ontology_inverse_property_parent_class=owlready2.ObjectProperty, + ontology=reasoning_ontology)) + + self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name=ENTITY_CONCEPT_NAME, + object_class_name=ENTITY_CONCEPT_NAME, + predicate_class_name=CORESIDE_PREDICATE_NAME, + ontology_property_parent_class=owlready2.ObjectProperty, + ontology=reasoning_ontology)) + + # Define rules for `transportability` & `co-residence` in [reasoning_ontology] + with reasoning_ontology: + def can_transport_itself(a: reasoning_ontology.Entity) -> bool: + return a in a.can_transport + + def can_transport_entity(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: + return b in a.can_transport + + def can_be_transported_by(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: + return b in a.transportable_by + + def coresidents(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: + return b in a.coreside + + # Rule1: Transitivity of transportability + self.ontology_manager.create_rule_transitivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, + predicate_name=CAN_TRANSPORT_PREDICATE_NAME, + ontology=reasoning_ontology) + + # Rule2: reflexivity of transportability + self.ontology_manager.create_rule_reflexivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, + predicate_name=CAN_TRANSPORT_PREDICATE_NAME, + ontology=reasoning_ontology) + + # Rule3 & 4: Symmetry & Transitivity of co-residence + self.ontology_manager.create_rule_transitivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, + predicate_name=CORESIDE_PREDICATE_NAME, + ontology=reasoning_ontology) + self.ontology_manager.create_rule_symmetry(ontology_concept_class_name=ENTITY_CONCEPT_NAME, + predicate_name=CORESIDE_PREDICATE_NAME, + ontology=reasoning_ontology) + + # Create entities + entities = [reasoning_ontology.Entity(name=f"e{i}") for i in range(3)] + entities[2].can_transport.append(entities[1]) + entities[1].can_transport.append(entities[0]) + entities[0].coreside.append(entities[1]) + entities[0].coreside.append(entities[2]) + + # Reason on [reasoning_world] + self.ontology_manager.reason(world=reasoning_world) + + # Test reflexivity + for entity in entities: + self.assertTrue(can_transport_itself(entity)) + + # Test transitivity + self.assertTrue(can_transport_entity(entities[2], entities[0])) + self.assertTrue(can_be_transported_by(entities[0], entities[2])) + + # Test symmetry + entities_num = len(entities) + for i in range(entities_num): + for j in range(entities_num): + if i != j: + self.assertTrue(coresidents(entities[i], entities[j])) + + @unittest.skipUnless(owlready2, 'Owlready2 is required') + def test_ontology_save(self): + save_dir = self.ontology_manager.get_main_ontology_dir() + owl_filepath = f"{save_dir}/{Path(self.ontology_manager.main_ontology_iri).stem}{ONTOLOGY_OWL_FILE_EXTENSION}" + self.assertTrue(self.ontology_manager.save(owl_filepath)) + self.assertTrue(Path(owl_filepath).is_file()) + sql_backend = self.ontology_manager.main_ontology_sql_backend + if sql_backend != ONTOLOGY_SQL_IN_MEMORY_BACKEND: + self.assertTrue(Path(sql_backend).is_file()) if __name__ == '__main__': unittest.main() diff --git a/test/test_orm.py b/test/test_orm.py index 482ce14e3..6609e3992 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,5 +1,7 @@ import os +import time import unittest +import time from sqlalchemy import select import sqlalchemy.orm import pycram.orm.action_designator @@ -9,18 +11,20 @@ import pycram.orm.tasktree import pycram.tasktree from bullet_world_testcase import BulletWorldTestCase +from pycram.datastructures.dataclasses import Color +from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycram.world_concepts.world_object import Object from pycram.designators import action_designator, object_designator, motion_designator -from pycram.designators.action_designator import ParkArmsActionPerformable, MoveTorsoActionPerformable, \ - SetGripperActionPerformable, PickUpActionPerformable, NavigateActionPerformable, TransportActionPerformable, \ - OpenActionPerformable, CloseActionPerformable, DetectActionPerformable, LookAtActionPerformable -from pycram.designators.object_designator import BelieveObject -from pycram.datastructures.enums import ObjectType +from pycram.designators.action_designator import * +from pycram.designators.object_designator import BelieveObject, ObjectPart +from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot -from pycram.tasktree import with_tree +from pycram.tasktree import with_tree, task_tree from pycram.orm.views import PickUpWithContextView -from pycram.datastructures.enums import Arms, Grasp, GripperState +from pycram.datastructures.enums import Arms, Grasp, GripperState, ObjectType +from pycram.worlds.bullet_world import BulletWorld class DatabaseTestCaseMixin(BulletWorldTestCase): @@ -39,7 +43,6 @@ def setUp(self): def tearDown(self): super().tearDown() - pycram.tasktree.reset_tree() pycram.orm.base.ProcessMetaData.reset() pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() @@ -56,7 +59,7 @@ def test_schema_creation(self): self.assertTrue("NavigateAction" in tables) self.assertTrue("MoveTorsoAction" in tables) self.assertTrue("SetGripperAction" in tables) - self.assertTrue("Release" in tables) + self.assertTrue("ReleaseAction" in tables) self.assertTrue("GripAction" in tables) self.assertTrue("PickUpAction" in tables) self.assertTrue("PlaceAction" in tables) @@ -183,7 +186,7 @@ def test_plan_serialization(self): PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT).perform() description.resolve().perform() pycram.orm.base.ProcessMetaData().description = "Unittest" - tt = pycram.tasktree.task_tree + tt = pycram.tasktree.task_tree.root tt.insert(self.session) action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() motion_results = self.session.scalars(select(pycram.orm.motion_designator.Motion)).all() @@ -226,6 +229,24 @@ def test_transportAction(self): milk_object = self.session.scalars(select(pycram.orm.object_designator.Object)).first() self.assertEqual(milk_object.pose, result[0].object.pose) + def test_pickUpAction(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + previous_position = object_description.resolve().pose + with simulated_robot: + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + PickUpActionPerformable(object_description.resolve(), Arms.LEFT, Grasp.FRONT).perform() + NavigateActionPerformable(Pose([1.3, 1, 0.9], [0, 0, 0, 1])).perform() + PlaceActionPerformable(object_description.resolve(), Arms.LEFT, Pose([2.0, 1.6, 1.8], [0, 0, 0, 1])).perform() + pycram.orm.base.ProcessMetaData().description = "pickUpAction_test" + pycram.tasktree.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.base.Position) + .join(pycram.orm.action_designator.PickUpAction.object) + .join(pycram.orm.object_designator.Object.pose) + .join(pycram.orm.base.Pose.position)).first() + self.assertEqual(result.x, previous_position.position.x) + self.assertEqual(result.y, previous_position.position.y) + self.assertEqual(result.z, previous_position.position.z) + def test_lookAt_and_detectAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) action = DetectActionPerformable(object_description.resolve()) @@ -252,7 +273,7 @@ def test_setGripperAction(self): def test_open_and_closeAction(self): apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") apartment_desig = BelieveObject(names=["apartment"]).resolve() - handle_desig = object_designator.ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig).resolve() + handle_desig = object_designator.ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig, type=ObjectType.ENVIRONMENT).resolve() self.kitchen.set_pose(Pose([20, 20, 0], [0, 0, 0, 1])) @@ -274,7 +295,61 @@ def test_open_and_closeAction(self): apartment.remove() +class BelieveObjectTestCase(unittest.TestCase): + engine: sqlalchemy.engine + session: sqlalchemy.orm.Session + + @classmethod + def setUpClass(cls): + cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) + environment_path = "apartment.urdf" + cls.world = BulletWorld(WorldMode.DIRECT) + cls.robot = Object("pr2", ObjectType.ROBOT, path="pr2.urdf", pose=Pose([1, 2, 0])) + cls.apartment = Object(environment_path[:environment_path.find(".")], ObjectType.ENVIRONMENT, environment_path) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]), + color=Color(1, 0, 0, 1)) + cls.viz_marker_publisher = VizMarkerPublisher() + OntologyManager(SOMA_ONTOLOGY_IRI) + + def setUp(self): + self.world.reset_world() + pycram.orm.base.Base.metadata.create_all(self.engine) + self.session = sqlalchemy.orm.Session(bind=self.engine) + + def tearDown(self): + pycram.tasktree.task_tree.reset_tree() + time.sleep(0.05) + pycram.orm.base.ProcessMetaData.reset() + pycram.orm.base.Base.metadata.drop_all(self.engine) + self.session.close() + self.world.reset_world() + + @classmethod + def tearDownClass(cls): + cls.viz_marker_publisher._stop_publishing() + cls.world.exit() + + def test_believe_object(self): + # TODO: Find better way to separate BelieveObject no pose from Object pose + + with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + NavigateAction(target_locations=[Pose([2, -1.89, 0])]).resolve().perform() + + LookAtAction(targets=[Pose([1, -1.78, 0.55])]).resolve().perform() + + object_desig = DetectAction(BelieveObject(types=[ObjectType.MILK])).resolve().perform() + TransportAction(object_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + pycram.orm.base.ProcessMetaData().description = "BelieveObject_test" + task_tree.root.insert(self.session) + + class ViewsSchemaTest(DatabaseTestCaseMixin): + def test_view_creation(self): pycram.orm.base.ProcessMetaData().description = "view_creation_test" pycram.tasktree.task_tree.root.insert(self.session) @@ -287,14 +362,16 @@ def test_view_creation(self): self.assertEqual(view.__table__.columns[3].name, "torso_height") self.assertEqual(view.__table__.columns[4].name, "relative_x") self.assertEqual(view.__table__.columns[5].name, "relative_y") - self.assertEqual(view.__table__.columns[6].name, "quaternion_x") - self.assertEqual(view.__table__.columns[7].name, "quaternion_y") - self.assertEqual(view.__table__.columns[8].name, "quaternion_z") - self.assertEqual(view.__table__.columns[9].name, "quaternion_w") + self.assertEqual(view.__table__.columns[6].name, "x") + self.assertEqual(view.__table__.columns[7].name, "y") + self.assertEqual(view.__table__.columns[8].name, "z") + self.assertEqual(view.__table__.columns[9].name, "w") self.assertEqual(view.__table__.columns[10].name, "obj_type") self.assertEqual(view.__table__.columns[11].name, "status") def test_pickUpWithContextView(self): + if self.engine.dialect.name == "sqlite": + return object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) self.assertEqual(description.ground().object_designator.name, "milk") @@ -315,6 +392,8 @@ def test_pickUpWithContextView(self): self.assertEqual(result.quaternion_w, 1) def test_pickUpWithContextView_conditions(self): + if self.engine.dialect.name == "sqlite": + return object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], [Arms.LEFT]) self.assertEqual(description.ground().object_designator.name, "milk") diff --git a/test/test_robot_description.py b/test/test_robot_description.py index 3e3985054..e845d9ec6 100644 --- a/test/test_robot_description.py +++ b/test/test_robot_description.py @@ -3,7 +3,7 @@ from pycram.robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ CameraDescription, RobotDescriptionManager from pycram.datastructures.enums import Arms, GripperState -from urdf_parser_py.urdf import URDF +from pycram.object_descriptors.urdf import ObjectDescription as URDF class TestRobotDescription(unittest.TestCase): @@ -11,7 +11,8 @@ class TestRobotDescription(unittest.TestCase): @classmethod def setUpClass(cls): cls.path = str(pathlib.Path(__file__).parent.resolve()) + '/../resources/robots/' + "pr2" + '.urdf' - cls.urdf_obj = URDF.from_xml_file(cls.path) + cls.path_turtlebot = str(pathlib.Path(__file__).parent.resolve()) + '/../resources/robots/' + "turtlebot" + '.urdf' + cls.urdf_obj = URDF(cls.path) def test_robot_description_construct(self): robot_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", self.path) @@ -190,3 +191,13 @@ def test_load_robot_description(self): rdm.register_description(robot_description) rdm.load_description("pr2_test2") self.assertIs(RobotDescription.current_robot_description, robot_description) + + def test_robot_description_turtlebot(self): + robot_description = RobotDescription("turtlebot", "base_link", "base_link", "base_joint", self.path_turtlebot) + self.assertEqual(robot_description.name, "turtlebot") + self.assertEqual(robot_description.base_link, "base_link") + self.assertEqual(robot_description.torso_link, "base_link") + self.assertEqual(robot_description.torso_joint, "base_joint") + self.assertTrue(type(robot_description.urdf_object) is URDF) + self.assertEqual(len(robot_description.links), 11) + self.assertEqual(len(robot_description.joints), 10) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index dee20a698..01bda73c8 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -8,7 +8,7 @@ import unittest import anytree from bullet_world_testcase import BulletWorldTestCase -import pycram.plan_failures +import pycram.failures from pycram.designators import object_designator, action_designator @@ -27,7 +27,7 @@ def plan(self): def setUp(self): super().setUp() - pycram.tasktree.reset_tree() + pycram.tasktree.task_tree.reset_tree() def test_tree_creation(self): """Test the creation and content of a task tree.""" @@ -48,11 +48,11 @@ def test_exception(self): @with_tree def failing_plan(): - raise pycram.plan_failures.PlanFailure("PlanFailure for UnitTesting") + raise pycram.failures.PlanFailure("PlanFailure for UnitTesting") - pycram.tasktree.reset_tree() + pycram.tasktree.task_tree.reset_tree() - self.assertRaises(pycram.plan_failures.PlanFailure, failing_plan) + self.assertRaises(pycram.failures.PlanFailure, failing_plan) tt = pycram.tasktree.task_tree @@ -85,6 +85,19 @@ def test_to_sql(self): result = tt.root.to_sql() self.assertIsNotNone(result) + def test_task_tree_singleton(self): + # Instantiate one TaskTree object + tree1 = pycram.tasktree.TaskTree() + + # Fill the tree + self.plan() + + # Instantiate another TaskTree object + tree2 = pycram.tasktree.TaskTree() + + # Check if both instances point to the same object and contain the same number of elements + self.assertEqual(len(tree1.root), len(tree2.root)) + self.assertIs(tree1, tree2) if __name__ == '__main__': unittest.main()