Merge pull request cram2#214 from tomsch420/dev

Move and Place

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
@@ -90,7 +91,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()
 ```
@@ -128,7 +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 = model.probabilistic_circuit
+model = ProbabilisticCircuit.from_other(model)
 print(model)
 ```
 

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.2
+random_events>=3.1.2
 sympy

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:
@@ -827,7 +823,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.
@@ -841,7 +837,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.
@@ -854,7 +850,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.
@@ -868,7 +864,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.
@@ -915,14 +911,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:
         """

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()