diff --git a/.github/workflows/pycram-ci.yml b/.github/workflows/pycram-ci.yml index c7bf4db3c..2c0e299c9 100644 --- a/.github/workflows/pycram-ci.yml +++ b/.github/workflows/pycram-ci.yml @@ -69,16 +69,16 @@ jobs: catkin_make echo 'export ROS_HOSTNAME=localhost' >> ~/.bashrc echo 'source $GITHUB_WORKSPACE/ros_ws/devel/setup.bash' >> ~/.bashrc + - name: Upgrade pip + run: | + sudo pip3 install --upgrade pip - name: Install requirements run: | cd $GITHUB_WORKSPACE/ros_ws/src/pycram sudo pip3 install -r requirements.txt - - name: upgrade numpy - run: | - sudo pip3 install --upgrade numpy - name: install additional requirements run: | - sudo pip3 install pytest pyjpt mlflow + sudo pip3 install --ignore-installed pytest pyjpt mlflow - name: start roscore run: | roslaunch pycram ik_and_description.launch & diff --git a/.github/workflows/reusable-sphinx-build.yml b/.github/workflows/reusable-sphinx-build.yml index d9de9b353..c6353b831 100644 --- a/.github/workflows/reusable-sphinx-build.yml +++ b/.github/workflows/reusable-sphinx-build.yml @@ -4,6 +4,10 @@ on: push: branches: - dev + pull_request: + branches: + - master + - dev # ---------------------------------------------------------------------------------------------------------------------- diff --git a/.gitmodules b/.gitmodules index 63abede05..a87f5a181 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,4 @@ [submodule "src/neem_interface_python"] path = src/neem_interface_python url = https://github.com/Tigul/neem_interface_python -[submodule "src/macropy"] - path = src/macropy - url = https://github.com/Tigul/macropy.git + diff --git a/README.md b/README.md index 3e127326d..c243da496 100644 --- a/README.md +++ b/README.md @@ -92,12 +92,3 @@ how to do that take a look at the respective `README` file. If you encounter some error please first take a look at the [troubleshooting](https://pycram.readthedocs.io/en/latest/troubleshooting.html) section and see if the error is mentioned there. - -## Authors - -* **Jonas Dech** -* **Andy Augsten** -* **Dustin Augsten** -* **Christopher Pollok** -* **Thomas Lipps** -* **Benjamin Alt** diff --git a/doc/source/designators.rst b/doc/source/designators.rst index d07af0a5e..08ec4d8f4 100644 --- a/doc/source/designators.rst +++ b/doc/source/designators.rst @@ -45,7 +45,7 @@ action looks like this: .. code-block:: python - NavigateAction.Action(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=((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)), target_location=[[1, 0, 0], [0, 0, 0, 1]]) A visual representation of the whole idea of designator and designator descriptions can be diff --git a/doc/source/examples.rst b/doc/source/examples.rst index 9cd424b2f..8810ec0a9 100644 --- a/doc/source/examples.rst +++ b/doc/source/examples.rst @@ -28,13 +28,15 @@ Misc notebooks/minimal_task_tree notebooks/pose notebooks/custom_resolver + notebooks/language + notebooks/local_transformer Interface Examples ================== .. nbgallery:: - notebooks/giskard - notebooks/robokudo + notebooks/interface_examples/giskard + notebooks/interface_examples/robokudo Object Relational Mapping ========================= diff --git a/doc/source/index.rst b/doc/source/index.rst index c8ab43941..fb1161d2d 100644 --- a/doc/source/index.rst +++ b/doc/source/index.rst @@ -52,30 +52,48 @@ The code for this plan can be seen below. .. code-block:: python - @with_simulated_robot - def plan(): - MotionDesignator(MoveArmJointsMotionDescription(left_arm_config='park', - right_arm_config='park')).perform() + from pycram.bullet_world import BulletWorld, 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 - MotionDesignator(MoveMotionDescription(target=moving_targets[robot_name]["sink"][0], - orientation=moving_targets[robot_name]["sink"][1])).perform() + world = BulletWorld() + 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]) - det_obj = MotionDesignator(DetectingMotionDescription(object_type="milk")).perform() + cereal_desig = ObjectDesignatorDescription(names=["cereal"]) + kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) + robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() - MotionDesignator(PickUpMotionDescription(object=milk, arm="left", grasp="front")).perform() + with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + MoveTorsoAction([0.3]).resolve().perform() - MotionDesignator(MoveMotionDescription(target=moving_targets[robot_name]["island"][0], - orientation=moving_targets[robot_name]["island"][1])).perform() + pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] - MotionDesignator(PlaceMotionDescription(object=milk, target=[-0.9, 1, 0.93], arm="left")).perform() + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() - MotionDesignator(MoveArmJointsMotionDescription(left_arm_config='park', - right_arm_config='park')).perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=["front"]).resolve().perform() - MotionDesignator(MoveMotionDescription(target=[0.0, 0.0, 0], - orientation=[0, 0, 0, 1])).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() + + world.exit() Tutorials --------- diff --git a/doc/source/installation.rst b/doc/source/installation.rst index 32625f5b1..49498a283 100644 --- a/doc/source/installation.rst +++ b/doc/source/installation.rst @@ -112,6 +112,7 @@ Then install the Python packages in the requirements.txt file Building your ROS workspace =========================== +.. _build-ws: Building and sourcing your ROS workspace using catkin compiles all ROS packages and manages the appending to the respective PATH variables. This is necessary to be able to import PyCRAM via the Python import system and to find the @@ -160,6 +161,7 @@ After installing pandoc, install sphinx on your device. sudo apt install python3-sphinx + Install the requirements in your python interpreter. .. code-block:: shell @@ -180,27 +182,67 @@ Show the index. firefox build/html/index.html + Setting up PyCRAM with PyCharm ============================== -Setting up PyCharm with packages that rely on rospy is non trivial. Follow this guide to get correct syntax highlighting -for the PyCRAM project. +Setting up PyCharm with packages that rely on rospy is non trivial. Follow this guide to get correct syntax highlighting for the PyCRAM project. + +Install PyCharm Professional +---------------------------- First, `install PyCharm Professional `_. -Next, if you have virtual environments that you want to use, you need to make sure that they have rospy available. -If you create a new environment, make sure to include `--system-site-packages` in your creation command. -You can check by activating your environment and calling the import +Create a JetBrains account and verify it for educational purpose. Now you can unlock the PyCharm Professional features in PyCharm. + +The next step will set up the virtual Python environment, so it can be used as a project interpreter in PyCharm. + + +Set up virtualenv +----------------- +.. _virtualenv: + +The virtualenvwrapper allows to manage virtual Python environments, where additional packages can be installed without the risk of breaking the system-wide Python configuration. Install `virtualenvwrapper `_ via pip and set it up. + +.. code-block:: shell + + sudo pip3 install virtualenvwrapper + echo "export WORKON_HOME=~/envs" >> ~/.bashrc + echo "source /usr/local/bin/virtualenvwrapper.sh" >> ~/.bashrc + mkdir -p $WORKON_HOME + source ~/.bashrc + +Create a virtual env based on the workspaces libraries (see build-ws_) and add the `--system-site-packages` to get them properly. The env will be registered in `$WORKON_HOME`. + +.. code-block:: shell + + source ~/workspace/ros/devel/setup.bash + mkvirtualenv pycram --system-site-packages + ls $WORKON_HOME + + +Check if the ROS libraries are available in the virtual env. .. code-block:: shell - workon your_env + workon pycram python -c "import rospy" +If it complains about `python`, install the following, to set `python` to Python 3 by default. + +.. code-block:: shell + + sudo apt install python-is-python3 + +If it finds `python` but complains about missing packages, make sure that the workspace is sourced before creating the virtual env. Also remember to create the virtual env with the `--system-site-packages` flag. + If this returns no errors, you can be sure that rospy is usable in your virtual environment. Next you have to build the -ros workspace including pycram and source it as described in install-pycram_. +ros workspace including pycram and source it as described in build-ws_. + +Configure PyCharm +----------------- -After that you have to start PyCharm from the terminal via +Always start PyCharm from the terminal via .. code-block:: shell @@ -212,6 +254,7 @@ or ~/pycharm/bin/pycharm.sh + Select **File | Open** and select the root folder of the PyCRAM package. Next go to **File | Settings | Project: pycram | Python Interpreter** and set up your virtual environment with rospy and the sourced workspace available as the python interpreter. @@ -221,6 +264,7 @@ folder as Tests and the resources as Resources. To verify that it works, you can execute any Testcase. + Using IPython as REPL ===================== diff --git a/doc/source/notebooks b/doc/source/notebooks new file mode 120000 index 000000000..da7b19653 --- /dev/null +++ b/doc/source/notebooks @@ -0,0 +1 @@ +../../examples/ \ No newline at end of file diff --git a/doc/source/notebooks/action_designator.ipynb b/doc/source/notebooks/action_designator.ipynb deleted file mode 120000 index d805f8db5..000000000 --- a/doc/source/notebooks/action_designator.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/action_designator.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/bullet_world.ipynb b/doc/source/notebooks/bullet_world.ipynb deleted file mode 120000 index bdb627d54..000000000 --- a/doc/source/notebooks/bullet_world.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/bullet_world.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/custom_resolver.ipynb b/doc/source/notebooks/custom_resolver.ipynb deleted file mode 120000 index ecd9fa1f8..000000000 --- a/doc/source/notebooks/custom_resolver.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/custom_resolver.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/giskard.ipynb b/doc/source/notebooks/giskard.ipynb deleted file mode 120000 index 6d8f4dcef..000000000 --- a/doc/source/notebooks/giskard.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/interface_examples/giskard.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/intro.ipynb b/doc/source/notebooks/intro.ipynb deleted file mode 120000 index 31a4f6830..000000000 --- a/doc/source/notebooks/intro.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/intro.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/location_designator.ipynb b/doc/source/notebooks/location_designator.ipynb deleted file mode 120000 index 60e30b0a0..000000000 --- a/doc/source/notebooks/location_designator.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/location_designator.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/migrate_neems.ipynb b/doc/source/notebooks/migrate_neems.ipynb deleted file mode 120000 index 031ee0a9e..000000000 --- a/doc/source/notebooks/migrate_neems.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/migrate_neems.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/minimal_task_tree.ipynb b/doc/source/notebooks/minimal_task_tree.ipynb deleted file mode 120000 index 6f3de6889..000000000 --- a/doc/source/notebooks/minimal_task_tree.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/minimal_task_tree.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/motion_designator.ipynb b/doc/source/notebooks/motion_designator.ipynb deleted file mode 120000 index 74539a9e4..000000000 --- a/doc/source/notebooks/motion_designator.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/motion_designator.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/object_designator.ipynb b/doc/source/notebooks/object_designator.ipynb deleted file mode 120000 index 065caac0b..000000000 --- a/doc/source/notebooks/object_designator.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/object_designator.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/orm_example.ipynb b/doc/source/notebooks/orm_example.ipynb deleted file mode 120000 index dd3b43ee0..000000000 --- a/doc/source/notebooks/orm_example.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/orm_example.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/orm_querying_examples.ipynb b/doc/source/notebooks/orm_querying_examples.ipynb deleted file mode 120000 index 853d8d1d2..000000000 --- a/doc/source/notebooks/orm_querying_examples.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/orm_querying_examples.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/pose.ipynb b/doc/source/notebooks/pose.ipynb deleted file mode 120000 index ca40edb56..000000000 --- a/doc/source/notebooks/pose.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/pose.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/robokudo.ipynb b/doc/source/notebooks/robokudo.ipynb deleted file mode 120000 index 97928bc1b..000000000 --- a/doc/source/notebooks/robokudo.ipynb +++ /dev/null @@ -1 +0,0 @@ -../../../examples/interface_examples/robokudo.ipynb \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/action_designator.png b/doc/source/notebooks/thumbnails/action_designator.png deleted file mode 120000 index f39b0478e..000000000 --- a/doc/source/notebooks/thumbnails/action_designator.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/action_designator.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/bullet_world.png b/doc/source/notebooks/thumbnails/bullet_world.png deleted file mode 120000 index ba0e38633..000000000 --- a/doc/source/notebooks/thumbnails/bullet_world.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/bullet_world.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/default.png b/doc/source/notebooks/thumbnails/default.png deleted file mode 120000 index 91394022f..000000000 --- a/doc/source/notebooks/thumbnails/default.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/pycram_logo.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/location_designator.png b/doc/source/notebooks/thumbnails/location_designator.png deleted file mode 120000 index bc2a3d989..000000000 --- a/doc/source/notebooks/thumbnails/location_designator.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/location_designator.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/motion_designator.png b/doc/source/notebooks/thumbnails/motion_designator.png deleted file mode 120000 index 25d82ec74..000000000 --- a/doc/source/notebooks/thumbnails/motion_designator.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/motion_designator.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/object_designator.png b/doc/source/notebooks/thumbnails/object_designator.png deleted file mode 120000 index 876198dde..000000000 --- a/doc/source/notebooks/thumbnails/object_designator.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/object_designator.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/overview.png b/doc/source/notebooks/thumbnails/overview.png deleted file mode 120000 index ec26a8933..000000000 --- a/doc/source/notebooks/thumbnails/overview.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/overview.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/pose.png b/doc/source/notebooks/thumbnails/pose.png deleted file mode 120000 index 5dcf9f1ff..000000000 --- a/doc/source/notebooks/thumbnails/pose.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/pose.png \ No newline at end of file diff --git a/doc/source/notebooks/thumbnails/tree.png b/doc/source/notebooks/thumbnails/tree.png deleted file mode 120000 index 251604eb3..000000000 --- a/doc/source/notebooks/thumbnails/tree.png +++ /dev/null @@ -1 +0,0 @@ -../../../images/thumbnails/tree.png \ No newline at end of file diff --git a/examples/action_designator.ipynb b/examples/action_designator.ipynb index 69cfa4ef3..9ee38d1f1 100644 --- a/examples/action_designator.ipynb +++ b/examples/action_designator.ipynb @@ -6,12 +6,12 @@ "metadata": {}, "source": [ "# Action Designator\n", - "This example will show the different kinds of Action Designator that are available. We will see how to create Action Designators and what they do.\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 Designator are high-level descriptions of actions which the robot should execute. \n", + "Action Designators are high-level descriptions of actions which the robot should execute. \n", "\n", - "Action Deisgnators are created from a Action Designator Descritpion, which describes the type of action as well as the parameter for this action. Parameter are given as a list of possible parameters.\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" ] @@ -103,7 +103,7 @@ "id": "30bd6765", "metadata": {}, "source": [ - "What we now did was create the pose were 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." + "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." ] }, { @@ -126,7 +126,7 @@ "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_robor```" + "There are also decorators which do the same thing but for whole methods, they are called ```with_real_robot``` and ```with_simulated_robot```." ] }, { @@ -137,7 +137,7 @@ "## 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 perfomed in a ```simulated_robot``` environment. " + "We start again by creating a description and resolving it to a designator. Afterwards, the designator is performed in a ```simulated_robot``` environment. " ] }, { @@ -216,9 +216,9 @@ "metadata": {}, "source": [ "## Pick Up and Place\n", - "Since these are depending on each other, meaning you can only place something when you picked it up beforehand, they will be shown together. \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 so please check the example on object designators for more details. \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." ] @@ -552,7 +552,7 @@ "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 alredy spawned it in a previous example." + "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." ] }, { diff --git a/examples/bullet_world.ipynb b/examples/bullet_world.ipynb index 67016f4d2..05d506f68 100644 --- a/examples/bullet_world.ipynb +++ b/examples/bullet_world.ipynb @@ -96,15 +96,15 @@ "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 easiert to work with types. But since the types of the enum might not fit your case you can also use strings. \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 sufficent 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", + "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 it's orientation" + "Since the Object is spawned, we can now interact with it. First we want to move it around and change its orientation" ] }, { @@ -142,7 +142,7 @@ "id": "5805be38", "metadata": {}, "source": [ - "In the same sense as setting the position or orientation you can also get the position and orientation." + "In the same sense as setting the position or orientation, you can also get the position and orientation." ] }, { @@ -200,7 +200,7 @@ "## 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 neeed another Object" + "First we need another Object" ] }, { @@ -265,9 +265,9 @@ "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 namley ```links``` and ```joints``` which contain every link or joint as key and a unique id, used by PyBullet, as value. \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" + "We will see this at the example of the PR2:" ] }, { @@ -369,7 +369,7 @@ " * ```set_joint_state```\n", " * ```get_joint_limits```\n", " \n", - "We will see how these methods work at the example of the torso_lift_joint" + "We will see how these methods work at the example of the torso_lift_joint:" ] }, { @@ -404,7 +404,7 @@ "metadata": {}, "source": [ "## Misc Methods\n", - "There are a few methods that don't fit any category but could be helpful anyways. 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. " + "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. " ] }, { @@ -440,7 +440,7 @@ "id": "ce5d910b", "metadata": {}, "source": [ - "Lastly, there is ```get_AABB``` AABB stands for axis aligned bounding box. This method returns two points in world coordinates which span a rectangle representing the AABB." + "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." ] }, { diff --git a/examples/cram_plan_tutorial.ipynb b/examples/cram_plan_tutorial.ipynb index d322d7be3..a26cd5b88 100644 --- a/examples/cram_plan_tutorial.ipynb +++ b/examples/cram_plan_tutorial.ipynb @@ -287,30 +287,30 @@ "def plan(obj, obj_desig, torso=0.2, place=\"countertop\"):\n", " world.reset_bullet_world()\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", - " MoveTorsoAction.Action(torso).perform()\n", + " MoveTorsoActionPerformable(torso).perform()\n", " location = CostmapLocation(target=obj_desig, reachable_for=robot_desig)\n", " pose = location.resolve()\n", " print()\n", - " NavigateAction.Action(pose.pose).perform()\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " NavigateActionPerformable(pose.pose).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " good_torsos.append(torso)\n", " picked_up_arm = pose.reachable_arms[0]\n", - " PickUpAction.Action(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=\"front\").perform()\n", + " PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=\"front\").perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\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", - " NavigateAction.Action(pose.pose).perform()\n", + " NavigateActionPerformable(pose.pose).perform()\n", "\n", - " PlaceAction.Action(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()\n", + " PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "good_torsos = []\n", "for obj_name in object_names:\n", @@ -489,7 +489,7 @@ "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" + "Task tree can be manipulated with ordinary anytree manipulation. If we for example want to discard the second plan, we would write:" ] }, { @@ -613,7 +613,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "The task tree can also be reset to an empty one by invoking" + "The task tree can also be reset to an empty one by invoking:" ] }, { @@ -720,10 +720,10 @@ "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" + "\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" ] } ], diff --git a/examples/custom_resolver.ipynb b/examples/custom_resolver.ipynb index 0660276e4..ef4413897 100644 --- a/examples/custom_resolver.ipynb +++ b/examples/custom_resolver.ipynb @@ -23,8 +23,8 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:13:58.323072599Z", - "start_time": "2023-12-13T10:13:56.448691067Z" + "end_time": "2024-01-29T16:05:37.443271489Z", + "start_time": "2024-01-29T16:05:35.328010988Z" } }, "outputs": [ @@ -32,15 +32,15 @@ "name": "stdout", "output_type": "stream", "text": [ - "Requirement already up-to-date: probabilistic_model in /home/dprueser/.local/lib/python3.8/site-packages (1.4.13)\r\n", - "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", + "Requirement already up-to-date: probabilistic_model in /home/dprueser/.local/lib/python3.8/site-packages (1.6.20)\r\n", "Requirement already satisfied, skipping upgrade: random-events>=1.2.5 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (1.2.5)\r\n", "Requirement already satisfied, skipping upgrade: anytree>=2.9.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.9.0)\r\n", "Requirement already satisfied, skipping upgrade: portion>=2.4.1 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.4.1)\r\n", - "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n", + "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", + "Requirement already satisfied, skipping upgrade: six in /usr/local/lib/python3.8/dist-packages (from anytree>=2.9.0->probabilistic_model) (1.16.0)\r\n", + "Requirement already satisfied, skipping upgrade: sortedcontainers~=2.2 in /home/dprueser/.local/lib/python3.8/site-packages (from portion>=2.4.1->probabilistic_model) (2.4.0)\r\n", "Requirement already satisfied, skipping upgrade: tenacity>=6.2.0 in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (8.2.3)\r\n", - "Requirement already satisfied, skipping upgrade: six in /usr/lib/python3/dist-packages (from anytree>=2.9.0->probabilistic_model) (1.14.0)\r\n", - "Requirement already satisfied, skipping upgrade: sortedcontainers~=2.2 in /home/dprueser/.local/lib/python3.8/site-packages (from portion>=2.4.1->probabilistic_model) (2.4.0)\r\n" + "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n" ] } ], @@ -53,8 +53,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:13:59.230429151Z", - "start_time": "2023-12-13T10:13:58.332536124Z" + "end_time": "2024-01-29T16:05:38.648889392Z", + "start_time": "2024-01-29T16:05:37.442437872Z" } }, "outputs": [ @@ -67,12 +67,13 @@ "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] [1702462438.857886]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702462438.862737]: Failed to import Giskard messages\n" + "[WARN] [1706544338.060994]: Failed to import Giskard messages\n", + "[WARN] [1706544338.065403]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], "source": [ + "from pycram.designators.actions.actions import ParkArmsActionPerformable, MoveTorsoActionPerformable\n", "from tf import transformations\n", "import itertools\n", "from typing import Optional, List, Tuple\n", @@ -189,13 +190,13 @@ " # try to execute a grasping plan\n", " with simulated_robot:\n", "\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(pycram.enums.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", - " MoveTorsoAction.Action(height).perform()\n", + " MoveTorsoActionPerformable(height).perform()\n", "\n", " # try to pick it up\n", " try:\n", @@ -241,8 +242,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.758891855Z", - "start_time": "2023-12-13T10:13:59.232324147Z" + "end_time": "2024-01-29T16:07:21.351710589Z", + "start_time": "2024-01-29T16:05:38.643086881Z" } }, "outputs": [ @@ -262,9 +263,9 @@ "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", - " 45%|████▍ | 430/960 [00:50<00:58, 9.02it/s, success_rate=0.0791]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_shoulder_lift_link (parent map) at time 1702462489.281120 according to authority default_authority\n", + " 96%|█████████▌| 923/960 [01:38<00:03, 9.52it/s, success_rate=0.0845]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame spoon_2 (parent map) at time 1706544437.280444 according to authority default_authority\n", " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", - "100%|██████████| 960/960 [01:51<00:00, 9.30it/s, success_rate=0.0854]" + "100%|██████████| 960/960 [01:42<00:00, 10.12it/s, success_rate=0.0854]" ] } ], @@ -290,8 +291,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.844627649Z", - "start_time": "2023-12-13T10:15:50.794715519Z" + "end_time": "2024-01-29T16:07:21.445516208Z", + "start_time": "2024-01-29T16:07:21.396046276Z" } }, "outputs": [], @@ -317,8 +318,8 @@ "metadata": { "scrolled": true, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.932190158Z", - "start_time": "2023-12-13T10:15:50.846547804Z" + "end_time": "2024-01-29T16:07:21.541375696Z", + "start_time": "2024-01-29T16:07:21.448402324Z" } }, "outputs": [], @@ -357,8 +358,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.983050833Z", - "start_time": "2023-12-13T10:15:50.939401673Z" + "end_time": "2024-01-29T16:07:21.605599589Z", + "start_time": "2024-01-29T16:07:21.552928567Z" } }, "outputs": [ @@ -366,7 +367,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"TaskTreeNode\".status AS \"TaskTreeNode_status\", \"Object\".type AS \"Object_type\", \"PickUpAction\".arm AS \"PickUpAction_arm\", \"PickUpAction\".grasp AS \"PickUpAction_grasp\", \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", + "SELECT \"TaskTreeNode\".status, \"Object\".type, \"PickUpAction\".arm, \"PickUpAction\".grasp, \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" AS \"Pose_1\" ON \"Pose_1\".id = \"RobotState\".pose_id JOIN \"Position\" AS \"Position_2\" ON \"Position_2\".id = \"Pose_1\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id JOIN \"Pose\" AS \"Pose_2\" ON \"Pose_2\".id = \"Object\".pose_id JOIN \"Position\" AS \"Position_1\" ON \"Position_1\".id = \"Pose_2\".position_id\n", " status type arm grasp \\\n", "0 FAILED ObjectType.BREAKFAST_CEREAL left left \n", @@ -399,6 +400,7 @@ } ], "source": [ + "from sqlalchemy import select\n", "from pycram.orm.base import Pose as ORMPose\n", "\n", "robot_pose = sqlalchemy.orm.aliased(ORMPose)\n", @@ -406,7 +408,7 @@ "robot_pos = sqlalchemy.orm.aliased(Position)\n", "object_pos = sqlalchemy.orm.aliased(Position)\n", "\n", - "query = (session.query(TaskTreeNode.status, Object.type, ORMPickUpAction.arm, ORMPickUpAction.grasp,\n", + "query = (select(TaskTreeNode.status, Object.type, ORMPickUpAction.arm, ORMPickUpAction.grasp,\n", " sqlalchemy.label(\"relative torso height\", object_pos.z - RobotState.torso_height),\n", " sqlalchemy.label(\"x\", robot_pos.x - object_pos.x), \n", " sqlalchemy.label(\"y\", robot_pos.y - object_pos.y))\n", @@ -420,7 +422,7 @@ " .join(object_pos, object_pose.position))\n", "print(query)\n", "\n", - "df = pd.read_sql(query.statement, session.get_bind())\n", + "df = pd.read_sql(query, session.get_bind())\n", "df[\"status\"] = df[\"status\"].apply(lambda x: str(x.name))\n", "print(df)" ] @@ -437,8 +439,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:15:51.329697221Z", - "start_time": "2023-12-13T10:15:50.982828383Z" + "end_time": "2024-01-29T16:07:21.939517021Z", + "start_time": "2024-01-29T16:07:21.601075366Z" } }, "outputs": [ @@ -446,7 +448,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "100%|██████████| 960/960 [01:51<00:00, 8.59it/s, success_rate=0.0854]\n" + "100%|██████████| 960/960 [01:43<00:00, 9.32it/s, success_rate=0.0854]\n" ] }, { @@ -475,10 +477,12 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 8, "metadata": { + "collapsed": false, "ExecuteTime": { - "start_time": "2023-12-13T10:15:51.329714184Z" + "end_time": "2024-01-29T16:13:01.579305735Z", + "start_time": "2024-01-29T16:07:21.942920319Z" } }, "outputs": [ @@ -502,6 +506,7 @@ } ], "source": [ + "from pycram.designators.actions.actions import NavigateActionPerformable, PickUpActionPerformable\n", "from pycram.resolver import JPTCostmapLocation\n", "\n", "\n", @@ -516,13 +521,11 @@ "cml.visualize()\n", "with simulated_robot:\n", " for sample in iter(cml):\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", - " NavigateAction.Action(sample.pose).perform()\n", - " MoveTorsoAction.Action(sample.torso_height).perform()\n", + " ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform()\n", + " NavigateActionPerformable(sample.pose).perform()\n", + " MoveTorsoActionPerformable(sample.torso_height).perform()\n", " try:\n", - " PickUpAction.Action(\n", - " ObjectDesignatorDescription(types=[pycram.enums.ObjectType.MILK]).resolve(),\n", - " arm=sample.reachable_arm, grasp=sample.grasp).perform()\n", + " PickUpActionPerformable(ObjectDesignatorDescription(types=[pycram.enums.ObjectType.MILK]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform()\n", " # time.sleep(5)\n", " except pycram.plan_failures.PlanFailure as p:\n", " continue\n", diff --git a/examples/interface_examples/giskard.ipynb b/examples/interface_examples/giskard.ipynb index 91c6b957b..54f81f4bc 100644 --- a/examples/interface_examples/giskard.ipynb +++ b/examples/interface_examples/giskard.ipynb @@ -8,17 +8,17 @@ "# 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 installing and starting 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 start giskard by calling \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", - "In this way you can start Giskard for any robot that is supported.\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 \"standalone\" with \"iai\". \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", @@ -33,9 +33,9 @@ "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 roughtly divided into three parts:\n", - " 1. Methods to manage the beliefe states between PyCRAM and Giskard\n", - " 2. Motion goals that should be send to Giskard for execution \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", @@ -84,11 +84,11 @@ "id": "f4c4f85d", "metadata": {}, "source": [ - "Now we have a PyCRAM belief state set up, belief state in this case just refeers to the BulletWorld since the BulletWorld represents what we belief the world to look like. \n", + "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 use the ```achive_joint_goal```. This method takes a dictionary with the joints that should be moved and the target value for the joint. \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." + "Look at RViz to see the robot move, since we call Giskard for movement the robot in the BulletWorld will not move." ] }, { @@ -110,11 +110,11 @@ "id": "77dbded9", "metadata": {}, "source": [ - "For Giskard everything is connected by joints (this is called [World Tree](https://github.com/SemRoCo/giskardpy/wiki/World-Tree) by Giskard) therefore we can move the robot by using a motion goal between the map origin and the robot base. \n", + "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 distance but keep in mind that Giskard has no collision avoidance for longer journeys. So using MoveBase for longer distances is a better idea." + "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." ] }, { @@ -135,9 +135,9 @@ "id": "98af5723", "metadata": {}, "source": [ - "Now for the last example: we will move the gripper using full body motion controll. \n", + "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\". This will not only move the robot (because the 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." + "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." ] }, { @@ -158,7 +158,7 @@ "id": "7dfe78ba", "metadata": {}, "source": [ - "That conludes this example you can now close the BulletWorld by using the \"exit\" method." + "That concludes this example you can now close the BulletWorld by using the \"exit\" method." ] }, { @@ -177,15 +177,15 @@ "metadata": {}, "source": [ "## How the Giskard interface works \n", - "The PyCRAM interface to Giskard mostly relies on the Python interface that Giskard already proviedes ([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 inteface provides methods to achive motion goals and load things into the Giskard believe state. \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 with this does is: Synchronize the believe state of Giskard with the one of PyCRAM by loading the environment URDF in Giskard, this is done before any motion goal is send. Furthermore, the motion goals are wrapped in methods that use PyCRAM data types. \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 the following colliion modes:\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 default back to avoid_all_collisions.\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. " ] diff --git a/examples/interface_examples/robokudo.ipynb b/examples/interface_examples/robokudo.ipynb index 7f7f0f33d..04268950b 100644 --- a/examples/interface_examples/robokudo.ipynb +++ b/examples/interface_examples/robokudo.ipynb @@ -6,7 +6,7 @@ "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", + "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", @@ -61,7 +61,7 @@ "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." + "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." ] }, { @@ -70,12 +70,12 @@ "metadata": {}, "source": [ "## How the RoboKudo interface in PyCRAM works\n", - "The interface to RoboKudo is designed around the ROS service that RoboKudo proviedes. The interface takes a ObjectDesignatorDescription which is PyCRAMs symbolic representation of objects and converts it to a RoboKudo ObjectDesignator, the RoboKudo ObjectDesignator is then send to RoboKudo. \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 possbile 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", + "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 gloabl variable ```robokudo_action_client```." + "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```." ] }, { @@ -84,7 +84,7 @@ "metadata": {}, "source": [ "## How to extend the RoboKudo interface in PyCRAM\n", - "At the moment the RoboKudo interface is tailored toward a specific scenarion in which only two types of objects need to be detected. The distiction is mainly made by the difference in color, which is written to the RoboKudo ObjectDesignator depending on the ObjectType of the PyCRAM ObjectDesignator. \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." ] diff --git a/examples/intro.ipynb b/examples/intro.ipynb index aea7c14fb..9096f380c 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -12,11 +12,25 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-04-05T16:37:51.973404Z", - "start_time": "2023-04-05T16:37:51.483749Z" + "end_time": "2024-01-29T16:14:50.953932170Z", + "start_time": "2024-01-29T16:14:50.284068023Z" } }, - "outputs": [], + "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" ] @@ -39,8 +53,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-04-05T16:37:59.788099Z", - "start_time": "2023-04-05T16:37:57.586879Z" + "end_time": "2024-01-29T16:14:51.215354072Z", + "start_time": "2024-01-29T16:14:50.980844838Z" } }, "outputs": [ @@ -48,8 +62,6 @@ "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 tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" ] @@ -67,115 +79,37 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "The world can also be closed with the 'exit' method" + "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": 37, + "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2023-04-05T16:38:04.800655Z", - "start_time": "2023-04-05T16:38:03.943911Z" + "end_time": "2024-01-29T16:14:51.251936758Z", + "start_time": "2024-01-29T16:14:51.234948638Z" } }, - "outputs": [], - "source": [ - "world.exit()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The BulletWorld allows to render images from arbitrary positoins. 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 at the left side of the BulletWorld window. These images are:\n", - "* A 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": 4, - "metadata": {}, "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)]" - ] + "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": 4, + "execution_count": 3, "metadata": {}, "output_type": "execute_result" } @@ -207,13 +141,18 @@ " * 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" + "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": 5, - "metadata": {}, + "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\")" @@ -230,8 +169,13 @@ }, { "cell_type": "code", - "execution_count": 6, - "metadata": {}, + "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]))" @@ -246,8 +190,13 @@ }, { "cell_type": "code", - "execution_count": 7, - "metadata": {}, + "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()" @@ -262,8 +211,13 @@ }, { "cell_type": "code", - "execution_count": 8, - "metadata": {}, + "execution_count": 7, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:14:54.380977290Z", + "start_time": "2024-01-29T16:14:51.404113811Z" + } + }, "outputs": [ { "name": "stderr", @@ -299,13 +253,18 @@ "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 moveable in height, for example if the robot has a movable torso. " + "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": 9, - "metadata": {}, + "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", @@ -314,8 +273,13 @@ }, { "cell_type": "code", - "execution_count": 10, - "metadata": {}, + "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()" @@ -323,8 +287,13 @@ }, { "cell_type": "code", - "execution_count": 11, - "metadata": {}, + "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()" @@ -340,8 +309,13 @@ }, { "cell_type": "code", - "execution_count": 12, - "metadata": {}, + "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]))" @@ -349,8 +323,13 @@ }, { "cell_type": "code", - "execution_count": 13, - "metadata": {}, + "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", @@ -368,8 +347,13 @@ }, { "cell_type": "code", - "execution_count": 14, - "metadata": {}, + "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()" @@ -377,8 +361,13 @@ }, { "cell_type": "code", - "execution_count": 15, - "metadata": {}, + "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()" @@ -393,8 +382,13 @@ }, { "cell_type": "code", - "execution_count": 16, - "metadata": {}, + "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" @@ -402,8 +396,13 @@ }, { "cell_type": "code", - "execution_count": 17, - "metadata": {}, + "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()" @@ -411,8 +410,13 @@ }, { "cell_type": "code", - "execution_count": 18, - "metadata": {}, + "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()" @@ -432,18 +436,27 @@ "* Blocking\n", "* Supporting\n", "\n", - "To show the geometric reasoning we first a robot as well as the milk Object again." + "To show the geometric reasoning we first spawn a robot as well as the milk Object again." ] }, { "cell_type": "code", - "execution_count": 19, - "metadata": {}, + "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", @@ -466,21 +479,15 @@ }, { "cell_type": "code", - "execution_count": 20, + "execution_count": 19, "metadata": { - "scrolled": true + "scrolled": true, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:13.246942739Z", + "start_time": "2024-01-29T16:15:12.963076402Z" + } }, "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" - ] - }, { "name": "stdout", "output_type": "stream", @@ -497,8 +504,13 @@ }, { "cell_type": "code", - "execution_count": 21, - "metadata": {}, + "execution_count": 20, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:15:13.334740244Z", + "start_time": "2024-01-29T16:15:13.249042978Z" + } + }, "outputs": [ { "name": "stdout", @@ -518,8 +530,13 @@ }, { "cell_type": "code", - "execution_count": 22, - "metadata": {}, + "execution_count": 21, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:15:13.430296220Z", + "start_time": "2024-01-29T16:15:13.334507309Z" + } + }, "outputs": [ { "name": "stdout", @@ -540,8 +557,8 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Designator\n", - "Designator are symbolic descriptions of Actions, Motions, Objects or Locations. In PyCRAM the different types of designators are representet by a class which takes a description, the description then tells the designator what to do. \n", + "# 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" @@ -551,17 +568,22 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "## Motion Designator\n", + "## 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. This can be done either with a decorator which can be added to a function and then executes every designator in this function on the specified robot. The other possibility is a \"with\" scope which wraps a code piece. \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 in the function is a \"with\" scope which executes something on the simulated robot for reasoning purposes. " + "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": 23, - "metadata": {}, + "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", @@ -570,15 +592,19 @@ "description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", "\n", "with simulated_robot:\n", - " description.resolve().perform()\n", + " description.perform()\n", " \n" ] }, { "cell_type": "code", - "execution_count": 24, + "execution_count": 23, "metadata": { - "scrolled": true + "scrolled": true, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:14.446281217Z", + "start_time": "2024-01-29T16:15:13.948689644Z" + } }, "outputs": [], "source": [ @@ -586,7 +612,7 @@ "\n", "@with_simulated_robot\n", "def move():\n", - " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).resolve().perform()\n", + " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).perform()\n", "\n", "move()" ] @@ -596,8 +622,6 @@ "metadata": {}, "source": [ "Other implemented Motion Designator descriptions are:\n", - "* Pick up\n", - "* Place\n", "* Accessing\n", "* Move TCP\n", "* Looking\n", @@ -611,183 +635,26 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "## Object Designator\n", + "## Object Designators\n", "\n", - "Object Designator represent objects, these objects could either be from the BulletWorld or the real world. Object Designator are used, for example, by the PickUpAction to know which object should be picked up." + "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": 25, - "metadata": {}, + "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/jdech/workspace/ros/src/pycram-1/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: 1699448180\n", - " nsecs: 830921888\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: 1699448184\n", - " nsecs: 735546827\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={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\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_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448185\n", - " nsecs: 354960680\n", - " frame_id: \"map\"\n", - "child_frame_id: \"Milk_4\"\n", - "transform: \n", - " translation: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': }), _pose=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/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: 1699448180\n", - " nsecs: 830921888\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: 1699448184\n", - " nsecs: 735546827\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={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\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_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448185\n", - " nsecs: 354960680\n", - " frame_id: \"map\"\n", - "child_frame_id: \"Milk_4\"\n", - "transform: \n", - " translation: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': })>)" - ] + "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \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, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \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={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \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, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \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={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...)>)" }, - "execution_count": 25, + "execution_count": 24, "metadata": {}, "output_type": "execute_result" } @@ -809,11 +676,7913 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 25, "metadata": { - "scrolled": false + "scrolled": false, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:14.493010865Z", + "start_time": "2024-01-29T16:15:14.451251059Z" + } }, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \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, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \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={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \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, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \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={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_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": { + "scrolled": false, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:22.177685226Z", + "start_time": "2024-01-29T16:15:14.482072615Z" + } + }, + "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", @@ -835,13 +8604,18 @@ "metadata": {}, "source": [ "# Action Designator\n", - "Action Designator are used to describe high-level actions. Action Designator are usually composed of other Designators to describe the high-level action in detail. " + "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": 29, - "metadata": {}, + "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", @@ -856,13 +8630,18 @@ "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 grab a cereal box from the kitchen counter and place it on the kitchen island. This is a simple pick and place 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." ] }, { "cell_type": "code", - "execution_count": 30, - "metadata": {}, + "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", @@ -871,8 +8650,13 @@ }, { "cell_type": "code", - "execution_count": 31, - "metadata": {}, + "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", @@ -911,28 +8695,45 @@ "metadata": {}, "source": [ "# Task Trees\n", - "Task trees are a hirachical representation of all Actions involved in a plan. The Task tree can later be used to inspect and restructre the execution order of Actions in the plan." + "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": 32, - "metadata": {}, + "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(ParkArmsAction, )\n", - "├── perform(ParkArmsAction, )\n", - "├── perform(MoveTorsoAction, )\n", - "├── perform(NavigateAction, )\n", - "├── perform(PickUpAction, )\n", - "├── perform(ParkArmsAction, )\n", - "├── perform(NavigateAction, )\n", - "├── perform(PlaceAction, )\n", - "└── perform(ParkArmsAction, )\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" ] } ], @@ -945,8 +8746,13 @@ }, { "cell_type": "code", - "execution_count": 33, - "metadata": {}, + "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", @@ -962,38 +8768,37 @@ }, { "cell_type": "code", - "execution_count": 34, - "metadata": {}, + "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%|██████████| 10/10 [00:00<00:00, 143.50it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 22/22 [00:00<00:00, 119.33it/s]\n" ] }, { "data": { - "text/plain": [ - "pycram.orm.task.TaskTreeNode(1, 2023-11-08 13:55:09.816577, None, TaskStatus.RUNNING, None, None, 1, 1)" - ] + "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": 34, + "execution_count": 32, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "import sqlalchemy\n", "import sqlalchemy.orm\n", "import pycram.orm.base\n", - "import pycram.orm.task\n", - "import pycram.orm.object_designator\n", - "import pycram.orm.motion_designator\n", "import pycram.orm.action_designator\n", "\n", "# set description of what we are doing\n", - "pycram.orm.base.MetaData().description = \"Tutorial for getting familiar with the ORM.\"\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", @@ -1006,45 +8811,80 @@ }, { "cell_type": "code", - "execution_count": 35, - "metadata": {}, + "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": [ - "pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4)\n", - "pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9)\n" + "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.query(pycram.orm.action_designator.NavigateAction).all()\n", + "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": 36, - "metadata": {}, + "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": [ - "(pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4), pycram.orm.base.Position(0.5799999999999998, 1.28, 0.0, 4, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16378361324016077, -0.9864962889104031, 4, None))\n", - "(pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9), pycram.orm.base.Position(-1.9074999952316287, 0.779200015068054, 0.0, 9, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16439898730535735, 0.9863939238321437, 9, None))\n" + "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.query(pycram.orm.action_designator.NavigateAction, \n", - " pycram.orm.base.Position, \n", - " pycram.orm.base.Quaternion).\\\n", - " join(pycram.orm.base.Position).\\\n", - " join(pycram.orm.base.Quaternion).all()\n", + "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", + "source": [ + "The world can also be closed with the 'exit' method" + ], + "metadata": { + "collapsed": false + } + }, + { + "cell_type": "code", + "execution_count": 35, + "outputs": [], + "source": [ + "world.exit()" + ], + "metadata": { + "collapsed": false, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:37.732405944Z", + "start_time": "2024-01-29T16:15:37.488193658Z" + } + } } ], "metadata": { diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb index 2d4d41c03..fc039ab60 100644 --- a/examples/local_transformer.ipynb +++ b/examples/local_transformer.ipynb @@ -5,6 +5,8 @@ "id": "611b7e76", "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", @@ -17,14 +19,14 @@ "id": "615cb805", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:51:08.901104329Z", - "start_time": "2023-09-08T16:51:08.139879012Z" + "end_time": "2024-02-07T09:18:56.397119146Z", + "start_time": "2024-02-07T09:18:56.384091718Z" } }, "outputs": [], "source": [ "from pycram.bullet_world import BulletWorld, Object\n", - "from pycram.pose import Transform\n", + "from pycram.pose import Transform, Pose\n", "from pycram.local_transformer import LocalTransformer" ] }, @@ -37,31 +39,17 @@ "## 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" + "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, + "execution_count": null, "id": "7a3222c2", - "metadata": { - "ExecuteTime": { - "end_time": "2023-09-08T16:51:11.202198685Z", - "start_time": "2023-09-08T16:51:10.916529902Z" - } - }, - "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 tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" - ] - } - ], + "metadata": {}, + "outputs": [], "source": [ "# Create an instance of the BulletWorld\n", "world = BulletWorld()\n" @@ -79,7 +67,12 @@ "cell_type": "code", "execution_count": 9, "id": "c019c259", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-02-07T09:20:31.566964425Z", + "start_time": "2024-02-07T09:20:31.309671668Z" + } + }, "outputs": [], "source": [ "world.exit()" @@ -95,7 +88,7 @@ "\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.\n" + "These objects will be used in subsequent tasks, to provide the frames to which we will transform poses.\n" ] }, { @@ -104,8 +97,8 @@ "id": "14494539", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:51:38.439962987Z", - "start_time": "2023-09-08T16:51:33.675944073Z" + "end_time": "2024-02-07T09:19:12.883651520Z", + "start_time": "2024-02-07T09:19:09.027873229Z" } }, "outputs": [ @@ -113,11 +106,6 @@ "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", - "Scalar element defined multiple times: limit\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" ] } @@ -126,8 +114,6 @@ "from pycram.bullet_world import Object\n", "from pycram.enums import ObjectType\n", "\n", - "plane = Object(\"floor\", ObjectType.ENVIRONMENT, \"plane.urdf\", world=world)\n", - "\n", "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n", "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", Pose([0.9, 1, 0.95]))\n", "bowl = Object(\"bowl\", ObjectType.BOWL, \"bowl.stl\", Pose([1.6, 1, 0.90]))" @@ -146,14 +132,19 @@ "cell_type": "code", "execution_count": 4, "id": "708606eb", - "metadata": {}, + "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" + "\n", + "\n" ] } ], @@ -182,8 +173,8 @@ "id": "988ffda1", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:42:15.478051994Z", - "start_time": "2023-09-08T16:42:15.472743693Z" + "end_time": "2024-02-07T09:19:19.412829961Z", + "start_time": "2024-02-07T09:19:19.398278560Z" } }, "outputs": [ @@ -194,9 +185,9 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448290\n", - " nsecs: 552922010\n", - " frame_id: \"milk_4\"\n", + " secs: 1707297559\n", + " nsecs: 407153367\n", + " frame_id: \"milk_3\"\n", "pose: \n", " position: \n", " x: 0.09999999999999998\n", @@ -207,11 +198,12 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0\n", + "-------------------\n", "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448290\n", - " nsecs: 553559541\n", + " secs: 1707297559\n", + " nsecs: 407913208\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -228,7 +220,6 @@ ], "source": [ "from pycram.local_transformer import LocalTransformer\n", - "from pycram.pose import Pose\n", "\n", "l = LocalTransformer()\n", "test_pose = Pose([1, 1, 1], [0, 0, 0, 1], \"map\")\n", @@ -236,6 +227,7 @@ "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)" ] @@ -246,14 +238,19 @@ "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 u can set a Transform." + "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": 6, "id": "dd2bafc5", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-02-07T09:19:24.941756539Z", + "start_time": "2024-02-07T09:19:24.929728567Z" + } + }, "outputs": [], "source": [ "from pycram.pose import Transform\n", @@ -278,8 +275,8 @@ "id": "6b23f540", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:47:16.375950749Z", - "start_time": "2023-09-08T16:47:16.332420624Z" + "end_time": "2024-02-07T09:19:26.995686220Z", + "start_time": "2024-02-07T09:19:26.983125610Z" } }, "outputs": [], @@ -305,14 +302,19 @@ "cell_type": "code", "execution_count": 8, "id": "bd619710", - "metadata": {}, + "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_4\n", - "kitchen_3/kitchen_island_surface\n" + "milk_3\n", + "kitchen_2/kitchen_island_surface\n" ] } ], diff --git a/examples/location_designator.ipynb b/examples/location_designator.ipynb index 9865e46f7..a6c368aec 100644 --- a/examples/location_designator.ipynb +++ b/examples/location_designator.ipynb @@ -6,18 +6,18 @@ "metadata": {}, "source": [ "# Location Designator\n", - "This example will show you waht location designator are, how to use them and what they are capable of. \n", + "This example will show you what location designators are, how to use them and what they are capable of. \n", "\n", - "Location Deisgnator 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", + "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", - " * Occupany \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 designator work similar to other designator, 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", + "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", @@ -63,7 +63,7 @@ "id": "8c09672d", "metadata": {}, "source": [ - "Next up we will create the location designator description, the ```CostmapLocation``` that we will be using needs a target as 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", + "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. " ] @@ -187,7 +187,7 @@ "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 rechable 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", + "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. " ] @@ -252,7 +252,7 @@ "## 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", + "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." ] @@ -359,7 +359,7 @@ "## 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." + "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." ] }, { diff --git a/examples/migrate_neems.ipynb b/examples/migrate_neems.ipynb index db3baabe5..8f8064c0d 100644 --- a/examples/migrate_neems.ipynb +++ b/examples/migrate_neems.ipynb @@ -128,7 +128,7 @@ " @with_tree\n", " def pick_and_place_plan(self):\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\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", @@ -147,7 +147,7 @@ "\n", " PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n" + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n" ] }, { diff --git a/examples/minimal_task_tree.ipynb b/examples/minimal_task_tree.ipynb index 1ac203580..ff4839c44 100644 --- a/examples/minimal_task_tree.ipynb +++ b/examples/minimal_task_tree.ipynb @@ -16,11 +16,25 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:45:06.870087Z", - "start_time": "2023-04-28T09:45:06.870015Z" + "end_time": "2024-01-05T11:47:15.273850937Z", + "start_time": "2024-01-05T11:47:14.439086041Z" } }, - "outputs": [], + "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] [1704455235.033913]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704455235.039023]: Failed to import Giskard messages\n" + ] + } + ], "source": [ "from pycram.bullet_world import BulletWorld\n", "from pycram.robot_descriptions import robot_description\n", @@ -48,8 +62,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:32:49.441717Z", - "start_time": "2023-04-27T17:32:49.441589Z" + "end_time": "2024-01-05T11:47:21.361713709Z", + "start_time": "2024-01-05T11:47:15.370855034Z" } }, "outputs": [ @@ -57,19 +71,19 @@ "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 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", "Scalar element defined multiple times: limit\n" ] } @@ -98,8 +112,8 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:32:58.124468Z", - "start_time": "2023-04-27T17:32:58.124325Z" + "end_time": "2024-01-05T11:47:35.217688853Z", + "start_time": "2024-01-05T11:47:21.361234790Z" } }, "outputs": [], @@ -107,7 +121,7 @@ "@pycram.task.with_tree\n", "def plan():\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " MoveTorsoAction([0.3]).resolve().perform()\n", " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", " pickup_arm = pickup_pose.reachable_arms[0]\n", @@ -126,7 +140,7 @@ "\n", " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "plan()\n" ] @@ -143,8 +157,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:33:02.572319Z", - "start_time": "2023-04-27T17:33:02.572253Z" + "end_time": "2024-01-05T11:47:35.222415842Z", + "start_time": "2024-01-05T11:47:35.217889291Z" } }, "outputs": [ @@ -154,15 +168,25 @@ "text": [ "no_operation()\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -183,8 +207,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:45:59.592864Z", - "start_time": "2023-04-28T09:45:58.928492Z" + "end_time": "2024-01-05T11:47:49.422118950Z", + "start_time": "2024-01-05T11:47:35.221784522Z" } }, "outputs": [ @@ -194,25 +218,45 @@ "text": [ "no_operation()\n", "├── plan()\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(MoveTorsoAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PickUpAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PlaceAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ └── perform(ParkArmsAction, )\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(MoveTorsoActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PickUpActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PlaceActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ └── perform(ParkArmsActionPerformable)\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -234,8 +278,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:37.041421Z", - "start_time": "2023-04-28T09:46:36.999813Z" + "end_time": "2024-01-05T11:47:49.434500008Z", + "start_time": "2024-01-05T11:47:49.422026208Z" } }, "outputs": [ @@ -246,25 +290,45 @@ "simulation()\n", "no_operation()\n", "├── plan()\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(MoveTorsoAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PickUpAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PlaceAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ └── perform(ParkArmsAction, )\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(MoveTorsoActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PickUpActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PlaceActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ └── perform(ParkArmsActionPerformable)\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -286,8 +350,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:45.637721Z", - "start_time": "2023-04-28T09:46:45.635535Z" + "end_time": "2024-01-05T11:47:49.438410783Z", + "start_time": "2024-01-05T11:47:49.433036544Z" } }, "outputs": [ @@ -297,15 +361,25 @@ "text": [ "no_operation()\n", "+-- plan()\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(MoveTorsoAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PickUpAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PlaceAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " +-- perform(ParkArmsAction, )\n" + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(MoveTorsoActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PickUpActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PlaceActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " +-- perform(ParkArmsActionPerformable)\n" ] } ], @@ -326,8 +400,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:48.259716Z", - "start_time": "2023-04-28T09:46:48.233474Z" + "end_time": "2024-01-05T11:47:57.076211143Z", + "start_time": "2024-01-05T11:47:49.437672197Z" } }, "outputs": [ @@ -337,15 +411,25 @@ "text": [ "no_operation()\n", "+-- plan()\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(MoveTorsoAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PickUpAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PlaceAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " +-- perform(ParkArmsAction, )\n" + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(MoveTorsoActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PickUpActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PlaceActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " +-- perform(ParkArmsActionPerformable)\n" ] } ], @@ -368,8 +452,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:52.656020Z", - "start_time": "2023-04-28T09:46:52.653083Z" + "end_time": "2024-01-05T11:47:57.081178616Z", + "start_time": "2024-01-05T11:47:57.077818445Z" } }, "outputs": [ @@ -378,9 +462,9 @@ "output_type": "stream", "text": [ "Code: plan() \n", - " start_time: 2023-11-08 14:01:12.716969 \n", + " start_time: 2024-01-05 12:47:21.363057 \n", " Status: TaskStatus.SUCCEEDED \n", - " end_time: 2023-11-08 14:01:24.675108 \n", + " end_time: 2024-01-05 12:47:35.214734 \n", " \n" ] } @@ -401,8 +485,8 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:55.385481Z", - "start_time": "2023-04-28T09:46:55.381316Z" + "end_time": "2024-01-05T11:47:57.090325704Z", + "start_time": "2024-01-05T11:47:57.081062518Z" } }, "outputs": [ @@ -431,8 +515,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:57.445908Z", - "start_time": "2023-04-28T09:46:57.443154Z" + "end_time": "2024-01-05T11:47:57.130889801Z", + "start_time": "2024-01-05T11:47:57.129989892Z" } }, "outputs": [ @@ -467,8 +551,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:47:00.292660Z", - "start_time": "2023-04-28T09:47:00.287579Z" + "end_time": "2024-01-05T11:47:57.131271385Z", + "start_time": "2024-01-05T11:47:57.130193323Z" } }, "outputs": [ @@ -479,9 +563,9 @@ "no_operation()\n", "+-- failing_plan()\n", "Code: failing_plan() \n", - " start_time: 2023-11-08 14:02:00.257042 \n", + " start_time: 2024-01-05 12:47:57.088480 \n", " Status: TaskStatus.FAILED \n", - " end_time: 2023-11-08 14:02:00.257507 \n", + " end_time: 2024-01-05 12:47:57.089024 \n", " \n" ] } @@ -496,8 +580,8 @@ "execution_count": 13, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:47:03.581366Z", - "start_time": "2023-04-28T09:47:03.447150Z" + "end_time": "2024-01-05T11:47:57.328473210Z", + "start_time": "2024-01-05T11:47:57.130261104Z" } }, "outputs": [], diff --git a/examples/motion_designator.ipynb b/examples/motion_designator.ipynb index 9dd9b5c66..2a0b256de 100644 --- a/examples/motion_designator.ipynb +++ b/examples/motion_designator.ipynb @@ -6,32 +6,44 @@ "metadata": {}, "source": [ "# Motion Designator\n", - "Motion designator are similar to action designator, but unlike action designator, motion designator represent atomar low-level motions. Motion designator only take the parameter that they should execute and not a list of possible parameter, like the other designator. Like action designator motion designator can be performed, performing motion designator verifies the parameter and passes the designator to the respective process module. \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 designaotr perform motion on the robot, we need a robot which we can use. Therefore, we will create a BulletWorld as well as a PR2 robot." + "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": "7f7c74ae", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.051126267Z", + "start_time": "2024-01-29T16:46:56.312023994Z" + } + }, "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", + "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] [1706546816.907019]: Failed to import Giskard messages\n", + "[WARN] [1706546816.911886]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\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 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 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" ] } @@ -54,9 +66,14 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 2, "id": "cc2e1bcd", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.051774984Z", + "start_time": "2024-01-29T16:47:00.048239557Z" + } + }, "outputs": [], "source": [ "world.exit()" @@ -73,27 +90,36 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 3, "id": "ad9c75e3", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.576314628Z", + "start_time": "2024-01-29T16:47:00.051456576Z" + } + }, "outputs": [], "source": [ + "from pycram.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_desig = motion_description.resolve()\n", - " \n", - " motion_desig.perform()" + " motion_description.perform()" ] }, { "cell_type": "code", "execution_count": 4, "id": "ac9f7766", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.594527954Z", + "start_time": "2024-01-29T16:47:00.591545638Z" + } + }, "outputs": [], "source": [ "world.reset_bullet_world()" @@ -112,7 +138,12 @@ "cell_type": "code", "execution_count": 5, "id": "52aa961d", - "metadata": {}, + "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", @@ -121,7 +152,7 @@ "with simulated_robot:\n", " motion_description = MoveTCPMotion(target=Pose([0.5, 0.6, 0.6], [0, 0, 0, 1]), arm=\"left\")\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -137,7 +168,12 @@ "cell_type": "code", "execution_count": 6, "id": "84ffcf03", - "metadata": {}, + "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", @@ -146,7 +182,7 @@ "with simulated_robot:\n", " motion_description = LookingMotion(target=Pose([1, 1, 1], [0, 0, 0, 1]))\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -155,14 +191,19 @@ "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 respectivly." + "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": 7, "id": "f90adb34", - "metadata": {}, + "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", @@ -171,7 +212,7 @@ "with simulated_robot:\n", " motion_description = MoveGripperMotion(motion=\"open\", gripper=\"left\")\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -189,7 +230,12 @@ "cell_type": "code", "execution_count": 8, "id": "4c9f3e27", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:02.199992775Z", + "start_time": "2024-01-29T16:47:02.150827945Z" + } + }, "outputs": [], "source": [ "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.5, 0, 1]))" @@ -197,21 +243,26 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 9, "id": "27eae08a", - "metadata": {}, + "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, type=ObjectType.MILK, bullet_world_object=Object(world=, \n", - "local_transformer=, \n", + "ObjectDesignatorDescription.Object(name=milk, type=ObjectType.MILK, bullet_world_object=Object(world=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -219,8 +270,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -237,8 +288,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -250,48 +301,17 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\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_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \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={'milk_main': }), _pose=, \n", - "local_transformer=, \n", + "link_to_geometry: ...), _pose=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -299,8 +319,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -317,8 +337,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -330,46 +350,15 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\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_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \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={'milk_main': })>, pose=header: \n", + "link_to_geometry: ...)>, pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -389,11 +378,11 @@ "from pycram.process_module import simulated_robot\n", "\n", "with simulated_robot:\n", - " LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).resolve().perform()\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.resolve().perform()\n", + " obj = motion_description.perform()\n", " \n", " print(obj)" ] @@ -404,23 +393,28 @@ "metadata": {}, "source": [ "## Move Arm Joints\n", - "This motion designator moves one or both arms. Movement targets can either be a dictionary with joint name as key and target pose as value or a pre-defined configuration like 'park'." + "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, + "execution_count": 10, "id": "ce7655c1", - "metadata": {}, + "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(left_arm_config=\"park\", right_arm_poses={\"r_shoulder_pan_joint\": -0.7})\n", + " motion_description = MoveArmJointsMotion(right_arm_poses={\"r_shoulder_pan_joint\": -0.7})\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -429,16 +423,21 @@ "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 beliefe state (BulletWorld) a resolved object designator will be returned.\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 the the previous example you can skip this step." + "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": null, + "execution_count": 11, "id": "0f9f0ca8", - "metadata": {}, + "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]))" @@ -446,21 +445,26 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 12, "id": "4382bfa3", - "metadata": {}, + "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(world=, \n", - "local_transformer=, \n", + "Object(world=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -468,8 +472,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -486,8 +490,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -499,42 +503,11 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\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_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \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={'milk_main': })\n" + "link_to_geometry: ...)\n" ] } ], @@ -545,7 +518,7 @@ "with simulated_robot:\n", " motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK)\n", " \n", - " obj = motion_description.resolve().perform()\n", + " obj = motion_description.perform()\n", " \n", " print(obj)" ] @@ -561,9 +534,14 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 13, "id": "1e0191c8", - "metadata": {}, + "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", @@ -572,7 +550,7 @@ "with simulated_robot:\n", " motion_description = MoveJointsMotion(names=[\"torso_lift_joint\", \"r_shoulder_pan_joint\"], positions=[0.2, -1.2])\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] } ], diff --git a/examples/object_designator.ipynb b/examples/object_designator.ipynb index bd03c2d3c..b5c09ca0b 100644 --- a/examples/object_designator.ipynb +++ b/examples/object_designator.ipynb @@ -6,11 +6,11 @@ "metadata": {}, "source": [ "# Object Designator\n", - "Object designator are used to describe objects located in the BulletWorld or the real environment and then resolve them during runtime to concrete objects.\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 designator 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. Nethertheless, object designator contain a reference to the BulletWorld object.\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", - "Object designator take two parameter, of which at least one has to be provided. These parameter are:\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", @@ -61,9 +61,9 @@ "metadata": {}, "source": [ "## Believe Object\n", - "This object designator is used to describe object that are located in the BulletWorld. So objects that are in the believe state, hence the name. In the futre when there is a perception interface there will be a ```RealObject``` description which will be used to describe objects in the real world. \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``` decribes Objects in the BulletWorld we create a few." + "Since ```BelieveObject``` describes Objects in the BulletWorld we create a few." ] }, { @@ -474,7 +474,7 @@ "metadata": {}, "source": [ "## Object Part \n", - "Part of object designators can be used to describe describe something as part of another obeject. 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", + "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." ] @@ -513,7 +513,7 @@ "## 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 obejcts, so if you didn't already spawn them you can use the next cell for this." + "For this we need some objects, so if you didn't already spawn them you can use the next cell for this." ] }, { @@ -861,8 +861,7 @@ " w: 1.0}, \n", "_current_joint_states={}, \n", "base_origin_shift=[0.00124406 0.00101732 0.1038567 ], \n", - "link_to_geometry={'cereal_main': })>) \n", - "\n" + "link_to_geometry={'cereal_main': })>) \n" ] } ], diff --git a/examples/orm_example.ipynb b/examples/orm_example.ipynb index 20a3a4532..6eca63f84 100644 --- a/examples/orm_example.ipynb +++ b/examples/orm_example.ipynb @@ -15,14 +15,14 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:05.213398212Z", - "start_time": "2023-12-13T10:22:05.061219318Z" + "end_time": "2024-01-29T16:51:09.128875369Z", + "start_time": "2024-01-29T16:51:08.956260897Z" } }, "outputs": [ { "data": { - "text/plain": "" + "text/plain": "" }, "execution_count": 1, "metadata": {}, @@ -50,8 +50,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:05.758910570Z", - "start_time": "2023-12-13T10:22:05.211301707Z" + "end_time": "2024-01-29T16:51:09.568683265Z", + "start_time": "2024-01-29T16:51:09.124071834Z" } }, "outputs": [ @@ -64,8 +64,8 @@ "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] [1702462925.736253]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702462925.741150]: Failed to import Giskard messages\n" + "[WARN] [1706547069.541495]: Failed to import Giskard messages\n", + "[WARN] [1706547069.546682]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], @@ -88,8 +88,8 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.532059166Z", - "start_time": "2023-12-13T10:22:05.759939989Z" + "end_time": "2024-01-29T16:51:35.305706324Z", + "start_time": "2024-01-29T16:51:09.568597034Z" } }, "outputs": [ @@ -105,38 +105,86 @@ "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", "Scalar element defined multiple times: limit\n" ] }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "argc=2\n", + "argv[0] = --unused\n", + "argv[1] = --start_demo_name=Physics Server\n", + "ExampleBrowserThreadFunc started\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "Creating context\n", + "Created GL 3.3 context\n", + "Direct GLX rendering context obtained\n", + "Making context current\n", + "GL_VENDOR=AMD\n", + "GL_RENDERER=AMD Radeon RX 6700 XT (NAVY_FLOUNDER, DRM 3.42.0, 5.15.0-89-generic, LLVM 12.0.0)\n", + "GL_VERSION=4.6 (Core Profile) Mesa 21.2.6\n", + "GL_SHADING_LANGUAGE_VERSION=4.60\n", + "pthread_getconcurrency()=0\n", + "Version = 4.6 (Core Profile) Mesa 21.2.6\n", + "Vendor = AMD\n", + "Renderer = AMD Radeon RX 6700 XT (NAVY_FLOUNDER, DRM 3.42.0, 5.15.0-89-generic, LLVM 12.0.0)\n", + "b3Printf: Selected demo: Physics Server\n", + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "MotionThreadFunc thread started\n", + "ven = AMD\n", + "ven = AMD\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/r_gripper_r_finger_tip_link (parent map) at time 1706547087.197074 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/r_gripper_palm_link (parent map) at time 1706547087.480158 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/projector_wg6802418_child_frame (parent map) at time 1706547089.689294 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/l_shoulder_pan_link (parent map) at time 1706547092.019791 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n" + ] + }, { "name": "stdout", "output_type": "stream", "text": [ "no_operation()\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " │ └── perform(MoveMotion, )\n", - " ├── perform(PickUpAction, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ └── perform(MoveTCPMotion, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " │ └── perform(MoveMotion, )\n", - " ├── perform(PlaceAction, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ └── perform(MoveTCPMotion, )\n", - " └── perform(ParkArmsAction, )\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" ] } ], @@ -165,7 +213,7 @@ "@with_tree\n", "def plan():\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " MoveTorsoAction([0.3]).resolve().perform()\n", " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", " pickup_arm = pickup_pose.reachable_arms[0]\n", @@ -182,7 +230,7 @@ "\n", " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "plan()\n", "\n", @@ -204,8 +252,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.715646672Z", - "start_time": "2023-12-13T10:22:25.534065783Z" + "end_time": "2024-01-29T16:51:35.505074180Z", + "start_time": "2024-01-29T16:51:35.307461245Z" } }, "outputs": [ @@ -213,12 +261,12 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 114.02it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 105.04it/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(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" + "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, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" }, "execution_count": 4, "metadata": {}, @@ -241,8 +289,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.720098203Z", - "start_time": "2023-12-13T10:22:25.717086864Z" + "end_time": "2024-01-29T16:51:35.508649071Z", + "start_time": "2024-01-29T16:51:35.506023628Z" } }, "outputs": [ @@ -250,12 +298,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "ProcessMetaData(id=1, created_at=datetime.datetime(2023, 12, 13, 10, 22, 25), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='3422aa2a9097bcb087795fccf89e5bd128ab21dd')\n" + "ProcessMetaData(id=1, created_at=datetime.datetime(2024, 1, 29, 16, 51, 35), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='5e32afdb9ba57a9d6befec2c78087fee1f5e5d9b')\n" ] } ], "source": [ - "print(*session.query(pycram.orm.base.ProcessMetaData).all())" + "from sqlalchemy import select\n", + "\n", + "print(*session.scalars(select(pycram.orm.base.ProcessMetaData)).all())" ] }, { @@ -271,8 +321,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.774954933Z", - "start_time": "2023-12-13T10:22:25.719710555Z" + "end_time": "2024-01-29T16:51:35.520518056Z", + "start_time": "2024-01-29T16:51:35.508477111Z" } }, "outputs": [ @@ -286,7 +336,7 @@ } ], "source": [ - "navigations = session.query(pycram.orm.action_designator.NavigateAction).all()\n", + "navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all()\n", "print(*navigations, sep=\"\\n\")" ] }, @@ -302,8 +352,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.775554799Z", - "start_time": "2023-12-13T10:22:25.774858881Z" + "end_time": "2024-01-29T16:51:35.560192686Z", + "start_time": "2024-01-29T16:51:35.520628950Z" } }, "outputs": [ @@ -323,7 +373,7 @@ } ], "source": [ - "actions = session.query(pycram.orm.action_designator.Action).all()\n", + "actions = session.scalars(select(pycram.orm.action_designator.Action)).all()\n", "print(*actions, sep=\"\\n\")" ] }, @@ -339,8 +389,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.775915781Z", - "start_time": "2023-12-13T10:22:25.774973642Z" + "end_time": "2024-01-29T16:51:35.561234983Z", + "start_time": "2024-01-29T16:51:35.535162323Z" } }, "outputs": [ @@ -348,14 +398,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "Pose(id=7, time=datetime.datetime(2023, 12, 13, 10, 22, 12, 75413), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" + "Pose(id=7, time=datetime.datetime(2024, 1, 29, 16, 51, 15, 804838), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" ] } ], "source": [ - "object_actions = (session.query(pycram.orm.base.Pose)\n", + "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", + " .join(pycram.orm.object_designator.Object.pose))\n", " .all())\n", "print(*object_actions, sep=\"\\n\")\n" ] @@ -384,8 +434,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.776819211Z", - "start_time": "2023-12-13T10:22:25.775011202Z" + "end_time": "2024-01-29T16:51:35.600701170Z", + "start_time": "2024-01-29T16:51:35.539887927Z" } }, "outputs": [ @@ -393,30 +443,30 @@ "name": "stdout", "output_type": "stream", "text": [ - "TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144323), end_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 650626), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 650720), end_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 154581), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704577), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214635), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704616), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214628), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704577), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214635), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 385460), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 894615), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 894706), end_time=datetime.datetime(2023, 12, 13, 11, 22, 15, 402177), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 15, 552840), end_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 62542), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 62656), end_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 570383), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 703869), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 214127), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504274), end_time=datetime.datetime(2023, 12, 13, 11, 22, 18, 13609), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957228), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462603), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957259), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462584), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957228), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462603), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 463717), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 974361), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 974450), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 482580), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 482694), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992063), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992156), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498594), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=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(2023, 12, 13, 11, 22, 5, 734143), 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=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872243), end_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 378193), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 378288), end_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 881639), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459617), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970219), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459689), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970208), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459617), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970219), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 122271), end_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 631460), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 631582), end_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 141918), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 273490), end_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 783950), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 784120), end_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 298210), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 457629), end_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 974496), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274678), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 782383), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 702999), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209586), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 703060), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209577), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 702999), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209586), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 210786), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 721856), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 722026), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 239587), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 241042), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751946), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), 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, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 752094), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265524), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=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, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" ] } ], "source": [ - "successful_tasks = session.query(pycram.orm.task.TaskTreeNode).filter(pycram.orm.task.TaskTreeNode.status == \"SUCCEEDED\")\n", + "successful_tasks = session.scalars(select(pycram.orm.task.TaskTreeNode).where(pycram.orm.task.TaskTreeNode.status == \"SUCCEEDED\"))\n", "print(*successful_tasks, sep=\"\\n\")" ] }, @@ -434,14 +484,15 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.827781799Z", - "start_time": "2023-12-13T10:22:25.782671460Z" + "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\n", + "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", @@ -452,32 +503,24 @@ " text: Mapped[str] \n", "\n", "# define brand new action designator\n", - "class Saying(ActionDesignatorDescription):\n", + "\n", + "@dataclass \n", + "class SayingActionPerformable(ActionDesignatorDescription.Action):\n", " \n", - " @dataclasses.dataclass \n", - " class Action(ActionDesignatorDescription.Action):\n", + " text: str\n", " \n", - " text: str\n", - " \n", - " @with_tree\n", - " def perform(self) -> Any:\n", - " print(self.text)\n", - " \n", - " def to_sql(self) -> ORMSaying:\n", - " return ORMSaying(self.text)\n", + " @with_tree\n", + " def perform(self) -> None:\n", + " print(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", - " \n", - " def __init__(self, texts: List[str], resolver=None):\n", - " super().__init__(resolver)\n", - " self.texts = texts\n", + " def to_sql(self) -> ORMSaying:\n", + " return ORMSaying(self.text)\n", "\n", - " def ground(self) -> Action:\n", - " return self.Action(self.texts[0])" + " def insert(self, session: Session, *args, **kwargs) -> ORMSaying:\n", + " action = super().insert(session)\n", + " session.add(action)\n", + " session.commit()\n", + " return action\n" ] }, { @@ -492,8 +535,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.828275817Z", - "start_time": "2023-12-13T10:22:25.826744665Z" + "end_time": "2024-01-29T16:51:35.601417935Z", + "start_time": "2024-01-29T16:51:35.600240790Z" } }, "outputs": [], @@ -513,8 +556,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:26.207983261Z", - "start_time": "2023-12-13T10:22:25.826841295Z" + "end_time": "2024-01-29T16:51:36.020873055Z", + "start_time": "2024-01-29T16:51:35.600296911Z" } }, "outputs": [ @@ -529,13 +572,13 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 122.69it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 116.25it/s]\n" ] } ], "source": [ "# create a saying action and insert it\n", - "Saying.Action(\"Patchie, Patchie; Where is my Patchie?\").perform()\n", + "SayingActionPerformable(\"Patchie, Patchie; Where is my Patchie?\").perform()\n", "pycram.task.task_tree.root.insert(session)\n", "session.commit()\n", "\n", @@ -552,11 +595,11 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 14, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:26.214535062Z", - "start_time": "2023-12-13T10:22:26.209490452Z" + "end_time": "2024-01-29T16:52:32.021378099Z", + "start_time": "2024-01-29T16:52:31.976674160Z" } }, "outputs": [ @@ -564,13 +607,13 @@ "data": { "text/plain": "[ORMSaying(id=37, process_metadata_id=1, dtype='ORMSaying', robot_state_id=17, robot_state=RobotState(id=17, torso_height=0.3, type=, pose_id=41, process_metadata_id=1), text='Patchie, Patchie; Where is my Patchie?')]" }, - "execution_count": 13, + "execution_count": 14, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "session.query(ORMSaying).all()" + "session.scalars(select(ORMSaying)).all()" ] } ], diff --git a/examples/orm_querying_examples.ipynb b/examples/orm_querying_examples.ipynb index 28f52f63d..12adedfe1 100644 --- a/examples/orm_querying_examples.ipynb +++ b/examples/orm_querying_examples.ipynb @@ -36,12 +36,13 @@ "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] [1702467193.499071]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702467193.504366]: Failed to import Giskard messages\n" + "[WARN] [1706547279.009069]: Failed to import Giskard messages\n", + "[WARN] [1706547279.013875]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], "source": [ + "from pycram.designators.actions.actions import MoveTorsoActionPerformable, ParkArmsActionPerformable\n", "from tf import transformations\n", "import itertools\n", "import time\n", @@ -167,13 +168,13 @@ " # try to execute a grasping plan\n", " with simulated_robot:\n", "\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(pycram.enums.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", - " MoveTorsoAction.Action(height).perform()\n", + " MoveTorsoActionPerformable(height).perform()\n", "\n", " # try to pick it up\n", " try:\n", @@ -205,8 +206,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:33:13.874158209Z", - "start_time": "2023-12-13T11:33:12.871008643Z" + "end_time": "2024-01-29T16:54:39.452156158Z", + "start_time": "2024-01-29T16:54:38.500054294Z" } }, "id": "dc0c9e6f15f126a3" @@ -242,7 +243,23 @@ "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", - "100%|██████████| 960/960 [01:41<00:00, 9.87it/s, success_rate=0.0854]" + " 7%|▋ | 69/960 [00:10<01:47, 8.33it/s, success_rate=0.058] Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame floor_0 (parent map) at time 1706547289.526924 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 28%|██▊ | 269/960 [00:38<01:37, 7.08it/s, success_rate=0.0929]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_frame (parent map) at time 1706547317.808086 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 33%|███▎ | 314/960 [00:44<01:35, 6.79it/s, success_rate=0.0955]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame floor_0 (parent map) at time 1706547323.909252 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 52%|█████▏ | 502/960 [01:10<01:07, 6.76it/s, success_rate=0.0837]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_frame (parent map) at time 1706547349.562933 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 57%|█████▋ | 543/960 [01:16<01:02, 6.65it/s, success_rate=0.081] Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_optical_frame (parent map) at time 1706547355.536726 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 78%|███████▊ | 749/960 [01:44<00:28, 7.28it/s, success_rate=0.0814]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/torso_lift_motor_screw_link (parent map) at time 1706547384.042323 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 81%|████████▏ | 782/960 [01:48<00:22, 7.94it/s, success_rate=0.0831]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/l_torso_lift_side_plate_link (parent map) at time 1706547388.456061 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 86%|████████▌ | 826/960 [01:55<00:18, 7.12it/s, success_rate=0.0835]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_shoulder_lift_link (parent map) at time 1706547394.924536 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "100%|██████████| 960/960 [02:13<00:00, 7.74it/s, success_rate=0.0854]" ] } ], @@ -258,8 +275,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.371037890Z", - "start_time": "2023-12-13T11:33:13.874635425Z" + "end_time": "2024-01-29T16:56:53.360247565Z", + "start_time": "2024-01-29T16:54:39.452769369Z" } }, "id": "f2dcacd942218899" @@ -293,9 +310,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"PickUpAction\".arm AS \"PickUpAction_arm\", \"PickUpAction\".grasp AS \"PickUpAction_grasp\", \"RobotState\".torso_height AS \"RobotState_torso_height\", \"Position\".x AS \"Position_x\", \"Position\".y AS \"Position_y\" \n", + "SELECT \"PickUpAction\".arm, \"PickUpAction\".grasp, \"RobotState\".torso_height, \"Position\".x, \"Position\".y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" ON \"Pose\".id = \"RobotState\".pose_id JOIN \"Position\" ON \"Position\".id = \"Pose\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id \n", - "WHERE \"Object\".type = ? AND \"TaskTreeNode\".status = ?\n", + "WHERE \"Object\".type = :type_1 AND \"TaskTreeNode\".status = :status_1\n", " arm grasp torso_height x y\n", "0 left left 0.198541 -0.679778 0.049746\n", "1 left left 0.092868 0.473199 0.612112\n", @@ -321,30 +338,31 @@ } ], "source": [ + "from sqlalchemy import select\n", "from pycram.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 = (session.query(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y)\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).filter(Object.type == milk.type)\n", - " .filter(TaskTreeNode.status == \"SUCCEEDED\"))\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.statement, session.get_bind())\n", + "df = pd.read_sql_query(query, session.get_bind())\n", "print(df)" ], "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.371818556Z", - "start_time": "2023-12-13T11:34:55.371020667Z" + "end_time": "2024-01-29T16:56:53.362279642Z", + "start_time": "2024-01-29T16:56:53.360090447Z" } }, "id": "b17c6366f317dd7" @@ -381,7 +399,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"TaskTreeNode\".status AS \"TaskTreeNode_status\", \"Object\".type AS \"Object_type\", \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", + "SELECT \"TaskTreeNode\".status, \"Object\".type, \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" AS \"Pose_1\" ON \"Pose_1\".id = \"RobotState\".pose_id JOIN \"Position\" AS \"Position_2\" ON \"Position_2\".id = \"Pose_1\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id JOIN \"Pose\" AS \"Pose_2\" ON \"Pose_2\".id = \"Object\".pose_id JOIN \"Position\" AS \"Position_1\" ON \"Position_1\".id = \"Pose_2\".position_id\n", " status type relative torso height x \\\n", "0 FAILED ObjectType.BREAKFAST_CEREAL 0.480005 -0.737416 \n", @@ -421,7 +439,7 @@ "robot_position = sqlalchemy.orm.aliased(Position)\n", "object_position = sqlalchemy.orm.aliased(Position)\n", "\n", - "query = (session.query(TaskTreeNode.status, Object.type, \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", @@ -435,15 +453,15 @@ " .join(object_position, object_pose.position))\n", "print(query)\n", "\n", - "df = pd.read_sql(query.statement, session.get_bind())\n", + "df = pd.read_sql(query, session.get_bind())\n", "df[\"status\"] = df[\"status\"].apply(lambda x: str(x.name))\n", "print(df)" ], "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.423112328Z", - "start_time": "2023-12-13T11:34:55.371312120Z" + "end_time": "2024-01-29T16:56:53.408414208Z", + "start_time": "2024-01-29T16:56:53.408012819Z" } }, "id": "a89d1a0f2a933475" diff --git a/examples/pose.ipynb b/examples/pose.ipynb index f3f94093f..91443c79b 100644 --- a/examples/pose.ipynb +++ b/examples/pose.ipynb @@ -18,8 +18,8 @@ "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 cartisian space, so the x, y, and z coordinates. \n", - "* **Orientation:** An orientation is the rotation in all thre axes represented as a quaternion with x, y, z, w.\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." ] }, @@ -192,8 +192,7 @@ "Set the position via method:\n", "x: 3\n", "y: 2\n", - "z: 1 \n", - "\n" + "z: 1 \n" ] } ], @@ -340,7 +339,7 @@ "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 additioinal parameter called ```child_frame_id``` which is the frame of reference to which the Transform is pointing. \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", @@ -392,7 +391,7 @@ "id": "acfe3d8c", "metadata": {}, "source": [ - "Transforms have the same methods to get and set values as Poses have, theresfore only a short showcase will be given. For more details please look at the Pose example or the API documentation." + "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." ] }, { @@ -524,7 +523,7 @@ "* 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 multiplicating these two we get the Transform from ```map``` to ```milk``` frame." + "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." ] }, { @@ -624,7 +623,7 @@ "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 examle of a hand holding a milk, but this time we have the Transforms from ```map``` to ```milk``` and ```hand``` to ```milk```. " + "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```. " ] }, { diff --git a/package.xml b/package.xml index af7fee250..f3b332d63 100644 --- a/package.xml +++ b/package.xml @@ -1,33 +1,25 @@ pycram - 0.0.0 + 0.0.1 The pycram package - Andy Augsten - Dustin Augsten + Jonas Dech - TODO + GPLv3 - - - - - - - - + https://pycram.readthedocs.io/en/latest/ @@ -44,13 +36,23 @@ + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + geometry_msgs + geometry_msgs + diff --git a/src/pycram/bullet_world.py b/src/pycram/bullet_world.py index 114d21cd7..a60d72a35 100644 --- a/src/pycram/bullet_world.py +++ b/src/pycram/bullet_world.py @@ -480,7 +480,7 @@ def run(self): self.check_for_pause() # self.check_for_equal() - time.sleep(1 / 240) + time.sleep(1 / 360) self.add_obj_queue.join() self.remove_obj_queue.join() diff --git a/src/pycram/bullet_world_reasoning.py b/src/pycram/bullet_world_reasoning.py index 9a6c0ee94..3d6af78a9 100644 --- a/src/pycram/bullet_world_reasoning.py +++ b/src/pycram/bullet_world_reasoning.py @@ -320,7 +320,7 @@ def blocking(pose_or_object: Union[Object, Pose], target_map.orientation.w = grasp_orientation[3] try: - inv = request_ik(target_map, robot, joints, gripper_name) + inv = request_ik(target_map, shadow_robot, joints, gripper_name) except IKError as e: rospy.logerr(f"Pose is not reachable: {e}") return None diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 13cdeaf87..d91b18beb 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -1,15 +1,15 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import dataclasses +from dataclasses import dataclass, field, fields from abc import ABC, abstractmethod -from copy import copy from inspect import isgenerator, isgeneratorfunction from sqlalchemy.orm.session import Session import rospy from .bullet_world import (Object as BulletWorldObject, BulletWorld) +from .enums import ObjectType from .helper import GeneratorList, bcolors from threading import Lock from time import time @@ -24,7 +24,6 @@ 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 Quaternion, Position, Base, RobotState, ProcessMetaData from .task import with_tree @@ -363,124 +362,27 @@ def copy(self) -> Type[DesignatorDescription]: return self -class MotionDesignatorDescription(DesignatorDescription, Language): - """ - Parent class of motion designator descriptions. - """ - - @dataclasses.dataclass - class Motion: - """ - Resolved motion designator which can be performed - """ - cmd: str - """ - Command of this motion designator, is used to match process modules to motion designator. Cmd is inherited by - every motion designator. - """ - - @with_tree - def perform(self): - """ - Passes this designator to the process module for execution. - - :return: The return value of the process module if there is any. - """ - raise NotImplementedError() - # return ProcessModule.perform(self) - - def to_sql(self) -> ORMMotionDesignator: - """ - Create an ORM object that corresponds to this description. - - :return: The created ORM object. - """ - return ORMMotionDesignator() - - 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 - :return: The completely instanced ORM motion. - """ - metadata = ProcessMetaData().insert(session) - - motion = self.to_sql() - motion.process_metadata_id = metadata.id - - return motion - - def ground(self) -> Motion: - """Fill all missing parameters and pass the designator to the process module. """ - raise NotImplementedError(f"{type(self)}.ground() is not implemented.") - - def __init__(self, resolver=None): - """ - Creates a new motion designator description - - :param resolver: An alternative resolver which overrides self.resolve() - """ - super().__init__(resolver) - - def get_slots(self): - """ - 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 list(self.__dict__.keys()).remove('cmd') - - def _check_properties(self, desig: str, exclude: List[str] = []) -> None: - """ - Checks the properties of this description. It will be checked if any attribute is - None and if any attribute has to wrong type according to the type hints in - the description class. - It is possible to provide a list of attributes which should not be checked. - - :param desig: The current type of designator, will be used when raising an - Exception as output. - :param exclude: A list of properties which should not be checked. - """ - right_types = get_type_hints(self.Motion) - attributes = self.__dict__.copy() - del attributes["resolve"] - missing = [] - wrong_type = {} - current_type = {} - for k in attributes.keys(): - if attributes[k] == None and not attributes[k] in exclude: - missing.append(k) - elif type(attributes[k]) != right_types[k] and not attributes[k] in exclude: - wrong_type[k] = right_types[k] - current_type[k] = type(attributes[k]) - if missing != [] or wrong_type != {}: - raise ResolutionError(missing, wrong_type, current_type, desig) - - class ActionDesignatorDescription(DesignatorDescription, Language): """ Abstract class for action designator descriptions. Descriptions hold possible parameter ranges for action designators. """ - @dataclasses.dataclass + @dataclass class Action: """ The performable designator with a single element for each list of possible parameter. """ - robot_position: Pose = dataclasses.field(init=False) + robot_position: Pose = field(init=False) """ The position of the robot at the start of the action. """ - robot_torso_height: float = dataclasses.field(init=False) + robot_torso_height: float = field(init=False) """ The torso height of the robot at the start of the action. """ - robot_type: str = dataclasses.field(init=False) + robot_type: str = field(init=False) """ The type of the robot at the start of the action. """ @@ -556,7 +458,7 @@ class LocationDesignatorDescription(DesignatorDescription): Parent class of location designator descriptions. """ - @dataclasses.dataclass + @dataclass class Location: """ Resolved location that represents a specific point in the world which satisfies the constraints of the location @@ -595,7 +497,7 @@ class ObjectDesignatorDescription(DesignatorDescription): Descriptions hold possible parameter ranges for object designators. """ - @dataclasses.dataclass + @dataclass class Object: """ A single element that fits the description. @@ -616,7 +518,7 @@ class Object: Reference to the BulletWorld object """ - _pose: Optional[Callable] = dataclasses.field(init=False) + _pose: Optional[Callable] = field(init=False) """ A callable returning the pose of this object. The _pose member is used overwritten for data copies which will not update when the original bullet_world_object is moved. @@ -655,7 +557,7 @@ def insert(self, session: Session) -> ORMObjectDesignator: session.commit() return obj - def data_copy(self) -> 'ObjectDesignatorDescription.Object': + def frozen_copy(self) -> 'ObjectDesignatorDescription.Object': """ :return: A copy containing only the fields of this class. The BulletWorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when @@ -688,7 +590,7 @@ def pose(self, value: Callable): def __repr__(self): return self.__class__.__qualname__ + f"(" + ', '.join( - [f"{f.name}={self.__getattribute__(f.name)}" for f in dataclasses.fields(self)] + [ + [f"{f.name}={self.__getattribute__(f.name)}" for f in fields(self)] + [ f"pose={self.pose}"]) + ')' def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 6935b0c69..828dc2bdb 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1,31 +1,16 @@ -import dataclasses import itertools -import time -from typing import List, Optional, Any, Tuple, Union - -import sqlalchemy.orm - -from .location_designator import CostmapLocation -from .motion_designator import * -from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from ..local_transformer import LocalTransformer -from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, - PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, - MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, - Action as ORMAction, LookAtAction as ORMLookAtAction, - DetectAction as ORMDetectAction, TransportAction as ORMTransportAction, - OpenAction as ORMOpenAction, CloseAction as ORMCloseAction, - GraspingAction as ORMGraspingAction) - -from ..orm.base import Quaternion, Position, Base, RobotState, ProcessMetaData -from ..plan_failures import ObjectUnfetchable, ReachabilityFailure -from ..robot_descriptions import robot_description -from ..task import with_tree +from typing_extensions import List, Union, Callable +from .object_designator import ObjectDesignatorDescription, ObjectPart from ..enums import Arms from ..designator import ActionDesignatorDescription -from ..bullet_world import BulletWorld from ..pose import Pose -from ..helper import multiply_quaternions +from pycram.designators.actions.actions import (ParkArmsActionPerformable, MoveTorsoActionPerformable, + SetGripperActionPerformable, GripActionPerformable, + PlaceActionPerformable, PickUpActionPerformable, + NavigateActionPerformable, TransportActionPerformable, + LookAtActionPerformable, DetectActionPerformable, OpenActionPerformable, + CloseActionPerformable, GraspingActionPerformable, + ReleaseActionPerformable) class MoveTorsoAction(ActionDesignatorDescription): @@ -33,30 +18,6 @@ class MoveTorsoAction(ActionDesignatorDescription): Action Designator for Moving the torso of the robot up and down """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - """ - Performable Move Torso Action designator. - """ - - position: float - """ - Target position of the torso joint - """ - - @with_tree - def perform(self) -> None: - MoveJointsMotion([robot_description.torso_joint], [self.position]).resolve().perform() - - def to_sql(self) -> ORMMoveTorsoAction: - return ORMMoveTorsoAction(self.position) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMMoveTorsoAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, positions: List[float], resolver=None): """ Create a designator description to move the torso of the robot up and down. @@ -67,13 +28,13 @@ def __init__(self, positions: List[float], resolver=None): super().__init__(resolver) self.positions: List[float] = positions - def ground(self) -> Action: + def ground(self) -> MoveTorsoActionPerformable: """ Creates a performable action designator with the first element from the list of possible torso heights. :return: A performable action designator """ - return self.Action(self.positions[0]) + return MoveTorsoActionPerformable(self.positions[0]) def __iter__(self): """ @@ -82,7 +43,7 @@ def __iter__(self): :return: A performable action designator """ for position in self.positions: - yield self.Action(position) + yield MoveTorsoActionPerformable(position) class SetGripperAction(ActionDesignatorDescription): @@ -90,30 +51,6 @@ class SetGripperAction(ActionDesignatorDescription): Set the gripper state of the robot """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - """ - The gripper that should be set - """ - motion: str - """ - The motion that should be set on the gripper - """ - - @with_tree - def perform(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).resolve().perform() - - def to_sql(self) -> ORMSetGripperAction: - return ORMSetGripperAction(self.gripper, self.motion) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMSetGripperAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, grippers: List[str], motions: List[str], resolver=None): """ Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. @@ -126,13 +63,13 @@ def __init__(self, grippers: List[str], motions: List[str], resolver=None): self.grippers: List[str] = grippers self.motions: List[str] = motions - def ground(self) -> Action: + def ground(self) -> SetGripperActionPerformable: """ Default resolver that returns a performable designator with the first element in the grippers and motions list. :return: A performable designator """ - return self.Action(self.grippers[0], self.motions[0]) + return SetGripperActionPerformable(self.grippers[0], self.motions[0]) def __iter__(self): """ @@ -141,7 +78,7 @@ def __iter__(self): :return: A performable designator with a combination of gripper and motion """ for parameter_combination in itertools.product(self.grippers, self.motions): - yield self.Action(*parameter_combination) + yield SetGripperActionPerformable(*parameter_combination) class ReleaseAction(ActionDesignatorDescription): @@ -151,29 +88,14 @@ class ReleaseAction(ActionDesignatorDescription): Note: This action can not be used yet. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - object_designator: ObjectDesignatorDescription.Object - - @with_tree - def perform(self) -> Any: - raise NotImplementedError() - - def to_sql(self) -> Base: - raise NotImplementedError() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> Base: - raise NotImplementedError() - def __init__(self, grippers: List[str], object_designator_description: ObjectDesignatorDescription, resolver=None): super().__init__(resolver) self.grippers: List[str] = grippers self.object_designator_description = object_designator_description - def ground(self) -> Action: - return self.Action(self.grippers[0], self.object_designator_description.ground()) + def ground(self) -> ReleaseActionPerformable: + return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) class GripAction(ActionDesignatorDescription): @@ -187,22 +109,6 @@ class GripAction(ActionDesignatorDescription): Note: This action can not be used yet. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - object_designator: ObjectDesignatorDescription.Object - effort: float - - @with_tree - def perform(self) -> Any: - raise NotImplementedError() - - def to_sql(self) -> Base: - raise NotImplementedError() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> Base: - raise NotImplementedError() - def __init__(self, grippers: List[str], object_designator_description: ObjectDesignatorDescription, efforts: List[float], resolver=None): super().__init__(resolver) @@ -210,8 +116,8 @@ def __init__(self, grippers: List[str], object_designator_description: ObjectDes self.object_designator_description: ObjectDesignatorDescription = object_designator_description self.efforts: List[float] = efforts - def ground(self) -> Action: - return self.Action(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) + def ground(self) -> GripActionPerformable: + return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) class ParkArmsAction(ActionDesignatorDescription): @@ -219,37 +125,6 @@ class ParkArmsAction(ActionDesignatorDescription): Park the arms of the robot. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - arm: Arms - """ - Entry from the enum for which arm should be parked - """ - - @with_tree - def perform(self) -> None: - # create the keyword arguments - kwargs = dict() - - # add park left arm if wanted - if self.arm in [Arms.LEFT, Arms.BOTH]: - kwargs["left_arm_config"] = "park" - - # add park right arm if wanted - if self.arm in [Arms.RIGHT, Arms.BOTH]: - kwargs["right_arm_config"] = "park" - MoveArmJointsMotion(**kwargs).resolve().perform() - - def to_sql(self) -> ORMParkArmsAction: - return ORMParkArmsAction(self.arm.name) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMParkArmsAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, arms: List[Arms], resolver=None): """ Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms @@ -260,13 +135,13 @@ def __init__(self, arms: List[Arms], resolver=None): super().__init__(resolver) self.arms: List[Arms] = arms - def ground(self) -> Action: + def ground(self) -> ParkArmsActionPerformable: """ Default resolver that returns a performable designator with the first element of the list of possible arms :return: A performable designator """ - return self.Action(self.arms[0]) + return ParkArmsActionPerformable(self.arms[0]) class PickUpAction(ActionDesignatorDescription): @@ -274,110 +149,6 @@ class PickUpAction(ActionDesignatorDescription): Designator to let the robot pick up an object. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be picked up - """ - - arm: str - """ - The arm that should be used for pick up - """ - - grasp: str - """ - The grasp that should be used. For example, 'left' or 'right' - """ - - object_at_execution: Optional[ObjectDesignatorDescription.Object] = dataclasses.field(init=False) - """ - The object at the time this Action got created. It is used to be a static, information holding entity. It is - not updated when the BulletWorld object is changed. - """ - - @with_tree - def perform(self) -> None: - # Store the object's data copy at execution - self.object_at_execution = self.object_designator.data_copy() - robot = BulletWorld.robot - # Retrieve object and robot from designators - object = self.object_designator.bullet_world_object - # Get grasp orientation and target pose - grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) - # oTm = Object Pose in Frame map - oTm = object.get_pose() - # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_to_object_frame(oTm, object) - # Adjust the pose according to the special knowledge of the object designator - adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) - # Transform the adjusted pose to the map frame - adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") - # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper - ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, - adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], - grasp) - - # Set the orientation of the object pose by grasp in MAP - adjusted_oTm.orientation.x = ori[0] - adjusted_oTm.orientation.y = ori[1] - adjusted_oTm.orientation.z = ori[2] - adjusted_oTm.orientation.w = ori[3] - - # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh - # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" - gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) - # First rotate the gripper, so the further calculations makes sense - tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - tmp_for_rotate_pose.pose.position.x = 0 - tmp_for_rotate_pose.pose.position.y = 0 - tmp_for_rotate_pose.pose.position.z = -0.1 - gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") - - #Perform Gripper Rotate - # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) - # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() - - oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - oTg.pose.position.x -= 0.07 # in x since this is how the gripper is oriented - prepose = object.local_transformer.transform_pose(oTg, "map") - - # Perform the motion with the prepose and open gripper - BulletWorld.current_bullet_world.add_vis_axis(prepose) - MoveTCPMotion(prepose, self.arm).resolve().perform() - MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() - - # Perform the motion with the adjusted pose -> actual grasp and close gripper - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() - adjusted_oTm.pose.position.z += 0.03 - MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() - tool_frame = robot_description.get_tool_frame(self.arm) - robot.attach(object, tool_frame) - - # Lift object - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() - - # Remove the vis axis from the world - BulletWorld.current_bullet_world.remove_vis_axis() - - def to_sql(self) -> ORMPickUpAction: - return ORMPickUpAction(self.arm, self.grasp) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMPickUpAction: - action = super().insert(session) - - od = self.object_at_execution.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[str], grasps: List[str], resolver=None): """ @@ -395,7 +166,7 @@ def __init__(self, object_designator_description: Union[ObjectDesignatorDescrip self.arms: List[str] = arms self.grasps: List[str] = grasps - def ground(self) -> Action: + def ground(self) -> PickUpActionPerformable: """ Default resolver, returns a performable designator with the first entries from the lists of possible parameter. @@ -404,7 +175,7 @@ def ground(self) -> Action: obj_desig = self.object_designator_description if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, self.arms[0], self.grasps[0]) + return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) class PlaceAction(ActionDesignatorDescription): @@ -412,59 +183,6 @@ class PlaceAction(ActionDesignatorDescription): Places an Object at a position using an arm. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be place - """ - arm: str - """ - Arm that is currently holding the object - """ - target_location: Pose - """ - Pose in the world at which the object should be placed - """ - - @with_tree - def perform(self) -> None: - object_pose = self.object_designator.bullet_world_object.get_pose() - local_tf = LocalTransformer() - - # Transformations such that the target position is the position of the object and not the tcp - tcp_to_object = local_tf.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame( - robot_description.get_tool_frame(self.arm))) - target_diff = self.target_location.to_transform("target").inverse_times( - tcp_to_object.to_transform("object")).to_pose() - - MoveTCPMotion(target_diff, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() - BulletWorld.robot.detach(self.object_designator.bullet_world_object) - retract_pose = local_tf.transform_pose(target_diff,BulletWorld.robot.get_link_tf_frame( - robot_description.get_tool_frame(self.arm))) - retract_pose.position.x -= 0.07 - MoveTCPMotion(retract_pose, self.arm).resolve().perform() - - def to_sql(self) -> ORMPlaceAction: - return ORMPlaceAction(self.arm) - - def insert(self, session, *args, **kwargs) -> ORMPlaceAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], target_locations: List[Pose], @@ -483,7 +201,7 @@ def __init__(self, self.target_locations: List[Pose] = target_locations self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> PlaceActionPerformable: """ Default resolver that returns a performable designator with the first entries from the list of possible entries. @@ -492,7 +210,7 @@ def ground(self) -> Action: obj_desig = self.object_designator_description if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, self.arms[0], self.target_locations[0]) + return PlaceActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) class NavigateAction(ActionDesignatorDescription): @@ -500,31 +218,6 @@ class NavigateAction(ActionDesignatorDescription): Navigates the Robot to a position. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - target_location: Pose - """ - Location to which the robot should be navigated - """ - - @with_tree - def perform(self) -> None: - MoveMotion(self.target_location).resolve().perform() - - def to_sql(self) -> ORMNavigateAction: - return ORMNavigateAction() - - def insert(self, session, *args, **kwargs) -> ORMNavigateAction: - action = super().insert(session) - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, target_locations: List[Pose], resolver=None): """ Navigates the robot to a location. @@ -535,13 +228,13 @@ def __init__(self, target_locations: List[Pose], resolver=None): super().__init__(resolver) self.target_locations: List[Pose] = target_locations - def ground(self) -> Action: + def ground(self) -> NavigateActionPerformable: """ Default resolver that returns a performable designator with the first entry of possible target locations. :return: A performable designator """ - return self.Action(self.target_locations[0]) + return NavigateActionPerformable(self.target_locations[0]) class TransportAction(ActionDesignatorDescription): @@ -549,67 +242,6 @@ class TransportAction(ActionDesignatorDescription): Transports an object to a position using an arm """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be transported. - """ - arm: str - """ - Arm that should be used - """ - target_location: Pose - """ - Target Location to which the object should be transported - """ - - @with_tree - def perform(self) -> None: - robot_desig = BelieveObject(names=[robot_description.name]) - ParkArmsAction.Action(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 - pickup_pose = None - for pose in pickup_loc: - if self.arm in pose.reachable_arms: - pickup_pose = pose - break - if not pickup_pose: - raise ObjectUnfetchable( - f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") - - NavigateAction([pickup_pose.pose]).resolve().perform() - PickUpAction.Action(self.object_designator, self.arm, "front").perform() - ParkArmsAction.Action(Arms.BOTH).perform() - try: - place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), - reachable_arm=self.arm).resolve() - except StopIteration: - raise ReachabilityFailure( - f"No location found from where the robot can reach the target location: {self.target_location}") - NavigateAction([place_loc.pose]).resolve().perform() - PlaceAction.Action(self.object_designator, self.arm, self.target_location).perform() - ParkArmsAction.Action(Arms.BOTH).perform() - - def to_sql(self) -> ORMTransportAction: - return ORMTransportAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMTransportAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[str], @@ -628,17 +260,17 @@ def __init__(self, self.arms: List[str] = arms self.target_locations: List[Pose] = target_locations - def ground(self) -> Action: + def ground(self) -> TransportActionPerformable: """ Default resolver that returns a performable designator with the first entries from the lists of possible parameter. :return: A performable designator """ - obj_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, - self.arms[0], - self.target_locations[0]) + obj_desig = self.object_designator_description \ + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object)\ + else self.object_designator_description.resolve() + + return TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) class LookAtAction(ActionDesignatorDescription): @@ -646,30 +278,6 @@ class LookAtAction(ActionDesignatorDescription): Lets the robot look at a position. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - target: Pose - """ - Position at which the robot should look, given as 6D pose - """ - - @with_tree - def perform(self) -> None: - LookingMotion(target=self.target).resolve().perform() - - def to_sql(self) -> ORMLookAtAction: - return ORMLookAtAction() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMLookAtAction: - action = super().insert(session) - - pose = self.target.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - return action - def __init__(self, targets: List[Pose], resolver=None): """ Moves the head of the robot such that it points towards the given target location. @@ -680,13 +288,13 @@ def __init__(self, targets: List[Pose], resolver=None): super().__init__(resolver) self.targets: List[Pose] = targets - def ground(self) -> Action: + def ground(self) -> LookAtActionPerformable: """ Default resolver that returns a performable designator with the first entry in the list of possible targets :return: A performable designator """ - return self.Action(self.targets[0]) + return LookAtActionPerformable(self.targets[0]) class DetectAction(ActionDesignatorDescription): @@ -694,31 +302,6 @@ class DetectAction(ActionDesignatorDescription): Detects an object that fits the object description and returns an object designator describing the object. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectDesignatorDescription.Object - """ - Object designator loosely describing the object, e.g. only type. - """ - - @with_tree - def perform(self) -> Any: - return DetectingMotion(object_type=self.object_designator.type).resolve().perform() - - def to_sql(self) -> ORMDetectAction: - return ORMDetectAction() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMDetectAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None): """ Tries to detect an object in the field of view (FOV) of the robot. @@ -729,13 +312,13 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, r super().__init__(resolver) self.object_designator_description: ObjectDesignatorDescription = object_designator_description - def ground(self) -> Action: + def ground(self) -> DetectActionPerformable: """ Default resolver that returns a performable designator with the resolved object description. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve()) + return DetectActionPerformable(self.object_designator_description.resolve()) class OpenAction(ActionDesignatorDescription): @@ -745,38 +328,6 @@ class OpenAction(ActionDesignatorDescription): Can currently not be used """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be opened - """ - arm: str - """ - Arm that should be used for opening the container - """ - - @with_tree - def perform(self) -> Any: - GraspingAction.Action(self.arm, self.object_designator).perform() - OpeningMotion(self.object_designator, self.arm).resolve().perform() - - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMOpenAction: - return ORMOpenAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMOpenAction: - action = super().insert(session) - - op = self.object_designator.insert(session) - action.object_id = op.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectPart, arms: List[str], resolver=None): """ Moves the arm of the robot to open a container. @@ -789,14 +340,14 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[str], r self.object_designator_description: ObjectPart = object_designator_description self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> OpenActionPerformable: """ Default resolver that returns a performable designator with the resolved object description and the first entries from the lists of possible parameter. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve(), self.arms[0]) + return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) class CloseAction(ActionDesignatorDescription): @@ -806,38 +357,6 @@ class CloseAction(ActionDesignatorDescription): Can currently not be used """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be closed - """ - arm: str - """ - Arm that should be used for closing - """ - - @with_tree - def perform(self) -> Any: - GraspingAction.Action(self.arm, self.object_designator).perform() - ClosingMotion(self.object_designator, self.arm).resolve().perform() - - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMCloseAction: - return ORMCloseAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMCloseAction: - action = super().insert(session) - - op = self.object_designator.insert(session) - action.object_id = op.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectPart, arms: List[str], resolver=None): """ @@ -851,14 +370,14 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[str], self.object_designator_description: ObjectPart = object_designator_description self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> CloseActionPerformable: """ Default resolver that returns a performable designator with the resolved object designator and the first entry from the list of possible arms. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve(), self.arms[0]) + return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) class GraspingAction(ActionDesignatorDescription): @@ -866,51 +385,6 @@ class GraspingAction(ActionDesignatorDescription): Grasps an object described by the given Object Designator description """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - arm: str - """ - The arm that should be used to grasp - """ - object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] - """ - Object Designator for the object that should be grasped - """ - - def perform(self) -> Any: - if isinstance(self.object_desig, ObjectPart.Object): - object_pose = self.object_desig.part_pose - else: - object_pose = self.object_desig.bullet_world_object.get_pose() - lt = LocalTransformer() - gripper_name = robot_description.get_tool_frame(self.arm) - - object_pose_in_gripper = lt.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame(gripper_name)) - - pre_grasp = object_pose_in_gripper.copy() - pre_grasp.pose.position.x -= 0.1 - - MoveTCPMotion(pre_grasp, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() - - MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).resolve().perform() - MoveGripperMotion("close", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMGraspingAction: - return ORMGraspingAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMGraspingAction: - action = super().insert(session) - - od = self.object_desig.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, arms: List[str], object_description: Union[ObjectDesignatorDescription, ObjectPart], resolver: Callable = None): """ @@ -925,11 +399,11 @@ def __init__(self, arms: List[str], object_description: Union[ObjectDesignatorDe self.arms: List[str] = arms self.object_description: ObjectDesignatorDescription = object_description - def ground(self) -> Action: + def ground(self) -> GraspingActionPerformable: """ Default resolver that takes the first element from the list of arms and the first solution for the object designator description ond returns it. :return: A performable action designator that contains specific arguments """ - return self.Action(self.arms[0], self.object_description.resolve()) + return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) diff --git a/src/pycram/designators/actions/__init__.py b/src/pycram/designators/actions/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py new file mode 100644 index 000000000..83ca6c29f --- /dev/null +++ b/src/pycram/designators/actions/actions.py @@ -0,0 +1,639 @@ +import abc +from typing_extensions import Union +from pycram.designator import ActionDesignatorDescription +from pycram.designators.motion_designator import * +from pycram.enums import Arms +from pycram.task import with_tree +from dataclasses import dataclass, field +from ..location_designator import CostmapLocation +from ..object_designator import BelieveObject +from ...bullet_world import BulletWorld +from ...helper import multiply_quaternions +from ...local_transformer import LocalTransformer +from ...orm.base import Base +from ...plan_failures import ObjectUnfetchable, ReachabilityFailure +from ...robot_descriptions import robot_description +from ...orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, + PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, + MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, + LookAtAction as ORMLookAtAction, DetectAction as ORMDetectAction, + TransportAction as ORMTransportAction, OpenAction as ORMOpenAction, + CloseAction as ORMCloseAction, GraspingAction as ORMGraspingAction, Action) + + +@dataclass +class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): + """Base class for performable actions.""" + + @abc.abstractmethod + def perform(self) -> None: + """ + Perform the action. Will be overwritten by each action. + """ + pass + + @abc.abstractmethod + def to_sql(self) -> Action: + """ + Convert this action to its ORM equivalent. Will be overwritten by each action. + """ + pass + + @abc.abstractmethod + def insert(self, session: Session, **kwargs) -> Action: + """ + Insert this action into the database. + + :param session: Session with a database that is used to add and commit the objects + :param kwargs: Possible extra keyword arguments + :return: The completely instanced ORM object + """ + + action = super().insert(session) + return action + + +@dataclass +class MoveTorsoActionPerformable(ActionAbstract): + """ + Move the torso of the robot up and down. + """ + + position: float + """ + Target position of the torso joint + """ + + @with_tree + def perform(self) -> None: + MoveJointsMotion([robot_description.torso_joint], [self.position]).perform() + + def to_sql(self) -> ORMMoveTorsoAction: + return ORMMoveTorsoAction(self.position) + + def insert(self, session: Session, **kwargs) -> ORMMoveTorsoAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class SetGripperActionPerformable(ActionAbstract): + """ + Set the gripper state of the robot. + """ + + gripper: str + """ + The gripper that should be set + """ + motion: str + """ + The motion that should be set on the gripper + """ + + @with_tree + def perform(self) -> None: + MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() + + def to_sql(self) -> ORMSetGripperAction: + return ORMSetGripperAction(self.gripper, self.motion) + + def insert(self, session: Session, *args, **kwargs) -> ORMSetGripperAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class ReleaseActionPerformable(ActionAbstract): + """ + Releases an Object from the robot. + + Note: This action can not ve used yet. + """ + + gripper: str + + object_designator: ObjectDesignatorDescription.Object + + def perform(self) -> None: + raise NotImplementedError + + def to_sql(self) -> ORMParkArmsAction: + raise NotImplementedError + + def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: + raise NotImplementedError + + +@dataclass +class GripActionPerformable(ActionAbstract): + """ + Grip an object with the robot. + + Note: This action can not be used yet. + """ + + gripper: str + object_designator: ObjectDesignatorDescription.Object + effort: float + + @with_tree + def perform(self) -> None: + raise NotImplementedError() + + def to_sql(self) -> Base: + raise NotImplementedError() + + def insert(self, session: Session, *args, **kwargs) -> Base: + raise NotImplementedError() + + +@dataclass +class ParkArmsActionPerformable(ActionAbstract): + """ + Park the arms of the robot. + """ + + arm: Arms + """ + Entry from the enum for which arm should be parked + """ + + @with_tree + def perform(self) -> None: + # create the keyword arguments + kwargs = dict() + left_poses = None + right_poses = None + + # add park left arm if wanted + if self.arm in [Arms.LEFT, Arms.BOTH]: + kwargs["left_arm_config"] = "park" + left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"]) + + # add park right arm if wanted + if self.arm in [Arms.RIGHT, Arms.BOTH]: + kwargs["right_arm_config"] = "park" + right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"]) + + MoveArmJointsMotion(left_poses, right_poses).perform() + + def to_sql(self) -> ORMParkArmsAction: + return ORMParkArmsAction(self.arm.name) + + def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class PickUpActionPerformable(ActionAbstract): + """ + Let the robot pick up an object. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be picked up + """ + + arm: str + """ + The arm that should be used for pick up + """ + + grasp: str + """ + The grasp that should be used. For example, 'left' or 'right' + """ + + object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False) + """ + The object at the time this Action got created. It is used to be a static, information holding entity. It is + not updated when the BulletWorld object is changed. + """ + + @with_tree + def perform(self) -> None: + # Store the object's data copy at execution + self.object_at_execution = self.object_designator.frozen_copy() + robot = BulletWorld.robot + # Retrieve object and robot from designators + object = self.object_designator.bullet_world_object + # Get grasp orientation and target pose + grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) + # oTm = Object Pose in Frame map + oTm = object.get_pose() + # Transform the object pose to the object frame, basically the origin of the object frame + mTo = object.local_transformer.transform_to_object_frame(oTm, object) + # Adjust the pose according to the special knowledge of the object designator + adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) + # Transform the adjusted pose to the map frame + adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") + # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper + ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, + adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], + grasp) + + # Set the orientation of the object pose by grasp in MAP + adjusted_oTm.orientation.x = ori[0] + adjusted_oTm.orientation.y = ori[1] + adjusted_oTm.orientation.z = ori[2] + adjusted_oTm.orientation.w = ori[3] + + # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh + # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" + gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) + # First rotate the gripper, so the further calculations makes sense + tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + tmp_for_rotate_pose.pose.position.x = 0 + tmp_for_rotate_pose.pose.position.y = 0 + tmp_for_rotate_pose.pose.position.z = -0.1 + gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") + + #Perform Gripper Rotate + # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) + # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() + + oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented + prepose = object.local_transformer.transform_pose(oTg, "map") + + # Perform the motion with the prepose and open gripper + BulletWorld.current_bullet_world.add_vis_axis(prepose) + MoveTCPMotion(prepose, self.arm).perform() + MoveGripperMotion(motion="open", gripper=self.arm).perform() + + # Perform the motion with the adjusted pose -> actual grasp and close gripper + BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm).perform() + adjusted_oTm.pose.position.z += 0.03 + MoveGripperMotion(motion="close", gripper=self.arm).perform() + tool_frame = robot_description.get_tool_frame(self.arm) + robot.attach(object, tool_frame) + + # Lift object + BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm).perform() + + # Remove the vis axis from the world + BulletWorld.current_bullet_world.remove_vis_axis() + + def to_sql(self) -> ORMPickUpAction: + return ORMPickUpAction(self.arm, self.grasp) + + def insert(self, session: Session, **kwargs) -> ORMPickUpAction: + action = super().insert(session) + + od = self.object_at_execution.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class PlaceActionPerformable(ActionAbstract): + """ + Places an Object at a position using an arm. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be place + """ + arm: str + """ + Arm that is currently holding the object + """ + target_location: Pose + """ + Pose in the world at which the object should be placed + """ + + @with_tree + def perform(self) -> None: + object_pose = self.object_designator.bullet_world_object.get_pose() + local_tf = LocalTransformer() + + # Transformations such that the target position is the position of the object and not the tcp + tcp_to_object = local_tf.transform_pose(object_pose, + BulletWorld.robot.get_link_tf_frame( + robot_description.get_tool_frame(self.arm))) + target_diff = self.target_location.to_transform("target").inverse_times( + tcp_to_object.to_transform("object")).to_pose() + + MoveTCPMotion(target_diff, self.arm).perform() + MoveGripperMotion("open", self.arm).perform() + BulletWorld.robot.detach(self.object_designator.bullet_world_object) + retract_pose = local_tf.transform_pose(target_diff, BulletWorld.robot.get_link_tf_frame( + robot_description.get_tool_frame(self.arm))) + retract_pose.position.x -= 0.07 + MoveTCPMotion(retract_pose, self.arm).perform() + + def to_sql(self) -> ORMPlaceAction: + return ORMPlaceAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMPlaceAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class NavigateActionPerformable(ActionAbstract): + """ + Navigates the Robot to a position. + """ + + target_location: Pose + """ + Location to which the robot should be navigated + """ + + @with_tree + def perform(self) -> None: + MoveMotion(self.target_location).perform() + + def to_sql(self) -> ORMNavigateAction: + return ORMNavigateAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMNavigateAction: + action = super().insert(session) + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class TransportActionPerformable(ActionAbstract): + """ + Transports an object to a position using an arm + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be transported. + """ + arm: str + """ + Arm that should be used + """ + target_location: Pose + """ + Target Location to which the object should be transported + """ + + @with_tree + def perform(self) -> None: + robot_desig = BelieveObject(names=[robot_description.name]) + # ParkArmsAction.Action(Arms.BOTH).perform() + 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 + pickup_pose = None + for pose in pickup_loc: + if self.arm in pose.reachable_arms: + pickup_pose = pose + break + if not pickup_pose: + raise ObjectUnfetchable( + f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") + + NavigateActionPerformable(pickup_pose.pose).perform() + PickUpActionPerformable(self.object_designator, self.arm, "front").perform() + # ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + try: + place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), + reachable_arm=self.arm).resolve() + except StopIteration: + raise ReachabilityFailure( + f"No location found from where the robot can reach the target location: {self.target_location}") + NavigateActionPerformable(place_loc.pose).perform() + PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + + def to_sql(self) -> ORMTransportAction: + return ORMTransportAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMTransportAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class LookAtActionPerformable(ActionAbstract): + """ + Lets the robot look at a position. + """ + + target: Pose + """ + Position at which the robot should look, given as 6D pose + """ + + @with_tree + def perform(self) -> None: + LookingMotion(target=self.target).perform() + + def to_sql(self) -> ORMLookAtAction: + return ORMLookAtAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMLookAtAction: + action = super().insert(session) + + pose = self.target.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + return action + + +@dataclass +class DetectActionPerformable(ActionAbstract): + """ + Detects an object that fits the object description and returns an object designator describing the object. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator loosely describing the object, e.g. only type. + """ + + @with_tree + def perform(self) -> None: + return DetectingMotion(object_type=self.object_designator.type).perform() + + def to_sql(self) -> ORMDetectAction: + return ORMDetectAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMDetectAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class OpenActionPerformable(ActionAbstract): + """ + Opens a container like object + """ + + object_designator: ObjectPart.Object + """ + Object designator describing the object that should be opened + """ + arm: str + """ + Arm that should be used for opening the container + """ + + @with_tree + def perform(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + OpeningMotion(self.object_designator, self.arm).perform() + + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).perform() + + def to_sql(self) -> ORMOpenAction: + return ORMOpenAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMOpenAction: + action = super().insert(session) + + op = self.object_designator.insert(session) + action.object_id = op.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class CloseActionPerformable(ActionAbstract): + """ + Closes a container like object. + """ + + object_designator: ObjectPart.Object + """ + Object designator describing the object that should be closed + """ + arm: str + """ + Arm that should be used for closing + """ + + @with_tree + def perform(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + ClosingMotion(self.object_designator, self.arm).perform() + + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).perform() + + def to_sql(self) -> ORMCloseAction: + return ORMCloseAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMCloseAction: + action = super().insert(session) + + op = self.object_designator.insert(session) + action.object_id = op.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class GraspingActionPerformable(ActionAbstract): + """ + Grasps an object described by the given Object Designator description + """ + + arm: str + """ + The arm that should be used to grasp + """ + object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] + """ + Object Designator for the object that should be grasped + """ + + @with_tree + def perform(self) -> None: + if isinstance(self.object_desig, ObjectPart.Object): + object_pose = self.object_desig.part_pose + else: + object_pose = self.object_desig.bullet_world_object.get_pose() + lt = LocalTransformer() + gripper_name = robot_description.get_tool_frame(self.arm) + + object_pose_in_gripper = lt.transform_pose(object_pose, + BulletWorld.robot.get_link_tf_frame(gripper_name)) + + pre_grasp = object_pose_in_gripper.copy() + pre_grasp.pose.position.x -= 0.1 + + MoveTCPMotion(pre_grasp, self.arm).perform() + MoveGripperMotion("open", self.arm).perform() + + MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() + MoveGripperMotion("close", self.arm, allow_gripper_collision=True).perform() + + def to_sql(self) -> ORMGraspingAction: + return ORMGraspingAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMGraspingAction: + action = super().insert(session) + + od = self.object_desig.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index c6535a3a6..31c163e0f 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -1,691 +1,398 @@ -import dataclasses +from abc import ABC, abstractmethod +from dataclasses import dataclass from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..bullet_world import Object, BulletWorld -from ..designator import DesignatorError +from ..designator import ResolutionError +from ..enums import ObjectType +from ..orm.base import ProcessMetaData from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager -from ..robot_descriptions import robot_description -from ..designator import MotionDesignatorDescription from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, AccessingMotion as ORMAccessingMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, WorldStateDetectingMotion as ORMWorldStateDetectingMotion, - OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion) + OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion, + Motion as ORMMotionDesignator) -from typing import List, Dict, Callable, Optional +from typing_extensions import Dict, Optional, get_type_hints, get_args, get_origin from ..pose import Pose from ..task import with_tree -class MoveMotion(MotionDesignatorDescription): - """ - Moves the robot to a designated location - """ +@dataclass +class BaseMotion(ABC): - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose + @abstractmethod + def perform(self): """ - Location to which the robot should be moved + Passes this designator to the process module for execution. Will be overwritten by each motion. """ + pass + # return ProcessModule.perform(self) - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.navigate().execute(self) - # return ProcessModule.perform(self) - - def to_sql(self) -> ORMMoveMotion: - return ORMMoveMotion() - - def insert(self, session, *args, **kwargs) -> ORMMoveMotion: - motion = super().insert(session) - - pose = self.target.insert(session) - motion.pose_id = pose.id - - session.add(motion) - session.commit() - - return motion - - def __init__(self, target: Pose, resolver: Callable = None): + @abstractmethod + def to_sql(self) -> ORMMotionDesignator: """ - Navigates to robot to the given target + Create an ORM object that corresponds to this description. Will be overwritten by each motion. - :param target: Position and Orientation of the navigation goal - :param resolver: A method with which to resolve the description + :return: The created ORM object. """ - super().__init__(resolver) - self.cmd: str = "navigate" - self.target: Pose = target + return ORMMotionDesignator() - def ground(self) -> Motion: + @abstractmethod + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: """ - Default resolver for moving the robot, this resolver simply creates the motion designator from the input of the - designator description. + 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. - :return: A resolved and performable motion designator + :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. """ - return self.Motion(self.cmd, self.target) + metadata = ProcessMetaData().insert(session) + motion = self.to_sql() + motion.process_metadata_id = metadata.id -class PickUpMotion(MotionDesignatorDescription): - """ - Lets the robot pick up a specific object - """ + return motion - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_desig: ObjectDesignatorDescription.Object - """ - Object designator describing the object to be picked up - """ - arm: str - """ - Arm that should be used for picking up the object - """ - grasp: str + def __post_init__(self): """ - From which direction the object should be grasped, e.g. 'left', 'front', etc. + Checks if types are missing or wrong """ + right_types = get_type_hints(self) + attributes = self.__dict__.copy() - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.pick_up().execute(self) + missing = [] + wrong_type = {} + current_type = {} - def __init__(self, object_desig: ObjectDesignatorDescription.Object, grasp: str = None, arm: str = None, - resolver: Callable = None): - """ - Motion designator implementation of the robot picking up an object. The robot will move its arm to the position - of the object and attach the object to the gripper of the robot. - - :param object_desig: Object designator of the object to be picked up - :param grasp: From which direction the object should be picked up - :param arm: Which arm should be used for picking up - :param resolver: Alternative resolver that produces a resolved and performable motion deisgnator - """ - super().__init__(resolver) - self.cmd: str = 'pick-up' - self.object_desig: ObjectDesignatorDescription.Object = object_desig - self.arm: Optional[str] = arm - self.grasp: Optional[str] = grasp - - def ground(self): - """ - Default resolver for picking up. Checks if all parameter are present and will fill missing parameter. Optional - parameter ``arm`` and ``grasp`` will default to ``'left'``. + 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__) - :return: A resolved motion designator that can be performed - """ - arm = "left" if not self.arm else self.arm - grasp = "left" if not self.grasp else self.grasp - return self.Motion(self.cmd, self.object_desig, arm, grasp) +@dataclass +class MoveMotion(BaseMotion): + """ + Moves the robot to a designated location + """ -class PlaceMotion(MotionDesignatorDescription): + target: Pose """ - Lets the robot place an object that was picked up + Location to which the robot should be moved """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object: ObjectDesignatorDescription.Object - """ - Object designator of the object to be placed - """ - target: Pose - """ - Pose at which the object should be placed - """ - arm: str - """ - Arm that is currently holding the object - """ + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.navigate().execute(self) + # return ProcessModule.perform(self) - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.place().execute(self) + def to_sql(self) -> ORMMoveMotion: + return ORMMoveMotion() - def __init__(self, object_desig: ObjectDesignatorDescription.Object, target: Pose, - arm: Optional[str] = None, resolver: Optional[Callable] = None): - """ - Places the object in object_desig at the position in target. If an arm is given then the arm is used, otherwise - arm defaults to ``'left'`` + def insert(self, session, *args, **kwargs) -> ORMMoveMotion: + motion = super().insert(session) - :param object_desig: Object designator describing the object to be placed - :param target: The target pose on which to place the object - :param arm: An arm to use for placing - :param resolver: An alternative resolver that resolves the list of parameters to a resolved motion designator. - """ - super().__init__(resolver) - self.cmd: str = 'place' - self.object_desig: ObjectDesignatorDescription.Object = object_desig - self.target: Pose = target - self.arm: str = arm + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self) -> Motion: - """ - Default resolver for placing an object which returns a resolved motion designator for the input. If no arm is - given then the arm parameter will default to ``'left'``. + session.add(motion) + session.commit() - :return: A resolved performable motion designator - """ - arm = "left" if not self.arm else self.arm - return self.Motion(self.cmd, self.object_desig, self.target, arm) + return motion -class MoveTCPMotion(MotionDesignatorDescription): +@dataclass +class MoveTCPMotion(BaseMotion): """ Moves the Tool center point (TCP) of the robot """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose - """ - Target pose to which the TCP should be moved - """ - arm: str - """ - Arm with the TCP that should be moved to the target - """ - allow_gripper_collision: bool - """ - If the gripper can collide with something - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_tcp().execute(self) - - def to_sql(self) -> ORMMoveTCPMotion: - return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision) - - def insert(self, session: Session, *args, **kwargs) -> ORMMoveTCPMotion: - motion = super().insert(session) - - pose = self.target.insert(session) - motion.pose_id = pose.id + target: Pose + """ + Target pose to which the TCP should be moved + """ + arm: str + """ + Arm with the TCP that should be moved to the target + """ + allow_gripper_collision: Optional[bool] = None + """ + If the gripper can collide with something + """ - session.add(motion) - session.commit() + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_tcp().execute(self) - return motion + def to_sql(self) -> ORMMoveTCPMotion: + return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision) - def __init__(self, target: Pose, arm: Optional[str] = None, - resolver: Optional[Callable] = None, allow_gripper_collision: Optional[bool] = None): - """ - Moves the TCP of the given arm to the given target pose. + def insert(self, session: Session, *args, **kwargs) -> ORMMoveTCPMotion: + motion = super().insert(session) - :param target: Target pose for the TCP - :param arm: Arm that should be moved - :param resolver: Alternative resolver which returns a resolved motion designator - :param allow_gripper_collision: If the gripper should be allowed to collide with something, only used on the real robot - """ - super().__init__(resolver) - self.cmd: str = 'move-tcp' - self.target: Pose = target - self.arm: Optional[str] = arm - self.allow_gripper_collision = allow_gripper_collision + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self) -> Motion: - """ - Default resolver that returns a resolved motion designator, arm defaults to ``'left'`` if no arm is given. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - arm = "left" if not self.arm else self.arm - return self.Motion(self.cmd, self.target, arm, self.allow_gripper_collision) + return motion -class LookingMotion(MotionDesignatorDescription): +@dataclass +class LookingMotion(BaseMotion): """ Lets the robot look at a point """ + target: Pose - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.looking().execute(self) + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.looking().execute(self) - def to_sql(self) -> ORMLookingMotion: - return ORMLookingMotion() + def to_sql(self) -> ORMLookingMotion: + return ORMLookingMotion() - def insert(self, session: Session, *args, **kwargs) -> ORMLookingMotion: - motion = super().insert(session) - - pose = self.target.insert(session) - motion.pose_id = pose.id - - session.add(motion) - session.commit() - - return motion - - def __init__(self, target: Optional[Pose] = None, object: Optional[ObjectDesignatorDescription.Object] = None, - resolver: Optional[Callable] = None): - """ - Moves the head of the robot such that the camera points towards the given location. If ``target`` and ``object`` - are given ``target`` will be preferred. + def insert(self, session: Session, *args, **kwargs) -> ORMLookingMotion: + motion = super().insert(session) - :param target: Position and orientation of the target - :param object: An Object in the BulletWorld - :param resolver: Alternative resolver that returns a resolved motion designator for parameter - """ - super().__init__(resolver) - self.cmd: str = 'looking' - self.target: Optional[Pose] = target - self.object: Object = object.bullet_world_object if object else object + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self) -> Motion: - """ - Default resolver for looking, chooses which pose to take if ``target`` and ``object`` are given. If both are given - ``target`` will be preferred. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - if not self.target and self.object: - self.target = self.object.get_pose() - return self.Motion(self.cmd, self.target) + return motion -class MoveGripperMotion(MotionDesignatorDescription): +@dataclass +class MoveGripperMotion(BaseMotion): """ Opens or closes the gripper """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - motion: str - """ - Motion that should be performed, either 'open' or 'close' - """ - gripper: str - """ - Name of the gripper that should be moved - """ - allow_gripper_collision: bool - """ - If the gripper is allowed to collide with something - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_gripper().execute(self) - - def to_sql(self) -> ORMMoveGripperMotion: - return ORMMoveGripperMotion(self.motion, self.gripper, self.allow_gripper_collision) + motion: str + """ + Motion that should be performed, either 'open' or 'close' + """ + gripper: str + """ + Name of the gripper that should be moved + """ + allow_gripper_collision: Optional[bool] = None + """ + If the gripper is allowed to collide with something + """ - def insert(self, session: Session, *args, **kwargs) -> ORMMoveGripperMotion: - motion = super().insert(session) + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_gripper().execute(self) - session.add(motion) - session.commit() - return motion + def to_sql(self) -> ORMMoveGripperMotion: + return ORMMoveGripperMotion(self.motion, self.gripper, self.allow_gripper_collision) - def __init__(self, motion: str, gripper: str, resolver: Optional[Callable] = None, - allow_gripper_collision: Optional[bool] = None): - """ - Moves the gripper into a given position. + def insert(self, session: Session, *args, **kwargs) -> ORMMoveGripperMotion: + motion = super().insert(session) - :param motion: Which motion to perform - :param gripper: Name of the gripper that should be moved - :param resolver: An alternative resolver that resolves the parameter to a motion designator - """ - super().__init__(resolver) - self.cmd: str = 'move-gripper' - self.motion: str = motion - self.gripper: str = gripper - self.allow_gripper_collision = allow_gripper_collision + session.add(motion) + session.commit() + return motion - def ground(self) -> Motion: - """ - Default resolver for moving the gripper, simply returns a resolved motion designator - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.motion, self.gripper, self.allow_gripper_collision) - - -class DetectingMotion(MotionDesignatorDescription): +@dataclass +class DetectingMotion(BaseMotion): """ Tries to detect an object in the FOV of the robot """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_type: str - """ - Type of the object that should be detected - """ + object_type: ObjectType + """ + Type of the object that should be detected + """ - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - bullet_world_object = pm_manager.detecting().execute(self) - if not bullet_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": - return RealObject.Object(bullet_world_object.name, bullet_world_object.type, - bullet_world_object, bullet_world_object.get_pose()) - - return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.type, - bullet_world_object) - - def to_sql(self) -> ORMDetectingMotion: - return ORMDetectingMotion(self.object_type) - - def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: - motion = super().insert(session) - session.add(motion) - session.commit() - return motion - - def __init__(self, object_type: str, resolver: Optional[Callable] = None): - """ - Checks for every object in the FOV of the robot if it fits the given object type. If the types match an object - designator describing the object will be returned. + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + bullet_world_object = pm_manager.detecting().execute(self) + if not bullet_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": + return RealObject.Object(bullet_world_object.name, bullet_world_object.type, + bullet_world_object, bullet_world_object.get_pose()) - :param object_type: Type of the object which should be detected - :param resolver: An alternative resolver which returns a resolved motion designator - """ - super().__init__(resolver) - self.cmd: str = 'detecting' - self.object_type: str = object_type + return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.type, + bullet_world_object) - def ground(self) -> Motion: - """ - Default resolver for detecting, simply returns a resolver motion designator without checking. + def to_sql(self) -> ORMDetectingMotion: + return ORMDetectingMotion(self.object_type) - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.object_type) + def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: + motion = super().insert(session) + session.add(motion) + session.commit() + return motion -class MoveArmJointsMotion(MotionDesignatorDescription): +@dataclass +class MoveArmJointsMotion(BaseMotion): """ Moves the joints of each arm into the given position """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - left_arm_poses: Dict[str, float] - """ - Target positions for the left arm joints - """ - right_arm_poses: Dict[str, float] - """ - Target positions for the right arm joints - """ - - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_arm_joints().execute(self) - - def __init__(self, left_arm_config: Optional[str] = None, right_arm_config: Optional[str] = None, - left_arm_poses: Optional[dict] = None, right_arm_poses: Optional[dict] = None, - resolver: Optional[Callable] = None): - """ - Moves the arm joints, target positions can be either be pre-defined configurations (like 'park') or a dictionary - with joint names as keys and joint positions as values. If a configuration and a dictionary are given the - dictionary will be preferred. - - :param left_arm_config: Target configuration for the left arm - :param right_arm_config: Target configuration for the right arm - :param left_arm_poses: Target Dict for the left arm - :param right_arm_poses: Target Dict for the right arm - :param resolver: An alternative resolver that returns a resolved motion designator for the given parameters. - """ - super().__init__(resolver) - self.cmd = 'move-arm-joints' - self.left_arm_config: str = left_arm_config - self.right_arm_config: str = right_arm_config - self.left_arm_poses: Dict[str, float] = left_arm_poses - self.right_arm_poses: Dict[str, float] = right_arm_poses - - def ground(self) -> Motion: - """ - Default resolver for moving the arms, returns a resolved motion designator containing the target positions for - the left and right arm joints. + left_arm_poses: Optional[Dict[str, float]] + """ + Target positions for the left arm joints + """ + right_arm_poses: Optional[Dict[str, float]] + """ + Target positions for the right arm joints + """ - :return: A resolved and performable motion designator - """ - left_poses = None - right_poses = None + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_arm_joints().execute(self) - if self.left_arm_poses: - left_poses = self.left_arm_poses - elif self.left_arm_config: - left_poses = robot_description.get_static_joint_chain("left", self.left_arm_config) + def to_sql(self) -> ORMMotionDesignator: + pass - if self.right_arm_poses: - right_poses = self.right_arm_poses - elif self.right_arm_config: - right_poses = robot_description.get_static_joint_chain("right", self.right_arm_config) - return self.Motion(self.cmd, left_poses, right_poses) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class WorldStateDetectingMotion(MotionDesignatorDescription): +@dataclass +class WorldStateDetectingMotion(BaseMotion): """ Detects an object based on the world state. """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_type: str - """ - Object type that should be detected - """ - - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.world_state_detecting().execute(self) - def __init__(self, object_type: str, resolver: Optional[Callable] = None): - """ - Tries to find an object using the belief state (BulletWorld), if there is an object in the belief state matching - the given object type an object designator will be returned. + object_type: str + """ + Object type that should be detected + """ - :param object_type: The object type which should be detected - :param resolver: An alternative resolver that returns a resolved motion designator for the input parameter - """ - super().__init__(resolver) - self.cmd: str = 'world-state-detecting' - self.object_type: str = object_type + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.world_state_detecting().execute(self) - def ground(self) -> Motion: - """ - Default resolver for world state detecting which simply returns a resolved motion designator for the input - parameter. + def to_sql(self) -> ORMMotionDesignator: + pass - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.object_type) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class MoveJointsMotion(MotionDesignatorDescription): +@dataclass +class MoveJointsMotion(BaseMotion): """ Moves any joint on the robot """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - names: list - """ - List of joint names that should be moved - """ - positions: list - """ - Target positions of joints, should correspond to the list of names - """ - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_joints().execute(self) - - def __init__(self, names: List[str], positions: List[float], resolver: Optional[Callable] = None): - """ - Moves the joints given by the list of names to the positions given by the list of positions. The index of a - joint name should correspond to the index of the target position. + names: list + """ + List of joint names that should be moved + """ + positions: list + """ + Target positions of joints, should correspond to the list of names + """ - :param names: List of joint names that should be moved - :param positions: List of joint positions that the joints should be moved in - :param resolver: An alternative resolver that resolves the input parameters to a performable motion designator. - """ - super().__init__(resolver) - self.cmd: str = "move-joints" - self.names: List[str] = names - self.positions: List[float] = positions + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_joints().execute(self) - def ground(self) -> Motion: - """ - Default resolver for move joints, checks if the length of both list match and checks if the target positions are - within the joint limits as stated in the URDF. + def to_sql(self) -> ORMMotionDesignator: + pass - :return: A resolved motion designator - """ - if len(self.names) != len(self.positions): - raise DesignatorError("[Motion Designator][Move Joints] The length of names and positions does not match") - for i in range(len(self.names)): - lower, upper = BulletWorld.robot.get_joint_limits(self.names[i]) - if self.positions[i] < lower or self.positions[i] > upper: - raise DesignatorError( - f"[Motion Designator][Move Joints] The given configuration for the Joint {self.names[i]} violates its limits") - return self.Motion(self.cmd, self.names, self.positions) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class OpeningMotion(MotionDesignatorDescription): +@dataclass +class OpeningMotion(BaseMotion): """ Designator for opening container """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_part: ObjectPart.Object - """ - Object designator for the drawer handle - """ - arm: str - """ - Arm that should be used - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.open().execute(self) - - def to_sql(self) -> ORMOpeningMotion: - return ORMOpeningMotion(self.arm) + object_part: ObjectPart.Object + """ + Object designator for the drawer handle + """ + arm: str + """ + Arm that should be used + """ - def insert(self, session: Session, *args, **kwargs) -> ORMOpeningMotion: - motion = super().insert(session) + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.open().execute(self) - op = self.object_part.insert(session) - motion.object_id = op.id + def to_sql(self) -> ORMOpeningMotion: + return ORMOpeningMotion(self.arm) - session.add(motion) - session.commit() + def insert(self, session: Session, *args, **kwargs) -> ORMOpeningMotion: + motion = super().insert(session) - return motion + op = self.object_part.insert(session) + motion.object_id = op.id - def __init__(self, object_part: ObjectPart.Object, arm: str, resolver: Optional[Callable] = None): - """ - Lets the robot open a container specified by the given parameter. This motion designator assumes that the handle - is already grasped. + session.add(motion) + session.commit() - :param object_part: Object designator describing the handle of the drawer - :param arm: Arm that should be used - :param resolver: An alternative resolver - """ - super().__init__(resolver) - self.cmd: str = 'open' - self.objet_part = object_part - self.arm: str = arm + return motion - def ground(self) -> Motion: - """ - Default resolver for opening motion designator, returns a resolved motion designator for the input parameters. - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.objet_part, self.arm) - - -class ClosingMotion(MotionDesignatorDescription): +@dataclass +class ClosingMotion(BaseMotion): """ Designator for closing a container """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_part: ObjectPart.Object - """ - Object designator for the drawer handle - """ - arm: str - """ - Arm that should be used - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.close().execute(self) - - def to_sql(self) -> ORMClosingMotion: - return ORMClosingMotion(self.arm) - - def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: - motion = super().insert(session) + object_part: ObjectPart.Object + """ + Object designator for the drawer handle + """ + arm: str + """ + Arm that should be used + """ - op = self.object_part.insert(session) - motion.object_id = op.id + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.close().execute(self) - session.add(motion) - session.commit() + def to_sql(self) -> ORMClosingMotion: + return ORMClosingMotion(self.arm) - return motion + def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: + motion = super().insert(session) - def __init__(self, object_part: ObjectPart.Object, arm: str, resolver: Optional[Callable] = None): - """ - Lets the robot close a container specified by the given parameter. This assumes that the handle is already grasped + op = self.object_part.insert(session) + motion.object_id = op.id - :param object_part: Object designator describing the handle of the drawer - :param arm: Arm that should be used - :param resolver: An alternative resolver - """ - super().__init__(resolver) - self.cmd: str = 'close' - self.objet_part = object_part - self.arm: str = arm - - def ground(self) -> Motion: - """ - Default resolver for opening motion designator, returns a resolved motion designator for the input parameters. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.objet_part, self.arm) + return motion diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index cc6356d5e..6bf7f55a9 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -2,6 +2,7 @@ import datetime import getpass import os +from dataclasses import field from typing import Optional import git @@ -27,7 +28,7 @@ def get_pycram_version_from_git() -> Optional[str]: return repo.head.object.hexsha -class _Base(DeclarativeBase): +class _Base(DeclarativeBase, MappedAsDataclass): """Dummy class""" type_annotation_map = { str: String(255) @@ -41,7 +42,7 @@ def __tablename__(self): return self.__name__ -class Base(_Base, MappedAsDataclass): +class Base(_Base): """ Base class to add orm functionality to all pycram mappings """ @@ -59,7 +60,7 @@ def process_metadata(self): tables""" -class MapperArgsMixin: +class MapperArgsMixin(MappedAsDataclass): """ MapperArgsMixin stores __mapper_args__ information for certain subclass-tables. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html @@ -72,14 +73,14 @@ def __mapper_args__(self): return {"polymorphic_identity": self.__tablename__} -class PositionMixin: +class PositionMixin(MappedAsDataclass): """ PositionMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - position_to_init: bool = False + position_to_init: bool = field(default=False, init=False) @declared_attr def position_id(self) -> Mapped[int]: @@ -90,14 +91,14 @@ def position(self): return relationship(Position.__tablename__, init=False) -class QuaternionMixin: +class QuaternionMixin(MappedAsDataclass): """ QuaternionMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - orientation_to_init: bool = False + orientation_to_init: bool = field(default=False, init=False) @declared_attr def orientation_id(self) -> Mapped[int]: @@ -108,14 +109,14 @@ def orientation(self): return relationship(Quaternion.__tablename__, init=False) -class PoseMixin: +class PoseMixin(MappedAsDataclass): """ PoseMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - pose_to_init: bool = False + pose_to_init: bool = field(default=False, init=False) @declared_attr def pose_id(self) -> Mapped[int]: @@ -126,7 +127,7 @@ def pose(self): return relationship(Pose.__tablename__, init=False) -class ProcessMetaData(MappedAsDataclass, _Base): +class ProcessMetaData(_Base): """ ProcessMetaData stores information about the context of this experiment. diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 78a584282..4c2bb6c4d 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -1,17 +1,20 @@ +from dataclasses import field +from typing import Optional -from pycram.orm.base import Base, MapperArgsMixin, PoseMixin -from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship +from pycram.orm.base import Base, MapperArgsMixin, PoseMixin, Pose +from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship, MappedAsDataclass from sqlalchemy import ForeignKey from ..enums import ObjectType -class ObjectMixin: + +class ObjectMixin(MappedAsDataclass): """ ObjectMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/13/orm/extensions/declarative/mixins.html """ __abstract__ = True - object_to_init: bool = False + object_to_init: bool = field(default=False, init=False) @declared_attr def object_id(self) -> Mapped[int]: @@ -26,7 +29,7 @@ class Object(PoseMixin, Base): """ORM class of pycram.designators.object_designator.ObjectDesignator""" dtype: Mapped[str] = mapped_column(init=False) - type: Mapped[ObjectType] + type: Mapped[Optional[ObjectType]] name: Mapped[str] __mapper_args__ = { diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 351798350..6fa95efcf 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -5,17 +5,11 @@ """ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations - import inspect import threading import time from abc import ABC -from threading import Lock - import rospy - -from .designator import MotionDesignatorDescription -from .fluent import Fluent from typing import Callable, List, Type, Any, Union from .language import Language @@ -40,14 +34,14 @@ def __init__(self, lock): """Create a new process module.""" self._lock = lock - def _execute(self, designator: MotionDesignatorDescription.Motion) -> Any: + def _execute(self, designator) -> Any: """ Helper method for internal usage only. This method is to be overwritten instead of the execute method. """ pass - def execute(self, designator: MotionDesignatorDescription.Motion) -> Any: + def execute(self, designator) -> Any: """ Execute the given designator. If there is already another process module of the same kind the `self._lock` will lock this thread until the execution of that process module is finished. This implicitly queues the execution of diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 4ce16bf96..9ecd818f0 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -1,19 +1,14 @@ -import time from threading import Lock - import numpy as np -import pybullet as p - import pycram.bullet_world_reasoning as btr import pycram.helper as helper -from ..bullet_world import BulletWorld +from ..bullet_world import BulletWorld, Object from ..designators.motion_designator import * from ..enums import JointType from ..external_interfaces.ik import request_ik -from ..local_transformer import LocalTransformer as local_tf, LocalTransformer +from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description -from tf.transformations import euler_from_quaternion, quaternion_from_euler def _park_arms(arm): @@ -37,66 +32,17 @@ class BoxyNavigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) -class BoxyPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class BoxyPlace(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion.Motion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() - local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose(object_pose, - robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) - target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() - - _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) - - class BoxyOpen(ProcessModule): """ Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -115,7 +61,7 @@ class BoxyClose(ProcessModule): """ Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -210,7 +156,7 @@ class BoxyMoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -223,7 +169,7 @@ class BoxyMoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -237,7 +183,7 @@ class BoxyWorldStateDetecting(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.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] @@ -256,8 +202,6 @@ class BoxyManager(ProcessModuleManager): def __init__(self): super().__init__("boxy") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -272,14 +216,6 @@ def navigate(self): if ProcessModuleManager.execution_type == "simulated": return BoxyNavigation(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return BoxyPickUp(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return BoxyPlace(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return BoxyMoveHead(self._looking_lock) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 390558bd9..87bea5cb8 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -5,7 +5,7 @@ from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld +from ..bullet_world import BulletWorld, Object from ..external_interfaces.ik import request_ik, IKError from ..helper import _apply_ik from ..local_transformer import LocalTransformer @@ -18,60 +18,18 @@ class DefaultNavigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) -class DefaultPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class DefaultPlace(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion.Motion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - _move_arm_tcp(desig.target, robot, arm) - robot.detach(object) - - class DefaultMoveHead(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.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -101,7 +59,7 @@ class DefaultMoveGripper(ProcessModule): Furthermore, it can only moved one gripper at a time. """ - def _execute(self, desig: MoveGripperMotion.Motion): + def _execute(self, desig: MoveGripperMotion): robot = BulletWorld.robot gripper = desig.gripper motion = desig.motion @@ -115,7 +73,7 @@ class DefaultDetecting(ProcessModule): the field of view of the robot. """ - def _execute(self, desig: DetectingMotion.Motion): + def _execute(self, desig: DetectingMotion): robot = BulletWorld.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" @@ -134,7 +92,7 @@ class DefaultMoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -147,7 +105,7 @@ class DefaultMoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -159,7 +117,7 @@ def _execute(self, desig: MoveArmJointsMotion.Motion): class DefaultMoveJoints(ProcessModule): - def _execute(self, desig: MoveJointsMotion.Motion): + def _execute(self, desig: MoveJointsMotion): robot = BulletWorld.robot for joint, pose in zip(desig.names, desig.positions): robot.set_joint_state(joint, pose) @@ -170,7 +128,7 @@ class DefaultWorldStateDetecting(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.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] @@ -180,7 +138,7 @@ class DefaultOpen(ProcessModule): Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -199,7 +157,7 @@ class DefaultClose(ProcessModule): """ Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -228,8 +186,6 @@ class DefaultManager(ProcessModuleManager): def __init__(self): super().__init__("default") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -244,14 +200,6 @@ def navigate(self): if ProcessModuleManager.execution_type == "simulated": return DefaultNavigation(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return DefaultPickUp(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return DefaultPlace(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return DefaultMoveHead(self._looking_lock) diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index ed5c8ae80..cd73d0895 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -161,7 +161,7 @@ class DonbotMoveJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.left_arm_poses: robot.set_joint_states(desig.left_arm_poses) @@ -172,7 +172,7 @@ class DonbotWorldStateDetecting(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.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 3930a8a9b..77cc067eb 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -50,66 +50,18 @@ class Pr2Navigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) -class Pr2PickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion.Motion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class Pr2Place(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion.Motion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() - local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose(object_pose, robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) - target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() - - _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) - - class Pr2MoveHead(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.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -133,7 +85,7 @@ class Pr2MoveGripper(ProcessModule): Furthermore, it can only moved one gripper at a time. """ - def _execute(self, desig: MoveGripperMotion.Motion): + def _execute(self, desig: MoveGripperMotion): robot = BulletWorld.robot gripper = desig.gripper motion = desig.motion @@ -147,7 +99,7 @@ class Pr2Detecting(ProcessModule): the field of view of the robot. """ - def _execute(self, desig: DetectingMotion.Motion): + def _execute(self, desig: DetectingMotion): robot = BulletWorld.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" @@ -166,7 +118,7 @@ class Pr2MoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -179,7 +131,7 @@ class Pr2MoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -192,7 +144,7 @@ class PR2MoveJoints(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.Motion): + def _execute(self, desig: MoveJointsMotion): robot = BulletWorld.robot robot.set_joint_states(dict(zip(desig.names, desig.positions))) @@ -202,7 +154,7 @@ class Pr2WorldStateDetecting(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.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] @@ -212,7 +164,7 @@ class Pr2Open(ProcessModule): Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -232,7 +184,7 @@ class Pr2Close(ProcessModule): Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -266,30 +218,18 @@ class Pr2NavigationReal(ProcessModule): Process module for the real PR2 that sends a cartesian goal to giskard to move the robot base """ - def _execute(self, designator: MoveMotion.Motion) -> Any: + def _execute(self, designator: MoveMotion) -> Any: rospy.logdebug(f"Sending goal to giskard to Move the robot") giskard.achieve_cartesian_goal(designator.target, robot_description.base_link, "map") -class Pr2PickUpReal(ProcessModule): - - def _execute(self, designator: PickUpMotion.Motion) -> Any: - pass - - -class Pr2PlaceReal(ProcessModule): - - def _execute(self, designator: MotionDesignatorDescription.Motion) -> Any: - pass - - class Pr2MoveHeadReal(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 """ - def _execute(self, desig: LookingMotion.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -314,7 +254,7 @@ class Pr2DetectingReal(ProcessModule): for perception of the environment. """ - def _execute(self, designator: DetectingMotion.Motion) -> Any: + def _execute(self, designator: DetectingMotion) -> Any: query_result = query(ObjectDesignatorDescription(types=[designator.object_type])) # print(query_result) obj_pose = query_result["ClusterPoseBBAnnotator"] @@ -339,15 +279,12 @@ def _execute(self, designator: DetectingMotion.Motion) -> Any: return bullet_obj[0] - - - class Pr2MoveTCPReal(ProcessModule): """ Moves the tool center point of the real PR2 while avoiding all collisions """ - def _execute(self, designator: MoveTCPMotion.Motion) -> Any: + def _execute(self, designator: MoveTCPMotion) -> Any: lt = LocalTransformer() pose_in_map = lt.transform_pose(designator.target, "map") @@ -362,7 +299,7 @@ class Pr2MoveArmJointsReal(ProcessModule): Moves the arm joints of the real PR2 to the given configuration while avoiding all collisions """ - def _execute(self, designator: MoveArmJointsMotion.Motion) -> Any: + def _execute(self, designator: MoveArmJointsMotion) -> Any: joint_goals = {} if designator.left_arm_poses: joint_goals.update(designator.left_arm_poses) @@ -377,7 +314,7 @@ class Pr2MoveJointsReal(ProcessModule): Moves any joint using giskard, avoids all collisions while doint this. """ - def _execute(self, designator: MoveJointsMotion.Motion) -> Any: + def _execute(self, designator: MoveJointsMotion) -> Any: name_to_position = dict(zip(designator.names, designator.positions)) giskard.avoid_all_collisions() giskard.achieve_joint_goal(name_to_position) @@ -388,7 +325,7 @@ class Pr2MoveGripperReal(ProcessModule): Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard """ - def _execute(self, designator: MoveGripperMotion.Motion) -> Any: + def _execute(self, designator: MoveGripperMotion) -> Any: def activate_callback(): rospy.loginfo("Started gripper Movement") @@ -414,7 +351,7 @@ class Pr2OpenReal(ProcessModule): Tries to open an already grasped container """ - def _execute(self, designator: OpeningMotion.Motion) -> Any: + def _execute(self, designator: OpeningMotion) -> Any: giskard.achieve_open_container_goal(robot_description.get_tool_frame(designator.arm), designator.object_part.name) @@ -424,7 +361,7 @@ class Pr2CloseReal(ProcessModule): Tries to close an already grasped container """ - def _execute(self, designator: ClosingMotion.Motion) -> Any: + def _execute(self, designator: ClosingMotion) -> Any: giskard.achieve_close_container_goal(robot_description.get_tool_frame(designator.arm), designator.object_part.name) @@ -434,8 +371,6 @@ class Pr2Manager(ProcessModuleManager): def __init__(self): super().__init__("pr2") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -452,18 +387,6 @@ def navigate(self): elif ProcessModuleManager.execution_type == "real": return Pr2NavigationReal(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return Pr2PickUp(self._pick_up_lock) - elif ProcessModuleManager.execution_type == "real": - return Pr2PickUpReal(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return Pr2Place(self._place_lock) - elif ProcessModuleManager.execution_type == "real": - return Pr2PlaceReal(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return Pr2MoveHead(self._looking_lock) diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index 4febdf360..d613b99a3 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -1,6 +1,7 @@ import numpy as np import sqlalchemy.orm import sqlalchemy.sql +from sqlalchemy import select, Select from tf import transformations import pycram.designators.location_designator import pycram.task @@ -34,25 +35,24 @@ def __init__(self, target, session: sqlalchemy.orm.Session = None, super().__init__(target, reachable_for, None, reachable_arm, resolver) self.session = session - def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query: + def create_query_from_occupancy_costmap(self) -> Select: """ Create a query that queries all relative robot positions from an object that are not occluded using an OccupancyCostmap. """ robot_pos = sqlalchemy.orm.aliased(Position) - object_pos = sqlalchemy.orm.aliased(Position) # query all relative robot positions in regard to an objects position # make sure to order the joins() correctly - query = (self.session.query(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x, - Position.y).join(TaskTreeNode.code) - .join(Code.designator.of_type(PickUpAction)) - .join(PickUpAction.robot_state) - .join(RobotState.pose) - .join(orm.base.Pose.position) - .join(PickUpAction.object).filter(Object.type == self.target.type) - .filter(TaskTreeNode.status == "SUCCEEDED")) + query = (select(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x, Position.y) + .join(TaskTreeNode.code) + .join(Code.designator.of_type(PickUpAction)) + .join(PickUpAction.robot_state) + .join(RobotState.pose) + .join(orm.base.Pose.position) + .join(PickUpAction.object).where(Object.type == self.target.type) + .where(TaskTreeNode.status == "SUCCEEDED")) # create Occupancy costmap for the target object position, orientation = self.target.pose.to_list() @@ -94,7 +94,7 @@ def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query: robot_pos.y >= rectangle[1][0], robot_pos.y < rectangle[1][1])) # query = self.model.bind({"x": list(rectangle[0]), "y": list(rectangle[1])}) - return query.filter(sqlalchemy.or_(*filters)) + return query.where(sqlalchemy.or_(*filters)) def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLocation.Location: """ @@ -112,7 +112,7 @@ def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLoc return result def __iter__(self) -> JPTCostmapLocation.Location: - query = self.create_query_from_occupancy_costmap().limit(200) - samples = query.all() + statement = self.create_query_from_occupancy_costmap().limit(200) + samples = self.session.execute(statement).all() for sample in samples: yield self.sample_to_location(sample) diff --git a/src/pycram/task.py b/src/pycram/task.py index 9d7113196..e6323fd0c 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -24,9 +24,13 @@ class TaskCode(Code): def __str__(self) -> str: - return "%s(%s)" % ( - self.function.__name__, ", ".join(["%s, " % (str(value.__class__).split(".")[-2]) for value in - self.kwargs.values()])) + if "self" in self.kwargs: + class_name = self.kwargs["self"].__class__.__name__ + else: + class_name = "" + function_name = self.function.__name__ + + return f"{function_name}({class_name})" def __eq__(self, other): return isinstance(other, Code) and other.function.__name__ == self.function.__name__ \ diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index b34b434c4..8687a294a 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -3,6 +3,7 @@ import numpy as np import rospkg +import pycram.task from pycram.bullet_world import BulletWorld, Object, fix_missing_inertial from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -34,6 +35,7 @@ def setUp(self): def tearDown(self): self.world.reset_bullet_world() + pycram.task.reset_tree() @classmethod def tearDownClass(cls): diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 0a452e9bf..0e1f995dc 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -1,6 +1,8 @@ import time import unittest from pycram.designators import action_designator, object_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.robot_descriptions import robot_description from pycram.process_module import simulated_robot from pycram.pose import Pose @@ -61,8 +63,8 @@ def test_pick_up(self): description = action_designator.PickUpAction(object_description, ["left"], ["front"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() description.resolve().perform() self.assertTrue(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) @@ -71,9 +73,9 @@ def test_place(self): description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() self.assertFalse(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 457bbd577..41f9567f0 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -17,7 +17,7 @@ def test_attachment_exclusion(self): self.milk.set_pose(Pose([0.5, 0, 1])) self.cereal.set_pose(Pose([50, 50, 0])) o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, origin=Pose([0, 0, 0], [0, 0, 0, 1])) - self.assertEquals(np.sum(o.map[115:135, 90:110]), 0) + self.assertEqual(np.sum(o.map[115:135, 90:110]), 0) self.robot.attach(self.milk) self.assertTrue(np.sum(o.map[80:90, 90:110]) != 0) diff --git a/test/test_database_merger.py b/test/test_database_merger.py index f3e8b0472..507331af4 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -40,7 +40,7 @@ def __init__(self): @with_tree def pick_and_place_plan(self): with simulated_robot: - ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).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] @@ -59,7 +59,7 @@ def pick_and_place_plan(self): PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() - ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() class MergerTestCaseBase(unittest.TestCase): @@ -92,19 +92,17 @@ def setUpClass(cls) -> None: cls.source_engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) cls.source_session_maker = sqlalchemy.orm.sessionmaker(bind=cls.source_engine) cls.destination_session_maker = sqlalchemy.orm.sessionmaker(bind=cls.destination_engine) - source_session = cls.source_session_maker() destination_session = cls.destination_session_maker() - pycram.orm.base.Base.metadata.create_all(cls.source_engine) pycram.orm.base.Base.metadata.create_all(cls.destination_engine) - source_session.commit() destination_session.commit() - source_session.close() destination_session.close() cls.numbers_of_example_runs = 3 def setUp(self) -> None: super().setUp() source_session = self.source_session_maker() + # If there is no session (connection to the memory database) it resets thus we need to define this here + pycram.orm.base.Base.metadata.create_all(self.source_engine) example_plans = ExamplePlans() for i in range(self.numbers_of_example_runs): try: @@ -151,7 +149,7 @@ def test_merge_databases(self): self.assertEqual(destination_content[key], destination_content[key].union(source_content[key])) def test_migrate_neems(self): - pycram.orm.utils.migrate_neems(self.source_session_maker,self.destination_session_maker) + pycram.orm.utils.migrate_neems(self.source_session_maker, self.destination_session_maker) destination_content = dict() source_content = dict() with self.destination_session_maker() as session: diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index a9103bd0a..e03f6d5db 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -5,8 +5,11 @@ import pycram.plan_failures from pycram import task from pycram.bullet_world import BulletWorld, Object -from pycram.designators import action_designator, object_designator +from pycram.designators import action_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.orm.base import Base +from pycram.designators.object_designator import ObjectDesignatorDescription from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.pose import Pose @@ -46,31 +49,33 @@ def setUpClass(cls) -> None: cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") ProcessModule.execution_delay = False cls.engine = sqlalchemy.create_engine(pycrorm_uri) - cls.session = sqlalchemy.orm.Session(bind=cls.engine) def setUp(self) -> None: self.world.reset_bullet_world() pycram.orm.base.Base.metadata.create_all(self.engine) + self.session = sqlalchemy.orm.Session(bind=self.engine) self.session.commit() def tearDown(self) -> None: self.world.reset_bullet_world() pycram.task.reset_tree() + pycram.orm.base.ProcessMetaData.reset() + self.session.rollback() + pycram.orm.base.Base.metadata.drop_all(self.engine) + self.session.close() @classmethod def tearDownClass(cls) -> None: cls.world.exit() - cls.session.commit() - cls.session.close() @with_tree def plan(self): - object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + object_description = ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() def test_costmap_no_obstacles(self): @@ -84,10 +89,9 @@ def test_costmap_no_obstacles(self): with simulated_robot: # action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() - action_designator.PickUpAction.Action( - object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() + PickUpActionPerformable(ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, + grasp=sample.grasp).perform() @unittest.skip def test_costmap_with_obstacles(self): @@ -98,15 +102,16 @@ def test_costmap_with_obstacles(self): for i in range(20): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() try: - action_designator.PickUpAction.Action( - object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), + PickUpActionPerformable( + ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() except pycram.plan_failures.PlanFailure: continue return + kitchen.remove() raise pycram.plan_failures.PlanFailure() diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 98884a0d7..9979d4a62 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -7,6 +7,8 @@ import pycram.plan_failures from pycram.bullet_world import BulletWorld, Object from pycram.designators import action_designator, object_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.robot_descriptions import robot_description @@ -63,9 +65,9 @@ def test_costmap_no_obstacles(self): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() - action_designator.PickUpAction.Action( + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() @@ -77,10 +79,10 @@ def test_costmap_with_obstacles(self): for i in range(20): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() try: - action_designator.PickUpAction.Action( + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() except pycram.plan_failures.PlanFailure: diff --git a/test/test_language.py b/test/test_language.py index 951db67c4..cb8746c4d 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -1,6 +1,8 @@ import threading +import time import unittest from pycram.designators.action_designator import * +from pycram.designators.object_designator import BelieveObject from pycram.enums import ObjectType, State from pycram.fluent import Fluent from pycram.plan_failures import PlanFailure @@ -8,6 +10,7 @@ from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Repeat, Code, RenderTree from pycram.process_module import simulated_robot import test_bullet_world +from pycram.robot_descriptions import robot_description class LanguageTestCase(test_bullet_world.BulletWorldTest): @@ -166,7 +169,7 @@ def test_set(param): def test_perform_parallel(self): def check_thread_id(main_id): - self.assertNotEquals(main_id, threading.get_ident()) + self.assertNotEqual(main_id, threading.get_ident()) act = Code(check_thread_id, {"main_id": threading.get_ident()}) act2 = Code(check_thread_id, {"main_id": threading.get_ident()}) act3 = Code(check_thread_id, {"main_id": threading.get_ident()}) diff --git a/test/test_object_designator.py b/test/test_object_designator.py index 625e107a2..3cdcdce0c 100644 --- a/test/test_object_designator.py +++ b/test/test_object_designator.py @@ -13,12 +13,12 @@ def test_object_grounding(self): self.assertEqual(obj.name, "milk") self.assertEqual(obj.type, ObjectType.MILK) - def test_data_copy(self): + def test_frozen_copy(self): description = ObjectDesignatorDescription(["milk"], [ObjectType.MILK]) obj = description.ground() - data_copy = obj.data_copy() - self.assertEqual(obj.pose, data_copy.pose) + frozen_copy = obj.frozen_copy() + self.assertEqual(obj.pose, frozen_copy.pose) if __name__ == '__main__': diff --git a/test/test_orm.py b/test/test_orm.py index ceec18c5d..4c5063fc7 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,9 +1,7 @@ import os import unittest - -import sqlalchemy +from sqlalchemy import select import sqlalchemy.orm - import pycram.orm.action_designator import pycram.orm.base import pycram.orm.motion_designator @@ -13,14 +11,20 @@ import pycram.task from bullet_world_testcase import BulletWorldTestCase import test_task_tree -from pycram.designators import action_designator, object_designator +from pycram.bullet_world import Object +from pycram.designators import action_designator, object_designator, motion_designator +from pycram.designators.actions.actions import ParkArmsActionPerformable, MoveTorsoActionPerformable, \ + SetGripperActionPerformable, PickUpActionPerformable, NavigateActionPerformable, TransportActionPerformable, \ + OpenActionPerformable, CloseActionPerformable, DetectActionPerformable, LookAtActionPerformable +from pycram.designators.object_designator import BelieveObject +from pycram.enums import ObjectType from pycram.pose import Pose from pycram.process_module import simulated_robot from pycram.task import with_tree -class ORMTestSchema(unittest.TestCase): - engine: sqlalchemy.engine.Engine +class DatabaseTestCaseMixin(BulletWorldTestCase): + engine: sqlalchemy.engine session: sqlalchemy.orm.Session @classmethod @@ -30,20 +34,19 @@ def setUpClass(cls): def setUp(self): super().setUp() + pycram.orm.base.Base.metadata.create_all(self.engine) self.session = sqlalchemy.orm.Session(bind=self.engine) self.session.commit() def tearDown(self): super().tearDown() + pycram.task.reset_tree() + pycram.orm.base.ProcessMetaData.reset() pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() +class ORMTestSchemaTestCase(DatabaseTestCaseMixin, unittest.TestCase): def test_schema_creation(self): pycram.orm.base.Base.metadata.create_all(self.engine) self.session.commit() @@ -68,33 +71,18 @@ def test_schema_creation(self): self.assertTrue("CloseAction" in tables) -class ORMTaskTreeTestCase(test_task_tree.TaskTreeTestCase): - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() - - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() +class ORMTaskTreeTestCase(DatabaseTestCaseMixin): - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() + @with_tree + def plan(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) + self.assertEqual(description.ground().object_designator.name, "milk") + with simulated_robot: + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() + description.resolve().perform() def test_node(self): """Test if the objects in the database is equal with the objects that got serialized.""" @@ -102,208 +90,194 @@ def test_node(self): pycram.orm.base.ProcessMetaData().description = "Unittest" pycram.task.task_tree.root.insert(self.session, ) - node_results = self.session.query(pycram.orm.task.TaskTreeNode).all() + node_results = self.session.scalars(select(pycram.orm.task.TaskTreeNode)).all() self.assertEqual(len(node_results), len(pycram.task.task_tree.root)) - code_results = self.session.query(pycram.orm.task.Code).all() + code_results = self.session.scalars(select(pycram.orm.task.Code)).all() self.assertEqual(len(code_results), len(pycram.task.task_tree.root)) - position_results = self.session.query(pycram.orm.base.Position).all() + position_results = self.session.scalars(select(pycram.orm.base.Position)).all() self.assertEqual(14, len(position_results)) - quaternion_results = self.session.query(pycram.orm.base.Quaternion).all() + quaternion_results = self.session.scalars(select(pycram.orm.base.Quaternion)).all() self.assertEqual(14, len(quaternion_results)) - park_arms_results = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + park_arms_results = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() self.assertEqual(0, len(park_arms_results)) - navigate_results = self.session.query(pycram.orm.action_designator.NavigateAction).all() + navigate_results = self.session.scalars(select(pycram.orm.action_designator.NavigateAction)).all() self.assertEqual(1, len(navigate_results)) - action_results = self.session.query(pycram.orm.action_designator.Action).all() + action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() self.assertEqual(4, len(action_results)) + def test_metadata_existence(self): + pycram.orm.base.ProcessMetaData().description = "metadata_existence_test" + self.plan() + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.base.Pose)).all() + self.assertTrue(all([r.process_metadata is not None for r in result])) + + def test_task_tree_node_parents(self): + self.plan() + pycram.orm.base.ProcessMetaData().description = "task_tree_node_parents_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.task.TaskTreeNode)).all() + self.assertTrue([result[i].parent == result[result[i].parent_id - 1] for i in range(len(result)) + if result[i].parent_id is not None]) + def test_meta_data(self): self.plan() pycram.orm.base.ProcessMetaData().description = "Unittest" pycram.task.task_tree.root.insert(self.session, ) - metadata_results = self.session.query(pycram.orm.base.ProcessMetaData).all() + metadata_results = self.session.scalars(select(pycram.orm.base.ProcessMetaData)).all() self.assertEqual(1, len(metadata_results)) - action_results = self.session.query(pycram.orm.action_designator.Action).all() + action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() self.assertTrue(all([a.process_metadata_id for a in action_results])) - park_arms_results = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + park_arms_results = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() self.assertTrue(all([a.process_metadata_id for a in park_arms_results])) - object_results = self.session.query(pycram.orm.object_designator.Object).all() + object_results = self.session.scalars(select(pycram.orm.object_designator.Object)).all() self.assertTrue(all([o.process_metadata_id for o in object_results])) def test_meta_data_alternation(self): - meta_data = pycram.orm.base.ProcessMetaData() - meta_data.description = "Test" self.plan() + pycram.orm.base.ProcessMetaData().description = "meta_data_alternation_test" pycram.task.task_tree.root.insert(self.session, ) - metadata_result = self.session.query(pycram.orm.base.ProcessMetaData).first() - self.assertEqual(metadata_result.description, "Test") + metadata_result = self.session.scalars(select(pycram.orm.base.ProcessMetaData)).first() + self.assertEqual(metadata_result.description, "meta_data_alternation_test") -class ORMObjectDesignatorTestCase(BulletWorldTestCase): - """Test ORM functionality with a plan including object designators. """ - - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - +class MixinTestCase(DatabaseTestCaseMixin): @with_tree def plan(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - cls.session = sqlalchemy.orm.Session(bind=cls.engine) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session.commit() - - def tearDown(self): - super().tearDown() - pycram.orm.base.Base.metadata.drop_all(self.engine) - pycram.task.reset_tree() - - @classmethod - def tearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() + def test_pose(self): + self.plan() + pycram.orm.base.ProcessMetaData().description = "pose_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.base.Pose)).all() + self.assertTrue(all([r.position is not None and r.orientation is not None for r in result])) - def test_plan_serialization(self): + def test_pose_mixin(self): self.plan() - pycram.orm.base.ProcessMetaData().description = "Unittest" - tt = pycram.task.task_tree - tt.insert(self.session) - action_results = self.session.query(pycram.orm.action_designator.Action).all() - motion_results = self.session.query(pycram.orm.motion_designator.Motion).all() - self.assertEqual(len(tt) - 2, len(action_results) + len(motion_results)) + pycram.orm.base.ProcessMetaData().description = "pose_mixin_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.base.RobotState)).all() + self.assertTrue(all([r.pose is not None and r.pose_id == r.pose.id for r in result])) -class RelationshipTestCase(test_task_tree.TaskTreeTestCase): - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session +class ORMObjectDesignatorTestCase(DatabaseTestCaseMixin): + """Test ORM functionality with a plan including object designators. """ - @with_tree - def plan(self): + def test_plan_serialization(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() + pycram.orm.base.ProcessMetaData().description = "Unittest" + tt = pycram.task.task_tree + 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() + self.assertEqual(len(tt) - 1, len(action_results) + len(motion_results)) - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() - - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() +class ORMActionDesignatorTestCase(DatabaseTestCaseMixin): - def test_metadata(self): - pycram.orm.base.ProcessMetaData().description = "MetaDataRelationshipTest" - self.plan() + def test_code_designator_type(self): + action = NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])) + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "code_designator_type_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.base.Position).all() - self.assertTrue(all([r.process_metadata is not None for r in result])) + result = self.session.scalars(select(pycram.orm.task.Code).where(pycram.orm.task.Code.function == "perform")).all() + self.assertEqual(result[0].designator.dtype, action_designator.NavigateAction.__name__) + self.assertEqual(result[1].designator.dtype, motion_designator.MoveMotion.__name__) - def test_task_tree_node_parents(self): - self.plan() - pycram.orm.base.ProcessMetaData().description = "taskTest" + def test_parkArmsAction(self): + action = ParkArmsActionPerformable(pycram.enums.Arms.BOTH) + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "parkArmsAction_test" pycram.task.task_tree.root.insert(self.session) - r = self.session.query(pycram.orm.task.TaskTreeNode).all() - self.assertTrue([r[i].parent == r[r[i].parent_id - 1] for i in range(len(r)) if r[i].parent_id is not None]) - - -class MotionDesigTest(BulletWorldTestCase): - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session + result = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() + self.assertTrue(all([result[i+1].dtype is not pycram.orm.action_designator.Action.dtype + if result[i].dtype is pycram.orm.action_designator.ParkArmsAction.dtype else None + for i in range(len(result)-1)])) - @with_tree - def plan(self): + def test_transportAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - description = action_designator.PlaceAction(object_description, [Pose([1.3, 0.9, 0.9], [0, 0, 0, 1])], ["left"]) - self.assertEqual(description.ground().object_designator.name, "milk") + action = TransportActionPerformable(object_description.resolve(), "left", + Pose([1.3, 0.9, 0.9], [0, 0, 0, 1])) with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.LookAtAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() - action_designator.SetGripperAction.Action("right", "open").perform() - description.resolve().perform() + action.perform() + pycram.orm.base.ProcessMetaData().description = "transportAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.action_designator.TransportAction)).all() + milk_object = self.session.scalars(select(pycram.orm.object_designator.Object)).first() + self.assertEqual(milk_object.pose, result[0].object.pose) - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) + def test_lookAt_and_detectAction(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + action = DetectActionPerformable(object_description.resolve()) + with simulated_robot: + ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + NavigateActionPerformable(Pose([0, 1, 0], [0, 0, 0, 1])).perform() + LookAtActionPerformable(object_description.resolve().pose).perform() + action.perform() + pycram.orm.base.ProcessMetaData().description = "detectAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.action_designator.DetectAction)).all() + self.assertEqual(result[0].object.name, "milk") - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() + def test_setGripperAction(self): + action = SetGripperActionPerformable("left", "open") + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "setGripperAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(pycram.orm.action_designator.SetGripperAction)).all() + self.assertEqual(result[0].gripper, "left") + self.assertEqual(result[0].motion, "open") - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() + 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() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() + self.kitchen.set_pose(Pose([20, 20, 0], [0, 0, 0, 1])) - def testTest(self): - self.plan() - pycram.orm.base.ProcessMetaData().description = "Unittest" - tt = pycram.task.task_tree - tt.insert(self.session) + with simulated_robot: + ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + NavigateActionPerformable(Pose([1.81, 1.73, 0.0], + [0.0, 0.0, 0.594, 0.804])).perform() + OpenActionPerformable(handle_desig, arm="left").perform() + CloseActionPerformable(handle_desig, arm="left").perform() - def test_insert_base_motion(self): - motion = pycram.orm.motion_designator.Motion() - self.assertIsNone(motion.id) - self.session.add(motion) - self.session.commit() - self.assertIsNotNone(motion.id) + pycram.orm.base.ProcessMetaData().description = "open_and_closeAction_test" + pycram.task.task_tree.root.insert(self.session) + open_result = self.session.scalars(select(pycram.orm.action_designator.OpenAction)).all() + close_result = self.session.scalars(select(pycram.orm.action_designator.CloseAction)).all() + self.assertTrue(open_result is not None) + self.assertEqual(open_result[0].object.name, "handle_cab10_t") + self.assertTrue(close_result is not None) + self.assertEqual(close_result[0].object.name, "handle_cab10_t") + apartment.remove() if __name__ == '__main__': diff --git a/test/test_pose.py b/test/test_pose.py index 97064a56d..6fbb2ef79 100644 --- a/test/test_pose.py +++ b/test/test_pose.py @@ -73,5 +73,5 @@ def test_transform_copy(self): t = Transform([1, 1, 1], [0, 0, 0, 1], "map", "test_frame") t_copy = t.copy() - self.assertEquals(t, t_copy) + self.assertEqual(t, t_copy) self.assertFalse(t is t_copy) diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 1a89f6650..3c534ba7d 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -1,3 +1,5 @@ +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.pose import Pose from pycram.process_module import simulated_robot import pycram.task @@ -17,9 +19,9 @@ def plan(self): description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() def setUp(self):