Skip to content

Commit

Permalink
[resolver][orm] fixed issues after merge
Browse files Browse the repository at this point in the history
  • Loading branch information
davidprueser committed Apr 9, 2024
1 parent 37a5f41 commit 23ab36a
Show file tree
Hide file tree
Showing 5 changed files with 26 additions and 24 deletions.
8 changes: 4 additions & 4 deletions src/pycram/orm/views.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
from sqlalchemy.orm import declarative_base, column_property, query_expression, deferred, contains_eager
from sqlalchemy.orm import declarative_base
from typing_extensions import Union
import sqlalchemy.orm
from sqlalchemy import table, inspect, event, select, engine, MetaData, Select, TableClause, ExecutableDDLElement
from sqlalchemy.ext.compiler import compiles
from pycram.orm.action_designator import PickUpAction, Action
from pycram.orm.base import Position, RobotState, Pose, Base, Quaternion, _Base
from pycram.orm.action_designator import PickUpAction
from pycram.orm.base import Position, RobotState, Pose, Base, Quaternion
from pycram.orm.object_designator import Object
from pycram.orm.task import TaskTreeNode

Expand Down Expand Up @@ -138,7 +138,7 @@ class PickUpWithContextView(base):
__relative_x.label("relative_x"), __relative_y.label("relative_y"),
Quaternion.x.label("quaternion_x"), Quaternion.y.label("quaternion_y"),
Quaternion.z.label("quaternion_z"), Quaternion.w.label("quaternion_w"),
Object.type.label("type"), TaskTreeNode.status.label("status"))
Object.obj_type.label("obj_type"), TaskTreeNode.status.label("status"))
.join(TaskTreeNode.action.of_type(PickUpAction))
.join(PickUpAction.robot_state)
.join(__robot_pose, RobotState.pose)
Expand Down
2 changes: 1 addition & 1 deletion src/pycram/resolver/location/database_location.py
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ def create_query_from_occupancy_costmap(self) -> Select:
query = self.select_statement(view)

# constraint query to correct object type and successful task status
query = query.where(view.type == self.target.type).where(view.status == "SUCCEEDED")
query = query.where(view.obj_type == self.target.obj_type).where(view.status == "SUCCEEDED")

filters = []

Expand Down
2 changes: 1 addition & 1 deletion src/pycram/resolver/probabilistic/probabilistic_action.py
Original file line number Diff line number Diff line change
Expand Up @@ -274,7 +274,7 @@ def iterate_without_occupancy_costmap(self) -> Iterator[MoveAndPickUpPerformable
def query_for_database():
view = PickUpWithContextView
query = (select(view.arm, view.grasp, view.relative_x, view.relative_y)
.where(TaskTreeNode.status == TaskStatus.SUCCEEDED))
.where(view.status == TaskStatus.SUCCEEDED))
return query

def batch_rollout(self):
Expand Down
36 changes: 19 additions & 17 deletions test/test_database_resolver.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
from pycram.task import with_tree
from pycram.datastructures.enums import ObjectType, WorldMode
from pycram.resolver.location.database_location import DatabaseCostmapLocation
from pycram.worlds.bullet_world import BulletWorld

pycrorm_uri = os.getenv('PYCRORM_URI')
if pycrorm_uri:
Expand All @@ -36,8 +37,8 @@ class DatabaseResolverTestCase(unittest.TestCase,):
@classmethod
def setUpClass(cls) -> None:
global pycrorm_uri
cls.world = World(WorldMode.DIRECT)
cls.milk = Object("milk", "milk", "milk.stl", pose=Pose([1.3, 1, 0.9]))
cls.world = BulletWorld(WorldMode.DIRECT)
cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf")
ProcessModule.execution_delay = False
cls.engine = sqlalchemy.create_engine(pycrorm_uri)
Expand Down Expand Up @@ -81,15 +82,15 @@ def test_costmap_no_obstacles(self):

