Point-to-Occupancy Grid Collision Avoidance

In trajectory planning, an Occupancy Grid Map is a commonly used environment representation method. It divides the environment into discrete grids, where each grid cell stores information about whether that area is occupied by an obstacle. The point-to-occupancy grid collision avoidance feature enables collision avoidance constraints between a point and an occupancy grid, ensuring that the point does not enter areas occupied by obstacles.

Version 0.7.0 supports both 2D and 3D occupancy grids. A 2D grid is defined with occupancy_map_2d, and a 3D grid is defined with occupancy_map_3d; both add collision avoidance constraints for points through point_to_occupancy_map_inequality.

2D Occupancy Grid

Modeling

Below is an example of a point-to-2D occupancy grid collision avoidance inequality constraint:

  occupancy_map = occupancy_map_2d(name, length, width, resolution, origin_x, origin_y, origin_phi)
  ineq = point_to_occupancy_map_inequality(occupancy_map, px, py, distance_to_avoid)
  prob.inequality(ineq)

First, define a 2D occupancy grid using the occupancy_map_2d interface. The parameters of the occupancy_map_2d interface are as follows:

• name: Name of the occupancy grid
• length, width: Number of cells in the length and width directions of the occupancy grid (unit: cell), must be integers greater than 1
• resolution: Resolution of the occupancy grid (unit: m/cell), can be a constant or an expression in terms of parameter $p$
• origin_x, origin_y, origin_phi: Pose of the occupancy grid origin (lower-left corner as the origin, counterclockwise is positive), can be constants or expressions in terms of parameter $p$

Then, define the point-to-2D occupancy grid collision avoidance inequality constraint using the point_to_occupancy_map_inequality interface. Its parameters are as follows:

• occupancy_map: A 2D occupancy grid defined using the occupancy_map_2d interface
• px, py: Coordinates of the point that needs collision avoidance, expressions in terms of optimization variable $v$
• distance_to_avoid: Avoidance distance, can be a constant, an expression in terms of parameter $p$, or an expression in terms of optimization variable $v$

Solving

Occupancy grid data is stored in the prob struct. Before solving, you need to set the occupancy grid data and compute the distance field (Euclidean Signed Distance Field, ESDF). Below is a C/C++ code example (assuming the occupancy grid name is static, with size 6x6):

/* set occupancy map data */
unsigned char occ[36] = {
    0, 0, 0, 0, 0, 0,
    0, 1, 1, 0, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 0, 0, 0, 0, 0
};

for (size_t row = 0; row < 6; row++) {
    for (size_t col = 0; col < 6; col++) {
        prob->occupancymap2d_static.occ_map[row * 6 + col] = occ[row * 6 + col];
    }
}
/* build esdf */ 
vehicle_static_occupancymap2d_build_esdf(&prob->occupancymap2d_static);

The following notes apply:

• vehicle is the problem name, static is the occupancy grid name, consistent with the names defined during modeling
• The vehicle_static_occupancymap2d_build_esdf function is an auto-generated function used to compute the distance field
• Occupancy grid data is accessed via occupancymap2d_<occ_name>.occ_map. The data format consists of 0/1 values, where 0 indicates that the cell is free, and 1 indicates that the cell is occupied
• Occupancy grid data is stored in row-major format (different from other parts of OPTIMake), stored row by row starting from the lower-left corner

info

The ESDF distance field is stored in occupancymap2d_<occ_name>.esdf, with the following notes:

• The length of the ESDF distance field is length+3, and the width is width+3, i.e., the distance field corresponding to the occupancy grid with an extra ring of free cells added around the original occupancy grid
• The ESDF distance field storage format is the same as the occupancy grid: row-major format, stored row by row starting from the lower-left corner
• The stored values are distance values in units of resolution, with a precision of ±1 resolution. For example, if a cell's ESDF value is 5 and the resolution is 0.1 m/cell, this means the distance from that cell to the nearest obstacle is 0.5 m
• A positive distance value indicates the cell is free, a negative value indicates the cell is occupied by an obstacle, and 0 indicates the cell is on the obstacle boundary
• You can compute the distance field using the <prob_name>_<occ_name>_occupancymap2d_build_esdf function, or alternatively not call this function and instead compute the distance field yourself and manually populate the occupancymap2d_<occ_name>.esdf data

The figure below shows the 6x6 occupancy grid from the example above (the inner layer is the original occupancy grid, the outer layer is the extended free cells, and the solid dot is the occupancy grid origin):

The ESDF distance field for this occupancy grid is (this value is consistent with the occupancymap2d_static.esdf data in the prob struct):

double esdf[81] = {
    2.828427, 2.236068, 2.000000,  2.000000,  2.000000, 2.236068, 2.828427, 3.535534, 4.242641, 
    2.236068, 1.414214, 1.000000,  1.000000,  1.000000, 1.414214, 2.121320, 2.828427, 3.605551, 
    2.000000, 1.000000, 0.000000,  0.000000,  0.000000, 0.707107, 1.414214, 2.236068, 3.162278, 
    2.000000, 1.000000, 0.000000, -1.000000, -0.707107, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000, -1.000000, -1.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000, -1.000000, -1.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000,  0.000000,  0.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.236068, 1.414214, 1.000000,  1.000000,  1.000000, 1.000000, 1.414214, 2.236068, 3.162278, 
    2.828427, 2.236068, 2.000000,  2.000000,  2.000000, 2.000000, 2.236068, 2.828427, 3.605551
};

