Source code for utils.vehicle_dynamics

"""
Implementation of all utility functions regarding the dynamic of the vehicle.
"""
import os

import numpy as np
import numpy.typing as npt
import yaml

def load_steering_model() -> dict:
    path = f'{os.path.abspath(os.path.dirname(__file__))}/../../config/model.yaml'
    with open(path) as input:
        steering_model = yaml.safe_load(input)
    return steering_model['steering_parameters']

def steering_wheel_to_ackermann(steering_wheel_angle: float, model: dict) -> float:
    """
    Calculate the ackermann angle with the steering wheel angle as input.

    Using a detailed formula, first implemented by Birk Blumhoff for his study work.

    Parameters
    ----------
    steering_wheel_angle : float
        Angle of the steering wheel [rad].
    model : dict
        

    Returns
    -------
    float
        Ackermann angle [rad].
    """
    # check if the setvalue is valid (not outside the steering kinematcs range)
    if(steering_wheel_angle < -0.5 * model['steering_range']):
        steering_wheel_angle = -0.5 * model['steering_range']
    elif(steering_wheel_angle > 0.5 * model['steering_range']):
        steering_wheel_angle = 0.5 * model['steering_range']

    
    # calculate left side
    delta_p = model['r_pin'] * steering_wheel_angle   #ggf Normalisierung hinzufügen
    l1_l = (model['track_width'] - model['l_rack'])/2 - delta_p
    l2_l = np.sqrt(l1_l**2 + model['D']**2)
    beta_l = np.pi/2 - np.arctan(model['D']/l1_l) - np.arccos((model['l_arm']**2 + l2_l**2 - model['l_tie']**2) / (2*model['l_arm']*l2_l))
    
    # calculate left tire Ackermann-angle
    delta_l = -(beta_l+model['normalization'])
    
    # calculate right side
    delta_p = -model['r_pin'] * steering_wheel_angle   #ggf Normalisierung hinzufügen
    l1_r = (model['track_width'] - model['l_rack'])/2 - delta_p
    l2_r = np.sqrt(l1_r**2 + model['D']**2)
    beta_r = np.pi/2 - np.arctan(model['D']/l1_r) - np.arccos((model['l_arm']**2 + l2_r**2 - model['l_tie']**2) / (2*model['l_arm']*l2_r))
    
    # calculate left tire Ackermann-angle
    delta_r = (beta_r+model['normalization'])

    # return both tire-angles [rad]
    return (delta_l + delta_r) / 2

def ackermann_to_radius(ackermann: npt.NDArray[np.floating], wheelbase: float = 1.550) -> npt.NDArray[np.floating]:
    """
    Calculate radius of turn with the ackermann angle.

    Using a simple bicycle model that disregards any side slip angle.

    Parameters
    ----------
    ackermann : npt.NDArray[np.floating]
        Ackermann angle, aka steering angle of the wheel [rad].
    wheelbase : float, optional
        Distance between front and rear axel., by default 1.550

    Returns
    -------
    npt.NDArray[np.floating]
        Radius of turn.
    """
    return wheelbase / np.tan(ackermann)