Skip to content
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

update DH names + notes for MPC output curvatures #24701

Merged
merged 4 commits into from
Jun 7, 2022
Merged

update DH names + notes for MPC output curvatures #24701

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fc18dfb
update names + notes for MPC outputs
ClockeNessMnstr Jun 1, 2022
54dd33f
more notes
ClockeNessMnstr Jun 1, 2022
621143d
match
ClockeNessMnstr Jun 1, 2022
bb0ffa1
Clarify #1
ClockeNessMnstr Jun 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 12 additions & 11 deletions selfdrive/controls/lib/drive_helpers.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,26 +97,27 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
curvature_rates = [0.0]*CONTROL_N

v_ego = max(v_ego, 0.1)

# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
current_curvature = curvatures[0]
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
desired_curvature_rate = curvature_rates[0]


# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
desired_curvature = current_curvature + 2 * curvature_diff_from_psi

v_ego = max(v_ego, 0.1)
current_curvature_desired = curvatures[0]
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
average_curvature_desired = psi / (v_ego * delay)
desired_curvature = 2 * average_curvature_desired - current_curvature_desired

# This is the "desired rate of the setpoint" not an actual desired rate
desired_curvature_rate = curvature_rates[0]
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2)
safe_desired_curvature_rate = clip(desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
safe_desired_curvature = clip(desired_curvature,
current_curvature - max_curvature_rate * DT_MDL,
current_curvature + max_curvature_rate * DT_MDL)
current_curvature_desired - max_curvature_rate * DT_MDL,
current_curvature_desired + max_curvature_rate * DT_MDL)

return safe_desired_curvature, safe_desired_curvature_rate
1 change: 1 addition & 0 deletions selfdrive/controls/lib/latcontrol_indi.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)

# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)

Expand Down
2 changes: 2 additions & 0 deletions selfdrive/controls/lib/latcontrol_torque.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi
actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
desired_lateral_accel = desired_curvature * CS.vEgo ** 2

# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo ** 2

Expand Down
3 changes: 3 additions & 0 deletions selfdrive/controls/lib/lateral_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,9 @@ def update(self, sm):
y_pts,
heading_pts)
# init state for next
# mpc.u_sol is the desired curvature rate from x0[3].
# we integrate here so that x_sol[:,3] is the desired curvature for lat_control
# if we set x0[3] = measured_curvature the output would be desired_curvature_rate
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand this whole comment block. Integrate or Interpolate.

Copy link
Contributor Author

@ClockeNessMnstr ClockeNessMnstr Jun 3, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wanted to clarify it's interpolating over the integral of rate, since rate is really the MPC's "output" state.

This is better maybe:

# mpc.u_sol is the desired curvature rate given x0 curv state. 
# with x0[3] = measured_curvature, this would be the actual desired rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah that makes sense!

self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])

# Check for infeasible MPC solution
Expand Down