def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.enfore_rate_limit = CP.carName == "toyota"
self.RC = CP.lateralTuning.indi.timeConstant
self.G = CP.lateralTuning.indi.actuatorEffectiveness
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.lane_hugging = LaneHugging()
self.reset()
def __init__(self, CP):
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0)
self.angle_steers_des = 0.
self.lane_hugging = LaneHugging()
def __init__(self, CP, rate=100):
self.sat_flag = False
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
self.dc_gain = CP.lateralTuning.lqr.dcGain
self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.lane_hugging = LaneHugging()
self.reset()
def __init__(self, CP):
self.LP = LanePlanner()
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.setup_mpc()
self.solution_invalid_cnt = 0
self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1'
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.lane_hugging = LaneHugging()
self.op_params = opParams()
self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True)
self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0)
class LatControlPID():
def __init__(self, CP):
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0)
self.angle_steers_des = 0.
self.lane_hugging = LaneHugging()
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = self.lane_hugging.lane_hug(
path_plan.angleSteers) # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.01
output_steer = self.pid.update(self.angle_steers_des,
angle_steers,
check_saturation=(v_ego > 10),
override=steer_override,
feedforward=steer_feedforward,
speed=v_ego,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
class LatControlLQR():
def __init__(self, CP, rate=100):
self.sat_flag = False
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
self.dc_gain = CP.lateralTuning.lqr.dcGain
self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.lane_hugging = LaneHugging()
self.reset()
def reset(self):
self.i_lqr = 0.0
self.output_steer = 0.0
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego)
torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = self.lane_hugging.lane_hug(path_plan.angleSteers - path_plan.angleOffset)
angle_steers -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = angle_steers - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
if v_ego < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0))):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer*0.7, -steers_max, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
return self.output_steer, float(self.angle_steers_des), lqr_log
class LatControlINDI():
def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.enfore_rate_limit = CP.carName == "toyota"
self.RC = CP.lateralTuning.indi.timeConstant
self.G = CP.lateralTuning.indi.actuatorEffectiveness
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.lane_hugging = LaneHugging()
self.reset()
def reset(self):
self.delayed_output = 0.
self.output_steer = 0.
self.counter = 0
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)],
[math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = self.lane_hugging.lane_hug(
path_plan.angleSteers)
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (
1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enfore_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output +
delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(
new_output_steer_cmd, prev_output_steer_cmd,
prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
self.sat_flag = False
return float(self.output_steer), float(self.angle_steers_des), indi_log
class PathPlanner():
def __init__(self, CP):
self.LP = LanePlanner()
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.setup_mpc()
self.solution_invalid_cnt = 0
self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1'
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.lane_hugging = LaneHugging()
self.op_params = opParams()
self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True)
self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0)
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.angle_steers_des = 0.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < self.alca_min_speed * CV.MPH_TO_MS
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = True
if self.alca_nudge_required:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out lanelines over 1s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.LP.update_d_poly(v_ego)
# account for actuation delay
angle_offset = self.lane_hugging.modify_offset(float(angle_offset), self.lane_change_direction, self.lane_change_state)
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = self.lane_hugging.modify_offset(float(sm['liveParameters'].angleOffsetAverage), self.lane_change_direction, self.lane_change_state)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)