Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactored action designators and added orm test cases #123

Merged
merged 14 commits into from
Feb 5, 2024
Merged

refactored action designators and added orm test cases #123

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
f3bcae7
[designator] refactored action_designator
davidprueser Jan 5, 2024
8d11a5d
[orm] fixed orm querying
davidprueser Jan 5, 2024
6f90a47
[tests] updated orm tests
davidprueser Jan 5, 2024
c4d6122
Merge branch 'dev' into tests
davidprueser Jan 5, 2024
af9e3e6
[tests] fixed object designator test
davidprueser Jan 5, 2024
b1f2a35
[designator] removed MotionDesignatorDescription
davidprueser Jan 18, 2024
01d01b1
Merge branch 'dev' into tests
davidprueser Jan 19, 2024
7520c8a
merge dev into tests
davidprueser Jan 19, 2024
5833a9d
[test] updated orm querying; added docstrings to actions and motions
davidprueser Jan 22, 2024
4081685
[test] fixed an issue where the orm session was not properly clossed …
davidprueser Jan 22, 2024
f6dd146
[test] fixed orm test inheritance pattern
davidprueser Jan 26, 2024
5e32afd
[designator] removed deprecated motions
davidprueser Jan 29, 2024
c9328d5
[doc] updated orm querying, motions and actions in documentation
davidprueser Feb 1, 2024
b03c6e2
[orm] fixed sqlalchemy deprecation warnings
davidprueser Feb 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion doc/source/designators.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
48 changes: 33 additions & 15 deletions doc/source/index.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
---------
Expand Down
18 changes: 9 additions & 9 deletions examples/cram_plan_tutorial.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
77 changes: 40 additions & 37 deletions examples/custom_resolver.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,24 +23,24 @@
"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": [
{
"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"
]
}
],
Expand All @@ -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": [
Expand All @@ -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",
Expand Down Expand Up @@ -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",
Expand Down Expand Up @@ -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": [
Expand All @@ -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]"
]
}
],
Expand All @@ -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": [],
Expand All @@ -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": [],
Expand Down Expand Up @@ -357,16 +358,16 @@
"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": [
{
"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",
Expand Down Expand Up @@ -399,14 +400,15 @@
}
],
"source": [
"from sqlalchemy import select\n",
"from pycram.orm.base import Pose as ORMPose\n",
"\n",
"robot_pose = sqlalchemy.orm.aliased(ORMPose)\n",
"object_pose = sqlalchemy.orm.aliased(ORMPose)\n",
"robot_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",
Expand All @@ -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)"
]
Expand All @@ -437,16 +439,16 @@
"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": [
{
"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"
]
},
{
Expand Down Expand Up @@ -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": [
Expand All @@ -502,6 +506,7 @@
}
],
"source": [
"from pycram.designators.actions.actions import NavigateActionPerformable, PickUpActionPerformable\n",
"from pycram.resolver import JPTCostmapLocation\n",
"\n",
"\n",
Expand All @@ -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",
Expand Down
Loading
Loading