From 9039fa5a4f74b5a2196d6c3e37b1f76e4cdc68de Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 30 Oct 2024 10:02:53 +0100 Subject: [PATCH 01/12] [Probabilistic Reasoning] Upgraded to newest PM version --- src/pycram/costmaps.py | 34 +++++++------------ .../probabilistic/probabilistic_action.py | 21 ++++++++---- 2 files changed, 26 insertions(+), 29 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 9ce0ecfdf..59407b79a 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -10,27 +10,23 @@ 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 probabilistic_model.probabilistic_circuit.nx.helper import uniform_measure_of_event +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit 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 tf.transformations import quaternion_from_matrix from typing_extensions import Tuple, List, Optional, Iterator -from .ros.ros_tools import wait_for_message -from .datastructures.dataclasses import AxisAlignedBoundingBox -from .datastructures.pose import Pose +from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color +from .datastructures.pose import Pose, Transform from .datastructures.world import UseProspectionWorld from .datastructures.world import World from .description import Link from .local_transformer import LocalTransformer +from .ros.ros_tools import wait_for_message from .world_concepts.world_object import Object -from .datastructures.pose import Pose, Transform -from .datastructures.world import World -from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color -from tf.transformations import quaternion_from_matrix - @dataclass class Rectangle: @@ -825,7 +821,7 @@ 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'") - def left(self, margin = 0.) -> Event: + def left(self, margin=0.) -> Event: """ Create an event left of the origins Y-Coordinate. :param margin: The margin of the events left bound. @@ -839,7 +835,7 @@ def left(self, margin = 0.) -> Event: {self.x: reals(), self.y: random_events.interval.open(left, y_origin)}).as_composite_set() return event - def right(self, margin = 0.) -> 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. @@ -852,7 +848,7 @@ def right(self, margin = 0.) -> Event: event = SimpleEvent({self.x: reals(), self.y: closed_open(y_origin, right)}).as_composite_set() return event - def top(self, margin = 0.) -> Event: + def top(self, margin=0.) -> Event: """ Create an event above the origins X-Coordinate. :param margin: The margin of the events upper bound. @@ -866,7 +862,7 @@ def top(self, margin = 0.) -> Event: {self.x: random_events.interval.closed_open(x_origin, top), self.y: reals()}).as_composite_set() return event - def bottom(self, margin = 0.) -> Event: + def bottom(self, margin=0.) -> Event: """ Create an event below the origins X-Coordinate. :param margin: The margin of the events lower bound. @@ -913,14 +909,8 @@ def generate_map(self) -> None: 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 + model = uniform_measure_of_event(self.valid_area) + return model def sample_to_pose(self, sample: np.ndarray) -> Pose: """ diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index 606969672..d56e00a9b 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -1,6 +1,8 @@ import numpy as np import tqdm -from probabilistic_model.probabilistic_circuit.nx.distributions import GaussianDistribution, SymbolicDistribution +from probabilistic_model.distributions import GaussianDistribution, SymbolicDistribution +from probabilistic_model.probabilistic_circuit.nx.distributions.distributions import UnivariateContinuousLeaf, \ + UnivariateDiscreteLeaf from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, \ ProductUnit from probabilistic_model.utils import MissingDict @@ -17,9 +19,9 @@ from ....datastructures.pose import Pose from ....datastructures.world import World from ....designator import ActionDesignatorDescription, ObjectDesignatorDescription +from ....failures import ObjectUnreachable, PlanFailure from ....local_transformer import LocalTransformer from ....orm.views import PickUpWithContextView -from ....failures import ObjectUnreachable, PlanFailure class Grasp(SetElement): @@ -125,17 +127,21 @@ def create_model_with_center(self) -> ProbabilisticCircuit: Create a fully factorized gaussian at the center of the map. """ 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))) + centered_model.add_subcircuit(UnivariateContinuousLeaf( + GaussianDistribution(self.relative_x, 0., np.sqrt(self.variance)))) + centered_model.add_subcircuit(UnivariateContinuousLeaf + (GaussianDistribution(self.relative_y, 0., np.sqrt(self.variance)))) grasp_probabilities = MissingDict(float, {int(element): 1 / len(self.grasp.domain.simple_sets) for element in self.grasp.domain.simple_sets}) - centered_model.add_subcircuit(SymbolicDistribution(self.grasp, grasp_probabilities)) + centered_model.add_subcircuit(UnivariateDiscreteLeaf( + SymbolicDistribution(self.grasp, grasp_probabilities))) arm_probabilities = MissingDict(float, {int(element): 1 / len(self.arm.domain.simple_sets) for element in self.arm.domain.simple_sets}) - centered_model.add_subcircuit(SymbolicDistribution(self.arm, arm_probabilities)) + centered_model.add_subcircuit(UnivariateDiscreteLeaf( + SymbolicDistribution(self.arm, arm_probabilities))) return centered_model.probabilistic_circuit def create_model(self) -> ProbabilisticCircuit: @@ -206,7 +212,8 @@ def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: pose = Pose(position, frame=self.object_designator.world_object.tf_frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 - action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], EGrasp(int(grasp))) + action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], + EGrasp(int(grasp))) return action def events_from_occupancy_and_visibility_costmap(self) -> Event: From d09d0976c873591afa0af65e7c55abfe2849b2b9 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Fri, 1 Nov 2024 10:40:37 +0100 Subject: [PATCH 02/12] [Probabilistic Reasoning] Started to work on move and place --- src/pycram/designators/action_designator.py | 34 ++++ .../probabilistic/probabilistic_action.py | 174 +++++++++++++++++- src/pycram/orm/views.py | 49 ++++- 3 files changed, 249 insertions(+), 8 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 2c77c29fe..c57ba1919 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1073,7 +1073,41 @@ class MoveAndPickUpPerformable(ActionAbstract): The grasp to use """ + @with_tree def perform(self): NavigateActionPerformable(self.standing_position).perform() FaceAtPerformable(self.object_designator.pose).perform() PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() + +@dataclass +class MoveAndPlacePerformable(ActionAbstract): + """ + Navigate to `standing_position`, then turn towards the object and pick it up. + """ + + standing_position: Pose + """ + The pose to stand before trying to pick up the object + """ + + object_designator: ObjectDesignatorDescription.Object + """ + The object to pick up + """ + + target_location: Pose + """ + The location to place the object. + """ + + arm: Arms + """ + The arm to use + """ + + @with_tree + def perform(self): + NavigateActionPerformable(self.standing_position).perform() + FaceAtPerformable(self.target_location).perform() + PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() + diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index d56e00a9b..d86957784 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -5,6 +5,7 @@ UnivariateDiscreteLeaf from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, \ ProductUnit +from probabilistic_model.probabilistic_model import ProbabilisticModel from probabilistic_model.utils import MissingDict from random_events.interval import * from random_events.product_algebra import Event, SimpleEvent @@ -40,7 +41,7 @@ class Arms(SetElement): class ProbabilisticAction: """ - Abstract class for probabilistic performables. + Abstract class for performables that have a probabilistic parametrization. """ @dataclass @@ -72,19 +73,18 @@ def __init__(self, policy: Optional[ProbabilisticCircuit] = None): self.policy = policy self.variables = self.Variables(*self.policy.variables) - def default_policy(self) -> ProbabilisticCircuit: + def default_policy(self) -> ProbabilisticModel: """ - Create a default policy for the action. - - :return: The default policy for the action + :return: The default policy for the action. """ raise NotImplementedError def sample_to_action(self, sample: List) -> ActionAbstract: """ Convert a sample from the policy to a performable action. - :param sample: The sample - :return: The action + + :param sample: The sample. + :return: The action. """ raise NotImplementedError @@ -161,6 +161,7 @@ def create_model(self) -> ProbabilisticCircuit: class MoveAndPickUp(ActionDesignatorDescription, ProbabilisticAction): + @dataclass class Variables: arm: Symbolic = Symbolic("arm", Arms) @@ -329,3 +330,162 @@ def batch_rollout(self): # reset world World.current_world.reset_world() + + +class MoveAndPlace(ActionDesignatorDescription, ProbabilisticAction): + + @dataclass + class Variables: + arm: Symbolic = Symbolic("arm", Arms) + relative_x: Continuous = Continuous("relative_x") + relative_y: Continuous = Continuous("relative_y") + + variables: Variables # Type hint variables + + sample_amount: int = 20 + """ + The amount of samples that should be drawn from the policy when iterating over it. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + The object designator that should be picked up. + """ + + arms: List[Arms] + """ + The arms that can be used for the pick up. + """ + + + def default_policy(self) -> ProbabilisticCircuit: + return GaussianCostmapModel().create_model() + + def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: + """ + Convert a sample from the underlying distribution to a performable action. + :param sample: The sample + :return: action + """ + arm, grasp, relative_x, relative_y = sample + position = [relative_x, relative_y, 0.] + pose = Pose(position, frame=self.object_designator.world_object.tf_frame) + standing_position = LocalTransformer().transform_pose(pose, "map") + standing_position.position.z = 0 + action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], + EGrasp(int(grasp))) + return action + + def events_from_occupancy_and_visibility_costmap(self) -> Event: + """ + Create events from the occupancy and visibility costmap. + + :return: The events that can be used as evidence for the model. + """ + + # create occupancy and visibility costmap for the target object + ocm = OccupancyCostmap(distance_to_obstacle=0.3, from_ros=False, size=200, resolution=0.1, + origin=self.object_designator.pose) + vcm = VisibilityCostmap(min_height=1.27, max_height=1.69, + size=200, resolution=0.1, origin=self.object_designator.pose) + mcm = ocm + vcm + + # convert rectangles to events + events = [] + for rectangle in mcm.partitioning_rectangles(): + event = SimpleEvent({self.variables.relative_x: open(rectangle.x_lower, rectangle.x_upper), + self.variables.relative_y: open(rectangle.y_lower, rectangle.y_upper)}) + events.append(event) + return Event(*events) + + def ground_model(self, model: Optional[ProbabilisticCircuit] = None, + event: Optional[Event] = None) -> ProbabilisticCircuit: + """ + Ground the model to the current evidence. + + :param model: The model that should be grounded. If None, the policy is used. + :param event: The events that should be used as evidence. If None, the occupancy costmap is used. + :return: The grounded model + """ + + if model is None: + model = self.policy + if event is None: + event = self.events_from_occupancy_and_visibility_costmap() + + result, probability = model.conditional(event) + + if probability == 0: + raise ObjectUnreachable("No possible locations found") + + return result + + def iter_with_mode(self) -> Iterator[MoveAndPickUpPerformable]: + """ + Generate performables by sampling from the mode of the policy conditioned on visibility and occupancy. + """ + model = self.ground_model() + modes, _ = model.mode() + model = self.ground_model(model, modes) + samples = model.sample(self.sample_amount) + + for sample in samples: + action = self.sample_to_action(sample) + yield action + + def __iter__(self) -> Iterator[MoveAndPickUpPerformable]: + """ + Generate performables by sampling from the policy conditioned on visibility and occupancy. + """ + model = self.ground_model(self.policy, self.events_from_occupancy_and_visibility_costmap()) + samples = model.sample(self.sample_amount) + likelihoods = model.likelihood(samples) + + # sort samples by likelihood + samples = [x for _, x in sorted(zip(likelihoods, samples), key=lambda pair: pair[0], reverse=True)] + + for sample in samples: + action = self.sample_to_action(sample) + yield action + + def iterate_without_occupancy_costmap(self) -> Iterator[MoveAndPickUpPerformable]: + """ + Generate performables by sampling from the policy without conditioning on visibility and occupancy. + """ + samples = self.policy.sample(self.sample_amount) + for sample in samples: + action = self.sample_to_action(sample) + yield action + + @staticmethod + def query_for_database(): + view = PickUpWithContextView + query = (select(view.arm, view.grasp, view.relative_x, view.relative_y) + .where(view.status == TaskStatus.SUCCEEDED)) + return query + + def batch_rollout(self): + """ + Try the policy without conditioning on visibility and occupancy and count the successful tries. + """ + + # initialize statistics + successful_tries = 0 + total_tries = 0 + + # create the progress bar + progress_bar = tqdm.tqdm(iter(self.iterate_without_occupancy_costmap()), total=self.sample_amount) + + for action in progress_bar: + total_tries += 1 + try: + action.perform() + successful_tries += 1 + except PlanFailure as p: + pass + + # update progress bar + progress_bar.set_postfix({"Success Probability": successful_tries / total_tries}) + + # reset world + World.current_world.reset_world() \ No newline at end of file diff --git a/src/pycram/orm/views.py b/src/pycram/orm/views.py index 999a4aabe..60760ab89 100644 --- a/src/pycram/orm/views.py +++ b/src/pycram/orm/views.py @@ -3,7 +3,7 @@ import sqlalchemy.orm from sqlalchemy import table, inspect, event, select, engine, MetaData, Select, TableClause, ExecutableDDLElement from sqlalchemy.ext.compiler import compiles -from .action_designator import PickUpAction +from .action_designator import PickUpAction, PlaceAction from .base import Position, RobotState, Pose, Base, Quaternion from .object_designator import Object from .tasktree import TaskTreeNode @@ -148,3 +148,50 @@ class PickUpWithContextView(base): quaternion_w: Mapped[float] = __table__.c.w obj_type: Mapped[str] = __table__.c.obj_type status: Mapped[str] = __table__.c.status + + +class PlaceWithContextView(base): + """ + View for pickup performables 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 + """ + + __table__ = view("PlaceWithContextView", Base.metadata, + (select(PlaceAction.id, PlaceAction.arm, RobotState.torso_height, + (__robot_position.x-__object_position.x).label("relative_x"), + (__robot_position.y-__object_position.y).label("relative_y"), Quaternion.x, Quaternion.y, + Quaternion.z, Quaternion.w, Object.obj_type, TaskTreeNode.status) + .join(TaskTreeNode.action.of_type(PlaceAction)) + .join(PlaceAction.robot_state) + .join(__robot_pose, RobotState.pose) + .join(__robot_position, __robot_pose.position) + .join(Pose.orientation) + .join(PlaceAction.object) + .join(Object.pose) + .join(__object_position, Pose.position))) + + id: Mapped[int] = __table__.c.id + arm: Mapped[str] = __table__.c.arm + torso_height: Mapped[float] = __table__.c.torso_height + relative_x: Mapped[float] = column_property(__table__.c.relative_x) + relative_y: Mapped[float] = column_property(__table__.c.relative_y) + quaternion_x: Mapped[float] = __table__.c.x + quaternion_y: Mapped[float] = __table__.c.y + quaternion_z: Mapped[float] = __table__.c.z + quaternion_w: Mapped[float] = __table__.c.w + obj_type: Mapped[str] = __table__.c.obj_type + status: Mapped[str] = __table__.c.status From 7e99e300b5f66c8754977438c7afe331c182fb3e Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Tue, 5 Nov 2024 17:17:42 +0100 Subject: [PATCH 03/12] [Probabilistic Reasoning] Test for move and place performable --- .../probabilistic/probabilistic_action.py | 144 +++++------------- test/test_move_and_pick_up.py | 25 +-- 2 files changed, 47 insertions(+), 122 deletions(-) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index d86957784..f9c1100c6 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -1,20 +1,17 @@ -import numpy as np +from dataclasses import fields + import tqdm -from probabilistic_model.distributions import GaussianDistribution, SymbolicDistribution -from probabilistic_model.probabilistic_circuit.nx.distributions.distributions import UnivariateContinuousLeaf, \ - UnivariateDiscreteLeaf -from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit, \ - ProductUnit +from probabilistic_model.probabilistic_circuit.nx.helper import fully_factorized +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit from probabilistic_model.probabilistic_model import ProbabilisticModel -from probabilistic_model.utils import MissingDict from random_events.interval import * from random_events.product_algebra import Event, SimpleEvent from random_events.set import SetElement -from random_events.variable import Symbolic, Continuous +from random_events.variable import Symbolic, Continuous, Variable from sqlalchemy import select from typing_extensions import Optional, List, Iterator -from ...action_designator import MoveAndPickUpPerformable, ActionAbstract +from ...action_designator import MoveAndPickUpPerformable, ActionAbstract, MoveAndPlacePerformable from ....costmaps import OccupancyCostmap, VisibilityCostmap from ....datastructures.enums import Arms as EArms, Grasp as EGrasp, TaskStatus from ....datastructures.pose import Pose @@ -22,7 +19,7 @@ from ....designator import ActionDesignatorDescription, ObjectDesignatorDescription from ....failures import ObjectUnreachable, PlanFailure from ....local_transformer import LocalTransformer -from ....orm.views import PickUpWithContextView +from ....orm.views import PickUpWithContextView, PlaceWithContextView class Grasp(SetElement): @@ -68,16 +65,10 @@ class Variables: """ def __init__(self, policy: Optional[ProbabilisticCircuit] = None): + self.variables = self.Variables() if policy is None: policy = self.default_policy() self.policy = policy - self.variables = self.Variables(*self.policy.variables) - - def default_policy(self) -> ProbabilisticModel: - """ - :return: The default policy for the action. - """ - raise NotImplementedError def sample_to_action(self, sample: List) -> ActionAbstract: """ @@ -88,76 +79,21 @@ def sample_to_action(self, sample: List) -> ActionAbstract: """ raise NotImplementedError - -class GaussianCostmapModel: - """ - Class that generates a Gaussian Costmap around the center of an object. The costmap cuts out a square in the middle - that has side lengths given by ``distance_to_center``. - """ - - distance_to_center: float - """ - The side length of the cut out square. - """ - - variance: float - """ - The variance of the distributions involved - """ - - relative_x = Continuous("relative_x") - relative_y = Continuous("relative_y") - grasp = Symbolic("grasp", Grasp) - arm = Symbolic("arm", Arms) - - def __init__(self, distance_to_center: float = 0.2, variance: float = 0.5): - self.distance_to_center = distance_to_center - self.variance = variance - - def center_event(self) -> Event: - """ - Create an event that describes the center of the map. + def all_variables(self) -> SortedSet: """ - return SimpleEvent({self.relative_x: open(-self.distance_to_center, self.distance_to_center), - self.relative_y: open(-self.distance_to_center, - self.distance_to_center)}).as_composite_set() - - def create_model_with_center(self) -> ProbabilisticCircuit: - """ - Create a fully factorized gaussian at the center of the map. + :return: All variables of the action. """ - centered_model = ProductUnit() - centered_model.add_subcircuit(UnivariateContinuousLeaf( - GaussianDistribution(self.relative_x, 0., np.sqrt(self.variance)))) - centered_model.add_subcircuit(UnivariateContinuousLeaf - (GaussianDistribution(self.relative_y, 0., np.sqrt(self.variance)))) - - grasp_probabilities = MissingDict(float, {int(element): 1 / len(self.grasp.domain.simple_sets) for element in - self.grasp.domain.simple_sets}) + return SortedSet([getattr(self.variables, field.name) for field in fields(self.variables) + if issubclass(field.type, Variable)]) - centered_model.add_subcircuit(UnivariateDiscreteLeaf( - SymbolicDistribution(self.grasp, grasp_probabilities))) - - arm_probabilities = MissingDict(float, {int(element): 1 / len(self.arm.domain.simple_sets) for element in - self.arm.domain.simple_sets}) - centered_model.add_subcircuit(UnivariateDiscreteLeaf( - SymbolicDistribution(self.arm, arm_probabilities))) - return centered_model.probabilistic_circuit - - def create_model(self) -> ProbabilisticCircuit: + def default_policy(self) -> ProbabilisticCircuit: """ - Create a gaussian model that assumes mass everywhere besides the center square. - - :return: The probabilistic circuit + :return: The default policy for the action. """ - centered_model = self.create_model_with_center() - outer_event = self.center_event().complement() - limiting_event = SimpleEvent({self.relative_x: open(-2, 2), - self.relative_y: open(-2, 2)}).as_composite_set() - event = outer_event & limiting_event - # go.Figure(event.plot()).show() - result, _ = centered_model.conditional(event) - return result + means = {v: 0 for v in self.all_variables() if v.is_numeric} + variances = {v: 1 for v in self.all_variables() if v.is_numeric} + model = fully_factorized(self.all_variables(), means, variances) + return model class MoveAndPickUp(ActionDesignatorDescription, ProbabilisticAction): @@ -169,7 +105,7 @@ class Variables: relative_x: Continuous = Continuous("relative_x") relative_y: Continuous = Continuous("relative_y") - variables: Variables # Type hint variables + variables = Variables() # Type hint variables sample_amount: int = 20 """ @@ -199,15 +135,7 @@ def __init__(self, object_designator: ObjectDesignatorDescription.Object, arms: self.arms = arms self.grasps = grasps - def default_policy(self) -> ProbabilisticCircuit: - return GaussianCostmapModel().create_model() - def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: - """ - Convert a sample from the underlying distribution to a performable action. - :param sample: The sample - :return: action - """ arm, grasp, relative_x, relative_y = sample position = [relative_x, relative_y, 0.] pose = Pose(position, frame=self.object_designator.world_object.tf_frame) @@ -336,12 +264,9 @@ class MoveAndPlace(ActionDesignatorDescription, ProbabilisticAction): @dataclass class Variables: - arm: Symbolic = Symbolic("arm", Arms) relative_x: Continuous = Continuous("relative_x") relative_y: Continuous = Continuous("relative_y") - variables: Variables # Type hint variables - sample_amount: int = 20 """ The amount of samples that should be drawn from the policy when iterating over it. @@ -352,27 +277,27 @@ class Variables: The object designator that should be picked up. """ + target_location: Pose + arms: List[Arms] """ The arms that can be used for the pick up. """ + def __init__(self, object_designator: ObjectDesignatorDescription.Object, + target_location: Pose, policy: Optional[ProbabilisticCircuit] = None): + ActionDesignatorDescription.__init__(self) + ProbabilisticAction.__init__(self, policy) + self.object_designator = object_designator + self.target_location = target_location - def default_policy(self) -> ProbabilisticCircuit: - return GaussianCostmapModel().create_model() - - def sample_to_action(self, sample: List) -> MoveAndPickUpPerformable: - """ - Convert a sample from the underlying distribution to a performable action. - :param sample: The sample - :return: action - """ + def sample_to_action(self, sample: List) -> MoveAndPlacePerformable: arm, grasp, relative_x, relative_y = sample position = [relative_x, relative_y, 0.] pose = Pose(position, frame=self.object_designator.world_object.tf_frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 - action = MoveAndPickUpPerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], + action = MoveAndPlacePerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], EGrasp(int(grasp))) return action @@ -385,9 +310,9 @@ def events_from_occupancy_and_visibility_costmap(self) -> Event: # create occupancy and visibility costmap for the target object ocm = OccupancyCostmap(distance_to_obstacle=0.3, from_ros=False, size=200, resolution=0.1, - origin=self.object_designator.pose) + origin=self.target_location) vcm = VisibilityCostmap(min_height=1.27, max_height=1.69, - size=200, resolution=0.1, origin=self.object_designator.pose) + size=200, resolution=0.1, origin=self.target_location) mcm = ocm + vcm # convert rectangles to events @@ -459,9 +384,8 @@ def iterate_without_occupancy_costmap(self) -> Iterator[MoveAndPickUpPerformable @staticmethod def query_for_database(): - view = PickUpWithContextView - query = (select(view.arm, view.grasp, view.relative_x, view.relative_y) - .where(view.status == TaskStatus.SUCCEEDED)) + view = PlaceWithContextView + query = (select(view.arm, view.relative_x, view.relative_y).where(view.status == TaskStatus.SUCCEEDED)) return query def batch_rollout(self): @@ -488,4 +412,4 @@ def batch_rollout(self): progress_bar.set_postfix({"Success Probability": successful_tries / total_tries}) # reset world - World.current_world.reset_world() \ No newline at end of file + World.current_world.reset_world() diff --git a/test/test_move_and_pick_up.py b/test/test_move_and_pick_up.py index 2c4268950..c6a6c6c50 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_move_and_pick_up.py @@ -2,28 +2,20 @@ import unittest import numpy as np +from random_events.variable import Continuous, Symbolic +from sortedcontainers import SortedSet from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import ObjectType, Arms, Grasp from pycram.designator import ObjectDesignatorDescription from pycram.designators.action_designator import MoveTorsoActionPerformable from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPickUp, - GaussianCostmapModel) + Arms as PMArms, + Grasp as PMGrasp) from pycram.failures import PlanFailure from pycram.process_module import simulated_robot -class GaussianCostmapModelTestCase(unittest.TestCase): - - def test_create_model(self): - gcm = GaussianCostmapModel() - model = gcm.create_model() - self.assertEqual(model.probability(gcm.center_event()), 0) - self.assertEqual(len(model.variables), 4) - # p_xy = model.marginal([gcm.relative_x, gcm.relative_y]) - # go.Figure(p_xy.plot(), p_xy.plotly_layout()).show() - - class MoveAndPickUpTestCase(BulletWorldTestCase): @classmethod @@ -32,6 +24,15 @@ def setUpClass(cls): np.random.seed(69) random.seed(69) + def test_variables(self): + object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], + grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP]) + result = SortedSet([Symbolic("arm", PMArms), Symbolic("grasp", PMGrasp), + Continuous("relative_x"), Continuous("relative_y")]) + all_variables = move_and_pick.all_variables() + self.assertEqual(all_variables, result) + def test_grounding(self): object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], From 7c09b6c52265bbded81518fa4b916575dc6cc793 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 13:29:39 +0100 Subject: [PATCH 04/12] [Probabilistic Reasoning] Investigating iter with mode --- src/pycram/designators/action_designator.py | 2 +- .../probabilistic/probabilistic_action.py | 46 ++++++++++++------- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index c57ba1919..23aad0f5d 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1079,6 +1079,7 @@ def perform(self): FaceAtPerformable(self.object_designator.pose).perform() PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() + @dataclass class MoveAndPlacePerformable(ActionAbstract): """ @@ -1110,4 +1111,3 @@ def perform(self): NavigateActionPerformable(self.standing_position).perform() FaceAtPerformable(self.target_location).perform() PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() - diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index f9c1100c6..35a5b0adc 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -1,13 +1,16 @@ -from dataclasses import fields +import json +from dataclasses import fields, dataclass +import random_events import tqdm from probabilistic_model.probabilistic_circuit.nx.helper import fully_factorized from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit from probabilistic_model.probabilistic_model import ProbabilisticModel -from random_events.interval import * +from random_events.interval import SimpleInterval from random_events.product_algebra import Event, SimpleEvent from random_events.set import SetElement from random_events.variable import Symbolic, Continuous, Variable +from sortedcontainers import SortedSet from sqlalchemy import select from typing_extensions import Optional, List, Iterator @@ -95,6 +98,12 @@ def default_policy(self) -> ProbabilisticCircuit: model = fully_factorized(self.all_variables(), means, variances) return model + def evidence_from_belief_state(self) -> Event: + """ + :return: the evidence from the belief state that influences the distribution. + """ + raise NotImplementedError + class MoveAndPickUp(ActionDesignatorDescription, ProbabilisticAction): @@ -105,7 +114,7 @@ class Variables: relative_x: Continuous = Continuous("relative_x") relative_y: Continuous = Continuous("relative_y") - variables = Variables() # Type hint variables + variables: Variables sample_amount: int = 20 """ @@ -162,8 +171,8 @@ def events_from_occupancy_and_visibility_costmap(self) -> Event: # convert rectangles to events events = [] for rectangle in mcm.partitioning_rectangles(): - event = SimpleEvent({self.variables.relative_x: open(rectangle.x_lower, rectangle.x_upper), - self.variables.relative_y: open(rectangle.y_lower, rectangle.y_upper)}) + event = SimpleEvent({self.variables.relative_x: random_events.interval.open(rectangle.x_lower, rectangle.x_upper), + self.variables.relative_y: random_events.interval.open(rectangle.y_lower, rectangle.y_upper)}) events.append(event) return Event(*events) @@ -194,8 +203,14 @@ def iter_with_mode(self) -> Iterator[MoveAndPickUpPerformable]: Generate performables by sampling from the mode of the policy conditioned on visibility and occupancy. """ model = self.ground_model() - modes, _ = model.mode() - model = self.ground_model(model, modes) + + modes, log_max = model.log_mode(check_determinism=False) + print(modes) + print(log_max) + model, _ = model.conditional(modes) + + print(model) + samples = model.sample(self.sample_amount) for sample in samples: @@ -267,6 +282,8 @@ class Variables: relative_x: Continuous = Continuous("relative_x") relative_y: Continuous = Continuous("relative_y") + variables: Variables + sample_amount: int = 20 """ The amount of samples that should be drawn from the policy when iterating over it. @@ -292,13 +309,13 @@ def __init__(self, object_designator: ObjectDesignatorDescription.Object, self.target_location = target_location def sample_to_action(self, sample: List) -> MoveAndPlacePerformable: - arm, grasp, relative_x, relative_y = sample + relative_x, relative_y = sample position = [relative_x, relative_y, 0.] - pose = Pose(position, frame=self.object_designator.world_object.tf_frame) + pose = Pose(position, frame=self.target_location.frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 - action = MoveAndPlacePerformable(standing_position, self.object_designator, EArms[Arms(int(arm)).name], - EGrasp(int(grasp))) + action = MoveAndPlacePerformable(standing_position, self.object_designator, + self.target_location, EArms.BOTH) return action def events_from_occupancy_and_visibility_costmap(self) -> Event: @@ -318,8 +335,8 @@ def events_from_occupancy_and_visibility_costmap(self) -> Event: # convert rectangles to events events = [] for rectangle in mcm.partitioning_rectangles(): - event = SimpleEvent({self.variables.relative_x: open(rectangle.x_lower, rectangle.x_upper), - self.variables.relative_y: open(rectangle.y_lower, rectangle.y_upper)}) + event = SimpleEvent({self.variables.relative_x: random_events.interval.open(rectangle.x_lower, rectangle.x_upper), + self.variables.relative_y: random_events.interval.open(rectangle.y_lower, rectangle.y_upper)}) events.append(event) return Event(*events) @@ -389,9 +406,6 @@ def query_for_database(): return query def batch_rollout(self): - """ - Try the policy without conditioning on visibility and occupancy and count the successful tries. - """ # initialize statistics successful_tries = 0 From 296801b7acd21deeeefcbbc3fbd52fa232891027 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 13:46:33 +0100 Subject: [PATCH 05/12] [Probabilistic Reasoning] Iter with mode is now test case to reduce flakyness --- .../probabilistic/probabilistic_action.py | 9 ++------- test/test_move_and_pick_up.py | 4 ++-- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index 35a5b0adc..c02ab17f9 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -162,7 +162,7 @@ def events_from_occupancy_and_visibility_costmap(self) -> Event: """ # create occupancy and visibility costmap for the target object - ocm = OccupancyCostmap(distance_to_obstacle=0.3, from_ros=False, size=200, resolution=0.1, + ocm = OccupancyCostmap(distance_to_obstacle=0.4, from_ros=False, size=200, resolution=0.1, origin=self.object_designator.pose) vcm = VisibilityCostmap(min_height=1.27, max_height=1.69, size=200, resolution=0.1, origin=self.object_designator.pose) @@ -205,12 +205,7 @@ def iter_with_mode(self) -> Iterator[MoveAndPickUpPerformable]: model = self.ground_model() modes, log_max = model.log_mode(check_determinism=False) - print(modes) - print(log_max) model, _ = model.conditional(modes) - - print(model) - samples = model.sample(self.sample_amount) for sample in samples: @@ -326,7 +321,7 @@ def events_from_occupancy_and_visibility_costmap(self) -> Event: """ # create occupancy and visibility costmap for the target object - ocm = OccupancyCostmap(distance_to_obstacle=0.3, from_ros=False, size=200, resolution=0.1, + ocm = OccupancyCostmap(distance_to_obstacle=0.4, from_ros=False, size=200, resolution=0.1, origin=self.target_location) vcm = VisibilityCostmap(min_height=1.27, max_height=1.69, size=200, resolution=0.1, origin=self.target_location) diff --git a/test/test_move_and_pick_up.py b/test/test_move_and_pick_up.py index c6a6c6c50..1a2008278 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_move_and_pick_up.py @@ -43,12 +43,12 @@ def test_grounding(self): self.assertTrue(event.is_disjoint()) self.assertIsNotNone(model) - def test_move_and_pick_up(self): + def test_move_and_pick_up_with_mode(self): object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP]) with simulated_robot: - for action in move_and_pick: + for action in move_and_pick.iter_with_mode(): try: MoveTorsoActionPerformable(0.3).perform() action.perform() From e2efb4fa7ca223ab8921fc7a302c17b95baf3343 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 14:34:10 +0100 Subject: [PATCH 06/12] [Probabilistic Reasoning] First version of move and place --- .../probabilistic/probabilistic_action.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index c02ab17f9..6ca5972e5 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -305,12 +305,12 @@ def __init__(self, object_designator: ObjectDesignatorDescription.Object, def sample_to_action(self, sample: List) -> MoveAndPlacePerformable: relative_x, relative_y = sample - position = [relative_x, relative_y, 0.] + position = [relative_x + self.target_location.position.x, relative_y + self.target_location.position.y, 0.] pose = Pose(position, frame=self.target_location.frame) standing_position = LocalTransformer().transform_pose(pose, "map") standing_position.position.z = 0 action = MoveAndPlacePerformable(standing_position, self.object_designator, - self.target_location, EArms.BOTH) + self.target_location, EArms.LEFT) return action def events_from_occupancy_and_visibility_costmap(self) -> Event: @@ -357,12 +357,12 @@ def ground_model(self, model: Optional[ProbabilisticCircuit] = None, return result - def iter_with_mode(self) -> Iterator[MoveAndPickUpPerformable]: + def iter_with_mode(self) -> Iterator[MoveAndPlacePerformable]: """ Generate performables by sampling from the mode of the policy conditioned on visibility and occupancy. """ model = self.ground_model() - modes, _ = model.mode() + modes, _ = model.log_mode(check_determinism=False) model = self.ground_model(model, modes) samples = model.sample(self.sample_amount) From 9eacd183509b700defe1eea85f132a29309f122a Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 14:35:03 +0100 Subject: [PATCH 07/12] [Probabilistic Reasoning] First version of move and place --- test/test_move_and_place.py | 44 +++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 test/test_move_and_place.py diff --git a/test/test_move_and_place.py b/test/test_move_and_place.py new file mode 100644 index 000000000..74cec82c4 --- /dev/null +++ b/test/test_move_and_place.py @@ -0,0 +1,44 @@ +import random +import unittest + +import numpy as np + +from bullet_world_testcase import BulletWorldTestCase +from pycram.datastructures.enums import ObjectType, Arms, Grasp +from pycram.datastructures.pose import Pose +from pycram.designator import ObjectDesignatorDescription +from pycram.designators.action_designator import MoveTorsoActionPerformable, NavigateActionPerformable, \ + PickUpActionPerformable +from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPlace) +from pycram.failures import PlanFailure +from pycram.process_module import simulated_robot + + +class MoveAndPlaceTestCase(BulletWorldTestCase): + + @classmethod + def setUpClass(cls): + super().setUpClass() + np.random.seed(69) + random.seed(69) + + def test_with_mode(self): + object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + target_location = Pose([1.3, 1, 0.9], [0, 0, 0, 1]) + designator = MoveAndPlace(object_designator, target_location) + + with simulated_robot: + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_designator, Arms.LEFT, Grasp.FRONT).perform() + with simulated_robot: + for action in designator: + try: + action.perform() + return # Success + except PlanFailure as e: + continue + raise AssertionError("No action performed successfully.") + +if __name__ == '__main__': + unittest.main() From c3b70c2db296577cb73328e50b7e996a0cd416ab Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 14:54:17 +0100 Subject: [PATCH 08/12] [Probabilistic Reasoning] Updated requirements --- requirements.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/requirements.txt b/requirements.txt index d4a612b1d..7c0ce41e2 100644 --- a/requirements.txt +++ b/requirements.txt @@ -20,6 +20,6 @@ gTTS~=2.5.3 dm_control trimesh deprecated -probabilistic_model>=5.1.0 -random_events>=3.0.7 +probabilistic_model>=6.0.0 +random_events>=3.1.2 sympy From bf0604d7d4a05ba0f2e78a32ec30cb57eb04039b Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 15:24:35 +0100 Subject: [PATCH 09/12] [Probabilistic Reasoning] Fixed bug in doc --- examples/improving_actions.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/improving_actions.md b/examples/improving_actions.md index e3e4cecf0..df15d9112 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -90,7 +90,7 @@ fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value]) print(world.current_world) p_xy = fpa.policy.marginal([fpa.variables.relative_x, fpa.variables.relative_y]) -fig = go.Figure(p_xy.root.plot(), p_xy.root.plotly_layout()) +fig = go.Figure(p_xy.plot(), p_xy.plotly_layout()) fig.update_layout(title="Marginal View of relative x and y position of the robot with respect to the object.") fig.show() ``` From 410dd80966b49a2e0bde7747e45e9d5b3ac32325 Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Wed, 6 Nov 2024 15:30:58 +0100 Subject: [PATCH 10/12] [Probabilistic Reasoning] Fixed bug in doc --- examples/improving_actions.md | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/improving_actions.md b/examples/improving_actions.md index df15d9112..0ccc039ac 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -128,7 +128,6 @@ variables = infer_variables_from_dataframe(samples, scale_continuous_types=False min_likelihood_improvement = 0.) model = JPT(variables, min_samples_leaf=25) model.fit(samples) -model = model.probabilistic_circuit print(model) ``` From cd9ac3c3a44f276a34fec8f3db6e2d053751caac Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck Date: Thu, 7 Nov 2024 11:15:46 +0100 Subject: [PATCH 11/12] [Probabilistic Reasoning] Fixed bug in example --- examples/improving_actions.md | 4 +++- requirements.txt | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/improving_actions.md b/examples/improving_actions.md index 0ccc039ac..cdd5eabfc 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -5,7 +5,7 @@ jupyter: extension: .md format_name: markdown format_version: '1.3' - jupytext_version: 1.16.2 + jupytext_version: 1.16.4 kernelspec: display_name: Python 3 language: python @@ -33,6 +33,7 @@ import pandas as pd import sqlalchemy.orm import plotly +from probabilistic_model.probabilistic_circuit.nx.probabilistic_circuit import ProbabilisticCircuit plotly.offline.init_notebook_mode() import plotly.graph_objects as go @@ -128,6 +129,7 @@ variables = infer_variables_from_dataframe(samples, scale_continuous_types=False min_likelihood_improvement = 0.) model = JPT(variables, min_samples_leaf=25) model.fit(samples) +model = ProbabilisticCircuit.from_other(model) print(model) ``` diff --git a/requirements.txt b/requirements.txt index 7c0ce41e2..8cabe8391 100644 --- a/requirements.txt +++ b/requirements.txt @@ -20,6 +20,6 @@ gTTS~=2.5.3 dm_control trimesh deprecated -probabilistic_model>=6.0.0 +probabilistic_model>=6.0.2 random_events>=3.1.2 sympy From 93f5d8070039dfdfe327e61df9e4dfe744832b9b Mon Sep 17 00:00:00 2001 From: Tom Schierenbeck <60885903+tomsch420@users.noreply.github.com> Date: Thu, 14 Nov 2024 17:30:59 +0100 Subject: [PATCH 12/12] Update probabilistic_action.py --- .../probabilistic/probabilistic_action.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index 6ca5972e5..cd48cd228 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -290,6 +290,9 @@ class Variables: """ target_location: Pose + """ + The position to place the object. + """ arms: List[Arms] """