Merging my costmap updates, including changes to costmap creation, changes to the gaussian costmap generation, as well as the addition of DirectionalCostmaps

demos/pycram_bullet_world_demo/demo.py

@@ -1,3 +1,4 @@
+from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher
 from pycram.worlds.bullet_world import BulletWorld
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
@@ -8,11 +9,11 @@
 from pycram.object_descriptors.urdf import ObjectDescription
 from pycram.world_concepts.world_object import Object
 from pycram.datastructures.dataclasses import Color
-from pycram.ros.viz_marker_publisher import VizMarkerPublisher
 
 extension = ObjectDescription.get_file_extension()
 
-world = BulletWorld(WorldMode.GUI)
+world = BulletWorld(WorldMode.DIRECT)
+viz = VizMarkerPublisher()
 
 robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0]))
 apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}")

src/pycram/datastructures/dataclasses.py

@@ -4,6 +4,8 @@
 from copy import deepcopy, copy
 from dataclasses import dataclass
 
+from matplotlib import pyplot as plt
+from std_msgs.msg import ColorRGBA
 from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING
 
 from .enums import JointType, Shape, VirtualMobileBaseJointName
@@ -86,6 +88,33 @@ def get_rgb(self) -> List[float]:
         """
         return [self.R, self.G, self.B]
 
+    @staticmethod
+    def gaussian_color_map(value, min_val=0.0, max_val=0.4, colormap='viridis'):
+        """
+        Maps a Gaussian-scaled value to a `ColorRGBA` object for color visualization.
+
+        This function normalizes `value` within a specified range and maps it to an RGB color
+        using a specified colormap. The alpha (opacity) is set to full (1.0).
+
+        Args:
+            value (float): The Gaussian value to map, expected within `min_val` and `max_val`.
+            min_val (float, optional): Minimum value of the mapping range. Defaults to 0.0.
+            max_val (float, optional): Maximum value of the mapping range. Defaults to 0.4.
+            colormap (str, optional): The name of the Matplotlib colormap to use. Defaults to 'viridis'.
+
+        Returns:
+            ColorRGBA: A `ColorRGBA` object where the RGB channels correspond to the color
+                       mapped from `value` using the specified colormap, and alpha is 1.0.
+        """
+        clamped_value = max(min(value, max_val), min_val)
+        normalized_value = (clamped_value - min_val) / (max_val - min_val)
+
+        color_map = plt.get_cmap(colormap)
+        rgba_color = color_map(normalized_value)
+
+        return ColorRGBA(r=rgba_color[0], g=rgba_color[1], b=rgba_color[2], a=1.0)
+
+
 
 @dataclass
 class AxisAlignedBoundingBox:

src/pycram/datastructures/enums.py

@@ -51,6 +51,8 @@ class Grasp(int, Enum):
     LEFT = 1
     RIGHT = 2
     TOP = 3
+    BOTTOM = 4
+    BACK = 5
 
 
 class ObjectType(int, Enum):

src/pycram/designators/action_designator.py

@@ -886,7 +886,7 @@ def perform(self) -> None:
         ParkArmsActionPerformable(Arms.BOTH).perform()
         try:
             place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(),
-                                        reachable_arm=self.arm).resolve()
+                                        reachable_arm=self.arm, used_grasp=Grasp.FRONT).resolve()
         except StopIteration:
             raise ReachabilityFailure(
                 f"No location found from where the robot can reach the target location: {self.target_location}")

src/pycram/designators/location_designator.py

@@ -8,8 +8,8 @@
 from ..local_transformer import LocalTransformer
 from ..world_reasoning import link_pose_for_joint_config
 from ..designator import DesignatorError, LocationDesignatorDescription
-from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap
-from ..datastructures.enums import JointType, Arms
+from ..costmaps import OccupancyCostmap, VisibilityCostmap, SemanticCostmap, GaussianCostmap, DirectionalCostmap
+from ..datastructures.enums import JointType, Arms, Grasp
 from ..pose_generator_and_validator import PoseGenerator, visibility_validator, reachability_validator
 from ..robot_description import RobotDescription
 from ..datastructures.pose import Pose
@@ -102,7 +102,7 @@ def __iter__(self) -> Iterable[Location]:
 
 class CostmapLocation(LocationDesignatorDescription):
     """
-    Uses Costmaps to create locations for complex constrains
+    Uses costmaps to create locations based on complex constraints, such as reachability and visibility.
     """
 
     @dataclasses.dataclass
@@ -115,7 +115,8 @@ class Location(LocationDesignatorDescription.Location):
     def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object],
                  reachable_for: Optional[ObjectDesignatorDescription.Object] = None,
                  visible_for: Optional[ObjectDesignatorDescription.Object] = None,
-                 reachable_arm: Optional[Arms] = None, resolver: Optional[Callable] = None):
+                 reachable_arm: Optional[Arms] = None, resolver: Optional[Callable] = None,
+                 used_grasp: Optional[Grasp] = None,  object_in_hand: Optional[ObjectDesignatorDescription.Object] = None):
         """
         Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or
         visible. In case of reachable the resolved location contains a list of arms with which the location is reachable.
