Calculates yaw and curvature of a 2D line with forward difference.

calculate_profile.py

from numpy import ediff1d, concatenate, arctan2, ndarray
from numpy.typing import ArrayLike


def solve_1st_derivative(x: ArrayLike, y: ArrayLike) -> tuple[ndarray, ndarray]:

    """
    :param x:       (ArrayLike) x-coordinates of the 2D line [m]
    :param y:       (ArrayLike) y-coordinates of the 2D line [m]

    :return dx:     (ndarray) x-coordinates after forward difference [m]
    :return dy:     (ndarray) y-coordinates after forward difference [m]
    """

    dx = ediff1d(x)
    dy = ediff1d(y)

    return concatenate((dx, [dx[-1]])), concatenate((dy, [dy[-1]]))


def calculate_yaw(x: ArrayLike, y: ArrayLike) -> ndarray:

    """
    :param x:       (ArrayLike) x-coordinates of the 2D line [m]
    :param y:       (ArrayLike) y-coordinates of the 2D line [m]

    :return yaw:    (ndarray) discrete yaw of the 2D line [rad]
    """
    
    return arctan2(*solve_1st_derivative(x, y))


def calculate_curvature(x: ArrayLike, y: ArrayLike) -> ndarray:

    """
    :param x:       (ArrayLike) x-coordinates of the 2D line [m]
    :param y:       (ArrayLike) y-coordinates of the 2D line [m]

    :return kappa:  (ndarray) discrete curvature of the 2D line [m]
    """

    dx, dy = solve_1st_derivative(x, y)
    ddx, ddy = solve_1st_derivative(dx, dy)

    return (ddy*dx - ddx*dy) / ((dx*dx + dy*dy)**1.5)