with simulated_robot:
MoveTorsoActionPerformable(sample.torso_height).perform()
PickUpActionPerformable(ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm,
PickUpActionPerformable(ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve(), arm=sample.reachable_arm,
grasp=sample.grasp).perform()

def test_costmap_with_obstacles(self):
kitchen = Object("kitchen", "environment", "kitchen.urdf")
kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
self.plan()
pycram.orm.base.ProcessMetaData().description = "costmap_with_obstacles_test"
pycram.task.task_tree.root.insert(self.session)
self.world.reset_bullet_world()
self.world.reset_current_world()

cml = DatabaseCostmapLocation(self.milk, self.session, reachable_for=self.robot)
sample = next(iter(cml))
Expand All @@ -99,22 +100,22 @@ def test_costmap_with_obstacles(self):
MoveTorsoActionPerformable(sample.torso_height).perform()
try:
PickUpActionPerformable(
ObjectDesignatorDescription(types=["milk"]).resolve(),
ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, grasp=sample.grasp).perform()
except pycram.plan_failures.PlanFailure as p:
kitchen.remove()
raise p
kitchen.remove()

def test_object_at_different_location(self):
kitchen = Object("kitchen", "environment", "kitchen.urdf")
kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
self.plan()

pycram.orm.base.ProcessMetaData().description = "object_at_different_location_test"
pycram.task.task_tree.root.insert(self.session)
self.world.reset_bullet_world()
self.world.reset_current_world()

new_milk = Object("new_milk", "milk", "milk.stl", pose=Pose([-1.45, 2.5, 0.95]))
new_milk = Object("new_milk", ObjectType.MILK, "milk.stl", pose=Pose([-1.45, 2.5, 0.95]))
cml = DatabaseCostmapLocation(new_milk, self.session, reachable_for=self.robot)

sample = next(iter(cml))
Expand All @@ -123,21 +124,21 @@ def test_object_at_different_location(self):
MoveTorsoActionPerformable(sample.torso_height).perform()
try:
PickUpActionPerformable(
ObjectDesignatorDescription(names=["new_milk"], types=["milk"]).resolve(),
ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, grasp=sample.grasp).perform()
except pycram.plan_failures.PlanFailure as p:
new_milk.remove()
kitchen.remove()
raise p
PlaceActionPerformable(ObjectDesignatorDescription(names=["new_milk"], types=["milk"]).resolve(),
PlaceActionPerformable(ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, target_location=Pose([-1.45, 2.5, 0.95])).perform()
new_milk.remove()
kitchen.remove()

@unittest.skip
def test_multiple_objects(self):
kitchen = Object("kitchen", "environment", "kitchen.urdf")
new_milk = Object("new_milk", "milk", "milk.stl", pose=Pose([-1.45, 2.5, 0.9]))
kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf")
new_milk = Object("new_milk", ObjectType.MILK, "milk.stl", pose=Pose([-1.45, 2.5, 0.9]))

object_description = ObjectDesignatorDescription(names=["milk"])
object_description_new_milk = ObjectDesignatorDescription(names=["new_milk"])
Expand All @@ -153,7 +154,7 @@ def test_multiple_objects(self):

pycram.orm.base.ProcessMetaData().description = "multiple_objects_test"
pycram.task.task_tree.root.insert(self.session)
self.world.reset_bullet_world()
self.world.reset_current_world()

cml = DatabaseCostmapLocation(self.milk, self.session, reachable_for=self.robot)
cml_new_milk = DatabaseCostmapLocation(new_milk, self.session, reachable_for=self.robot)
Expand All @@ -167,17 +168,18 @@ def test_multiple_objects(self):
try:
if dcl.target.name == "milk":
PickUpActionPerformable(
ObjectDesignatorDescription(names=["milk"], types=["milk"]).resolve(),
ObjectDesignatorDescription(names=["milk"], types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, grasp=sample.grasp).perform()
else:
PickUpActionPerformable(
ObjectDesignatorDescription(names=["new_milk"], types=["milk"]).resolve(),
ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, grasp=sample.grasp).perform()
except pycram.plan_failures.PlanFailure as p:
new_milk.remove()
kitchen.remove()
raise p
PlaceActionPerformable(ObjectDesignatorDescription(names=[dcl.target.name], types=["milk"]).resolve(),
PlaceActionPerformable(ObjectDesignatorDescription(names=[dcl.target.name],
types=[ObjectType.MILK]).resolve(),
arm=sample.reachable_arm, target_location=Pose([dcl.target.pose.position.x,
dcl.target.pose.position.y,
dcl.target.pose.position.z])
Expand Down
2 changes: 1 addition & 1 deletion test/test_orm.py
Original file line number Diff line number Diff line change
Expand Up @@ -291,7 +291,7 @@ def test_view_creation(self):
self.assertEqual(view.__table__.columns[7].name, "quaternion_y")
self.assertEqual(view.__table__.columns[8].name, "quaternion_z")
self.assertEqual(view.__table__.columns[9].name, "quaternion_w")
self.assertEqual(view.__table__.columns[10].name, "type")
self.assertEqual(view.__table__.columns[10].name, "obj_type")
self.assertEqual(view.__table__.columns[11].name, "status")

def test_pickUpWithContextView(self):
Expand Down

0 comments on commit 23ab36a

Please sign in to comment.