pycram.costmaps

pycram.costmaps#

Attributes#

`logger`
`cmap`

Classes#

`Rectangle`	A rectangle that is described by a lower and upper x and y value.
`Costmap`	The base class of all Costmaps which implements the visualization of costmaps
`OccupancyCostmap`	The occupancy Costmap represents a map of the environment where obstacles or
`VisibilityCostmap`	A costmap that represents the visibility of a specific point for every position around
`GaussianCostmap`	Gaussian Costmaps are 2D gaussian distributions around the origin with the given mean and sigma
`SemanticCostmap`	Semantic Costmaps represent a 2D distribution over a link of an Object. An example of this would be a Costmap for a
`AlgebraicSemanticCostmap`	Class for a semantic costmap that is based on an algebraic set-description of the valid area.

Functions#

plot_grid(→ None)

An auxiliary method only used for debugging, it will plot a 2D numpy array using MatplotLib.

Module Contents#

pycram.costmaps.logger#

class pycram.costmaps.Rectangle#

A rectangle that is described by a lower and upper x and y value.

x_lower: float#

x_upper: float#

y_lower: float#

y_upper: float#

translate(x: float, y: float)#: Translate the rectangle by x and y

scale(x_factor: float, y_factor: float)#: Scale the rectangle by x_factor and y_factor

class pycram.costmaps.Costmap#

The base class of all Costmaps which implements the visualization of costmaps in the World.

resolution: float#: The distance in metre in the real-world which is represented by a single entry in the costmap.

height: int#: Height of the costmap.

width: int#: Width of the costmap.

origin: pycram.datastructures.pose.PoseStamped#: Origin pose of the costmap.

map: numpy.ndarray#: Numpy array representing the costmap.

world: semantic_digital_twin.world.World#: The world from which this costmap was created.

vis_ids: typing_extensions.List[int] = []#

visualize() → None#: Visualizes a costmap in the BulletWorld, the visualisation works by subdividing the costmap in rectangles which are then visualized as pybullet visual shapes.

_chunks(lst: typing_extensions.List, n: int) → typing_extensions.Iterator[typing_extensions.List]#

Yield successive n-sized chunks from lst.

Parameters:

lst – The list from which chunks should be yielded
n – Size of the chunks

Returns:

A list of size n from lst

close_visualization() → None#: Removes the visualization from the World.

_find_consectuive_line(start: typing_extensions.Tuple[int, int], map: numpy.ndarray) → int#

Finds the number of consecutive entries in the costmap which are greater than zero.

Parameters:

start – The indices in the costmap from which the consecutive line should be found.
map – The costmap in which the line should be found.

Returns:

The length of the consecutive line of entries greater than zero.

_find_max_box_height(start: typing_extensions.Tuple[int, int], length: int, map: numpy.ndarray) → int#

Finds the maximal height for a rectangle with a given width in a costmap. The method traverses one row at a time and checks if all entries for the given width are greater than zero. If an entry is less or equal than zero the height is returned.

Parameters:

start – The indices in the costmap from which the method should start.
length – The given width for the rectangle
map – The costmap in which should be searched.

Returns:

The height of the rectangle.

merge(other_cm: Costmap) → Costmap#

Merges the values of two costmaps and returns a new costmap that has for every cell the merged values of both inputs. To merge two costmaps they need to fulfill 3 constrains:

They need to have the same size
They need to have the same x and y coordinates in the origin
They need to have the same resolution

If any of these constrains is not fulfilled a ValueError will be raised.

Parameters:: other_cm – The other costmap with which this costmap should be merged.
Returns:: A new costmap that contains the merged values

__add__(other: Costmap) → Costmap#

Overloading of the “+” operator for merging of Costmaps. Furthermore, checks if ‘other’ is actual a Costmap and raises a ValueError if this is not the case. Please check merge() for further information of merging.

Parameters:: other – Another Costmap
Returns:: A new Costmap that contains the merged values from this Costmap and the other Costmap

partitioning_rectangles() → typing_extensions.List[Rectangle]#

Partition the map attached to this costmap into rectangles. The rectangles are axis aligned, exhaustive and disjoint sets.

Returns:: A list containing the partitioning rectangles

class pycram.costmaps.OccupancyCostmap#

Bases: Costmap

The occupancy Costmap represents a map of the environment where obstacles or positions which are inaccessible for a robot have a value of -1.

distance_to_obstacle: float#

robot_view: semantic_digital_twin.robots.abstract_robot.AbstractRobot#

_distance_to_obstacle_index: int = None#

__post_init__()#

create_ray_mask_around_origin()#

Determines the occupied space around the origin position using ray testing. A ray is cast from the ground straight up 10m and if it hits something the position is considered occupied.

Returns:: A 2d numpy array of the occupied space

inflate_obstacles(map: numpy.ndarray)#

Inflates found obstacles in the environment by the distance_to_obstacle factor.

Parameters:: map – Map of obstacles to inflate.
Returns:: The map with inflated obstacles.

_create_from_world() → numpy.ndarray#: Creates an Occupancy Costmap for the specified World. This map marks every position as valid that has no object above it. After creating the costmap the distance to obstacle parameter is applied.

class pycram.costmaps.VisibilityCostmap#

Bases: Costmap

A costmap that represents the visibility of a specific point for every position around this point. For a detailed explanation on how the creation of the costmap works please look here: PhD Thesis (page 173)

min_height: float#

max_height: float#

target_object: typing_extensions.Optional[semantic_digital_twin.world_description.world_entity.Body | pycram.datastructures.pose.PoseStamped] = None#

__post_init__()#

_create_images() → typing_extensions.List[numpy.ndarray]#

Creates four depth images in every direction around the point for which the costmap should be created. The depth images are converted to metre, meaning that every entry in the depth images represents the distance to the next object in metre.

Returns:: A list of four depth images, the images are represented as 2D arrays.

_generate_map()#: This method generates the resulting density map by using the algorithm explained in Lorenz Mösenlechners PhD Thesis (page 178) The resulting map is then saved to self.map

class pycram.costmaps.GaussianCostmap(mean: int, sigma: float, world: semantic_digital_twin.world.World, resolution: typing_extensions.Optional[float] = 0.02, origin: typing_extensions.Optional[pycram.datastructures.pose.PoseStamped] = None)#

Bases: Costmap

Gaussian Costmaps are 2D gaussian distributions around the origin with the given mean and sigma

gau: numpy.ndarray#

map: numpy.ndarray#: Numpy array representing the costmap.

size: float#

width#: Width of the costmap.

height#: Height of the costmap.

resolution: float = 0.02#: The distance in metre in the real-world which is represented by a single entry in the costmap.

world#: The world from which this costmap was created.

origin: pycram.datastructures.pose.PoseStamped#: Origin pose of the costmap.

_gaussian_window(mean: int, std: float) → numpy.ndarray#: This method creates a window of values with a gaussian distribution of size “mean” and standart deviation “std”. Code from Scipy

class pycram.costmaps.SemanticCostmap(body: semantic_digital_twin.world_description.world_entity.Body, resolution: float = 0.02)#

Bases: Costmap

Semantic Costmaps represent a 2D distribution over a link of an Object. An example of this would be a Costmap for a table surface.

world: semantic_digital_twin.world.World#: The world from which this costmap was created.

body: semantic_digital_twin.world_description.world_entity.Body#

resolution: float = 0.02#: The distance in metre in the real-world which is represented by a single entry in the costmap.

origin: pycram.datastructures.pose.PoseStamped#: Origin pose of the costmap.

height: int = 0#: Height of the costmap.

width: int = 0#: Width of the costmap.

map: numpy.ndarray#: Numpy array representing the costmap.

get_edges_map(margin_in_meters: float, horizontal_only: bool = False) → Costmap#

Return a Costmap with only the edges of the original Costmap marked as possible positions.

Parameters:

margin_in_meters – The edge thickness in meters that should be marked as possible positions.
horizontal_only – If True only the horizontal edges will be marked as possible positions.

Returns:

The modified Costmap.

generate_map() → None#: Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) for the link name will be used. Height and width of the final Costmap will be the x and y sizes of the AABB.

static points_in_poly(points, poly)#

class pycram.costmaps.AlgebraicSemanticCostmap(body: semantic_digital_twin.world_description.world_entity.Body, number_of_samples=1000)#

Bases: SemanticCostmap

Class for a semantic costmap that is based on an algebraic set-description of the valid area.

x: random_events.variable.Continuous#: The variable for height.

y: random_events.variable.Continuous#: The variable for width.

original_valid_area: typing_extensions.Optional[random_events.product_algebra.SimpleEvent]#: The original rectangle of the valid area.

valid_area: typing_extensions.Optional[random_events.product_algebra.Event]#: A description of the valid positions as set.

number_of_samples: int#: The number of samples to generate for the iter.

world#: The world from which this costmap was created.

check_valid_area_exists()#

left(margin=0.0) → random_events.product_algebra.Event#: Create an event left of the origins Y-Coordinate. :param margin: The margin of the events left bound. :return: The left event.

right(margin=0.0) → random_events.product_algebra.Event#: Create an event right of the origins Y-Coordinate. :param margin: The margin of the events right bound. :return: The right event.

top(margin=0.0) → random_events.product_algebra.Event#: Create an event above the origins X-Coordinate. :param margin: The margin of the events upper bound. :return: The top event.

bottom(margin=0.0) → random_events.product_algebra.Event#: Create an event below the origins X-Coordinate. :param margin: The margin of the events lower bound. :return: The bottom event.

inner(margin=0.2)#

border(margin=0.2)#

generate_map() → None#: Generates the semantic costmap according to the provided parameters. To do this the axis aligned bounding box (AABB) for the link name will be used. Height and width of the final Costmap will be the x and y sizes of the AABB.

as_distribution() → probabilistic_model.probabilistic_circuit.rx.probabilistic_circuit.ProbabilisticCircuit#

sample_to_pose(sample: numpy.ndarray) → pycram.datastructures.pose.PoseStamped#

Convert a sample from the costmap to a pose.

Parameters:: sample – The sample to convert
Returns:: The pose corresponding to the sample

__iter__() → typing_extensions.Iterator[pycram.datastructures.pose.PoseStamped]#

pycram.costmaps.cmap#

pycram.costmaps.plot_grid(data: numpy.ndarray) → None#: An auxiliary method only used for debugging, it will plot a 2D numpy array using MatplotLib.