sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 1 Packages Projects Releases Wiki Activity

no heading cost (#20594)

Browse Source 
* no heading cost

* live mpc weight config

* need to add stds

* make work on empty data

* no divide by 0

* update refs

* update model replay

* update proc replat

* new model replay ref
pull/214/head
HaraldSchafer 4 years ago committed by GitHub
parent 399ac30250
commit d72d433ec7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 39 additions and 220 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
  2. 4
      selfdrive/controls/lib/lateral_mpc/libmpc_py.py
  3. 10
      selfdrive/controls/lib/lateral_planner.py
  4. 4
      selfdrive/controls/tests/test_lateral_mpc.py
  5. 3
      selfdrive/debug/mpc/test_mpc_wobble.py
  6. 187
      selfdrive/debug/mpc/tune_lateral.py
  7. 39
      selfdrive/modeld/models/driving.cc
  8. 2
      selfdrive/test/process_replay/model_ref_commit
  9. 2
      selfdrive/test/process_replay/model_replay_ref_commit
  10. 2
      selfdrive/test/process_replay/ref_commit

6
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
Unescape View File

@ -29,7 +29,7 @@ typedef struct {
double cost;
} log_t;
void init_weights(double pathCost, double headingCost, double steerRateCost){
void set_weights(double pathCost, double headingCost, double steerRateCost){
int i;
const int STEP_MULTIPLIER = 3.0;
@ -44,7 +44,7 @@ void init_weights(double pathCost, double headingCost, double steerRateCost){
acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER;
}
void init(double pathCost, double headingCost, double steerRateCost){
void init(){
acado_initializeSolver();
int i;
@ -58,8 +58,6 @@ void init(double pathCost, double headingCost, double steerRateCost){
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
init_weights(pathCost, headingCost, steerRateCost);
}
int run_mpc(state_t * x0, log_t * solution, double v_ego,

4
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
Unescape View File

@ -22,8 +22,8 @@ typedef struct {
double cost;
} log_t;
void init(double pathCost, double headingCost, double steerRateCost);
void init_weights(double pathCost, double headingCost, double steerRateCost);
void init();
void set_weights(double pathCost, double headingCost, double steerRateCost);
int run_mpc(state_t * x0, log_t * solution,
double v_ego, double rotation_radius,
double target_y[N+1], double target_psi[N+1]);

10
selfdrive/controls/lib/lateral_planner.py
Unescape View File

@ -63,13 +63,14 @@ class LateralPlanner():
self.desire = log.LateralPlan.Desire.none
self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.libmpc.init()
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
@ -94,6 +95,8 @@ class LateralPlanner():
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
if len(md.orientation.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
@ -161,8 +164,10 @@ class LateralPlanner():
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
if self.use_lanelines:
std_cost_mult = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0)
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
else:
std_cost_mult = 1.0
d_path_xyz = self.path_xyz
y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
@ -170,6 +175,7 @@ class LateralPlanner():
assert len(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1
self.libmpc.set_weights(std_cost_mult*MPC_COST_LAT.PATH, 0.0, CP.steerRateCost)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
float(v_ego),
CAR_ROTATION_RADIUS,
@ -207,7 +213,7 @@ class LateralPlanner():
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.libmpc.init()
self.cur_state.curvature = measured_curvature
if t > self.last_cloudlog_t + 5.0:

4
selfdrive/controls/tests/test_lateral_mpc.py
Unescape View File

@ -8,7 +8,9 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
lane_width=3.6, poly_shift=0.):
libmpc = libmpc_py.libmpc
libmpc.init(1.0, 1.0, 1.0)
libmpc.init()
libmpc.set_weights(1., 1., 1.)
mpc_solution = libmpc_py.ffi.new("log_t *")

3
selfdrive/debug/mpc/test_mpc_wobble.py
Unescape View File

@ -6,7 +6,8 @@ from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATI
import math
libmpc = libmpc_py.libmpc
libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.)
libmpc.init()
libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.)
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = 0.0

187
selfdrive/debug/mpc/tune_lateral.py
Unescape View File

@ -1,187 +0,0 @@
#! /usr/bin/env python
# type: ignore
import numpy as np
from collections import OrderedDict
import matplotlib.pyplot as plt
from selfdrive.car.honda.interface import CarInterface
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.vehicle_model import VehicleModel
# plot lateral MPC trajectory by defining boundary conditions:
# lane lines, p_poly and vehicle states. Use this script to tune MPC costs
libmpc = libmpc_py.libmpc
mpc_solution = libmpc_py.ffi.new("log_t *")
points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454])
points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037])
points_c = (points_l + points_r) / 2.0
def compute_path_pinv():
deg = 3
x = np.arange(50.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points):
path_pinv = compute_path_pinv()
return np.dot(path_pinv, map(float, points))
xx = []
yy = []
deltas = []
psis = []
times = []
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
VM = VehicleModel(CP)
v_ref = 32.00 # 45 mph
curvature_factor = VM.curvature_factor(v_ref)
print(curvature_factor)
LANE_WIDTH = 3.9
p_l = map(float, model_polyfit(points_l))
p_r = map(float, model_polyfit(points_r))
p_p = map(float, model_polyfit(points_c))
l_poly = libmpc_py.ffi.new("double[4]", p_l)
r_poly = libmpc_py.ffi.new("double[4]", p_r)
p_poly = libmpc_py.ffi.new("double[4]", p_p)
l_prob = 1.0
r_prob = 1.0
p_prob = 1.0 # This is always 1
mpc_x_points = np.linspace(0., 2.5*v_ref, num=50)
points_poly_l = np.polyval(p_l, mpc_x_points)
points_poly_r = np.polyval(p_r, mpc_x_points)
points_poly_p = np.polyval(p_p, mpc_x_points)
print(points_poly_l)
lanes_x = np.linspace(0, 49)
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = 0.0
cur_state[0].y = 0.5
cur_state[0].psi = 0.0
cur_state[0].delta = 0.0
xs = []
ys = []
deltas = []
titles = [
'Steer rate cost',
'Heading cost',
'Lane cost',
'Path cost',
]
# Steer rate cost
sol_x = OrderedDict()
sol_y = OrderedDict()
delta = OrderedDict()
for cost in np.logspace(-1, 1.0, 5):
libmpc.init(1.0, 3.0, 1.0, cost)
for _ in range(10):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
curvature_factor, v_ref, LANE_WIDTH)
sol_x[cost] = map(float, list(mpc_solution[0].x))
sol_y[cost] = map(float, list(mpc_solution[0].y))
delta[cost] = map(float, list(mpc_solution[0].delta))
xs.append(sol_x)
ys.append(sol_y)
deltas.append(delta)
# Heading cost
sol_x = OrderedDict()
sol_y = OrderedDict()
delta = OrderedDict()
for cost in np.logspace(-1, 1.0, 5):
libmpc.init(1.0, 3.0, cost, 1.0)
for _ in range(10):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
curvature_factor, v_ref, LANE_WIDTH)
sol_x[cost] = map(float, list(mpc_solution[0].x))
sol_y[cost] = map(float, list(mpc_solution[0].y))
delta[cost] = map(float, list(mpc_solution[0].delta))
xs.append(sol_x)
ys.append(sol_y)
deltas.append(delta)
# Lane cost
sol_x = OrderedDict()
sol_y = OrderedDict()
delta = OrderedDict()
for cost in np.logspace(-1, 2.0, 5):
libmpc.init(1.0, cost, 1.0, 1.0)
for _ in range(10):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
curvature_factor, v_ref, LANE_WIDTH)
sol_x[cost] = map(float, list(mpc_solution[0].x))
sol_y[cost] = map(float, list(mpc_solution[0].y))
delta[cost] = map(float, list(mpc_solution[0].delta))
xs.append(sol_x)
ys.append(sol_y)
deltas.append(delta)
# Path cost
sol_x = OrderedDict()
sol_y = OrderedDict()
delta = OrderedDict()
for cost in np.logspace(-1, 1.0, 5):
libmpc.init(cost, 3.0, 1.0, 1.0)
for _ in range(10):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
curvature_factor, v_ref, LANE_WIDTH)
sol_x[cost] = map(float, list(mpc_solution[0].x))
sol_y[cost] = map(float, list(mpc_solution[0].y))
delta[cost] = map(float, list(mpc_solution[0].delta))
xs.append(sol_x)
ys.append(sol_y)
deltas.append(delta)
plt.figure()
for i in range(len(xs)):
ax = plt.subplot(2, 2, i + 1)
sol_x = xs[i]
sol_y = ys[i]
for cost in sol_x.keys():
plt.plot(sol_x[cost], sol_y[cost])
plt.plot(lanes_x, points_r, '.b')
plt.plot(lanes_x, points_l, '.b')
plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g')
plt.plot(mpc_x_points, points_poly_l, 'b')
plt.plot(mpc_x_points, points_poly_r, 'b')
plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g')
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3)
plt.title(titles[i])
plt.grid(True)
# ax.set_aspect('equal', 'datalim')
plt.figure()
for i in range(len(xs)):
plt.subplot(2, 2, i + 1)
sol_x = xs[i]
delta = deltas[i]
for cost in sol_x.keys():
plt.plot(delta[cost])
plt.title(titles[i])
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3)
plt.grid(True)
plt.show()

