Vehicle Researcher
8 years ago
parent
f566c5b099
commit
490307fe50
55 changed files with 1189 additions and 654 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:85daa6764a264b9639ee3693fd577259ec6e73c83ae9355f5a62ae12049fb832 |
||||
size 6238486 |
||||
oid sha256:2617aa99a01fd0994e53b34a32699ac75186ac8a40c2e3021e4ce3ca2ba03de2 |
||||
size 6335294 |
||||
|
||||
@ -1 +1 @@ |
||||
Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85 |
||||
Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21 |
||||
@ -1 +1 @@ |
||||
Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383 |
||||
Subproject commit 3cab37297566962fd6e48a674db3e1f6de8fa4da |
||||
@ -0,0 +1,8 @@ |
||||
class CAR: |
||||
PRIUS = "TOYOTA PRIUS 2017" |
||||
RAV4 = "TOYOTA RAV4 2017" |
||||
|
||||
class ECU: |
||||
CAM = 0 # camera |
||||
DSU = 1 # driving support unit |
||||
APGS = 2 # advanced parking guidance system |
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.3.8.2-openpilot" |
||||
#define COMMA_VERSION "0.3.9-openpilot" |
||||
|
||||
@ -1,294 +0,0 @@ |
||||
import math |
||||
import numpy as np |
||||
from common.numpy_fast import clip, interp |
||||
import selfdrive.messaging as messaging |
||||
|
||||
# TODO: we compute a_pcm but we don't use it, as accelOverride is hardcoded in controlsd |
||||
|
||||
# lookup tables VS speed to determine min and max accels in cruise |
||||
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] |
||||
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] |
||||
|
||||
# need fast accel at very low speed for stop and go |
||||
_A_CRUISE_MAX_V = [1., 1., .8, .5, .30] |
||||
_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] |
||||
|
||||
def calc_cruise_accel_limits(v_ego): |
||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) |
||||
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) |
||||
return np.vstack([a_cruise_min, a_cruise_max]) |
||||
|
||||
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2] |
||||
_A_TOTAL_MAX_BP = [0., 20., 40.] |
||||
|
||||
def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP): |
||||
#*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration |
||||
# this should avoid accelerating when losing the target in turns |
||||
deg_to_rad = np.pi / 180. # from can reading to rad |
||||
|
||||
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) |
||||
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l) |
||||
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) |
||||
|
||||
a_target[1] = min(a_target[1], a_x_allowed) |
||||
a_pcm = min(a_pcm, a_x_allowed) |
||||
return a_target, a_pcm |
||||
|
||||
def process_a_lead(a_lead): |
||||
# soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead |
||||
a_lead_threshold = 0.5 |
||||
a_lead = min(a_lead + a_lead_threshold, 0) |
||||
return a_lead |
||||
|
||||
def calc_desired_distance(v_lead): |
||||
#*** compute desired distance *** |
||||
t_gap = 1.7 # good to be far away |
||||
d_offset = 4 # distance when at zero speed |
||||
return d_offset + v_lead * t_gap |
||||
|
||||
|
||||
#linear slope |
||||
_L_SLOPE_V = [0.40, 0.10] |
||||
_L_SLOPE_BP = [0., 40] |
||||
|
||||
# parabola slope |
||||
_P_SLOPE_V = [1.0, 0.25] |
||||
_P_SLOPE_BP = [0., 40] |
||||
|
||||
def calc_desired_speed(d_lead, d_des, v_lead, a_lead): |
||||
#*** compute desired speed *** |
||||
# the desired speed curve is divided in 4 portions: |
||||
# 1-constant |
||||
# 2-linear to regain distance |
||||
# 3-linear to shorten distance |
||||
# 4-parabolic (constant decel) |
||||
|
||||
max_runaway_speed = -2. # no slower than 2m/s over the lead |
||||
|
||||
# interpolate the lookups to find the slopes for a give lead speed |
||||
l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V) |
||||
p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V) |
||||
|
||||
# this is where parabola and linear curves are tangents |
||||
x_linear_to_parabola = p_slope / l_slope**2 |
||||
|
||||
# parabola offset to have the parabola being tangent to the linear curve |
||||
x_parabola_offset = p_slope / (2 * l_slope**2) |
||||
|
||||
if d_lead < d_des: |
||||
# calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des |
||||
v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des) |
||||
# calculate v_rel_des on one third of the linear slope |
||||
v_rel_des_2 = (d_lead - d_des) * l_slope / 3. |
||||
# take the min of the 2 above |
||||
v_rel_des = min(v_rel_des_1, v_rel_des_2) |
||||
v_rel_des = max(v_rel_des, max_runaway_speed) |
||||
elif d_lead < d_des + x_linear_to_parabola: |
||||
v_rel_des = (d_lead - d_des) * l_slope |
||||
v_rel_des = max(v_rel_des, max_runaway_speed) |
||||
else: |
||||
v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope) |
||||
|
||||
# compute desired speed |
||||
v_target = v_rel_des + v_lead |
||||
|
||||
# compute v_coast: above this speed we want to coast |
||||
t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region |
||||
v_coast_shift = max(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0 |
||||
v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line |
||||
v_coast = min(v_coast, v_target) |
||||
|
||||
return v_target, v_coast |
||||
|
||||
def calc_critical_decel(d_lead, v_rel, d_offset, v_offset): |
||||
# this function computes the required decel to avoid crashing, given safety offsets |
||||
a_critical = - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5) |
||||
return a_critical |
||||
|
||||
|
||||
# maximum acceleration adjustment |
||||
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0] |
||||
# speeds |
||||
_A_CORR_BY_SPEED_BP = [0., 2., 10.] |
||||
|
||||
# max acceleration allowed in acc, which happens in restart |
||||
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V) |
||||
|
||||
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max): |
||||
a_coast_min = -1.0 # never coast faster then -1m/s^2 |
||||
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease, |
||||
# regardless v_target |
||||
if v_ref > min(v_coast, v_target): |
||||
# for smooth coast we can be aggressive and target a point where car would actually crash |
||||
v_offset_coast = 0. |
||||
d_offset_coast = d_des/2. - 4. |
||||
|
||||
# acceleration value to smoothly coast until we hit v_target |
||||
if d_lead > d_offset_coast + 0.1: |
||||
a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast) |
||||
# if lead is decelerating, then offset the coast decel |
||||
a_coast += a_lead_contr |
||||
a_max = max(a_coast, a_coast_min) |
||||
else: |
||||
a_max = a_coast_min |
||||
else: |
||||
# same as cruise accel, plus add a small correction based on relative lead speed |
||||
# if the lead car is faster, we can accelerate more, if the car is slower, then we can reduce acceleration |
||||
a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \ |
||||
* clip(-v_rel / 4., -.5, 1) |
||||
return a_max |
||||
|
||||
# arbitrary limits to avoid too high accel being computed |
||||
_A_SAT = [-10., 5.] |
||||
|
||||
# do not consider a_lead at 0m/s, fully consider it at 10m/s |
||||
_A_LEAD_LOW_SPEED_V = [0., 1.] |
||||
|
||||
# speed break points |
||||
_A_LEAD_LOW_SPEED_BP = [0., 10.] |
||||
|
||||
# add a small offset to the desired decel, just for safety margin |
||||
_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3] |
||||
|
||||
# speed bp: different offset based on the likelyhood that lead decels abruptly |
||||
_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.] |
||||
|
||||
|
||||
def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead, |
||||
v_target, v_coast, a_target, a_pcm): |
||||
#*** compute max accel *** |
||||
# v_rel is now your velocity in lead car frame |
||||
v_rel *= -1 # this simplifies things when thinking in d_rel-v_rel diagram |
||||
|
||||
v_rel_pid = v_pid - v_lead |
||||
|
||||
# this is how much lead accel we consider in assigning the desired decel |
||||
a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP, |
||||
_A_LEAD_LOW_SPEED_V) * 0.8 |
||||
|
||||
# first call of calc_positive_accel_limit is used to shape v_pid |
||||
a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid, |
||||
v_rel_pid, v_coast, v_target, |
||||
a_lead_contr, a_target[1]) |
||||
# second call of calc_positive_accel_limit is used to limit the pcm throttle |
||||
# control (only useful when we don't control throttle directly) |
||||
a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, |
||||
v_rel, v_coast, v_target, |
||||
a_lead_contr, a_pcm) |
||||
|
||||
#*** compute max decel *** |
||||
v_offset = 1. # assume the car is 1m/s slower |
||||
d_offset = 1. # assume the distance is 1m lower |
||||
if v_target - v_ego > 0.5: |
||||
pass # acc target speed is above vehicle speed, so we can use the cruise limits |
||||
elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions |
||||
# compute needed accel to get to 1m distance with -1m/s rel speed |
||||
decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V) |
||||
|
||||
critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset) |
||||
a_target[0] = min(decel_offset + critical_decel + a_lead_contr, |
||||
a_target[0]) |
||||
else: |
||||
a_target[0] = _A_SAT[0] |
||||
# a_min can't be higher than a_max |
||||
a_target[0] = min(a_target[0], a_target[1]) |
||||
# final check on limits |
||||
a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1]) |
||||
a_target = a_target.tolist() |
||||
return a_target, a_pcm |
||||
|
||||
def calc_jerk_factor(d_lead, v_rel): |
||||
# we don't have an explicit jerk limit, so this function calculates a factor |
||||
# that is used by the PID controller to scale the gains. Not the cleanest solution |
||||
# but we need this for the demo. |
||||
# TODO: Calculate Kp and Ki directly in this function. |
||||
|
||||
# the higher is the decel required to avoid a crash, the higher is the PI factor scaling |
||||
d_offset = 0.5 |
||||
v_offset = 2. |
||||
a_offset = 1. |
||||
jerk_factor_max = 1.0 # can't increase Kp and Ki more than double. |
||||
if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions |
||||
jerk_factor = jerk_factor_max |
||||
else: |
||||
a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset) |
||||
# increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2 |
||||
jerk_factor = max(a_critical - a_offset, 0.)/5. |
||||
jerk_factor = min(jerk_factor, jerk_factor_max) |
||||
return jerk_factor |
||||
|
||||
|
||||
|
||||
MAX_SPEED_POSSIBLE = 55. |
||||
|
||||
def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP): |
||||
# drive limits |
||||
# TODO: Make lims function of speed (more aggressive at low speed). |
||||
a_lim = [-3., 1.5] |
||||
|
||||
#*** set target speed pretty high, as lead hasn't been considered yet |
||||
v_target_lead = MAX_SPEED_POSSIBLE |
||||
|
||||
#*** set accel limits as cruise accel/decel limits *** |
||||
a_target = calc_cruise_accel_limits(v_ego) |
||||
|
||||
# start with 1 |
||||
a_pcm = 1. |
||||
|
||||
#*** limit max accel in sharp turns |
||||
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP) |
||||
jerk_factor = 0. |
||||
|
||||
if l1 is not None and l1.status: |
||||
#*** process noisy a_lead signal from radar processing *** |
||||
a_lead_p = process_a_lead(l1.aLeadK) |
||||
|
||||
#*** compute desired distance *** |
||||
d_des = calc_desired_distance(l1.vLead) |
||||
|
||||
#*** compute desired speed *** |
||||
v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p) |
||||
|
||||
if l2 is not None and l2.status: |
||||
#*** process noisy a_lead signal from radar processing *** |
||||
a_lead_p2 = process_a_lead(l2.aLeadK) |
||||
|
||||
#*** compute desired distance *** |
||||
d_des2 = calc_desired_distance(l2.vLead) |
||||
|
||||
#*** compute desired speed *** |
||||
v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2) |
||||
|
||||
# listen to lead that makes you go slower |
||||
if v_target_lead2 < v_target_lead: |
||||
l1 = l2 |
||||
d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2 |
||||
|
||||
# l1 is the main lead now |
||||
|
||||
#*** compute accel limits *** |
||||
a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead, |
||||
l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm) |
||||
|
||||
# we can now limit a_target to a_lim |
||||
a_target = np.clip(a_target1, a_lim[0], a_lim[1]) |
||||
a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist() |
||||
|
||||
#*** compute max factor *** |
||||
jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel) |
||||
|
||||
# force coasting decel if driver hasn't been controlling car in a while |
||||
return v_target_lead, a_target, a_pcm, jerk_factor |
||||
|
||||
|
||||
class AdaptiveCruise(object): |
||||
def __init__(self): |
||||
self.l1, self.l2 = None, None |
||||
def update(self, v_ego, angle_steers, v_pid, CP, l20): |
||||
if l20 is not None: |
||||
self.l1 = l20.live20.leadOne |
||||
self.l2 = l20.live20.leadTwo |
||||
|
||||
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \ |
||||
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP) |
||||
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE |
||||
@ -1,66 +0,0 @@ |
||||
import numpy as np |
||||
from common.realtime import sec_since_boot |
||||
|
||||
#Time to collisions greater than 5s are iognored |
||||
MAX_TTC = 5. |
||||
|
||||
def calc_ttc(l1): |
||||
# if l1 is None, return max ttc immediately |
||||
if not l1: |
||||
return MAX_TTC |
||||
# this function returns the time to collision (ttc), assuming that |
||||
# ARel will stay constant TODO: review this assumptions |
||||
# change sign to rel quantities as it's going to be easier for calculations |
||||
vRel = -l1.vRel |
||||
aRel = -l1.aRel |
||||
|
||||
# assuming that closing gap ARel comes from lead vehicle decel, |
||||
# then limit ARel so that v_lead will get to zero in no sooner than t_decel. |
||||
# This helps underweighting ARel when v_lead is close to zero. |
||||
t_decel = 2. |
||||
aRel = np.minimum(aRel, l1.vLead/t_decel) |
||||
|
||||
# delta of the quadratic equation to solve for ttc |
||||
delta = vRel**2 + 2 * l1.dRel * aRel |
||||
|
||||
# assign an arbitrary high ttc value if there is no solution to ttc |
||||
if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1): |
||||
ttc = MAX_TTC |
||||
else: |
||||
ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC) |
||||
return ttc |
||||
|
||||
class ForwardCollisionWarning(object): |
||||
def __init__(self, dt): |
||||
self.last_active = 0. |
||||
self.violation_time = 0. |
||||
self.active = False |
||||
self.dt = dt # time step |
||||
|
||||
def process(self, CS, AC): |
||||
# send an fcw alert if the violation time > violation_thrs |
||||
violation_thrs = 0.3 # fcw turns on after a continuous violation for this time |
||||
fcw_t_delta = 5. # no more than one fcw alert within this time |
||||
a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw |
||||
a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw |
||||
ttc_thrs = 2.5 # ttc threshold for fcw |
||||
v_fcw_min = 5. # no fcw below 5m/s |
||||
steer_angle_th = 40. # deg, no fcw above this steer angle |
||||
cur_time = sec_since_boot() |
||||
|
||||
ttc = calc_ttc(AC.l1) |
||||
a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off |
||||
|
||||
# increase violation time if we want to decelerate quite fast |
||||
if AC.l1 and ( \ |
||||
(CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \ |
||||
and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \ |
||||
and AC.l1.fcw): |
||||
self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs) |
||||
else: |
||||
self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0) |
||||
|
||||
# fire FCW |
||||
self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta) |
||||
if self.active: |
||||
self.last_active = cur_time |
||||
@ -0,0 +1,94 @@ |
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
PHONELIBS = ../../../../phonelibs
|
||||
|
||||
UNAME_M := $(shell uname -m)
|
||||
|
||||
CFLAGS = -O3 -fPIC -I.
|
||||
CXXFLAGS = -O3 -fPIC -I.
|
||||
|
||||
QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
|
||||
|
||||
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
|
||||
|
||||
ifeq ($(UNAME_M),aarch64) |
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
else |
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
endif |
||||
|
||||
OBJS = \
|
||||
qp/Bounds.o \
|
||||
qp/Constraints.o \
|
||||
qp/CyclingManager.o \
|
||||
qp/Indexlist.o \
|
||||
qp/MessageHandling.o \
|
||||
qp/QProblem.o \
|
||||
qp/QProblemB.o \
|
||||
qp/SubjectTo.o \
|
||||
qp/Utils.o \
|
||||
qp/EXTRAS/SolutionAnalysis.o \
|
||||
mpc_export/acado_qpoases_interface.o \
|
||||
mpc_export/acado_integrator.o \
|
||||
mpc_export/acado_solver.o \
|
||||
mpc_export/acado_auxiliary_functions.o \
|
||||
mpc.o
|
||||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
.PHONY: all |
||||
all: libcommampc1.so libcommampc2.so |
||||
|
||||
libcommampc1.so: $(OBJS) |
||||
$(CXX) -shared -o '$@' $^ -lm
|
||||
|
||||
libcommampc2.so: libcommampc1.so |
||||
cp libcommampc1.so libcommampc2.so
|
||||
|
||||
qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp |
||||
@echo "[ CXX ] $@"
|
||||
mkdir -p qp
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp |
||||
@echo "[ CXX ] $@"
|
||||
mkdir -p qp/EXTRAS
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.cpp |
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.c |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
generator: generator.cpp |
||||
$(CXX) -Wall -std=c++11 \
|
||||
generator.cpp \
|
||||
-o generator \
|
||||
$(ACADO_FLAGS) \
|
||||
$(ACADO_LIBS)
|
||||
|
||||
.PHONY: generate |
||||
generate: generator |
||||
./generator
|
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS)
|
||||
|
||||
-include $(DEPS) |
||||
@ -0,0 +1,98 @@ |
||||
#include <acado_code_generation.hpp> |
||||
|
||||
const int controlHorizon = 50; |
||||
const double samplingTime = 0.2; |
||||
|
||||
using namespace std; |
||||
|
||||
#define G 9.81 |
||||
#define TR 1.8 |
||||
|
||||
#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) |
||||
#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) |
||||
|
||||
int main( ) |
||||
{ |
||||
USING_NAMESPACE_ACADO |
||||
|
||||
|
||||
DifferentialEquation f; |
||||
|
||||
DifferentialState x_ego, v_ego, a_ego; |
||||
DifferentialState x_l, v_l, a_l; |
||||
|
||||
OnlineData lambda; |
||||
|
||||
Control j_ego; |
||||
|
||||
auto desired = 4.0 + RW(v_ego, v_l); |
||||
auto d_l = x_l - x_ego; |
||||
|
||||
// Equations of motion
|
||||
f << dot(x_ego) == v_ego; |
||||
f << dot(v_ego) == a_ego; |
||||
f << dot(a_ego) == j_ego; |
||||
|
||||
f << dot(x_l) == v_l; |
||||
f << dot(v_l) == a_l; |
||||
f << dot(a_l) == -lambda * a_l; |
||||
|
||||
// Running cost
|
||||
Function h; |
||||
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); |
||||
h << (d_l - desired) / (0.1 * v_ego + 0.5); |
||||
h << a_ego * (1.0 + v_ego / 10.0); |
||||
h << j_ego * (1.0 + v_ego / 10.0); |
||||
|
||||
DMatrix Q(4,4); |
||||
Q(0,0) = 5.0; |
||||
Q(1,1) = 0.1; |
||||
Q(2,2) = 10.0; |
||||
Q(3,3) = 20.0; |
||||
|
||||
// Terminal cost
|
||||
Function hN; |
||||
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); |
||||
hN << (d_l - desired) / (0.1 * v_ego + 0.5); |
||||
hN << a_ego * (1.0 + v_ego / 10.0); |
||||
|
||||
DMatrix QN(3,3); |
||||
QN(0,0) = 5.0; |
||||
QN(1,1) = 0.1; |
||||
QN(2,2) = 10.0; |
||||
|
||||
// Setup Optimal Control Problem
|
||||
const double tStart = 0.0; |
||||
const double tEnd = samplingTime * controlHorizon; |
||||
|
||||
OCP ocp( tStart, tEnd, controlHorizon ); |
||||
ocp.subjectTo(f); |
||||
|
||||
ocp.minimizeLSQ(Q, h); |
||||
ocp.minimizeLSQEndTerm(QN, hN); |
||||
|
||||
ocp.subjectTo( 0.0 <= v_ego); |
||||
ocp.setNOD(1); |
||||
|
||||
OCPexport mpc(ocp); |
||||
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); |
||||
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); |
||||
mpc.set( INTEGRATOR_TYPE, INT_RK4 ); |
||||
mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); |
||||
mpc.set( MAX_NUM_QP_ITERATIONS, 500); |
||||
|
||||
mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); |
||||
mpc.set( QP_SOLVER, QP_QPOASES ); |
||||
mpc.set( HOTSTART_QP, YES ); |
||||
mpc.set( GENERATE_TEST_FILE, NO); |
||||
mpc.set( GENERATE_MAKE_FILE, NO ); |
||||
mpc.set( GENERATE_MATLAB_INTERFACE, NO ); |
||||
mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); |
||||
|
||||
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN) |
||||
exit( EXIT_FAILURE ); |
||||
|
||||
mpc.printDimensionsQP( ); |
||||
|
||||
return EXIT_SUCCESS; |
||||
} |
||||
@ -0,0 +1,43 @@ |
||||
import os |
||||
import subprocess |
||||
|
||||
from cffi import FFI |
||||
|
||||
mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) |
||||
subprocess.check_output(["make", "-j4"], cwd=mpc_dir) |
||||
|
||||
|
||||
def _get_libmpc(mpc_id): |
||||
libmpc_fn = os.path.join(mpc_dir, "libcommampc%d.so" % mpc_id) |
||||
|
||||
ffi = FFI() |
||||
ffi.cdef(""" |
||||
typedef struct { |
||||
double x_ego, v_ego, a_ego, x_l, v_l, a_l; |
||||
} state_t; |
||||
|
||||
|
||||
typedef struct { |
||||
double x_ego[50]; |
||||
double v_ego[50]; |
||||
double a_ego[50]; |
||||
double j_ego[50]; |
||||
double x_l[50]; |
||||
double v_l[50]; |
||||
double a_l[50]; |
||||
} log_t; |
||||
|
||||
void init(); |
||||
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); |
||||
int run_mpc(state_t * x0, log_t * solution, |
||||
double l); |
||||
""") |
||||
|
||||
return (ffi, ffi.dlopen(libmpc_fn)) |
||||
|
||||
|
||||
mpcs = [_get_libmpc(1), _get_libmpc(2)] |
||||
|
||||
|
||||
def get_libmpc(mpc_id): |
||||
return mpcs[mpc_id - 1] |
||||
@ -0,0 +1,118 @@ |
||||
#include "acado_common.h" |
||||
#include "acado_auxiliary_functions.h" |
||||
|
||||
#include <stdio.h> |
||||
|
||||
#define NX ACADO_NX /* Number of differential state variables. */ |
||||
#define NXA ACADO_NXA /* Number of algebraic variables. */ |
||||
#define NU ACADO_NU /* Number of control inputs. */ |
||||
#define NOD ACADO_NOD /* Number of online data values. */ |
||||
|
||||
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ |
||||
#define NYN ACADO_NYN /* Number of measurements/references on node N. */ |
||||
|
||||
#define N ACADO_N /* Number of intervals in the horizon. */ |
||||
|
||||
ACADOvariables acadoVariables; |
||||
ACADOworkspace acadoWorkspace; |
||||
|
||||
typedef struct { |
||||
double x_ego, v_ego, a_ego, x_l, v_l, a_l; |
||||
} state_t; |
||||
|
||||
|
||||
typedef struct { |
||||
double x_ego[N]; |
||||
double v_ego[N]; |
||||
double a_ego[N]; |
||||
double j_ego[N]; |
||||
double x_l[N]; |
||||
double v_l[N]; |
||||
double a_l[N]; |
||||
} log_t; |
||||
|
||||
void init(){ |
||||
acado_initializeSolver(); |
||||
int i; |
||||
|
||||
/* Initialize the states and controls. */ |
||||
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; |
||||
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; |
||||
|
||||
/* Initialize the measurements/reference. */ |
||||
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; |
||||
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; |
||||
|
||||
/* MPC: initialize the current state feedback. */ |
||||
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; |
||||
} |
||||
|
||||
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){ |
||||
int i; |
||||
double x_ego = 0.0; |
||||
double a_ego = 0.0; |
||||
|
||||
if (v_ego > v_l){ |
||||
a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l; |
||||
} |
||||
double dt = 0.2; |
||||
|
||||
for (i = 0; i < N + 1; ++i){ |
||||
acadoVariables.x[i*NX] = x_ego; |
||||
acadoVariables.x[i*NX+1] = v_ego; |
||||
acadoVariables.x[i*NX+2] = a_ego; |
||||
|
||||
acadoVariables.x[i*NX+3] = x_l; |
||||
acadoVariables.x[i*NX+4] = v_l; |
||||
acadoVariables.x[i*NX+5] = a_l; |
||||
|
||||
x_ego += v_ego * dt; |
||||
v_ego += a_ego * dt; |
||||
|
||||
x_l += v_l * dt; |
||||
v_l += a_l * dt; |
||||
a_l += -l * a_l * dt; |
||||
|
||||
if (v_ego <= 0.0) { |
||||
v_ego = 0.0; |
||||
a_ego = 0.0; |
||||
} |
||||
} |
||||
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; |
||||
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; |
||||
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; |
||||
} |
||||
|
||||
int run_mpc(state_t * x0, log_t * solution, double l){ |
||||
int i; |
||||
|
||||
for (i = 0; i <= NOD * N; i+= NOD){ |
||||
acadoVariables.od[i] = l; |
||||
} |
||||
|
||||
acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; |
||||
acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; |
||||
acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; |
||||
acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l; |
||||
acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l; |
||||
acadoVariables.x[5] = acadoVariables.x0[5] = x0->a_l; |
||||
|
||||
acado_preparationStep(); |
||||
acado_feedbackStep(); |
||||
|
||||
for (i = 0; i <= N; i++){ |
||||
solution->x_ego[i] = acadoVariables.x[i*NX]; |
||||
solution->v_ego[i] = acadoVariables.x[i*NX+1]; |
||||
solution->a_ego[i] = acadoVariables.x[i*NX+2]; |
||||
solution->x_l[i] = acadoVariables.x[i*NX+3]; |
||||
solution->v_l[i] = acadoVariables.x[i*NX+4]; |
||||
solution->a_l[i] = acadoVariables.x[i*NX+5]; |
||||
|
||||
solution->j_ego[i] = acadoVariables.u[i]; |
||||
} |
||||
|
||||
// Dont shift states here. Current solution is closer to next timestep than if
|
||||
// we shift by 0.2 seconds.
|
||||
|
||||
return acado_getNWSR(); |
||||
} |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357 |
||||
size 4906 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270 |
||||
size 3428 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:6db0b40b4f066266c4382512de1752cd8fd2260bcd58355f1291e5db272b7ec1 |
||||
size 8655 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:fbe29a0acd4c2ec8fea880b159440ac68b8d0b6ab7437c95c6f84443db13abef |
||||
size 33237 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:ac20ce29802179e0d9b91f2a43673e6a0a41c2d1abcecadd54daca7a08befda8 |
||||
size 1948 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:e1ccfb2ae276bc3bc787e2bfc68861de08017e8ff97411fe5ceab65ae9997f18 |
||||
size 1821 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:5d9df24f3df44e3fbd8d4d9695c8f3f452e391179f01ba8c52d28473e1bbd6a3 |
||||
size 303043 |
||||
@ -0,0 +1,88 @@ |
||||
import numpy as np |
||||
|
||||
|
||||
def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin): |
||||
|
||||
tDelta = 0. |
||||
if aEgo > aMax: |
||||
tDelta = (aMax - aEgo) / jMin |
||||
elif aEgo < aMin: |
||||
tDelta = (aMin - aEgo) / jMax |
||||
|
||||
return tDelta |
||||
|
||||
|
||||
def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): |
||||
|
||||
dV = vT - vEgo |
||||
|
||||
tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin) |
||||
|
||||
if (ts <= tDelta): |
||||
if (aEgo < aMin): |
||||
vEgo += ts * aEgo + 0.5 * ts**2 * jMax |
||||
aEgo += ts * jMax |
||||
return vEgo, aEgo |
||||
elif (aEgo > aMax): |
||||
vEgo += ts * aEgo + 0.5 * ts**2 * jMin |
||||
aEgo += ts * jMin |
||||
return vEgo, aEgo |
||||
|
||||
if aEgo > aMax: |
||||
dV -= 0.5 * (aMax**2 - aEgo**2) / jMin |
||||
vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin |
||||
aEgo += tDelta * jMin |
||||
elif aEgo < aMin: |
||||
dV -= 0.5 * (aMin**2 - aEgo**2) / jMax |
||||
vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax |
||||
aEgo += tDelta * jMax |
||||
|
||||
ts -= tDelta |
||||
|
||||
jLim = jMin if aEgo >= 0 else jMax |
||||
# if we reduce the accel to zero immediately, how much delta speed we generate? |
||||
dv_min_shift = - 0.5 * aEgo**2 / jLim |
||||
|
||||
# flip signs so we can consider only one case |
||||
flipped = False |
||||
if dV < dv_min_shift: |
||||
flipped = True |
||||
dV *= -1 |
||||
vEgo *= -1 |
||||
aEgo *= -1 |
||||
aMax = -aMin |
||||
jMaxcopy = -jMin |
||||
jMin = -jMax |
||||
jMax = jMaxcopy |
||||
|
||||
# small addition needed to avoid numerical issues with sqrt of ~zero |
||||
aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin)) |
||||
|
||||
if aPeak > aMax: |
||||
aPeak = aMax |
||||
t1 = (aPeak - aEgo) / jMax |
||||
vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin |
||||
if vChange < aPeak * ts: |
||||
t2 = t1 + vChange / aPeak |
||||
else: |
||||
t2 = t1 + ts |
||||
else: |
||||
t1 = (aPeak - aEgo) / jMax |
||||
t2 = t1 |
||||
t3 = t2 - aPeak / jMin |
||||
|
||||
dt1 = min(ts, t1) |
||||
dt2 = max(min(ts, t2) - t1, 0.) |
||||
dt3 = max(min(ts, t3) - t2, 0.) |
||||
|
||||
if ts > t3: |
||||
vEgo += dV |
||||
aEgo = 0. |
||||
else: |
||||
vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin |
||||
aEgo += jMax * dt1 + dt3 * jMin |
||||
|
||||
vEgo *= -1 if flipped else 1 |
||||
aEgo *= -1 if flipped else 1 |
||||
|
||||
return float(vEgo), float(aEgo) |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:a1adb2870715bfac1640fc3afa1b7a274a24ff3aa98b3a8bbbab622039a7868a |
||||
oid sha256:b7988b5c9f39ef6650f25e1a8181d4eb2cfa1ded0a89d871440f121f0234f02e |
||||
size 1412368 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:c1db908ac63c3036eb0e4758c2b3af19524df2c34f011de055cdbef16b655c0d |
||||
size 981608 |
||||
oid sha256:15804c73a05556fcbb976a266e66d4c22ec6f7dd4a98aeff89f15437252bdb3c |
||||
size 981400 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:056e3f9de2d89dfee1a7092615c1958959f7d01a0e3c660798b19b084dce8819 |
||||
oid sha256:51c5ed132e3984edfc2fbfd856169ea3d999cada499ef90ceba0122a35f857bb |
||||
size 972296 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:9818be4dff22fef5dbcf5b6fce468c15c34594f7e3ae45b8be0cb6164694919a |
||||
size 13330512 |
||||
oid sha256:9fe67ea99d1cc0e003fb68cb9c71bbc6ff0b0124b043bcb58ee4df43bbdb7547 |
||||
size 13334208 |
||||
|
||||
Loading…
Reference in new issue