brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

The rest

Browse Source
This commit is contained in:
FrogAi
2024-01-15 06:17:49 -07:00
parent deb2b8d247
commit 458b51c60b
43 changed files with 376 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -56,7 +56,7 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
def get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality=log.LongitudinalPersonality.standard):
def get_jerk_factor(custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
if custom_personalities:
if personality==log.LongitudinalPersonality.relaxed:
return relaxed_jerk
@@ -241,6 +241,13 @@ def gen_long_ocp():
class LongitudinalMpc:
def __init__(self, mode='acc'):
# FrogPilot variables
self.safe_obstacle_distance = 0
self.safe_obstacle_distance_stock = 0
self.stopped_equivalence_factor = 0
self.t_follow = 0
self.t_follow_offset = 1
self.mode = mode
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.reset()
@@ -292,8 +299,9 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
def set_weights(self, prev_accel_constraint=True, custom_personalities=False, aggressive_jerk=0.5, standard_jerk=1.0, relaxed_jerk=1.0, personality=log.LongitudinalPersonality.standard):
jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality)
jerk_factor /= np.mean(self.t_follow_offset)
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
@@ -351,7 +359,7 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, smoother_braking, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, increased_stopping_distance, smoother_braking, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality)
self.t_follow = t_follow
v_ego = self.x0[1]
@@ -363,8 +371,9 @@ class LongitudinalMpc:
# Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
if aggressive_acceleration:
distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE + increased_stopping_distance - v_ego), 1, distance_factor)
t_follow = t_follow / t_follow_offset
standstill_offset = max(STOP_DISTANCE + increased_stopping_distance - (v_ego**COMFORT_BRAKE), 0)
self.t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + standstill_offset, 1, distance_factor)
t_follow = t_follow / self.t_follow_offset
# Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
if smoother_braking:

2
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -135,7 +135,7 @@ class LongitudinalPlanner:
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_weights(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, prev_accel_constraint, personality=self.personality)
self.mpc.set_weights(prev_accel_constraint, frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, personality=self.personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)