Results

Below is the problem setup and result for vehicle collision avoidance trajectory planning using the point-to-2D occupancy grid collision avoidance feature:

• The dashed line is the vehicle's reference line
• The vehicle dimensions are 4x2 m. Vehicle collision avoidance is achieved by applying point-to-2D occupancy grid collision avoidance constraints on 30 sampled points on the vehicle body
• The avoidance distance is 0.5 m (grid resolution is 0.1 m/cell)

3D Occupancy Grid

A 3D occupancy grid divides space into voxels, where each voxel records whether that region is occupied by an obstacle. Compared with the 2D interface, the 3D interface adds the size in the height direction and the z coordinate of the point to be kept clear, and describes the pose of the map in the world frame through a rotation matrix and a translation vector.

Modeling

The interfaces of the 3D occupancy grid and its collision avoidance constraint are as follows:

occupancy_map = occupancy_map_3d(name, length, width, height, resolution=0.1, T=None)
ineq = point_to_occupancy_map_inequality(
    occupancy_map, px, py, pz, distance_to_avoid, scaling_ratio)
prob.inequality(ineq)

The input arguments of occupancy_map_3d are as follows:

Argument	Description
name	Name of the map, which must be a valid C identifier, e.g., static
length, width, height	Number of voxels along the local x, y, and z directions of the map. They must be integers greater than or equal to 1 and are fixed at code generation time
resolution	Edge length of each voxel, 0.1 by default; if world coordinates are in m, the unit is m/voxel. It can be a positive constant or an expression of declared parameters, and must be greater than 0 at runtime
T	Pose of the map in the format (R, t). R is the 3×3 rotation matrix that rotates the map frame into the world frame, and t is the position of the map origin in the world frame

R must be defined as a 3×3 list/tuple, and t must be defined as a list/tuple of length 3. If T is omitted, the identity rotation and zero translation are used; R or t can also be set to None individually to use the corresponding default value. t corresponds to the starting corner of voxel (0, 0, 0), not the voxel center. The interface does not check the orthogonality of R; the caller must ensure that it is a valid rotation matrix.

The elements of resolution, R, and t can be constants, declared parameters, or expressions that depend only on these parameters; stage parameters are also supported. They must not depend on optimization variables; using undeclared parameters or optimization variables causes an error during code generation.

The map origin $O_m$ is located at the starting corner of the first voxel (0, 0, 0), and the three positive coordinate axes extend along the voxel edges that meet at this corner. The coordinates of this corner in the world frame are t.

The 3D input arguments of point_to_occupancy_map_inequality are as follows:

Argument	Description
occupancy_map	3D occupancy grid defined by occupancy_map_3d
px, py, pz	World coordinates of the point to be kept clear, which can be expressions of optimization variables
distance_to_avoid	Safety distance, using the same length unit as the world coordinates; it can be a constant or an expression of parameters or optimization variables
scaling_ratio	Numerical scaling factor of the inequality, a numeric constant in (1e-6, 1.0], 0.1 by default; it does not change the actual safety distance

Below is a modeling snippet from the occ3d_traj trajectory planning problem. The map size is 40×30×24, and the map pose, resolution, and safety distance are all set through parameters; the objective, dynamics, and start/end constraints can be set as in a regular multi-stage problem:

from optimake import multi_stage_problem
from optimake.extension import occupancy_map_3d, point_to_occupancy_map_inequality

prob = multi_stage_problem('occ3d_traj', 41)
x, y, z = [prob.variable(name) for name in ('x', 'y', 'z')]
resolution = prob.parameter('resolution')
safe_distance = prob.parameter('safe_distance')
R = [list(prob.parameters([f'r{i}{j}' for j in range(3)])) for i in range(3)]
t = list(prob.parameters(['tx', 'ty', 'tz']))

occ_map = occupancy_map_3d('static', 40, 30, 24, resolution, T=(R, t))
ineq = point_to_occupancy_map_inequality(
    occ_map, x, y, z, safe_distance)
prob.inequality(ineq)

Solving

The 3D occupancy data is stored in prob->occupancymap3d_<occ_name>.occ_map. Before solving, fill in the complete occupancy array, and then call the automatically generated <prob_name>_<occ_name>_occupancymap3d_build_esdf function to build the distance field.

The following C/C++ snippet corresponds to the problem name occ3d_traj, the map name static, and a 40×30×24 grid. It assumes that prob, option, ws, and out have been allocated and initialized with occ3d_traj_init:

