From 5bc624787ef401bcaeae681f52d0cf7840932c08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Tue, 9 Apr 2024 13:38:49 +0200 Subject: [PATCH 1/2] [orm] created database views --- src/pycram/orm/queries/__init__.py | 0 src/pycram/orm/queries/queries.py | 51 ------ src/pycram/orm/views.py | 149 ++++++++++++++++++ .../resolver/location/database_location.py | 31 ++-- .../probabilistic/probabilistic_action.py | 12 +- test/test_orm.py | 63 +++++++- 6 files changed, 225 insertions(+), 81 deletions(-) delete mode 100644 src/pycram/orm/queries/__init__.py delete mode 100644 src/pycram/orm/queries/queries.py create mode 100644 src/pycram/orm/views.py diff --git a/src/pycram/orm/queries/__init__.py b/src/pycram/orm/queries/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/pycram/orm/queries/queries.py b/src/pycram/orm/queries/queries.py deleted file mode 100644 index 7854b8c54..000000000 --- a/src/pycram/orm/queries/queries.py +++ /dev/null @@ -1,51 +0,0 @@ -import sqlalchemy.orm -from sqlalchemy import Select, alias -from ..task import TaskTreeNode -from ..action_designator import PickUpAction -from ..base import Position, RobotState, Pose -from ..object_designator import Object - - -class Query: - """ - Abstract class for queries - """ - - -class PickUpWithContext(Query): - """ - Query for pickup actions with context. - """ - - robot_position = sqlalchemy.orm.aliased(Position) - """ - 3D Vector of robot position - """ - - robot_pose = sqlalchemy.orm.aliased(Pose) - """ - Complete robot pose - """ - - object_position = sqlalchemy.orm.aliased(Position) - """ - 3D Vector for object position - """ - - relative_x = (robot_position.x - object_position.x).label("relative_x") - """ - Distance on x axis between robot and object - """ - - relative_y = (robot_position.y - object_position.y).label("relative_y") - """ - Distance on y axis between robot and object - """ - - def join_statement(self, query: Select): - return (query.join(TaskTreeNode).join(TaskTreeNode.action.of_type(PickUpAction)) - .join(PickUpAction.robot_state).join(self.robot_pose, RobotState.pose) - .join(self.robot_position, self.robot_pose.position) - .join(Pose.orientation) - .join(PickUpAction.object) - .join(Object.pose).join(self.object_position, Pose.position)) diff --git a/src/pycram/orm/views.py b/src/pycram/orm/views.py new file mode 100644 index 000000000..352b3c304 --- /dev/null +++ b/src/pycram/orm/views.py @@ -0,0 +1,149 @@ +from sqlalchemy.orm import declarative_base, column_property, query_expression, deferred, contains_eager +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.object_designator import Object +from pycram.orm.task import TaskTreeNode + + +class CreateView(ExecutableDDLElement): + """ + Class that is used to create a view. Every instance will be compiled into a SQL CREATE VIEW statement. + """ + + def __init__(self, name: str, selectable: Select): + self.name = name + self.selectable = selectable + + +class DropView(ExecutableDDLElement): + """ + Class that is used to drop a view. Every instance will be compiled into a SQL DROP VIEW statement. + """ + + def __init__(self, name: str): + self.name = name + + +@compiles(CreateView) +def _create_view(element: CreateView, compiler, **kw) -> str: + """ + Compiles a CreateView instance into a SQL CREATE VIEW statement. + :param element: CreateView instance + :param compiler: compiler + :param kw: keyword arguments + :return: SQL CREATE VIEW statement + """ + + return "CREATE VIEW %s AS %s" % ( + element.name, + compiler.sql_compiler.process(element.selectable, literal_binds=True), + ) + + +@compiles(DropView) +def _drop_view(element: DropView, compiler, **kw) -> str: + """ + Compiles a DropView instance into a SQL DROP VIEW statement. + :param element: DropView instance + :param compiler: compiler + :param kw: keyword arguments + :return: SQL DROP VIEW statement + """ + return "DROP VIEW %s" % element.name + + +def view_exists(ddl: Union[CreateView, DropView], target, connection: engine, **kw) -> bool: + """ + Check if a view exists. + :param ddl: ddl instance + :param target: target object + :param connection: connection + :param kw: keyword arguments + :return: True if the view exists, False otherwise + """ + + return ddl.name in inspect(connection).get_view_names() + + +def view_doesnt_exist(ddl: Union[CreateView, DropView], target, connection: engine, **kw) -> bool: + """ + Check if a view does not exist. + :param ddl: ddl instance + :param target: target object + :param connection: connection + :param kw: keyword arguments + :return: True if the view does not exist, False otherwise + """ + + return not view_exists(ddl, target, connection, **kw) + + +def view(name: str, metadata: MetaData, selectable: Select) -> TableClause: + """ + Function used to control view creation and deletion. It will listen to the after_create and before_drop events + of the metadata object in order to either create or drop the view. The view needs to have a column id. + """ + view = table(name) + + view._columns._populate_separate_keys( + col._make_proxy(view) for col in selectable.selected_columns + ) + + event.listen(metadata, "after_create", CreateView(name, selectable).execute_if(callable_=view_doesnt_exist)) + event.listen(metadata, "before_drop", DropView(name).execute_if(callable_=view_exists)) + + return view + + +base = declarative_base(metadata=Base.metadata) + + +class PickUpWithContextView(base): + """ + View for pickup actions with context. + """ + + __robot_position: Position = sqlalchemy.orm.aliased(Position, flat=True) + """ + 3D Vector of robot position + """ + + __robot_pose: Pose = sqlalchemy.orm.aliased(Pose, flat=True) + """ + Complete robot pose + """ + + __object_position: Position = sqlalchemy.orm.aliased(Position, flat=True) + """ + 3D Vector for object position + """ + + __relative_x = (__robot_position.x - __object_position.x) + """ + Distance on x axis between robot and object + """ + + __relative_y = (__robot_position.y - __object_position.y) + """ + Distance on y axis between robot and object + """ + + __table__ = view("PickUpWithContextView", Base.metadata, + (select(PickUpAction.id.label("id"), PickUpAction.arm.label("arm"), + PickUpAction.grasp.label("grasp"), RobotState.torso_height.label("torso_height"), + __relative_x.label("relative_x"), __relative_y.label("relative_y"), + Quaternion.x.label("quaternion_x"), Quaternion.y.label("quaternion_y"), + Quaternion.z.label("quaternion_z"), Quaternion.w.label("quaternion_w"), + Object.type.label("type"), TaskTreeNode.status.label("status")) + .join(TaskTreeNode.action.of_type(PickUpAction)) + .join(PickUpAction.robot_state) + .join(__robot_pose, RobotState.pose) + .join(__robot_position, __robot_pose.position) + .join(Pose.orientation) + .join(PickUpAction.object) + .join(Object.pose) + .join(__object_position, Pose.position))) diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index 38115476c..20949df77 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -1,20 +1,14 @@ from dataclasses import dataclass -import numpy as np import sqlalchemy.orm import sqlalchemy.sql from sqlalchemy import select, Select -from typing_extensions import List - +from typing_extensions import List, Type from ...costmaps import Rectangle, OccupancyCostmap from ...designator import LocationDesignatorDescription from ...designators.location_designator import CostmapLocation -from ...orm.action_designator import PickUpAction -from ...orm.base import RobotState, Quaternion -from ...orm.object_designator import Object -from ...orm.task import TaskTreeNode +from ...orm.views import PickUpWithContextView from ...pose import Pose -from ...orm.queries.queries import PickUpWithContext @dataclass @@ -72,10 +66,9 @@ def __init__(self, target, session: sqlalchemy.orm.Session = None, reachable_for self.session = session @staticmethod - def select_statement(query_context: PickUpWithContext) -> Select: - return query_context.join_statement(select(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, - query_context.relative_x, query_context.relative_y, Quaternion.x, - Quaternion.y, Quaternion.z, Quaternion.w).distinct()) + def select_statement(view: Type[PickUpWithContextView]) -> Select: + return (select(view.arm, view.grasp, view.torso_height, view.relative_x, view.relative_y, view.quaternion_x, + view.quaternion_y, view.quaternion_z, view.quaternion_w).distinct()) def create_query_from_occupancy_costmap(self) -> Select: """ @@ -83,23 +76,23 @@ def create_query_from_occupancy_costmap(self) -> Select: OccupancyCostmap. """ - query_context = PickUpWithContext() + view = PickUpWithContextView # get query - query = self.select_statement(query_context) + query = self.select_statement(view) # constraint query to correct object type and successful task status - query = query.where(Object.type == self.target.type).where(TaskTreeNode.status == "SUCCEEDED") + query = query.where(view.type == self.target.type).where(view.status == "SUCCEEDED") filters = [] # for every rectangle for rectangle in self.create_occupancy_rectangles(): # add sql filter - filters.append(sqlalchemy.and_(query_context.relative_x >= rectangle.x_lower, - query_context.relative_x < rectangle.x_upper, - query_context.relative_y >= rectangle.y_lower, - query_context.relative_y < rectangle.y_upper)) + filters.append(sqlalchemy.and_(view.relative_x >= rectangle.x_lower, + view.relative_x < rectangle.x_upper, + view.relative_y >= rectangle.y_lower, + view.relative_y < rectangle.y_upper)) return query.where(sqlalchemy.or_(*filters)) diff --git a/src/pycram/resolver/probabilistic/probabilistic_action.py b/src/pycram/resolver/probabilistic/probabilistic_action.py index f5feaaa32..7af6932c3 100644 --- a/src/pycram/resolver/probabilistic/probabilistic_action.py +++ b/src/pycram/resolver/probabilistic/probabilistic_action.py @@ -4,7 +4,7 @@ import portion from probabilistic_model.probabilistic_circuit.distributions import GaussianDistribution, SymbolicDistribution from probabilistic_model.probabilistic_circuit.probabilistic_circuit import ProbabilisticCircuit, \ - DecomposableProductUnit, DeterministicSumUnit + DecomposableProductUnit from random_events.events import Event, ComplexEvent from random_events.variables import Symbolic, Continuous import tqdm @@ -15,9 +15,8 @@ from ...designators.actions.actions import MoveAndPickUpPerformable, ActionAbstract from ...enums import Arms, Grasp, TaskStatus from ...local_transformer import LocalTransformer -from ...orm.queries.queries import PickUpWithContext from ...orm.task import TaskTreeNode -from ...orm.action_designator import PickUpAction as ORMPickUpAction +from ...orm.views import PickUpWithContextView from ...plan_failures import ObjectUnreachable, PlanFailure from ...pose import Pose import plotly.graph_objects as go @@ -274,10 +273,9 @@ def iterate_without_occupancy_costmap(self) -> Iterator[MoveAndPickUpPerformable @staticmethod def query_for_database(): - query_context = PickUpWithContext() - query = select(ORMPickUpAction.arm, ORMPickUpAction.grasp, - query_context.relative_x, query_context.relative_y) - query = query_context.join_statement(query).where(TaskTreeNode.status == TaskStatus.SUCCEEDED) + view = PickUpWithContextView + query = (select(view.arm, view.grasp, view.relative_x, view.relative_y) + .where(TaskTreeNode.status == TaskStatus.SUCCEEDED)) return query def batch_rollout(self): diff --git a/test/test_orm.py b/test/test_orm.py index ce5775a84..f27b24c51 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -17,6 +17,7 @@ OpenActionPerformable, CloseActionPerformable, DetectActionPerformable, LookAtActionPerformable from pycram.designators.object_designator import BelieveObject from pycram.enums import ObjectType +from pycram.orm.views import PickUpWithContextView from pycram.pose import Pose from pycram.process_module import simulated_robot from pycram.task import with_tree @@ -46,8 +47,6 @@ def tearDown(self): class ORMTestSchemaTestCase(DatabaseTestCaseMixin, unittest.TestCase): def test_schema_creation(self): - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session.commit() tables = list(pycram.orm.base.Base.metadata.tables.keys()) self.assertTrue("Position" in tables) self.assertTrue("Quaternion" in tables) @@ -211,9 +210,9 @@ def test_parkArmsAction(self): pycram.orm.base.ProcessMetaData().description = "parkArmsAction_test" pycram.task.task_tree.root.insert(self.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 + 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)])) + for i in range(len(result) - 1)])) def test_transportAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) @@ -275,5 +274,61 @@ def test_open_and_closeAction(self): apartment.remove() +class ViewsSchemaTest(DatabaseTestCaseMixin): + def test_view_creation(self): + pycram.orm.base.ProcessMetaData().description = "view_creation_test" + pycram.task.task_tree.root.insert(self.session) + view = PickUpWithContextView + self.assertEqual(len(view.__table__.columns), 12) + self.assertEqual(view.__table__.name, "PickUpWithContextView") + self.assertEqual(view.__table__.columns[0].name, "id") + self.assertEqual(view.__table__.columns[1].name, "arm") + self.assertEqual(view.__table__.columns[2].name, "grasp") + self.assertEqual(view.__table__.columns[3].name, "torso_height") + self.assertEqual(view.__table__.columns[4].name, "relative_x") + self.assertEqual(view.__table__.columns[5].name, "relative_y") + self.assertEqual(view.__table__.columns[6].name, "quaternion_x") + self.assertEqual(view.__table__.columns[7].name, "quaternion_y") + self.assertEqual(view.__table__.columns[8].name, "quaternion_z") + self.assertEqual(view.__table__.columns[9].name, "quaternion_w") + self.assertEqual(view.__table__.columns[10].name, "type") + self.assertEqual(view.__table__.columns[11].name, "status") + + def test_pickUpWithContextView(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() + pycram.orm.base.ProcessMetaData().description = "pickUpWithContextView_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(PickUpWithContextView)).first() + self.assertEqual(result.arm, "left") + self.assertEqual(result.grasp, "front") + self.assertEqual(result.torso_height, 0.3) + self.assertAlmostEqual(result.relative_x, -0.7, 6) + self.assertAlmostEqual(result.relative_y, -0.6, 6) + self.assertEqual(result.quaternion_x, 0) + self.assertEqual(result.quaternion_w, 1) + + def test_pickUpWithContextView_conditions(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() + pycram.orm.base.ProcessMetaData().description = "pickUpWithContextView_conditions_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.scalars(select(PickUpWithContextView) + .where(PickUpWithContextView.arm == "right")).all() + self.assertEqual(result, []) + + if __name__ == '__main__': unittest.main() From 23ab36a0096cb1ffb77a3caedd7a54d326db5da4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Tue, 9 Apr 2024 19:13:22 +0200 Subject: [PATCH 2/2] [resolver][orm] fixed issues after merge --- src/pycram/orm/views.py | 8 ++--- .../resolver/location/database_location.py | 2 +- .../probabilistic/probabilistic_action.py | 2 +- test/test_database_resolver.py | 36 ++++++++++--------- test/test_orm.py | 2 +- 5 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/pycram/orm/views.py b/src/pycram/orm/views.py index 352b3c304..f49b349f5 100644 --- a/src/pycram/orm/views.py +++ b/src/pycram/orm/views.py @@ -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 @@ -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) diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index 412ba4ffc..35daae321 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -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 = [] diff --git a/src/pycram/resolver/probabilistic/probabilistic_action.py b/src/pycram/resolver/probabilistic/probabilistic_action.py index 53867488c..348f0627e 100644 --- a/src/pycram/resolver/probabilistic/probabilistic_action.py +++ b/src/pycram/resolver/probabilistic/probabilistic_action.py @@ -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): diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 6dfd121de..e6021ef60 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -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: @@ -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) @@ -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)) @@ -99,7 +100,7 @@ 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() @@ -107,14 +108,14 @@ def test_costmap_with_obstacles(self): 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)) @@ -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"]) @@ -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) @@ -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]) diff --git a/test/test_orm.py b/test/test_orm.py index f9fbbd737..f9c96c126 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -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):