"""Debug desired_heading calculation"""
|
from math import atan2, pi
|
|
def wrap_angle(a):
|
while a > pi:
|
a -= 2 * pi
|
while a < -pi:
|
a += 2 * pi
|
return a
|
|
# From log at t=10s
|
x, y = -0.282, -0.638
|
heading = -0.470
|
tx, ty = 0.000, 0.000
|
xte = 0.573
|
heading_err_log = -0.095
|
|
# Calculate path_heading
|
path_heading = atan2(ty - y, tx - x)
|
print(f"path_heading = atan2({ty} - {y}, {tx} - {x}) = atan2({ty-y:.3f}, {tx-x:.3f}) = {path_heading:.3f}")
|
|
# Calculate xte_correction
|
k_xte = 1.2
|
max_xte = 1.0
|
xte_gain_scale = 1.0
|
xte_normalized = max(-1.0, min(1.0, xte / max_xte))
|
xte_correction = -k_xte * xte_normalized * xte_gain_scale
|
print(f"xte_correction = -k_xte * xte_normalized * xte_gain_scale")
|
print(f" = -{k_xte} * {xte_normalized:.3f} * {xte_gain_scale}")
|
print(f" = {xte_correction:.3f}")
|
|
# Calculate desired_heading
|
desired_heading_raw = path_heading + xte_correction
|
desired_heading = wrap_angle(desired_heading_raw)
|
print(f"desired_heading = wrap_angle({path_heading:.3f} + {xte_correction:.3f})")
|
print(f" = wrap_angle({desired_heading_raw:.3f})")
|
print(f" = {desired_heading:.3f}")
|
|
# Calculate heading_err
|
heading_err_calc = wrap_angle(desired_heading - heading)
|
print(f"\nheading_err = wrap_angle({desired_heading:.3f} - {heading:.3f})")
|
print(f" = wrap_angle({desired_heading - heading:.3f})")
|
print(f" = {heading_err_calc:.3f}")
|
|
print(f"\n heading_err from log: {heading_err_log:.3f}")
|
print(f"heading_err calculated: {heading_err_calc:.3f}")
|
print(f" Difference: {abs(heading_err_log - heading_err_calc):.3f}")
|
|
if abs(heading_err_log - heading_err_calc) > 0.1:
|
print("\n⚠️ WARNING: Significant difference detected!")
|
print("Either:")
|
print("1. xte_correction is not being applied")
|
print("2. Different target point is being used")
|
print("3. Log data is from a different code path")
|
