Hi Song,
The reference to these conversion functions can be found in the help text of the trajectoryOptimalFrenet feature, but you bring up a good point - we should add this reference to the help text of referencePathFrenet and trajectoryGeneratorFrenet as well.
The derivation of the global2frenet and frenet2global methods are defined in the appendix of Werling's Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame.
% [1] M. Werling, J. Ziegler, S. Kammel, and S. Thrun. "Optimal
% trajectory generation for dynamic street scenarios in a frenet
% frame." In 2010 IEEE International Conference on Robotics and
% Automation, 2010, pp. 987-993.
As for how these values are calculated, here is the plain english explanation:
frenetState = global2frenet(refPath, globalState)
- Find the point,
, along the path that is nearest to the xy-coordinate of globalState, [x y θ κ v a]
. Since the reference path is C1 continuous (tangentially continuous), the distance will be shortest where 
-
is orthogonal to the path's tangent. - With the nearest point found, we evaluate the full state of the path [x y θ κ
s]
(x, y, tangent-angle, curvature, change-in-curvature vs change in arclength, arclength). This is equivalent to a moving reference frame located at [x y]
whose longitudinal axis points along 
and whose lateral axis intersects with 
(right-hand rule). The instantaneous motion of this frame is described by [κ 
]
- The velocity and acceleration components of the globalState act along the tangent defined by
, which itself evolves based on 
. Using the derivations from Werling's paper, we then map the global position, velocity, and acceleration to the local Frenet frame, giving us our frenetState. A visual representation of this is shown on the referencePathFrenet documentation page.
globalState = frenet2global(refPath, frenetState)
The process for frenet2global is the inverse of global2frenet:
- Evaluate the path at the frenetState's arclength,
=
, once again giving us [x y θ κ 
s]
- This point once again defines a reference frame whose axes and motion can be used to project the lon/lat and their 1st&2nd derivatives back into the global frame.
Lastly, we should note the following:
- The closestPoint orthogonality only holds when the path "covers" the region containing the global state. If the global state lies beyond the end of the path (i.e. the nearest point on the path lies at either end of the path), closestPoint's behavior is to simply snap to the limit.
- Werling offers two different ways of representing state in the Frenet frame, one where lateral derivatives are w.r.t. time [l
], and another where they are w.r.t. longitudinal [l 
]. We currently only support lateral derivatives w.r.t longitude, which is the reason you see your lateral "velocity" = 1 rather than 0. - We currently do not check for or handle cases where
lies on a level set, or where there are multiple-equidistant points along the path, as depicted in your 3-sided square example. That said, we may consider doing so in the future, and we value any feedback you may have.
Hope this helps!
-Cameron
