Squared Distance to Path

In trajectory planning, it is sometimes necessary to compute the distance from a point to a path for path following. The squared distance to path extension feature computes the nearest squared distance from a point to a path composed of multiple line segments.

Before use, you need to import the relevant module:

from optimake.extension import *

Modeling

Below is an example of defining the squared distance from a point to a path:

path = path_2d('road_middle', max_dim=10, free_extrapolation=False)
sqd_dist = squared_distance_to_path(path, px, py)

First, define a 2D path using the path_2d interface. Its parameters are as follows:

• name: Path name
• max_dim: Maximum number of points composing the path, must be an integer greater than or equal to 2
• free_extrapolation: bool, specifies whether the path can extend infinitely. When False, the path extends infinitely in the reverse direction of the first line segment and in the direction of the last line segment. When True, the path does not extend. The default value of this parameter is False, i.e., extension is applied.

info

When free_extrapolation is True, the path does not extend, and the distance for points beyond the path is the distance to the endpoint.

When free_extrapolation is False, the extension rules are as follows:

• The path extends infinitely in the reverse direction of the first line segment
• The path extends infinitely in the direction of the last line segment
• When the above two rays intersect with each other or with other line segments of the path, the extension stops

Then, define the squared distance from a point to the path using the squared_distance_to_path interface. Its parameters are as follows:

• path: A path defined using the path_2d interface
• px, py: Coordinates of the point for which to compute the squared distance, expressions in terms of optimization variable $v$

The sqd_dist defined through this interface can be used for modeling as an objective function or constraint.

For example, below is an example of minimizing the squared distance from a point to a path as an objective function, which can be used for path following problems:

prob.objective(sqd_dist + w_v * (v - v_ref) ** 2)

Below is an example of modeling the squared distance from a point to a path as an inequality constraint, stating that the squared distance from the point to the path is less than or equal to 1.0:

ineq = general_inequality(expr=[sqd_dist], sign='<=', bound=[1.0])
prob.inequality(ineq)

Solving

Path data is stored in the prob struct. Before solving, you need to set the path data. Below is a C/C++ code example:

prob.path2d_road_middle.valid_dim = 4;
prob.path2d_road_middle.x[0] = 0.0; prob.path2d_road_middle.y[0] = 0.0;
prob.path2d_road_middle.x[1] = 10.0; prob.path2d_road_middle.y[1] = 0.0;
prob.path2d_road_middle.x[2] = 20.0; prob.path2d_road_middle.y[2] = 2.0;
prob.path2d_road_middle.x[3] = 30.0; prob.path2d_road_middle.y[3] = 2.0;

The following notes apply:

• road_middle is the path name, consistent with the name defined during modeling
• valid_dim is the number of valid points of the path, i.e., how many points define the path. It must be less than or equal to max_dim defined during modeling and greater than or equal to 2
• x and y are arrays of coordinates for each point on the path

info

• When valid_dim is less than 2, the solver returns a distance of 0
• When valid_dim is greater than max_dim, the solver uses max_dim as the number of valid points
• The computational cost is proportional to valid_dim, so it is recommended to merge adjacent collinear points to reduce valid_dim