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)
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

info

• 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