New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Calculation of yaw differences does not generallize #9

Open

wohnjayne opened this issue Jan 7, 2025 · 0 comments

wohnjayne commented Jan 7, 2025

The calculation of yaw differences does not generalize (e.g., to angles > 2*Pi, or if one angle is negative).

The current cost calculation takes the angle difference directly:

stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \
                     self.stage_cost_weight[2]*(yaw-ref_yaw)**2 + self.stage_cost_weight[3]*(v-ref_v)**2

Proposed fix: instead, calculate the generalized yaw diff first:

yaw_norm = np.mod(yaw + 2 * np.pi, 2 * np.pi)
ref_yaw_norm = np.mod(ref_yaw + 2 * np.pi, 2 * np.pi)
diff = float(ref_yaw_norm - yaw_norm)
yaw_diff = abs((diff + np.pi) % (2 * np.pi) - np.pi)
stage_cost = self.stage_cost_weight[0]*(x-ref_x)**2 + self.stage_cost_weight[1]*(y-ref_y)**2 + \
                     self.stage_cost_weight[2]*(yaw_diff)**2 + self.stage_cost_weight[3]*(v-ref_v)**2

(note: needs to be fixed in multiple places)

The text was updated successfully, but these errors were encountered:

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment