Skip to content

Planning graph guide

Jump to bottom
Olivier Lamarre edited this page Nov 12, 2023 · 3 revisions

Planning graph generation

A graph with directed edges can be defined over the terrain grid, assigning a vertex/node to every free pixel/cell and connecting them with their (free) neighbourhood. The following code snippets are retrieved from the site graph demo notebook, which you can refer to for complete examples.

Generating a grid graph

Two configuration dictionaries are required (here, we define them in separate yaml files):

  1. A "world" configuration, indicating the dataset with which the graph is associated and a few graph parameters. velocity_model describes what traverse velocity model to use to calculate traverse times along each edge. It is an optional collection - omitting it will not calculate edge traverse times during graph generaiton. power_model describes the mobility power model used to calculate the mobility energy expenditure along every edge. It is also an optional parameter and it requires velocity_model to be defined. Refer to the edge weights calculation section below for more information.
# world.yaml

site_name: "JEZ5_sample" # Dataset name

graph:
  connectivity: 8 # connected grid pattern (4 or 8)
  velocity_model:
    name: 'ConstantVelocity'
    params: null

  power_model:
    name: 'ConstantPower'
    params: null
  1. A "rover" configuration, describing the rover mobility model employed (effective velocity and drive power consumption):
# rover.yaml

motion:
  # m/s
  velocity: 0.045  # Average/effective traverse speed
  # W
  power: 137    # Effective mobility power consumption

solar_panel: # for the solar loader module
  # m^2
  area: 1.5
  # 0...1 (0%-100%)
  efficiency: 0.3

These configurations are loaded as dictionaries. The complete path to the dataset directory is added to the world configuration with the site_dirpath key.

# Load world config, add full path to dataset to load
world_fpath = Path(GPLANETARY_CFG.params_dir, "world.yaml")
with open(mission_fpath) as f:
    world_cfg = yaml.load(f, Loader=yaml.FullLoader)
world_cfg['site_dirpath'] = Path(os.environ.get('GNAV_DATASET_PATH'), world_cfg['site_name'])

# Load rover config
rover_fpath = Path(GPLANETARY_CFG.params_dir,"rover.yaml")
with open(rover_fpath) as f:
    rover_cfg = yaml.load(f, Loader=yaml.FullLoader)

The graph is generated and/or loaded using the load method. A DiGraph instance from the networkx python package is returned:

from gplanetary_nav.graph.grid_graph import GridGraph

gg = GridGraph.load(world_cfg, rover_cfg)
# gg.G is of type networkx.DiGraph

If no existing graph matching the world and rover configurations in the current dataset, this method generates a new one and saves it in the dataset directory under a (new) graph/ subdirectory.

Graph inspection

The data stored into every (directed) edge is the following:

for _, _, features_dict in G.edges(data=True):
    log.info(f"Graph edge attributes: {features_dict.keys()}")
    break
# Graph edge attributes: ['length', 'pitch_orig', 'roll_orig', 'pitch_dest', 'roll_dest', 'time_orig', 'time_dest', 'time_total', 'energy_orig', 'energy_dest', 'energy_total']

  • lenght: physical length of the edge in metres. This quantity is calculated using the Euclidean distance in three dimensions between the center of neighbouring grid cells according to the elevation map. As a first order approximation, we assume that half of this drive length takes place in the originating grid cell and the other half takes place in the destination grid cell.

  • pitch_{orig/dest}: the pitch of the rover in radians along the first/second half of the drive (i.e., in the "originating"/"destination" grid cell).

  • roll_{orig/dest}: the roll of the rover in radians along the first/second half of the drive.

If a velocity_model was defined in the world configuration:

  • time_{orig/dest}: the time in seconds required to traverse the first/second half of the edge at the speed indicated in the rover mobility model.

  • time_total: total time in seconds to traverse the whole edge (time_orig+time_dest).

If a power_model was also defined in the world configuration:

  • energy_{orig/dest}: the energy in Watt-hours required to traverse the first/second half of the edge given the (constant) rover mobility power and the duration time_{orig/dest}.

  • energy_total: total energy cost in Watt-hours of the whole edge (energy_orig+energy_dest).

Optimal path planning

Optimal graph-based path planning can be carried out using the networkx package:

import networkx as nx

G = gg.G

start_cell, goal_cell = (152, 152), (40,60)
path = nx.shortest_path(G, start_cell, goal_cell, weight='length')

path_length = path_cost(G, path, 'length')
path_duration = path_cost(G, path, 'time_total')

The example above finds the physically shortest path between two grid cells and then computes the path length in addition to the time required to traverse it.

The site graph demo notebook also provides an example that uses the M2020 Terrain Traversability analysis Tools (MTTT) velocity model and computes the fastest path between the same start/goal cells:

Edge weight calculation

Different velocity and power models can be implemented to calculate traverse times and drive energy, respectively. This section describes the process associated with the calculation of edge traverse times, but a similar reasoning applies to the calculation of energy expenditure.

The velocity model defined in the world configuration takes two parameters: name and params:

  • name: name of the class that implements the traverse time calculation. This class must inherit the BaseCost class

  • params: a collection of parameters specific to this cost class.

As an example, you can refer to the ConstantVelocity class. This class does not have any custom parameter - it is initialized using BaseCost.__init__(). The MTTTVelocity class, on the other hand, takes in 1 specific parameter (optimistic) in addition to the generic arguments required by BaseCost.

A world configuration using this model would look like this:

# Site name
site_name: "JEZ5_sample"

graph:
  connectivity: 8 # connected grid pattern (4 or 8)
  velocity_model:
    name: 'MTTTVelocity'
    params:
      optimistic: False

Implementing your own velocity model

Velocity models can be added to gplanetary_nav by following the format of the existing models. The new model should be defined as a class inheriting BaseCost and implemented in itw own module under (gplanetary_nav/cost/time). The class has to be imported in the subpackage's init file. The class name is the one that must be specified in the name field in the world configuration file.

The new class needs its own init function if it requires custom parameters (i.e., parameters listed under params in the world configuration file) and an eval() method. This method must return the traverse time (in seconds) associated with the arguments provided to it. These arguments are documented in the BaseCost.eval docstring.

Implementing your own power model

The same reasoning as with the velocity model applies, but in the energy subpackage. The eval() method of the new power model class must return the energy consumption (in Watt-hours) associated with the arguments provided to it.

Clone this wiki locally