Source code for suboptimumg.log_analysis.steering

import numpy as np
from perda.core_data_structures.data_instance import DataInstance
from perda.core_data_structures.single_run_data import SingleRunData

from ..vehicle.irl_vehicle import IrlCar
from .kinematics import CURVATURE_BODY, CURVATURE_TRACK

BODY_FRAME_STEER_ANGLE = "body.steerAngle"
TRACK_FRAME_STEER_ANGLE = "track.steerAngle"

FRONT_LEFT_TIRE_STEERING_ANGLE = "front.steerAngle.left"
FRONT_RIGHT_TIRE_STEERING_ANGLE = "front.steerAngle.right"
FRONT_BICYCLE_MODEL_STEER_ANGLE = "front.steerAngle.bicycle"




def add_kinematic_steer_angle(
    data: SingleRunData,
    wb: float,
) -> SingleRunData:
    """Add the kinematic steer angle from curvature using the bicycle model.
    Expects curvature DataInstances to already be present, as added by `add_curvature()`.

    Computes ``delta = degrees(arctan(wheelbase * curvature))`` for both
    body and track frames and stores the results as ``body.steerAngle`` and
    ``track.steerAngle``. All angles are in degrees.

    Parameters
    ----------
    data : SingleRunData
    wb : float
        Wheelbase (m).

    Returns
    -------
    SingleRunData
    """
    missing = [c for c in (CURVATURE_BODY, CURVATURE_TRACK) if c not in data]
    if missing:
        raise KeyError(
            f"add_kinematic_steer_angle: missing variable(s) {missing}.\n"
            f"Try running run add_curvature first."
        )

    curv_body = data[CURVATURE_BODY]
    data[BODY_FRAME_STEER_ANGLE] = DataInstance(
        timestamp_np=curv_body.timestamp_np,
        value_np=np.degrees(np.arctan(wb * curv_body.value_np)),
        label="Kinematic Steer Angle — Body Frame (deg)",
        cpp_name=BODY_FRAME_STEER_ANGLE,
    )

    curv_track = data[CURVATURE_TRACK]
    data[TRACK_FRAME_STEER_ANGLE] = DataInstance(
        timestamp_np=curv_track.timestamp_np,
        value_np=np.degrees(np.arctan(wb * curv_track.value_np)),
        label="Kinematic Steer Angle — Track Frame (deg)",
        cpp_name=TRACK_FRAME_STEER_ANGLE,
    )
    return data





def add_bicycle_steer_angle(
    data: SingleRunData,
    irl_car: IrlCar,
    sw_col: str = "ludwig.steeringWheel.angle",
) -> SingleRunData:
    """Add front tire steer angles from the steering wheel sensor.

    Converts the steering wheel angle to individual left/right tire angles
    using the Ackermann polynomial in ``irl_car``, then averages them for
    the bicycle-model front axle steer angle. Stores the results as
    'front.steerAngle.left', 'front.steerAngle.right', and 'front.steerAngle.bicycle'.
    All outputs are in degrees.

    Parameters
    ----------
    data : SingleRunData
    irl_car : IrlCar
        Provides the Ackermann polynomial via ``tire_angles()``.
    sw_col : str
        Steering wheel angle variable name (degrees).

    Returns
    -------
    SingleRunData
    """
    if sw_col not in data:
        raise KeyError(f"add_bicycle_steer_angle: missing variable '{sw_col}'")

    sw = data[sw_col]
    left_vals, right_vals = irl_car.tire_angles(sw.value_np)
    avg_vals = (left_vals + right_vals) / 2.0

    data[FRONT_LEFT_TIRE_STEERING_ANGLE] = DataInstance(
        timestamp_np=sw.timestamp_np,
        value_np=left_vals,
        label="Left Tire Steer Angle (deg)",
        cpp_name=FRONT_LEFT_TIRE_STEERING_ANGLE,
    )
    data[FRONT_RIGHT_TIRE_STEERING_ANGLE] = DataInstance(
        timestamp_np=sw.timestamp_np,
        value_np=right_vals,
        label="Right Tire Steer Angle (deg)",
        cpp_name=FRONT_RIGHT_TIRE_STEERING_ANGLE,
    )
    data[FRONT_BICYCLE_MODEL_STEER_ANGLE] = DataInstance(
        timestamp_np=sw.timestamp_np,
        value_np=avg_vals,
        label="Bicycle Model Steer Angle (deg)",
        cpp_name=FRONT_BICYCLE_MODEL_STEER_ANGLE,
    )
    return data