@@ -125,12 +126,16 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object],
         :param visible_for: Object for which the visibility should be calculated, usually a robot
         :param reachable_arm: An optional arm with which the target should be reached
         :param resolver: An alternative specialized_designators that returns a resolved location for the given input of this description
+        :param used_grasp: The grasp that should be used for the target
+        :param object_in_hand: The object that is in the hand of the robot
         """
         super().__init__(resolver)
         self.target: Union[Pose, ObjectDesignatorDescription.Object] = target
         self.reachable_for: ObjectDesignatorDescription.Object = reachable_for
         self.visible_for: ObjectDesignatorDescription.Object = visible_for
         self.reachable_arm: Optional[Arms] = reachable_arm
+        self.used_grasp: Optional[Grasp] = used_grasp
+        self.object_in_hand: Optional[ObjectDesignatorDescription.Object] = object_in_hand
 
     def ground(self) -> Location:
         """
@@ -142,19 +147,22 @@ def ground(self) -> Location:
 
     def __iter__(self):
         """
-           Generates positions for a given set of constrains from a costmap and returns
-           them. The generation is based of a costmap which itself is the product of
-           merging costmaps, each for a different purpose. In any case an occupancy costmap
-           is used as the base, then according to the given constrains a visibility or
-           gaussian costmap is also merged with this. Once the costmaps are merged,
-           a generator generates pose candidates from the costmap. Each pose candidate
-           is then validated against the constraints given by the designator if all validators
-           pass the pose is considered valid and yielded.
-
-           :yield: An instance of CostmapLocation.Location with a valid position that satisfies the given constraints
-           """
+        Generates positions that satisfy the given constraints from a costmap.
+
+        This method creates a costmap by merging different costmaps, each serving a specific purpose.
+        An occupancy costmap is always used as the base. Depending on the provided constraints,
+        a visibility costmap and/or a Gaussian costmap are also merged with the base costmap.
+
+        Once the costmaps are merged, a pose generator produces candidate poses from the costmap.
+        Each candidate pose is then validated against the specified constraints.
+        If all validators pass, the pose is considered valid and yielded.
+
+        Yields:
+            Location: An instance of `CostmapLocation.Location` containing a valid position that satisfies the given constraints.
+        """
         min_height = RobotDescription.current_robot_description.get_default_camera().minimal_height
         max_height = RobotDescription.current_robot_description.get_default_camera().maximal_height
+
         # This ensures that the costmaps always get a position as their origin.
         if isinstance(self.target, ObjectDesignatorDescription.Object):
             target_pose = self.target.world_object.get_pose()
@@ -165,39 +173,51 @@ def __iter__(self):
         ground_pose = Pose(target_pose.position_as_list())
         ground_pose.position.z = 0
 
-        occupancy = OccupancyCostmap(0.32, False, 200, 0.02, ground_pose)
+        map_resolution = 0.15
+
+        occupancy = OccupancyCostmap(0.32, False, 300, map_resolution, ground_pose)
         final_map = occupancy
 
         if self.reachable_for:
-            gaussian = GaussianCostmap(200, 15, 0.02, ground_pose)
+            gaussian = GaussianCostmap(200, 1.5, map_resolution, ground_pose, True, 0.5)
             final_map += gaussian
+
         if self.visible_for:
-            visible = VisibilityCostmap(min_height, max_height, 200, 0.02, Pose(target_pose.position_as_list()))
+            visible = VisibilityCostmap(min_height, max_height, 200, map_resolution, Pose(target_pose.position_as_list()))
             final_map += visible
 
+        if self.used_grasp is not None and self.used_grasp not in [Grasp.TOP, Grasp.BOTTOM]:
+            directional = DirectionalCostmap(200, self.used_grasp, map_resolution, target_pose,
+                                             self.object_in_hand is not None)
+
+            final_map *= directional
+
+        final_map.publish(weighted=True)
+
         if self.visible_for or self.reachable_for:
             robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object
             test_robot = World.current_world.get_prospection_object_for_object(robot_object)
 
         with UseProspectionWorld():
             for maybe_pose in PoseGenerator(final_map, number_of_samples=600):
-                res = True
+                is_valid = True
                 arms = None
                 if self.visible_for:
                     visible_prospection_object = World.current_world.get_prospection_object_for_object(self.target.world_object)
-                    res = res and visibility_validator(maybe_pose, test_robot, visible_prospection_object,
+                    is_valid = is_valid and visibility_validator(maybe_pose, test_robot, visible_prospection_object,
                                                        World.current_world)
                 if self.reachable_for:
                     hand_links = []
                     for description in RobotDescription.current_robot_description.get_manipulator_chains():
                         hand_links += description.end_effector.links
-                    valid, arms = reachability_validator(maybe_pose, test_robot, target_pose,
+
+                    is_reachable, arms = reachability_validator(maybe_pose, test_robot, target_pose,
                                                          allowed_collision={test_robot: hand_links})
                     if self.reachable_arm:
-                        res = res and valid and self.reachable_arm in arms
+                        is_valid = is_valid and is_reachable and self.reachable_arm in arms
                     else:
-                        res = res and valid
-                if res:
+                        is_valid = is_valid and is_reachable
+                if is_valid:
                     yield self.Location(maybe_pose, arms)
 
 
@@ -245,9 +265,14 @@ def __iter__(self) -> Location:
         # ground_pose = [[self.handle.part_pose[0][0], self.handle.part_pose[0][1], 0], self.handle.part_pose[1]]
         ground_pose = Pose(self.handle.part_pose.position_as_list())
         ground_pose.position.z = 0
-        occupancy = OccupancyCostmap(distance_to_obstacle=0.25, from_ros=False, size=200, resolution=0.02,
+
+        map_resolution = 0.15
+
+        occupancy = OccupancyCostmap(distance_to_obstacle=0.32, from_ros=False, size=300, resolution=map_resolution,
                                      origin=ground_pose)
-        gaussian = GaussianCostmap(200, 15, 0.02, ground_pose)
+
+        gaussian = GaussianCostmap(200, 1.5, map_resolution, ground_pose, True, 0.6)
+
 
         final_map = occupancy + gaussian
 

src/pycram/pose_generator_and_validator.py

@@ -31,11 +31,20 @@ class PoseGenerator:
 
     def __init__(self, costmap: Costmap, number_of_samples=100, orientation_generator=None):
         """
