Skip to content

Commit

Permalink
Merge pull request #123 from davidprueser/tests
Browse files Browse the repository at this point in the history 
refactored action designators and added orm test cases
  • Loading branch information
@Tigul
Tigul authored Feb 5, 2024
2 parents d3eb6ef + b03c6e2 commit 15225f1
Show file tree
Hide file tree
Showing 33 changed files with 10,111 additions and 2,565 deletions.
2 changes: 1 addition & 1 deletion doc/source/designators.rst
View file
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
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
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
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

0 comments on commit 15225f1

Please sign in to comment.