39
selfdrive/modeld/models/driving.cc
Unescape View File

@ -180,40 +180,39 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
int columns, int column_offset, float * plan_t_arr) {
int columns, int column_offset, float * plan_t_arr, bool fill_std) {
float x_arr[TRAJECTORY_SIZE] = {};
float y_arr[TRAJECTORY_SIZE] = {};
float z_arr[TRAJECTORY_SIZE] = {};
//float x_std_arr[TRAJECTORY_SIZE];
//float y_std_arr[TRAJECTORY_SIZE];
//float z_std_arr[TRAJECTORY_SIZE];
float x_std_arr[TRAJECTORY_SIZE];
float y_std_arr[TRAJECTORY_SIZE];
float z_std_arr[TRAJECTORY_SIZE];
float t_arr[TRAJECTORY_SIZE];
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// column_offset == -1 means this data is X indexed not T indexed
if (column_offset >= 0) {
t_arr[i] = T_IDXS[i];
x_arr[i] = data[i*columns + 0 + column_offset];
//x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
} else {
t_arr[i] = plan_t_arr[i];
x_arr[i] = X_IDXS[i];
//x_std_arr[i] = NAN;
x_std_arr[i] = NAN;
}
y_arr[i] = data[i*columns + 1 + column_offset];
//y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
z_arr[i] = data[i*columns + 2 + column_offset];
//z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
}
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
xyzt.setX(x_arr);
xyzt.setY(y_arr);
xyzt.setZ(z_arr);
//xyzt.setXStd(x_std);
//xyzt.setYStd(y_std);
//xyzt.setZStd(z_std);
xyzt.setT(t_arr);
if (fill_std) {
xyzt.setXStd(x_std_arr);
xyzt.setYStd(y_std_arr);
xyzt.setZStd(z_std_arr);
}
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
@ -224,17 +223,17 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
}
fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true);
fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr, false);
fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr, false);
fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr, false);
// lane lines
auto lane_lines = framed.initLaneLines(4);
float lane_line_probs_arr[4];
float lane_line_stds_arr[4];
for (int i = 0; i < 4; i++) {
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
@ -245,7 +244,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
auto road_edges = framed.initRoadEdges(2);
float road_edge_stds_arr[2];
for (int i = 0; i < 2; i++) {
fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false);
road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
}
framed.setRoadEdgeStds(road_edge_stds_arr);

2
selfdrive/test/process_replay/model_ref_commit
Unescape View File

@ -1 +1 @@
6829c5c76f3527af06e1c2b685f98a5e1bbef00a
1d17a97b34258507720b39cdf059a3c769aaf998

2
selfdrive/test/process_replay/model_replay_ref_commit
Unescape View File

@ -1 +1 @@
6c2409d2b1a93b675e4cd4ae7e67fc56ec3824dc
15390f9a445a1fd775079d1938ed14b0d6afacc9

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
724ca5ef28a601d5c78e63fb59890c6c93bd07d7
305c7a50812c20094998975b2966a7a5ad768e96
Write Preview
Loading…
Cancel
Save