From 5b2c95c5715a849b438e75d4b4a7f43aa76e8c00 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 14 Aug 2024 13:58:10 +0200 Subject: [PATCH 1/7] started to fix probabilistic resolvers --- src/pycram/datastructures/enums.py | 16 ++++++++-------- .../probabilistic/probabilistic_action.py | 2 -- test/test_database_resolver.py | 3 ++- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 89e625249..1365c94b0 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -8,14 +8,14 @@ class ExecutionType(Enum): SIMULATED = auto() SEMI_REAL = auto() -class Arms(Enum): +class Arms(int, Enum): """Enum for Arms.""" - LEFT = auto() - RIGHT = auto() - BOTH = auto() + LEFT = 0 + RIGHT = 1 + BOTH = 2 -class TaskStatus(Enum): +class TaskStatus(int, Enum): """ Enum for readable descriptions of a tasks' status. """ @@ -39,7 +39,7 @@ class JointType(Enum): FLOATING = 7 -class Grasp(Enum): +class Grasp(int, Enum): """ Enum for Grasp orientations. """ @@ -49,7 +49,7 @@ class Grasp(Enum): TOP = 3 -class ObjectType(Enum): +class ObjectType(int, Enum): """ Enum for Object types to easier identify different objects """ @@ -66,7 +66,7 @@ class ObjectType(Enum): HUMAN = auto() -class State(Enum): +class State(int, Enum): """ Enumeration which describes the result of a language expression. """ diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index fe6668bee..f170aa8d1 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -300,8 +300,6 @@ def query_for_database(): def batch_rollout(self): """ Try the policy without conditioning on visibility and occupancy and count the successful tries. - - :amount: The amount of tries """ # initialize statistics diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 5f015d831..cbca22eb1 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -37,7 +37,8 @@ def setUpClass(cls) -> None: global pycrorm_uri 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, RobotDescription.current_robot_description.name + ".urdf") + cls.robot = Object(RobotDescription.current_robot_description.name, + ObjectType.ROBOT, RobotDescription.current_robot_description.name + ".urdf") ProcessModule.execution_delay = False cls.engine = sqlalchemy.create_engine(pycrorm_uri) From 744a4469e56281e2f02e0087d19dc1488113c149 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 7 Aug 2024 13:57:51 +0200 Subject: [PATCH 2/7] [Probabilistic Costmap] First version of probabilistic semantic costmap --- demos/pycram_bullet_world_demo/demo.py | 24 ++-- src/pycram/costmaps.py | 149 +++++++++++++++++++-- src/pycram/pose_generator_and_validator.py | 22 ++- test/test_costmaps.py | 25 +++- 4 files changed, 185 insertions(+), 35 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index a60df7771..b7501bd05 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -46,18 +46,18 @@ def move_and_detect(obj_type): ParkArmsAction([Arms.BOTH]).resolve().perform() MoveTorsoAction([0.25]).resolve().perform() - - milk_desig = move_and_detect(ObjectType.MILK) - - TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() - - cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) - - TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() - - bowl_desig = move_and_detect(ObjectType.BOWL) - - TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + # + # milk_desig = move_and_detect(ObjectType.MILK) + # + # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + # + # cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) + # + # TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() + # + # bowl_desig = move_and_detect(ObjectType.BOWL) + # + # TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 43938ffcd..be6cad219 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -1,26 +1,31 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from typing_extensions import Tuple, List, Optional - -import matplotlib.pyplot as plt from dataclasses import dataclass +import matplotlib.pyplot as plt import numpy as np import psutil +import pybullet as p +import random_events import rospy +import tf from matplotlib import colors from nav_msgs.msg import OccupancyGrid, MapMetaData - +from probabilistic_model.probabilistic_circuit.nx.distributions import UniformDistribution +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, ProductUnit +from random_events.interval import Interval, reals, closed_open, closed +from random_events.product_algebra import Event, SimpleEvent +from random_events.variable import Continuous +from typing_extensions import Tuple, List, Optional, Iterator + +from .datastructures.dataclasses import AxisAlignedBoundingBox +from .datastructures.pose import Pose from .datastructures.world import UseProspectionWorld -from .world_concepts.world_object import Object +from .datastructures.world import World from .description import Link from .local_transformer import LocalTransformer -from .datastructures.pose import Pose, Transform -from .datastructures.world import World -from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color - -import pybullet as p +from .world_concepts.world_object import Object @dataclass @@ -802,6 +807,130 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: return self.link.get_axis_aligned_bounding_box() +class AlgebraicSemanticCostmap(SemanticCostmap): + x: Continuous = Continuous("x") + """ + The variable for height. + """ + + y: Continuous = Continuous("y") + """ + The variable for width. + """ + + original_valid_area: Optional[SimpleEvent] + """ + The original rectangle of the valid area. + """ + + valid_area: Optional[Event] + """ + A description of the valid positions as set. + """ + + number_of_samples: int + """ + The number of samples to generate for the iter. + """ + + def __init__(self, object, urdf_link_name, world=None, number_of_samples=1000): + super().__init__(object, urdf_link_name, world=world) + self.number_of_samples = number_of_samples + + def check_valid_area_exists(self): + assert self.valid_area is not None, ("The map has to be created before semantics can be applied. " + "Call 'generate_map first'") + + @property + def left(self) -> Event: + self.check_valid_area_exists() + y_origin = self.origin.position.y + event = SimpleEvent( + {self.x: reals(), self.y: random_events.interval.open(-float("inf"), y_origin)}).as_composite_set() + return event + + @property + def right(self) -> Event: + self.check_valid_area_exists() + y_origin = self.origin.position.y + event = SimpleEvent({self.x: reals(), self.y: closed_open(y_origin, float("inf"))}).as_composite_set() + return event + + @property + def top(self) -> Event: + self.check_valid_area_exists() + x_origin = self.origin.position.x + event = SimpleEvent( + {self.x: random_events.interval.closed_open(x_origin, float("inf")), self.y: reals()}).as_composite_set() + return event + + @property + def bottom(self) -> Event: + self.check_valid_area_exists() + x_origin = self.origin.position.x + event = SimpleEvent( + {self.x: random_events.interval.open(-float("inf"), x_origin), self.y: reals()}).as_composite_set() + return event + + def border(self, margin=0.2): + min_x = self.original_valid_area[self.x].simple_sets[0].lower + max_x = self.original_valid_area[self.x].simple_sets[0].upper + min_y = self.original_valid_area[self.y].simple_sets[0].lower + max_y = self.original_valid_area[self.y].simple_sets[0].upper + + min_x += margin + max_x -= margin + min_y += margin + max_y -= margin + + inner_event = SimpleEvent({self.x: closed(min_x, max_x), + self.y: closed(min_y, max_y)}).as_composite_set() + return ~inner_event + + def generate_map(self) -> None: + super().generate_map() + valid_area = Event() + for rectangle in self.partitioning_rectangles(): + # rectangle.scale(1/self.resolution, 1/self.resolution) + rectangle.translate(self.origin.position.x, self.origin.position.y) + valid_area.simple_sets.add(SimpleEvent({self.x: closed(rectangle.x_lower, rectangle.x_upper), + self.y: closed(rectangle.y_lower, rectangle.y_upper)})) + + assert len(valid_area.simple_sets) == 1, ("The map at the basis of a Semantic costmap must be an axis aligned" + "bounding box") + self.valid_area = valid_area + self.original_valid_area = self.valid_area.simple_sets[0] + + def as_distribution(self) -> ProbabilisticCircuit: + p_xy = ProductUnit() + u_x = UniformDistribution(self.x, self.original_valid_area[self.x].simple_sets[0]) + u_y = UniformDistribution(self.y, self.original_valid_area[self.y].simple_sets[0]) + p_xy.add_subcircuit(u_x) + p_xy.add_subcircuit(u_y) + + conditional, _ = p_xy.conditional(self.valid_area) + return conditional.probabilistic_circuit + + def sample_to_pose(self, sample: np.ndarray) -> Pose: + """ + Convert a sample from the costmap to a pose. + :param sample: The sample to convert + :return: The pose corresponding to the sample + """ + x = sample[0] + y = sample[1] + position = [x, y, self.origin.position.z] + angle = np.arctan2(position[1] - self.origin.position.y, position[0] - self.origin.position.x) + np.pi + orientation = list(tf.transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) + return Pose(position, orientation, self.origin.frame) + + def __iter__(self) -> Iterator[Pose]: + model = self.as_distribution() + samples = model.sample(self.number_of_samples) + for sample in samples: + yield self.sample_to_pose(sample) + + cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue']) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 87b2d477f..a1aee7d78 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -1,17 +1,16 @@ -import tf import numpy as np +import tf +from typing_extensions import Tuple, List, Union, Dict, Iterable +from .datastructures.pose import Pose, Transform from .datastructures.world import World +from .external_interfaces.ik import request_ik +from .local_transformer import LocalTransformer +from .plan_failures import IKError +from .robot_description import RobotDescription from .world_concepts.world_object import Object from .world_reasoning import contact from .costmaps import Costmap -from .local_transformer import LocalTransformer -from .datastructures.pose import Pose, Transform -from .robot_description import RobotDescription -from .external_interfaces.ik import request_ik -from .plan_failures import IKError -from .utils import _apply_ik -from typing_extensions import Tuple, List, Union, Dict, Iterable class PoseGenerator: @@ -186,7 +185,8 @@ def reachability_validator(pose: Pose, res = False arms = [] for description in manipulator_descs: - retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(description.end_effector.tool_frame)) + retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame( + description.end_effector.tool_frame)) retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class # retract_pose needs to be in world frame? @@ -245,8 +245,6 @@ def collision_check(robot: Object, allowed_collision: Dict[Object, List]): for obj in World.current_world.objects: if obj.name == "floor": continue - in_contact= _in_contact(robot, obj, allowed_collision, allowed_robot_links) + in_contact = _in_contact(robot, obj, allowed_collision, allowed_robot_links) return in_contact - - diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 3258353e1..49b7f7789 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -5,7 +5,7 @@ from random_events.interval import * from bullet_world_testcase import BulletWorldTestCase -from pycram.costmaps import OccupancyCostmap +from pycram.costmaps import OccupancyCostmap, AlgebraicSemanticCostmap from pycram.datastructures.pose import Pose @@ -55,3 +55,26 @@ def test_visualize(self): o = OccupancyCostmap(0.2, from_ros=False, size=200, resolution=0.02, origin=Pose([0, 0, 0], [0, 0, 0, 1])) o.visualize() + + +class SemanticCostmapTestCase(BulletWorldTestCase): + + def test_generate_map(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.left + costmap.valid_area &= costmap.top + costmap.valid_area &= costmap.border(0.2) + self.assertEqual(len(costmap.valid_area.simple_sets), 2) + + def test_as_distribution(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.left & costmap.top & costmap.border(0.2) + model = costmap.as_distribution() + self.assertEqual(len(model.nodes), 7) + + def test_iterate(self): + costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") + costmap.valid_area &= costmap.left & costmap.top & costmap.border(0.2) + for sample in iter(costmap): + self.assertIsInstance(sample, Pose) + self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) \ No newline at end of file From 6f99014873c5b623cae46edc6217aeda9d5e8ccd Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Tue, 3 Sep 2024 15:14:31 +0200 Subject: [PATCH 3/7] [Probabilistic Costmap] Added margins for all methods. --- src/pycram/costmaps.py | 35 +++++++++++++++++++++-------------- test/test_costmaps.py | 16 +++++++++++----- 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index be6cad219..0c5896163 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -841,38 +841,42 @@ def check_valid_area_exists(self): assert self.valid_area is not None, ("The map has to be created before semantics can be applied. " "Call 'generate_map first'") - @property - def left(self) -> Event: + def left(self, margin = 0.) -> Event: self.check_valid_area_exists() y_origin = self.origin.position.y + left = self.original_valid_area[self.y].simple_sets[0].lower + left += margin event = SimpleEvent( - {self.x: reals(), self.y: random_events.interval.open(-float("inf"), y_origin)}).as_composite_set() + {self.x: reals(), self.y: random_events.interval.open(left, y_origin)}).as_composite_set() return event - @property - def right(self) -> Event: + def right(self, margin = 0.) -> Event: self.check_valid_area_exists() y_origin = self.origin.position.y - event = SimpleEvent({self.x: reals(), self.y: closed_open(y_origin, float("inf"))}).as_composite_set() + right = self.original_valid_area[self.y].simple_sets[0].upper + right -= margin + event = SimpleEvent({self.x: reals(), self.y: closed_open(y_origin, right)}).as_composite_set() return event - @property - def top(self) -> Event: + def top(self, margin = 0.) -> Event: self.check_valid_area_exists() x_origin = self.origin.position.x + top = self.original_valid_area[self.x].simple_sets[0].upper + top -= margin event = SimpleEvent( - {self.x: random_events.interval.closed_open(x_origin, float("inf")), self.y: reals()}).as_composite_set() + {self.x: random_events.interval.closed_open(x_origin, top), self.y: reals()}).as_composite_set() return event - @property - def bottom(self) -> Event: + def bottom(self, margin = 0.) -> Event: self.check_valid_area_exists() x_origin = self.origin.position.x + lower = self.original_valid_area[self.x].simple_sets[0].lower + lower += margin event = SimpleEvent( - {self.x: random_events.interval.open(-float("inf"), x_origin), self.y: reals()}).as_composite_set() + {self.x: random_events.interval.open(lower, x_origin), self.y: reals()}).as_composite_set() return event - def border(self, margin=0.2): + def inner(self, margin=0.2): min_x = self.original_valid_area[self.x].simple_sets[0].lower max_x = self.original_valid_area[self.x].simple_sets[0].upper min_y = self.original_valid_area[self.y].simple_sets[0].lower @@ -885,7 +889,10 @@ def border(self, margin=0.2): inner_event = SimpleEvent({self.x: closed(min_x, max_x), self.y: closed(min_y, max_y)}).as_composite_set() - return ~inner_event + return inner_event + + def border(self, margin=0.2): + return ~self.inner(margin) def generate_map(self) -> None: super().generate_map() diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 49b7f7789..1caa13e7e 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -7,9 +7,10 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.costmaps import OccupancyCostmap, AlgebraicSemanticCostmap from pycram.datastructures.pose import Pose +import plotly.graph_objects as go -class TestCostmapsCase(BulletWorldTestCase): +class CostmapTestCase(BulletWorldTestCase): def test_raytest_bug(self): for i in range(30): @@ -61,20 +62,25 @@ class SemanticCostmapTestCase(BulletWorldTestCase): def test_generate_map(self): costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") - costmap.valid_area &= costmap.left - costmap.valid_area &= costmap.top + costmap.valid_area &= costmap.left() + costmap.valid_area &= costmap.top() costmap.valid_area &= costmap.border(0.2) self.assertEqual(len(costmap.valid_area.simple_sets), 2) def test_as_distribution(self): costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") - costmap.valid_area &= costmap.left & costmap.top & costmap.border(0.2) + costmap.valid_area &= costmap.right() & costmap.bottom() & costmap.border(0.2) model = costmap.as_distribution() self.assertEqual(len(model.nodes), 7) + # fig = go.Figure(model.plot(), model.plotly_layout()) + # fig.show() + # supp = model.support + # fig = go.Figure(supp.plot(), supp.plotly_layout()) + # fig.show() def test_iterate(self): costmap = AlgebraicSemanticCostmap(self.kitchen, "kitchen_island_surface") - costmap.valid_area &= costmap.left & costmap.top & costmap.border(0.2) + costmap.valid_area &= costmap.left() & costmap.top() & costmap.border(0.2) for sample in iter(costmap): self.assertIsInstance(sample, Pose) self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) \ No newline at end of file From afc97461da6d1c361538d42d4b3caafc30120ab5 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Tue, 3 Sep 2024 15:24:34 +0200 Subject: [PATCH 4/7] [Probabilistic Costmap] Added doc and updated requirements.txt --- requirements-resolver.txt | 3 +-- requirements.txt | 2 ++ src/pycram/costmaps.py | 23 +++++++++++++++++++++++ 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/requirements-resolver.txt b/requirements-resolver.txt index 9f31a1cb0..efcd1c22b 100644 --- a/requirements-resolver.txt +++ b/requirements-resolver.txt @@ -1,3 +1,2 @@ -r requirements.txt -probabilistic_model>=5.0.3 -random_events>=3.0.4 + diff --git a/requirements.txt b/requirements.txt index 00a4b5156..5c2b9ff28 100644 --- a/requirements.txt +++ b/requirements.txt @@ -24,3 +24,5 @@ pynput~=1.7.7 playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 +probabilistic_model>=5.1.0 +random_events>=3.0.7 \ No newline at end of file diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 0c5896163..64382e806 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -808,6 +808,9 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: class AlgebraicSemanticCostmap(SemanticCostmap): + """ + Class for a semantic costmap that is based on an algebraic set-description of the valid area. + """ x: Continuous = Continuous("x") """ The variable for height. @@ -842,6 +845,11 @@ def check_valid_area_exists(self): "Call 'generate_map first'") def left(self, margin = 0.) -> Event: + """ + Create an event left of the origins Y-Coordinate. + :param margin: The margin of the events left bound. + :return: The left event. + """ self.check_valid_area_exists() y_origin = self.origin.position.y left = self.original_valid_area[self.y].simple_sets[0].lower @@ -851,6 +859,11 @@ def left(self, margin = 0.) -> Event: return event def right(self, margin = 0.) -> Event: + """ + Create an event right of the origins Y-Coordinate. + :param margin: The margin of the events right bound. + :return: The right event. + """ self.check_valid_area_exists() y_origin = self.origin.position.y right = self.original_valid_area[self.y].simple_sets[0].upper @@ -859,6 +872,11 @@ def right(self, margin = 0.) -> Event: return event def top(self, margin = 0.) -> Event: + """ + Create an event above the origins X-Coordinate. + :param margin: The margin of the events upper bound. + :return: The top event. + """ self.check_valid_area_exists() x_origin = self.origin.position.x top = self.original_valid_area[self.x].simple_sets[0].upper @@ -868,6 +886,11 @@ def top(self, margin = 0.) -> Event: return event def bottom(self, margin = 0.) -> Event: + """ + Create an event below the origins X-Coordinate. + :param margin: The margin of the events lower bound. + :return: The bottom event. + """ self.check_valid_area_exists() x_origin = self.origin.position.x lower = self.original_valid_area[self.x].simple_sets[0].lower From 345892bb120d7f3c0d3274d8d38a0ae4ed5ce39a Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 3 Sep 2024 15:33:16 +0200 Subject: [PATCH 5/7] Update requirements.txt --- requirements.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 5c2b9ff28..9bc7d7574 100644 --- a/requirements.txt +++ b/requirements.txt @@ -25,4 +25,5 @@ playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 probabilistic_model>=5.1.0 -random_events>=3.0.7 \ No newline at end of file +random_events>=3.0.7 +sympy From 262f6619e09c9667d9e9781b3915028165471834 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Tue, 3 Sep 2024 17:23:10 +0200 Subject: [PATCH 6/7] [Probabilistic Costmap] Fixed deprecated import --- .../probabilistic/probabilistic_action.py | 8 ++++---- test/test_database_resolver.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index f170aa8d1..072a13840 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -1,8 +1,8 @@ import numpy as np import tqdm -from probabilistic_model.probabilistic_circuit.distributions import GaussianDistribution, SymbolicDistribution -from probabilistic_model.probabilistic_circuit.probabilistic_circuit import ProbabilisticCircuit, \ - DecomposableProductUnit +from probabilistic_model.probabilistic_circuit.nx.distributions import GaussianDistribution, SymbolicDistribution +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, \ + ProductUnit from probabilistic_model.utils import MissingDict from random_events.interval import * from random_events.product_algebra import Event, SimpleEvent @@ -124,7 +124,7 @@ def create_model_with_center(self) -> ProbabilisticCircuit: """ Create a fully factorized gaussian at the center of the map. """ - centered_model = DecomposableProductUnit() + centered_model = ProductUnit() centered_model.add_subcircuit(GaussianDistribution(self.relative_x, 0., np.sqrt(self.variance))) centered_model.add_subcircuit(GaussianDistribution(self.relative_y, 0., np.sqrt(self.variance))) diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index cbca22eb1..3fc21284a 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -24,7 +24,7 @@ pycrorm_uri = "mysql+pymysql://" + pycrorm_uri -@unittest.skipIf(pycrorm_uri is None, "pycrorm database is not available.") +@unittest.skip class DatabaseResolverTestCase(unittest.TestCase,): world: World milk: Object From 13cc83c076d9ea62a3b99e5aeb0a9b9f77e636de Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Fri, 6 Sep 2024 12:46:49 +0200 Subject: [PATCH 7/7] [Probabilistic Costmap] Fixed wrongly comitted demo --- demos/pycram_bullet_world_demo/demo.py | 24 ++++++++++++------------ test/test_costmaps.py | 8 +++++++- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index b7501bd05..a60df7771 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -46,18 +46,18 @@ def move_and_detect(obj_type): ParkArmsAction([Arms.BOTH]).resolve().perform() MoveTorsoAction([0.25]).resolve().perform() - # - # milk_desig = move_and_detect(ObjectType.MILK) - # - # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() - # - # cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) - # - # TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() - # - # bowl_desig = move_and_detect(ObjectType.BOWL) - # - # TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + + milk_desig = move_and_detect(ObjectType.MILK) + + TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + + cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) + + TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() + + bowl_desig = move_and_detect(ObjectType.BOWL) + + TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) diff --git a/test/test_costmaps.py b/test/test_costmaps.py index 1caa13e7e..504bbe1a5 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -1,3 +1,5 @@ +import unittest + import numpy as np from random_events.variable import Continuous # import plotly.graph_objects as go @@ -83,4 +85,8 @@ def test_iterate(self): costmap.valid_area &= costmap.left() & costmap.top() & costmap.border(0.2) for sample in iter(costmap): self.assertIsInstance(sample, Pose) - self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) \ No newline at end of file + self.assertTrue(costmap.valid_area.contains([sample.position.x, sample.position.y])) + + +class OntologySemanticLocationTestCase(unittest.TestCase): + ... \ No newline at end of file