-        :param costmap: The costmap from which poses should be sampled.
-        :param number_of_samples: The number of samples from the costmap that should be returned at max
-        :param orientation_generator: function that generates an orientation given a position and the origin of the costmap
-        """
+        Initializes a PoseGenerator for sampling poses from a given costmap.
+
+        This class generates sampled poses within the specified costmap, using a defined number of samples
+        and an optional orientation generator. If no orientation generator is provided, a default generator
+        is used.
+
+        Args:
+            costmap (Costmap): The costmap from which poses should be sampled.
+            number_of_samples (int, optional): Maximum number of samples to be returned from the costmap. Defaults to 100.
+            orientation_generator (callable, optional): Function that generates an orientation given a position and the origin of the costmap.
 
+        Returns:
+            None
+        """
         if not PoseGenerator.current_orientation_generator:
             PoseGenerator.current_orientation_generator = PoseGenerator.generate_orientation
 
@@ -47,23 +56,40 @@ def __init__(self, costmap: Costmap, number_of_samples=100, orientation_generato
 
     def __iter__(self) -> Iterable:
         """
-        A generator that crates pose candidates from a given costmap. The generator
-        selects the highest 100 values and returns the corresponding positions.
-        Orientations are calculated such that the Robot faces the center of the costmap.
+        Generates pose candidates from a costmap by randomly sampling positions weighted by their values.
+        The number of samples is determined by `self.number_of_samples`. The orientations are calculated
+        such that the robot faces the center of the costmap.
 
-        :Yield: A tuple of position and orientation
+        Yields:
+            Pose: A Pose object containing position and orientation.
         """
+        np.random.seed(42)
 
-        # Determines how many positions should be sampled from the costmap
         if self.number_of_samples == -1:
             self.number_of_samples = self.costmap.map.flatten().shape[0]
-        indices = np.argpartition(self.costmap.map.flatten(), -self.number_of_samples)[-self.number_of_samples:]
-        indices = np.dstack(np.unravel_index(indices, self.costmap.map.shape)).reshape(self.number_of_samples, 2)
+        number_of_samples = min(self.number_of_samples, self.costmap.map.size)
 
-        height = self.costmap.map.shape[0]
-        width = self.costmap.map.shape[1]
+        height, width = self.costmap.map.shape
         center = np.array([height // 2, width // 2])
-        for ind in indices:
+
+        flat_values = self.costmap.map.flatten()
+
+        non_zero_indices = np.nonzero(flat_values)[0]
+        non_zero_weights = flat_values[non_zero_indices]
+        number_of_samples = min(number_of_samples, len(non_zero_indices))
+
+        if non_zero_weights.sum() > 0:
+            sampled_indices = np.random.choice(non_zero_indices, size=number_of_samples, replace=False,
+                                               p=non_zero_weights / non_zero_weights.sum())
+        else:
+            sampled_indices = []
+
+        indices = np.column_stack(np.unravel_index(sampled_indices, self.costmap.map.shape))
+
+        sampled_weights = flat_values[sampled_indices]
+        sorted_indices = indices[np.argsort(-sampled_weights)]
+
+        for ind in sorted_indices:
             if self.costmap.map[ind[0]][ind[1]] == 0:
                 continue
             # The position is calculated by creating a vector from the 2D position in the costmap (given by x and y)