mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-08 14:32:06 +08:00
Code Issues 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
This commit is contained in:
HaraldSchafer
2021-04-06 23:49:29 -07:00
committed by GitHub
parent 399ac30250
commit d72d433ec7
10 changed files with 39 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+2 -4
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,
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+2 -2
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]);
selfdrive/controls/lib/lateral_planner.py
+8 -2
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:
selfdrive/controls/tests/test_lateral_mpc.py
+3 -1
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 *")
selfdrive/debug/mpc/test_mpc_wobble.py
+2 -1
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
selfdrive/debug/mpc/tune_lateral.py
-187
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()
selfdrive/modeld/models/driving.cc
+19 -20
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);
selfdrive/test/process_replay/model_ref_commit
+1 -1
View File
@@ -1 +1 @@
6829c5c76f3527af06e1c2b685f98a5e1bbef00a
1d17a97b34258507720b39cdf059a3c769aaf998
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
6c2409d2b1a93b675e4cd4ae7e67fc56ec3824dc
15390f9a445a1fd775079d1938ed14b0d6afacc9
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
724ca5ef28a601d5c78e63fb59890c6c93bd07d7
305c7a50812c20094998975b2966a7a5ad768e96