/* Fill all voxels: x is contiguous, followed by y and z. */
for (size_t z = 0; z < 24; ++z) {
    for (size_t y = 0; y < 30; ++y) {
        for (size_t x = 0; x < 40; ++x) {
            size_t index = ((z * 30) + y) * 40 + x;
            prob->occupancymap3d_static.occ_map[index] =
                (x >= 16 && x < 24 && y >= 6 && y < 24 && z < 8) ? 1 : 0;
        }
    }
}
occ3d_traj_static_occupancymap3d_build_esdf(&prob->occupancymap3d_static);

/* Set the map pose, resolution, and safety distance. The identity rotation is used here. */
const double rotation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
for (size_t i = 0; i < 9; ++i) {
    prob->param[OCC3D_TRAJ_PARAM_R00 + i] = rotation[i];
}
prob->param[OCC3D_TRAJ_PARAM_TX] = 0.3;
prob->param[OCC3D_TRAJ_PARAM_TY] = -0.4;
prob->param[OCC3D_TRAJ_PARAM_TZ] = 0.2;
prob->param[OCC3D_TRAJ_PARAM_RESOLUTION] = 0.1;
prob->param[OCC3D_TRAJ_PARAM_SAFE_DISTANCE] = 0.15;

/* Set the remaining parameters and initial guess, such as start/end points and reference trajectory, then solve. */
int status = occ3d_traj_solve(prob, option, ws, out);

Notes:

• occ_map is of type unsigned char with length length * width * height; 0 means free and nonzero means occupied.
• The index of voxel (x, y, z) is ((z * width) + y) * length + x, where 0 <= x < length, 0 <= y < width, and 0 <= z < height.
• prob_name and occ_name are the problem name and map name used in modeling, respectively; the parameter macros above are generated automatically by the corresponding model.
• After modifying the occupancy data, build_esdf must be called again; if only T or resolution passed in as parameters is modified, the ESDF does not need to be rebuilt. If the map size or modeling constants change, the code must be regenerated and recompiled.
• Both the occupancy array and the ESDF are embedded in the problem struct, so large maps increase memory usage. It is recommended to allocate the problem struct on the heap as in the example.

Distance Field and Queries

The 3D ESDF is stored in prob->occupancymap3d_<occ_name>.esdf, sampled at the grid vertices:

• The number of vertices is (length + 1) * (width + 1) * (height + 1); the extra ring of free cells used by the 2D interface is not added.
• The index of vertex (x, y, z) is ((z * (width + 1)) + y) * (length + 1) + x, where the indices along each axis can reach length, width, and height, respectively.
• The stored distances are in units of voxels and must be multiplied by resolution to obtain lengths in world coordinates. Positive values indicate the free side, negative values indicate the inside of obstacles, and 0 indicates the sampled obstacle boundary.
• Inside the grid, the distance and gradient are queried by trilinear interpolation; outside the grid, an approximate exterior distance is used. A completely free map returns a grid distance of 1e20 and a zero gradient.

The automatically generated C query function can also be called directly. For example, to query the distance at grid coordinates (5, 15, 5):

Occ3d_traj_ESDFQueryResult3D result;
occ3d_traj_static_occupancymap3d_query(
    &prob->occupancymap3d_static, 5.0, 15.0, 5.0, &result);
double distance = prob->param[OCC3D_TRAJ_PARAM_RESOLUTION] * result.val;

result contains val, grad_x, grad_y, and grad_z, which are the grid distance and its gradient with respect to the grid coordinates. The explicit C query takes grid coordinates and cannot be passed world coordinates directly; to query a point in world coordinates, first transform the coordinates by $q = R^T(p-t)/r$, and multiply the returned distance by resolution. Collision avoidance constraints generated through the modeling interface automatically perform the coordinate transformation, distance conversion, and gradient chain rule.

info

ESDF interpolation is a distance approximation and is generally not differentiable at the boundaries between grid cells. The interior of a one-voxel-thick obstacle may be interpolated to 0, so a positive distance_to_avoid should be set to avoid treating zero distance as satisfying the safety requirement. Trajectory collision avoidance constraints apply to the added trajectory nodes; the continuous motion between nodes still needs to be checked separately.

The symbolic derivative of the ESDF gradient uses a zero approximation, so even with the exact setting, the true ESDF Hessian is not obtained.

Results

Below are the problem setup and results of a single 3D trajectory avoiding two occupancy grids simultaneously:

• The sizes of the two maps are 60×30×24 and 20×18×12, with resolutions of 0.10 m/voxel and 0.15 m/voxel, respectively, and each map has its own origin.
• Collision avoidance constraints for both maps are applied to each trajectory node, with a safety distance of 0.15 m from the occupied regions in both maps.
• The red dashed line is a straight reference passing through the occupied regions, the orange dashed line is the initial trajectory passing over the obstacles, and the blue solid line is the optimized trajectory avoiding both occupied regions.

Below is the result of an iiwa14 robot arm carrying a box while using a 3D occupancy grid to avoid the workbenches and the central fixture:

• The central fixture and the workbenches on both sides are written into the same 32×32×28 3D occupancy grid with a resolution of 0.05 m/voxel.
• Bounding spheres cover the arm links, the wrist, and the box, and point-to-occupancy grid collision avoidance constraints are applied to the sphere centers; the avoidance distance includes the sphere radius and a 0.02 m safety margin.