"""Derived quantities computed from raw log data."""
from __future__ import annotations
from typing import TYPE_CHECKING
import numpy as np
import pandas as pd
from suboptimumg.constants import G
if TYPE_CHECKING:
from .irl_car import IrlCar
# ---------------------------------------------------------------------------
# Private helpers
# ---------------------------------------------------------------------------
def _require_col(df: pd.DataFrame, col: str, remedy: str) -> None:
"""Raise a helpful KeyError if *col* is missing from *df*."""
if col in df.columns or df.index.name == col:
return
raise KeyError(f"Column '{col}' not found. {remedy}")
def _safe_gradient(signal: np.ndarray, dt: np.ndarray) -> np.ndarray:
"""Time derivative with forward/back-fill where dt == 0."""
result = np.where(dt != 0, np.gradient(signal) / dt, np.nan)
return pd.Series(result).ffill().bfill().to_numpy()
# ---------------------------------------------------------------------------
# Ground speed
# ---------------------------------------------------------------------------
[docs]
def compute_groundspeed(
df: pd.DataFrame,
fl_col: str = "pcm.wheelSpeeds.frontLeft",
fr_col: str = "pcm.wheelSpeeds.frontRight",
out_col: str = "groundSpeed",
) -> pd.DataFrame:
"""Average front wheel speeds to estimate ground speed.
Uses undriven (front) wheels to avoid slip contamination from
the driven (rear) axle.
"""
print(
"""
DEPRECATION WARNING: compute_groundspeed is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_groundspeed instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(
df,
fl_col,
"Ensure wheel speed columns exist after intake + convert_wheelspeeds_to_m_per_s().",
)
_require_col(
df,
fr_col,
"Ensure wheel speed columns exist after intake + convert_wheelspeeds_to_m_per_s().",
)
df[out_col] = (df[fl_col] + df[fr_col]) / 2.0
print(f"[derived] Added: {out_col} = avg({fl_col}, {fr_col}) ({len(df)} rows)")
return df
# ---------------------------------------------------------------------------
# Track frame velocities
# ---------------------------------------------------------------------------
[docs]
def compute_track_frame_velocities(
df: pd.DataFrame,
vel_n_col: str = "velN",
vel_e_col: str = "velE",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
low_speed_thresh: float = 3.0,
out_vx: str = "track.velocity.x",
out_vy: str = "track.velocity.y",
out_heading: str = "track.heading",
) -> pd.DataFrame:
"""Compute track-frame (velocity-frame) velocities from NED components.
Track heading is the course angle (direction of travel), same
convention as yaw (0 = North, 90 = East, wraps at +/-180).
At low speed the heading falls back to INS yaw.
"""
print(
"""
DEPRECATION WARNING: compute_track_frame_velocities is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_track_frame_velocities instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(
df, vel_n_col, "Run patch_ned_velocity() first to generate NED columns."
)
_require_col(
df, vel_e_col, "Run patch_ned_velocity() first to generate NED columns."
)
_require_col(df, yaw_col, "Yaw column not found in DataFrame.")
vel_n = df[vel_n_col].values.astype(np.float64)
vel_e = df[vel_e_col].values.astype(np.float64)
df[out_vx] = np.sqrt(vel_n**2 + vel_e**2)
df[out_vy] = 0.0
df[out_heading] = np.degrees(np.arctan2(vel_e, vel_n))
low_speed = df[out_vx].values < low_speed_thresh
df.loc[low_speed, out_heading] = df.loc[low_speed, yaw_col]
print(
f"[derived] Added: {out_vx}, {out_vy}, {out_heading} "
f"({len(df)} rows, low-speed gate @ {low_speed_thresh} m/s)"
)
return df
# ---------------------------------------------------------------------------
# Body slip angle
# ---------------------------------------------------------------------------
[docs]
def compute_body_slip_angle(
df: pd.DataFrame,
track_heading_col: str = "track.heading",
yaw_col: str = "pcm.vnav.yawPitchRoll.yaw",
vel_body_col: str = "pcm.vnav.velocityBody.x",
low_speed_thresh: float = 3.0,
out_col: str = "body.slipAngle",
) -> pd.DataFrame:
"""Compute body slip angle (beta) from track heading and INS yaw.
Standard SAE sideslip: beta = track_heading - yaw, wrapped to +/-180.
Positive beta = velocity vector to the right of the nose.
Zeroed below *low_speed_thresh* where heading is unreliable.
"""
print(
"""
DEPRECATION WARNING: compute_body_slip_angle is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_body_slip_angle instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(df, track_heading_col, "Run compute_track_frame_velocities() first.")
_require_col(df, yaw_col, "Yaw column not found in DataFrame.")
_require_col(
df, vel_body_col, "Run patch_ned_velocity() first to get body-frame velocity."
)
slip = df[track_heading_col].values - df[yaw_col].values
df[out_col] = (slip + 180.0) % 360.0 - 180.0
low_speed = np.abs(df[vel_body_col].values) < low_speed_thresh
df.loc[low_speed, out_col] = 0.0
print(
f"[derived] Added: {out_col} "
f"({len(df)} rows, low-speed gate @ {low_speed_thresh} m/s)"
)
return df
# ---------------------------------------------------------------------------
# Curvature & radius
# ---------------------------------------------------------------------------
[docs]
def compute_curvature(
df: pd.DataFrame,
yaw_rate_col: str = "pcm.vnav.compensatedAngularRate.z",
vel_body_col: str = "pcm.vnav.velocityBody.x",
track_heading_col: str = "track.heading",
vel_track_col: str = "track.velocity.x",
time_col: str = "time_s",
low_speed_thresh: float = 3.0,
median_window: int = 8,
radius_clip: float = 500.0,
out_body_curv: str = "body.curvature",
out_body_radius: str = "body.radius",
out_track_curv: str = "track.curvature",
out_track_radius: str = "track.radius",
) -> pd.DataFrame:
"""Compute path curvature and radius in body and track frames.
Body frame uses the measured yaw rate directly. Track frame
differentiates the track heading. Both apply a rolling median
and radius clipping.
"""
print(
"""
DEPRECATION WARNING: compute_curvature is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_curvature instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(
df,
yaw_rate_col,
"Yaw rate column not found. Ensure compensatedAngularRate.z is in intake.",
)
_require_col(
df, vel_body_col, "Run patch_ned_velocity() first to get body-frame velocity."
)
_require_col(df, track_heading_col, "Run compute_track_frame_velocities() first.")
_require_col(df, vel_track_col, "Run compute_track_frame_velocities() first.")
from ._utils import get_time_array
t = get_time_array(df, time_col)
dt = np.gradient(t)
# --- Body frame: curv = yaw_rate_rad / v_body ---
yaw_rate_rad = df[yaw_rate_col].values.astype(np.float64) # already rad/s
speed_body = df[vel_body_col].values.astype(np.float64)
with np.errstate(divide="ignore", invalid="ignore"):
curv_body_raw = np.where(
np.abs(speed_body) > low_speed_thresh,
yaw_rate_rad / speed_body,
0.0,
)
df[out_body_curv] = (
pd.Series(curv_body_raw)
.rolling(window=median_window, center=True, min_periods=1)
.median()
.fillna(0.0)
.values
)
with np.errstate(divide="ignore", invalid="ignore"):
r_body = np.where(
df[out_body_curv].values != 0,
1.0 / df[out_body_curv].values,
radius_clip,
)
df[out_body_radius] = np.clip(r_body, -radius_clip, radius_clip)
# --- Track frame: curv = d(heading)/dt / v_track ---
heading_unwrapped = np.unwrap(np.radians(df[track_heading_col].values))
heading_rate = _safe_gradient(heading_unwrapped, dt) # rad/s
speed_track = df[vel_track_col].values.astype(np.float64)
with np.errstate(divide="ignore", invalid="ignore"):
curv_track_raw = np.where(
speed_track > low_speed_thresh,
heading_rate / speed_track,
0.0,
)
df[out_track_curv] = (
pd.Series(curv_track_raw)
.rolling(window=median_window, center=True, min_periods=1)
.median()
.fillna(0.0)
.values
)
with np.errstate(divide="ignore", invalid="ignore"):
r_track = np.where(
df[out_track_curv].values != 0,
1.0 / df[out_track_curv].values,
radius_clip,
)
df[out_track_radius] = np.clip(r_track, -radius_clip, radius_clip)
print(
f"[derived] Added: {out_body_curv}, {out_body_radius}, "
f"{out_track_curv}, {out_track_radius} "
f"({len(df)} rows, median_window={median_window}, "
f"radius_clip={radius_clip}m)"
)
return df
# ---------------------------------------------------------------------------
# Lateral acceleration
# ---------------------------------------------------------------------------
[docs]
def compute_accelerations(
df: pd.DataFrame,
vel_body: str = "pcm.vnav.velocityBody.x",
curvature_body: str = "body.curvature",
vel_track: str = "track.velocity.x",
curvature_track: str = "track.curvature",
out_body: str = "body.accLat",
out_track: str = "track.accLat",
) -> pd.DataFrame:
"""Compute lateral acceleration from v^2 * curvature.
Computes both body and track frames by default. All column
name arguments have defaults matching upstream function outputs.
"""
print(
"""
DEPRECATION WARNING: compute_accelerations is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_accelerations instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(
df, vel_body, "Run patch_ned_velocity() first to get body-frame velocity."
)
_require_col(df, curvature_body, "Run compute_curvature() first.")
_require_col(df, vel_track, "Run compute_track_frame_velocities() first.")
_require_col(df, curvature_track, "Run compute_curvature() first.")
df[out_body] = df[vel_body].values ** 2 * df[curvature_body].values
df[out_track] = df[vel_track].values ** 2 * df[curvature_track].values
print(f"[derived] Added: {out_body}, {out_track} ({len(df)} rows)")
return df
# ---------------------------------------------------------------------------
# Kinematic steer angle
# ---------------------------------------------------------------------------
[docs]
def compute_kinematic_steer_angle(
df: pd.DataFrame,
curvature_body: str = "body.curvature",
curvature_track: str = "track.curvature",
wheelbase: float | None = None,
car_yaml: str = "parameters/car.yaml",
out_body: str = "body.steerAngle",
out_track: str = "track.steerAngle",
) -> pd.DataFrame:
"""Compute kinematic steer angle from curvature (bicycle model).
``delta = atan(wheelbase * curvature)`` in degrees. Wheelbase
is auto-loaded from car.yaml if not specified.
"""
print(
"""
DEPRECATION WARNING: compute_kinematic_steer_angle is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_kinematic_steer_angle instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(df, curvature_body, "Run compute_curvature() first.")
_require_col(df, curvature_track, "Run compute_curvature() first.")
if wheelbase is None:
import yaml
with open(car_yaml, "r") as f:
car_cfg = yaml.safe_load(f)
wheelbase = car_cfg["wb"]
print(f"[derived] Loaded wheelbase={wheelbase}m from {car_yaml}")
df[out_body] = np.degrees(np.arctan(wheelbase * df[curvature_body].values))
df[out_track] = np.degrees(np.arctan(wheelbase * df[curvature_track].values))
print(
f"[derived] Added: {out_body}, {out_track} ({len(df)} rows, wb={wheelbase}m)"
)
return df
# ---------------------------------------------------------------------------
# Rear slip ratio
# ---------------------------------------------------------------------------
[docs]
def compute_rear_slip_ratio(
df: pd.DataFrame,
wheel_speed_col: str = "pcm.moc.motor.wheelSpeed",
ground_speed_col: str = "groundSpeed",
out_col: str = "rear.slipRatio",
low_speed_thresh: float = 1.0,
) -> pd.DataFrame:
"""Compute rear axle longitudinal slip ratio.
``slip = (v_wheel - v_ground) / v_ground``
Positive = driven wheels spinning faster (traction loss).
Zeroed below *low_speed_thresh* to avoid division blow-up.
"""
print(
"""
DEPRECATION WARNING: compute_rear_slip_ratio is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_rear_slip_ratio instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(df, wheel_speed_col, "Run correct_motor_data() first.")
_require_col(df, ground_speed_col, "Run compute_groundspeed() first.")
v_wheel = df[wheel_speed_col].values.astype(np.float64)
v_ground = df[ground_speed_col].values.astype(np.float64)
with np.errstate(divide="ignore", invalid="ignore"):
slip = np.where(
np.abs(v_ground) > low_speed_thresh,
(v_wheel - v_ground) / v_ground,
0.0,
)
df[out_col] = slip
print(
f"[derived] Added: {out_col} "
f"({len(df)} rows, low-speed gate @ {low_speed_thresh} m/s)"
)
return df
# ---------------------------------------------------------------------------
# Aerodynamic coefficients
# ---------------------------------------------------------------------------
[docs]
def compute_cla(
df: pd.DataFrame,
irl_car: IrlCar,
ref_time_range: tuple[float, float],
vel_col: str = "groundSpeed",
pot_fl: str = "ludwig.shockpot.frontLeft",
pot_fr: str = "ludwig.shockpot.frontRight",
pot_rl: str = "ludwig.shockpot.rearLeft",
pot_rr: str = "ludwig.shockpot.rearRight",
ax_col: str = "pcm.vnav.linearAccelBody.x",
rho: float = 1.225,
low_speed_thresh: float = 5.0,
out_cla: str = "aero.cla",
out_cop: str = "aero.cop",
) -> pd.DataFrame:
"""Estimate CLA and aero CoP from shock pot compression.
A reference time range defines the "zero-aero" baseline (e.g. car
sitting still). At each timestep, the change in average axle pot
compression is converted to vertical force via the ``IrlCar``
suspension math, and the aero contribution is isolated by
subtracting longitudinal weight transfer through the springs.
Parameters
----------
df : DataFrame
Must contain shock pot columns, a velocity column, and a
longitudinal acceleration column.
irl_car : IrlCar
Provides mass, frontal area, heave stiffness, motion ratios,
and anti-dive/squat percentages.
ref_time_range : (t_start, t_end)
Index (``time_s``) window where the car is stationary or at a
known-zero-aero condition.
vel_col : str
Velocity source for dynamic pressure.
rho : float
Air density in kg/m^3.
low_speed_thresh : float
Below this speed (m/s) CLA and CoP are zeroed.
out_cla, out_cop : str
Output column names.
"""
print(
"""
DEPRECATION WARNING: compute_cla is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_cla instead, which operates directly on a PERDA SingleRunData object.
"""
)
for col, remedy in [
(vel_col, "Run compute_groundspeed() first."),
(pot_fl, "Shock pot column not found in DataFrame."),
(pot_fr, "Shock pot column not found in DataFrame."),
(pot_rl, "Shock pot column not found in DataFrame."),
(pot_rr, "Shock pot column not found in DataFrame."),
(ax_col, "Longitudinal accel column not found in DataFrame."),
]:
_require_col(df, col, remedy)
t_start, t_end = ref_time_range
ref = df.loc[t_start:t_end]
ref_front_avg = ((ref[pot_fl] + ref[pot_fr]) / 2.0).mean()
ref_rear_avg = ((ref[pot_rl] + ref[pot_rr]) / 2.0).mean()
front_avg = (df[pot_fl] + df[pot_fr]) / 2.0
rear_avg = (df[pot_rl] + df[pot_rr]) / 2.0
front_delta_mm = (front_avg - ref_front_avg).values
rear_delta_mm = (rear_avg - ref_rear_avg).values
ax_g = df[ax_col].values / G
f_front, f_rear = irl_car.aero_force_from_pots(front_delta_mm, rear_delta_mm, ax_g)
f_total = f_front + f_rear
v = df[vel_col].values.astype(np.float64)
q_A = 0.5 * rho * v**2 * irl_car.front_area
with np.errstate(divide="ignore", invalid="ignore"):
cla = np.where(np.abs(v) > low_speed_thresh, f_total / q_A, 0.0)
cop = np.where(np.abs(f_total) > 1.0, f_front / f_total, np.nan)
df[out_cla] = cla
df[out_cop] = cop
low_speed = np.abs(v) < low_speed_thresh
df.loc[low_speed, out_cla] = 0.0
df.loc[low_speed, out_cop] = np.nan
print(
f"[derived] Added: {out_cla}, {out_cop} "
f"({len(df)} rows, ref=[{t_start:.1f}, {t_end:.1f}]s, "
f"low-speed gate @ {low_speed_thresh} m/s)"
)
return df
[docs]
def compute_cda(
df: pd.DataFrame,
irl_car: IrlCar,
vel_col: str = "groundSpeed",
time_col: str = "time_s",
rho: float = 1.225,
low_speed_thresh: float = 5.0,
out_col: str = "aero.cda",
) -> pd.DataFrame:
"""Estimate CDA from coastdown deceleration (free-rolling assumption).
``F_drag = -m * a_x - F_rolling`` then ``CDA = F_drag / (0.5 * rho * v^2)``
The user should trim the DataFrame to a coasting segment (no
throttle, no braking) before calling this, or filter the output to
regions where throttle and brake are zero.
Parameters
----------
df : DataFrame
Must contain a velocity column and time index.
irl_car : IrlCar
Provides mass, frontal area, and rolling resistance coefficient.
vel_col : str
Velocity source (m/s).
rho : float
Air density in kg/m^3.
low_speed_thresh : float
Below this speed (m/s) CDA is zeroed.
out_col : str
Output column name.
"""
print(
"""
DEPRECATION WARNING: compute_cda is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_cda instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(df, vel_col, "Run compute_groundspeed() first.")
from ._utils import get_time_array
t = get_time_array(df, time_col)
dt = np.gradient(t)
v = df[vel_col].values.astype(np.float64)
a_x = _safe_gradient(v, dt)
m = irl_car.mass
f_rolling = m * G * irl_car.rolling_coeff
f_drag = -m * a_x - f_rolling
q = 0.5 * rho * v**2
with np.errstate(divide="ignore", invalid="ignore"):
cda = np.where(np.abs(v) > low_speed_thresh, f_drag / q, 0.0)
df[out_col] = cda
print(
f"[derived] Added: {out_col} "
f"({len(df)} rows, low-speed gate @ {low_speed_thresh} m/s)"
)
return df
# ---------------------------------------------------------------------------
# Bicycle-model front steer angle (from steering wheel + Ackermann)
# ---------------------------------------------------------------------------
[docs]
def compute_bicycle_steer_angle(
df: pd.DataFrame,
irl_car: IrlCar,
sw_col: str = "ludwig.steeringWheel.angle",
out_left: str = "front.steerAngle.left",
out_right: str = "front.steerAngle.right",
out_avg: str = "front.steerAngle.bicycle",
) -> pd.DataFrame:
"""Compute front tire steer angles from the steering wheel sensor.
Uses the Ackermann polynomial fit in *irl_car* to convert the
steering wheel angle to individual left/right tire angles, then
averages them for the bicycle-model front axle steer angle.
All outputs are in degrees.
"""
print(
"""
DEPRECATION WARNING: compute_bicycle_steer_angle is deprecated and will be removed in a future release.
Use suboptimumg.log_analysis.add_bicycle_steer_angle instead, which operates directly on a PERDA SingleRunData object.
"""
)
_require_col(df, sw_col, "Steering wheel angle column not found in DataFrame.")
sw = df[sw_col].values.astype(np.float64)
left, right = irl_car.tire_angles(sw)
df[out_left] = left
df[out_right] = right
df[out_avg] = (left + right) / 2.0
print(f"[derived] Added: {out_left}, {out_right}, {out_avg} ({len(df)} rows)")
return df