George Hotz
8 years ago
committed by
GitHub
commit
5ae7119646
63 changed files with 4807 additions and 849 deletions
Binary file not shown.
@ -0,0 +1,23 @@ |
||||
import numpy as np |
||||
|
||||
|
||||
class KF1D: |
||||
# this EKF assumes constant covariance matrix, so calculations are much simpler |
||||
# the Kalman gain also needs to be precomputed using the control module |
||||
|
||||
def __init__(self, x0, A, C, K): |
||||
self.x = x0 |
||||
self.A = A |
||||
self.C = C |
||||
self.K = K |
||||
|
||||
self.A_K = self.A - np.dot(self.K, self.C) |
||||
|
||||
# K matrix needs to be pre-computed as follow: |
||||
# import control |
||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) |
||||
# self.K = np.transpose(K) |
||||
|
||||
def update(self, meas): |
||||
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) |
||||
return self.x |
||||
@ -1 +1 @@ |
||||
Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85 |
||||
Subproject commit 242698f80038bab677a4a6e58127309f9ed38d93 |
||||
@ -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,212 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
#include "acado_auxiliary_functions.h" |
||||
|
||||
#include <stdio.h> |
||||
|
||||
real_t* acado_getVariablesX( ) |
||||
{ |
||||
return acadoVariables.x; |
||||
} |
||||
|
||||
real_t* acado_getVariablesU( ) |
||||
{ |
||||
return acadoVariables.u; |
||||
} |
||||
|
||||
#if ACADO_NY > 0 |
||||
real_t* acado_getVariablesY( ) |
||||
{ |
||||
return acadoVariables.y; |
||||
} |
||||
#endif |
||||
|
||||
#if ACADO_NYN > 0 |
||||
real_t* acado_getVariablesYN( ) |
||||
{ |
||||
return acadoVariables.yN; |
||||
} |
||||
#endif |
||||
|
||||
real_t* acado_getVariablesX0( ) |
||||
{ |
||||
#if ACADO_INITIAL_VALUE_FIXED |
||||
return acadoVariables.x0; |
||||
#else |
||||
return 0; |
||||
#endif |
||||
} |
||||
|
||||
/** Print differential variables. */ |
||||
void acado_printDifferentialVariables( ) |
||||
{ |
||||
int i, j; |
||||
printf("\nDifferential variables:\n[\n"); |
||||
for (i = 0; i < ACADO_N + 1; ++i) |
||||
{ |
||||
for (j = 0; j < ACADO_NX; ++j) |
||||
printf("\t%e", acadoVariables.x[i * ACADO_NX + j]); |
||||
printf("\n"); |
||||
} |
||||
printf("]\n\n"); |
||||
} |
||||
|
||||
/** Print control variables. */ |
||||
void acado_printControlVariables( ) |
||||
{ |
||||
int i, j; |
||||
printf("\nControl variables:\n[\n"); |
||||
for (i = 0; i < ACADO_N; ++i) |
||||
{ |
||||
for (j = 0; j < ACADO_NU; ++j) |
||||
printf("\t%e", acadoVariables.u[i * ACADO_NU + j]); |
||||
printf("\n"); |
||||
} |
||||
printf("]\n\n"); |
||||
} |
||||
|
||||
/** Print ACADO code generation notice. */ |
||||
void acado_printHeader( ) |
||||
{ |
||||
printf( |
||||
"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n" |
||||
"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n"
|
||||
"Milan Vukov and Rien Quirynen, KU Leuven.\n" |
||||
); |
||||
|
||||
printf( |
||||
"Developed within the Optimization in Engineering Center (OPTEC) under\n" |
||||
"supervision of Moritz Diehl. All rights reserved.\n\n" |
||||
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n" |
||||
"General Public License 3 in the hope that it will be useful,\n" |
||||
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n" |
||||
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" |
||||
"GNU Lesser General Public License for more details.\n\n" |
||||
); |
||||
} |
||||
|
||||
#if !(defined _DSPACE) |
||||
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) |
||||
|
||||
void acado_tic( acado_timer* t ) |
||||
{ |
||||
QueryPerformanceFrequency(&t->freq); |
||||
QueryPerformanceCounter(&t->tic); |
||||
} |
||||
|
||||
real_t acado_toc( acado_timer* t ) |
||||
{ |
||||
QueryPerformanceCounter(&t->toc); |
||||
return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart); |
||||
} |
||||
|
||||
|
||||
#elif (defined __APPLE__) |
||||
|
||||
void acado_tic( acado_timer* t ) |
||||
{ |
||||
/* read current clock cycles */ |
||||
t->tic = mach_absolute_time(); |
||||
} |
||||
|
||||
real_t acado_toc( acado_timer* t ) |
||||
{ |
||||
|
||||
uint64_t duration; /* elapsed time in clock cycles*/ |
||||
|
||||
t->toc = mach_absolute_time(); |
||||
duration = t->toc - t->tic; |
||||
|
||||
/*conversion from clock cycles to nanoseconds*/ |
||||
mach_timebase_info(&(t->tinfo)); |
||||
duration *= t->tinfo.numer; |
||||
duration /= t->tinfo.denom; |
||||
|
||||
return (real_t)duration / 1e9; |
||||
} |
||||
|
||||
#else |
||||
|
||||
#if __STDC_VERSION__ >= 199901L |
||||
/* C99 mode */ |
||||
|
||||
/* read current time */ |
||||
void acado_tic( acado_timer* t ) |
||||
{ |
||||
gettimeofday(&t->tic, 0); |
||||
} |
||||
|
||||
/* return time passed since last call to tic on this timer */ |
||||
real_t acado_toc( acado_timer* t ) |
||||
{ |
||||
struct timeval temp; |
||||
|
||||
gettimeofday(&t->toc, 0); |
||||
|
||||
if ((t->toc.tv_usec - t->tic.tv_usec) < 0) |
||||
{ |
||||
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; |
||||
temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec; |
||||
} |
||||
else |
||||
{ |
||||
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; |
||||
temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec; |
||||
} |
||||
|
||||
return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6; |
||||
} |
||||
|
||||
#else |
||||
/* ANSI */ |
||||
|
||||
/* read current time */ |
||||
void acado_tic( acado_timer* t ) |
||||
{ |
||||
clock_gettime(CLOCK_MONOTONIC, &t->tic); |
||||
} |
||||
|
||||
|
||||
/* return time passed since last call to tic on this timer */ |
||||
real_t acado_toc( acado_timer* t ) |
||||
{ |
||||
struct timespec temp; |
||||
|
||||
clock_gettime(CLOCK_MONOTONIC, &t->toc);
|
||||
|
||||
if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) |
||||
{ |
||||
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; |
||||
temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec; |
||||
} |
||||
else |
||||
{ |
||||
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; |
||||
temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; |
||||
} |
||||
|
||||
return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9; |
||||
} |
||||
|
||||
#endif /* __STDC_VERSION__ >= 199901L */ |
||||
|
||||
#endif /* (defined _WIN32 || _WIN64) */ |
||||
|
||||
#endif |
||||
@ -0,0 +1,138 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
#ifndef ACADO_AUXILIARY_FUNCTIONS_H |
||||
#define ACADO_AUXILIARY_FUNCTIONS_H |
||||
|
||||
#include "acado_common.h" |
||||
|
||||
#ifndef __MATLAB__ |
||||
#ifdef __cplusplus |
||||
extern "C" |
||||
{ |
||||
#endif /* __cplusplus */ |
||||
#endif /* __MATLAB__ */ |
||||
|
||||
/** Get pointer to the matrix with differential variables. */ |
||||
real_t* acado_getVariablesX( ); |
||||
|
||||
/** Get pointer to the matrix with control variables. */ |
||||
real_t* acado_getVariablesU( ); |
||||
|
||||
#if ACADO_NY > 0 |
||||
/** Get pointer to the matrix with references/measurements. */ |
||||
real_t* acado_getVariablesY( ); |
||||
#endif |
||||
|
||||
#if ACADO_NYN > 0 |
||||
/** Get pointer to the vector with references/measurement on the last node. */ |
||||
real_t* acado_getVariablesYN( ); |
||||
#endif |
||||
|
||||
/** Get pointer to the current state feedback vector. Only applicable for NMPC. */ |
||||
real_t* acado_getVariablesX0( ); |
||||
|
||||
/** Print differential variables. */ |
||||
void acado_printDifferentialVariables( ); |
||||
|
||||
/** Print control variables. */ |
||||
void acado_printControlVariables( ); |
||||
|
||||
/** Print ACADO code generation notice. */ |
||||
void acado_printHeader( ); |
||||
|
||||
/*
|
||||
* A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for
|
||||
* providing us with the following timing routines. |
||||
*/ |
||||
|
||||
#if !(defined _DSPACE) |
||||
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) |
||||
|
||||
/* Use Windows QueryPerformanceCounter for timing. */ |
||||
#include <Windows.h> |
||||
|
||||
/** A structure for keeping internal timer data. */ |
||||
typedef struct acado_timer_ |
||||
{ |
||||
LARGE_INTEGER tic; |
||||
LARGE_INTEGER toc; |
||||
LARGE_INTEGER freq; |
||||
} acado_timer; |
||||
|
||||
|
||||
#elif (defined __APPLE__) |
||||
|
||||
#include "unistd.h" |
||||
#include <mach/mach_time.h> |
||||
|
||||
/** A structure for keeping internal timer data. */ |
||||
typedef struct acado_timer_ |
||||
{ |
||||
uint64_t tic; |
||||
uint64_t toc; |
||||
mach_timebase_info_data_t tinfo; |
||||
} acado_timer; |
||||
|
||||
#else |
||||
|
||||
/* Use POSIX clock_gettime() for timing on non-Windows machines. */ |
||||
#include <time.h> |
||||
|
||||
#if __STDC_VERSION__ >= 199901L |
||||
/* C99 mode of operation. */ |
||||
|
||||
#include <sys/stat.h> |
||||
#include <sys/time.h> |
||||
|
||||
typedef struct acado_timer_ |
||||
{ |
||||
struct timeval tic; |
||||
struct timeval toc; |
||||
} acado_timer; |
||||
|
||||
#else |
||||
/* ANSI C */ |
||||
|
||||
/** A structure for keeping internal timer data. */ |
||||
typedef struct acado_timer_ |
||||
{ |
||||
struct timespec tic; |
||||
struct timespec toc; |
||||
} acado_timer; |
||||
|
||||
#endif /* __STDC_VERSION__ >= 199901L */ |
||||
|
||||
#endif /* (defined _WIN32 || defined _WIN64) */ |
||||
|
||||
/** A function for measurement of the current time. */ |
||||
void acado_tic( acado_timer* t ); |
||||
|
||||
/** A function which returns the elapsed time. */ |
||||
real_t acado_toc( acado_timer* t ); |
||||
|
||||
#endif |
||||
|
||||
#ifndef __MATLAB__ |
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif /* __cplusplus */ |
||||
#endif /* __MATLAB__ */ |
||||
|
||||
#endif /* ACADO_AUXILIARY_FUNCTIONS_H */ |
||||
@ -0,0 +1,349 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
#ifndef ACADO_COMMON_H |
||||
#define ACADO_COMMON_H |
||||
|
||||
#include <math.h> |
||||
#include <string.h> |
||||
|
||||
#ifndef __MATLAB__ |
||||
#ifdef __cplusplus |
||||
extern "C" |
||||
{ |
||||
#endif /* __cplusplus */ |
||||
#endif /* __MATLAB__ */ |
||||
|
||||
/** \defgroup ACADO ACADO CGT generated module. */ |
||||
/** @{ */ |
||||
|
||||
/** qpOASES QP solver indicator. */ |
||||
#define ACADO_QPOASES 0 |
||||
#define ACADO_QPOASES3 1 |
||||
/** FORCES QP solver indicator.*/ |
||||
#define ACADO_FORCES 2 |
||||
/** qpDUNES QP solver indicator.*/ |
||||
#define ACADO_QPDUNES 3 |
||||
/** HPMPC QP solver indicator. */ |
||||
#define ACADO_HPMPC 4 |
||||
#define ACADO_GENERIC 5 |
||||
|
||||
/** Indicator for determining the QP solver used by the ACADO solver code. */ |
||||
#define ACADO_QP_SOLVER ACADO_QPOASES |
||||
|
||||
#include "acado_qpoases_interface.hpp" |
||||
|
||||
|
||||
/*
|
||||
* Common definitions |
||||
*/ |
||||
/** User defined block based condensing. */ |
||||
#define ACADO_BLOCK_CONDENSING 0 |
||||
/** Compute covariance matrix of the last state estimate. */ |
||||
#define ACADO_COMPUTE_COVARIANCE_MATRIX 0 |
||||
/** Flag indicating whether constraint values are hard-coded or not. */ |
||||
#define ACADO_HARDCODED_CONSTRAINT_VALUES 1 |
||||
/** Indicator for fixed initial state. */ |
||||
#define ACADO_INITIAL_STATE_FIXED 1 |
||||
/** Number of control/estimation intervals. */ |
||||
#define ACADO_N 50 |
||||
/** Number of online data values. */ |
||||
#define ACADO_NOD 1 |
||||
/** Number of path constraints. */ |
||||
#define ACADO_NPAC 0 |
||||
/** Number of control variables. */ |
||||
#define ACADO_NU 1 |
||||
/** Number of differential variables. */ |
||||
#define ACADO_NX 6 |
||||
/** Number of algebraic variables. */ |
||||
#define ACADO_NXA 0 |
||||
/** Number of differential derivative variables. */ |
||||
#define ACADO_NXD 0 |
||||
/** Number of references/measurements per node on the first N nodes. */ |
||||
#define ACADO_NY 4 |
||||
/** Number of references/measurements on the last (N + 1)st node. */ |
||||
#define ACADO_NYN 3 |
||||
/** Total number of QP optimization variables. */ |
||||
#define ACADO_QP_NV 56 |
||||
/** Number of integration steps per shooting interval. */ |
||||
#define ACADO_RK_NIS 1 |
||||
/** Number of Runge-Kutta stages per integration step. */ |
||||
#define ACADO_RK_NSTAGES 4 |
||||
/** Providing interface for arrival cost. */ |
||||
#define ACADO_USE_ARRIVAL_COST 0 |
||||
/** Indicator for usage of non-hard-coded linear terms in the objective. */ |
||||
#define ACADO_USE_LINEAR_TERMS 0 |
||||
/** Indicator for type of fixed weighting matrices. */ |
||||
#define ACADO_WEIGHTING_MATRICES_TYPE 0 |
||||
|
||||
|
||||
/*
|
||||
* Globally used structure definitions |
||||
*/ |
||||
|
||||
/** The structure containing the user data.
|
||||
*
|
||||
* Via this structure the user "communicates" with the solver code. |
||||
*/ |
||||
typedef struct ACADOvariables_ |
||||
{ |
||||
int dummy; |
||||
/** Matrix of size: 51 x 6 (row major format)
|
||||
*
|
||||
* Matrix containing 51 differential variable vectors. |
||||
*/ |
||||
real_t x[ 306 ]; |
||||
|
||||
/** Column vector of size: 50
|
||||
*
|
||||
* Matrix containing 50 control variable vectors. |
||||
*/ |
||||
real_t u[ 50 ]; |
||||
|
||||
/** Column vector of size: 51
|
||||
*
|
||||
* Matrix containing 51 online data vectors. |
||||
*/ |
||||
real_t od[ 51 ]; |
||||
|
||||
/** Column vector of size: 200
|
||||
*
|
||||
* Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes. |
||||
*/ |
||||
real_t y[ 200 ]; |
||||
|
||||
/** Column vector of size: 3
|
||||
*
|
||||
* Reference/measurement vector for the 51. node. |
||||
*/ |
||||
real_t yN[ 3 ]; |
||||
|
||||
/** Column vector of size: 6
|
||||
*
|
||||
* Current state feedback vector. |
||||
*/ |
||||
real_t x0[ 6 ]; |
||||
|
||||
|
||||
} ACADOvariables; |
||||
|
||||
/** Private workspace used by the auto-generated code.
|
||||
*
|
||||
* Data members of this structure are private to the solver. |
||||
* In other words, the user code should not modify values of this
|
||||
* structure.
|
||||
*/ |
||||
typedef struct ACADOworkspace_ |
||||
{ |
||||
real_t rk_ttt; |
||||
|
||||
/** Row vector of size: 50 */ |
||||
real_t rk_xxx[ 50 ]; |
||||
|
||||
/** Matrix of size: 4 x 48 (row major format) */ |
||||
real_t rk_kkk[ 192 ]; |
||||
|
||||
/** Row vector of size: 50 */ |
||||
real_t state[ 50 ]; |
||||
|
||||
/** Column vector of size: 300 */ |
||||
real_t d[ 300 ]; |
||||
|
||||
/** Column vector of size: 200 */ |
||||
real_t Dy[ 200 ]; |
||||
|
||||
/** Column vector of size: 3 */ |
||||
real_t DyN[ 3 ]; |
||||
|
||||
/** Matrix of size: 300 x 6 (row major format) */ |
||||
real_t evGx[ 1800 ]; |
||||
|
||||
/** Column vector of size: 300 */ |
||||
real_t evGu[ 300 ]; |
||||
|
||||
/** Column vector of size: 32 */ |
||||
real_t objAuxVar[ 32 ]; |
||||
|
||||
/** Row vector of size: 8 */ |
||||
real_t objValueIn[ 8 ]; |
||||
|
||||
/** Row vector of size: 32 */ |
||||
real_t objValueOut[ 32 ]; |
||||
|
||||
/** Matrix of size: 300 x 6 (row major format) */ |
||||
real_t Q1[ 1800 ]; |
||||
|
||||
/** Matrix of size: 300 x 4 (row major format) */ |
||||
real_t Q2[ 1200 ]; |
||||
|
||||
/** Column vector of size: 50 */ |
||||
real_t R1[ 50 ]; |
||||
|
||||
/** Matrix of size: 50 x 4 (row major format) */ |
||||
real_t R2[ 200 ]; |
||||
|
||||
/** Column vector of size: 300 */ |
||||
real_t S1[ 300 ]; |
||||
|
||||
/** Matrix of size: 6 x 6 (row major format) */ |
||||
real_t QN1[ 36 ]; |
||||
|
||||
/** Matrix of size: 6 x 3 (row major format) */ |
||||
real_t QN2[ 18 ]; |
||||
|
||||
/** Column vector of size: 6 */ |
||||
real_t Dx0[ 6 ]; |
||||
|
||||
/** Matrix of size: 6 x 6 (row major format) */ |
||||
real_t T[ 36 ]; |
||||
|
||||
/** Column vector of size: 7650 */ |
||||
real_t E[ 7650 ]; |
||||
|
||||
/** Column vector of size: 7650 */ |
||||
real_t QE[ 7650 ]; |
||||
|
||||
/** Matrix of size: 300 x 6 (row major format) */ |
||||
real_t QGx[ 1800 ]; |
||||
|
||||
/** Column vector of size: 300 */ |
||||
real_t Qd[ 300 ]; |
||||
|
||||
/** Column vector of size: 306 */ |
||||
real_t QDy[ 306 ]; |
||||
|
||||
/** Matrix of size: 50 x 6 (row major format) */ |
||||
real_t H10[ 300 ]; |
||||
|
||||
/** Matrix of size: 56 x 56 (row major format) */ |
||||
real_t H[ 3136 ]; |
||||
|
||||
/** Matrix of size: 50 x 56 (row major format) */ |
||||
real_t A[ 2800 ]; |
||||
|
||||
/** Column vector of size: 56 */ |
||||
real_t g[ 56 ]; |
||||
|
||||
/** Column vector of size: 56 */ |
||||
real_t lb[ 56 ]; |
||||
|
||||
/** Column vector of size: 56 */ |
||||
real_t ub[ 56 ]; |
||||
|
||||
/** Column vector of size: 50 */ |
||||
real_t lbA[ 50 ]; |
||||
|
||||
/** Column vector of size: 50 */ |
||||
real_t ubA[ 50 ]; |
||||
|
||||
/** Column vector of size: 56 */ |
||||
real_t x[ 56 ]; |
||||
|
||||
/** Column vector of size: 106 */ |
||||
real_t y[ 106 ]; |
||||
|
||||
|
||||
} ACADOworkspace; |
||||
|
||||
/*
|
||||
* Forward function declarations.
|
||||
*/ |
||||
|
||||
|
||||
/** Performs the integration and sensitivity propagation for one shooting interval.
|
||||
* |
||||
* \param rk_eta Working array to pass the input values and return the results. |
||||
* \param resetIntegrator The internal memory of the integrator can be reset. |
||||
* |
||||
* \return Status code of the integrator. |
||||
*/ |
||||
int acado_integrate( real_t* const rk_eta, int resetIntegrator ); |
||||
|
||||
/** Export of an ACADO symbolic function.
|
||||
* |
||||
* \param in Input to the exported function. |
||||
* \param out Output of the exported function. |
||||
*/ |
||||
void acado_rhs_forw(const real_t* in, real_t* out); |
||||
|
||||
/** Preparation step of the RTI scheme.
|
||||
* |
||||
* \return Status of the integration module. =0: OK, otherwise the error code. |
||||
*/ |
||||
int acado_preparationStep( ); |
||||
|
||||
/** Feedback/estimation step of the RTI scheme.
|
||||
* |
||||
* \return Status code of the qpOASES QP solver. |
||||
*/ |
||||
int acado_feedbackStep( ); |
||||
|
||||
/** Solver initialization. Must be called once before any other function call.
|
||||
* |
||||
* \return =0: OK, otherwise an error code of a QP solver. |
||||
*/ |
||||
int acado_initializeSolver( ); |
||||
|
||||
/** Initialize shooting nodes by a forward simulation starting from the first node.
|
||||
*/ |
||||
void acado_initializeNodesByForwardSimulation( ); |
||||
|
||||
/** Shift differential variables vector by one interval.
|
||||
* |
||||
* \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation. |
||||
* \param xEnd Value for the x vector on the last node. If =0 the old value is used. |
||||
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. |
||||
*/ |
||||
void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ); |
||||
|
||||
/** Shift controls vector by one interval.
|
||||
* |
||||
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. |
||||
*/ |
||||
void acado_shiftControls( real_t* const uEnd ); |
||||
|
||||
/** Get the KKT tolerance of the current iterate.
|
||||
* |
||||
* \return The KKT tolerance value. |
||||
*/ |
||||
real_t acado_getKKT( ); |
||||
|
||||
/** Calculate the objective value.
|
||||
* |
||||
* \return Value of the objective function. |
||||
*/ |
||||
real_t acado_getObjective( ); |
||||
|
||||
|
||||
/*
|
||||
* Extern declarations.
|
||||
*/ |
||||
|
||||
extern ACADOworkspace acadoWorkspace; |
||||
extern ACADOvariables acadoVariables; |
||||
|
||||
/** @} */ |
||||
|
||||
#ifndef __MATLAB__ |
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif /* __cplusplus */ |
||||
#endif /* __MATLAB__ */ |
||||
|
||||
#endif /* ACADO_COMMON_H */ |
||||
@ -0,0 +1,383 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
#include "acado_common.h" |
||||
|
||||
|
||||
void acado_rhs_forw(const real_t* in, real_t* out) |
||||
{ |
||||
const real_t* xd = in; |
||||
const real_t* u = in + 48; |
||||
const real_t* od = in + 49; |
||||
|
||||
/* Compute outputs: */ |
||||
out[0] = xd[1]; |
||||
out[1] = xd[2]; |
||||
out[2] = u[0]; |
||||
out[3] = xd[4]; |
||||
out[4] = xd[5]; |
||||
out[5] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[5]); |
||||
out[6] = xd[12]; |
||||
out[7] = xd[13]; |
||||
out[8] = xd[14]; |
||||
out[9] = xd[15]; |
||||
out[10] = xd[16]; |
||||
out[11] = xd[17]; |
||||
out[12] = xd[18]; |
||||
out[13] = xd[19]; |
||||
out[14] = xd[20]; |
||||
out[15] = xd[21]; |
||||
out[16] = xd[22]; |
||||
out[17] = xd[23]; |
||||
out[18] = (real_t)(0.0000000000000000e+00); |
||||
out[19] = (real_t)(0.0000000000000000e+00); |
||||
out[20] = (real_t)(0.0000000000000000e+00); |
||||
out[21] = (real_t)(0.0000000000000000e+00); |
||||
out[22] = (real_t)(0.0000000000000000e+00); |
||||
out[23] = (real_t)(0.0000000000000000e+00); |
||||
out[24] = xd[30]; |
||||
out[25] = xd[31]; |
||||
out[26] = xd[32]; |
||||
out[27] = xd[33]; |
||||
out[28] = xd[34]; |
||||
out[29] = xd[35]; |
||||
out[30] = xd[36]; |
||||
out[31] = xd[37]; |
||||
out[32] = xd[38]; |
||||
out[33] = xd[39]; |
||||
out[34] = xd[40]; |
||||
out[35] = xd[41]; |
||||
out[36] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[36]); |
||||
out[37] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[37]); |
||||
out[38] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[38]); |
||||
out[39] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[39]); |
||||
out[40] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[40]); |
||||
out[41] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[41]); |
||||
out[42] = xd[43]; |
||||
out[43] = xd[44]; |
||||
out[44] = (real_t)(1.0000000000000000e+00); |
||||
out[45] = xd[46]; |
||||
out[46] = xd[47]; |
||||
out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]); |
||||
} |
||||
|
||||
/* Fixed step size:0.2 */ |
||||
int acado_integrate( real_t* const rk_eta, int resetIntegrator ) |
||||
{ |
||||
int error; |
||||
|
||||
int run1; |
||||
acadoWorkspace.rk_ttt = 0.0000000000000000e+00; |
||||
rk_eta[6] = 1.0000000000000000e+00; |
||||
rk_eta[7] = 0.0000000000000000e+00; |
||||
rk_eta[8] = 0.0000000000000000e+00; |
||||
rk_eta[9] = 0.0000000000000000e+00; |
||||
rk_eta[10] = 0.0000000000000000e+00; |
||||
rk_eta[11] = 0.0000000000000000e+00; |
||||
rk_eta[12] = 0.0000000000000000e+00; |
||||
rk_eta[13] = 1.0000000000000000e+00; |
||||
rk_eta[14] = 0.0000000000000000e+00; |
||||
rk_eta[15] = 0.0000000000000000e+00; |
||||
rk_eta[16] = 0.0000000000000000e+00; |
||||
rk_eta[17] = 0.0000000000000000e+00; |
||||
rk_eta[18] = 0.0000000000000000e+00; |
||||
rk_eta[19] = 0.0000000000000000e+00; |
||||
rk_eta[20] = 1.0000000000000000e+00; |
||||
rk_eta[21] = 0.0000000000000000e+00; |
||||
rk_eta[22] = 0.0000000000000000e+00; |
||||
rk_eta[23] = 0.0000000000000000e+00; |
||||
rk_eta[24] = 0.0000000000000000e+00; |
||||
rk_eta[25] = 0.0000000000000000e+00; |
||||
rk_eta[26] = 0.0000000000000000e+00; |
||||
rk_eta[27] = 1.0000000000000000e+00; |
||||
rk_eta[28] = 0.0000000000000000e+00; |
||||
rk_eta[29] = 0.0000000000000000e+00; |
||||
rk_eta[30] = 0.0000000000000000e+00; |
||||
rk_eta[31] = 0.0000000000000000e+00; |
||||
rk_eta[32] = 0.0000000000000000e+00; |
||||
rk_eta[33] = 0.0000000000000000e+00; |
||||
rk_eta[34] = 1.0000000000000000e+00; |
||||
rk_eta[35] = 0.0000000000000000e+00; |
||||
rk_eta[36] = 0.0000000000000000e+00; |
||||
rk_eta[37] = 0.0000000000000000e+00; |
||||
rk_eta[38] = 0.0000000000000000e+00; |
||||
rk_eta[39] = 0.0000000000000000e+00; |
||||
rk_eta[40] = 0.0000000000000000e+00; |
||||
rk_eta[41] = 1.0000000000000000e+00; |
||||
rk_eta[42] = 0.0000000000000000e+00; |
||||
rk_eta[43] = 0.0000000000000000e+00; |
||||
rk_eta[44] = 0.0000000000000000e+00; |
||||
rk_eta[45] = 0.0000000000000000e+00; |
||||
rk_eta[46] = 0.0000000000000000e+00; |
||||
rk_eta[47] = 0.0000000000000000e+00; |
||||
acadoWorkspace.rk_xxx[48] = rk_eta[48]; |
||||
acadoWorkspace.rk_xxx[49] = rk_eta[49]; |
||||
|
||||
for (run1 = 0; run1 < 1; ++run1) |
||||
{ |
||||
acadoWorkspace.rk_xxx[0] = + rk_eta[0]; |
||||
acadoWorkspace.rk_xxx[1] = + rk_eta[1]; |
||||
acadoWorkspace.rk_xxx[2] = + rk_eta[2]; |
||||
acadoWorkspace.rk_xxx[3] = + rk_eta[3]; |
||||
acadoWorkspace.rk_xxx[4] = + rk_eta[4]; |
||||
acadoWorkspace.rk_xxx[5] = + rk_eta[5]; |
||||
acadoWorkspace.rk_xxx[6] = + rk_eta[6]; |
||||
acadoWorkspace.rk_xxx[7] = + rk_eta[7]; |
||||
acadoWorkspace.rk_xxx[8] = + rk_eta[8]; |
||||
acadoWorkspace.rk_xxx[9] = + rk_eta[9]; |
||||
acadoWorkspace.rk_xxx[10] = + rk_eta[10]; |
||||
acadoWorkspace.rk_xxx[11] = + rk_eta[11]; |
||||
acadoWorkspace.rk_xxx[12] = + rk_eta[12]; |
||||
acadoWorkspace.rk_xxx[13] = + rk_eta[13]; |
||||
acadoWorkspace.rk_xxx[14] = + rk_eta[14]; |
||||
acadoWorkspace.rk_xxx[15] = + rk_eta[15]; |
||||
acadoWorkspace.rk_xxx[16] = + rk_eta[16]; |
||||
acadoWorkspace.rk_xxx[17] = + rk_eta[17]; |
||||
acadoWorkspace.rk_xxx[18] = + rk_eta[18]; |
||||
acadoWorkspace.rk_xxx[19] = + rk_eta[19]; |
||||
acadoWorkspace.rk_xxx[20] = + rk_eta[20]; |
||||
acadoWorkspace.rk_xxx[21] = + rk_eta[21]; |
||||
acadoWorkspace.rk_xxx[22] = + rk_eta[22]; |
||||
acadoWorkspace.rk_xxx[23] = + rk_eta[23]; |
||||
acadoWorkspace.rk_xxx[24] = + rk_eta[24]; |
||||
acadoWorkspace.rk_xxx[25] = + rk_eta[25]; |
||||
acadoWorkspace.rk_xxx[26] = + rk_eta[26]; |
||||
acadoWorkspace.rk_xxx[27] = + rk_eta[27]; |
||||
acadoWorkspace.rk_xxx[28] = + rk_eta[28]; |
||||
acadoWorkspace.rk_xxx[29] = + rk_eta[29]; |
||||
acadoWorkspace.rk_xxx[30] = + rk_eta[30]; |
||||
acadoWorkspace.rk_xxx[31] = + rk_eta[31]; |
||||
acadoWorkspace.rk_xxx[32] = + rk_eta[32]; |
||||
acadoWorkspace.rk_xxx[33] = + rk_eta[33]; |
||||
acadoWorkspace.rk_xxx[34] = + rk_eta[34]; |
||||
acadoWorkspace.rk_xxx[35] = + rk_eta[35]; |
||||
acadoWorkspace.rk_xxx[36] = + rk_eta[36]; |
||||
acadoWorkspace.rk_xxx[37] = + rk_eta[37]; |
||||
acadoWorkspace.rk_xxx[38] = + rk_eta[38]; |
||||
acadoWorkspace.rk_xxx[39] = + rk_eta[39]; |
||||
acadoWorkspace.rk_xxx[40] = + rk_eta[40]; |
||||
acadoWorkspace.rk_xxx[41] = + rk_eta[41]; |
||||
acadoWorkspace.rk_xxx[42] = + rk_eta[42]; |
||||
acadoWorkspace.rk_xxx[43] = + rk_eta[43]; |
||||
acadoWorkspace.rk_xxx[44] = + rk_eta[44]; |
||||
acadoWorkspace.rk_xxx[45] = + rk_eta[45]; |
||||
acadoWorkspace.rk_xxx[46] = + rk_eta[46]; |
||||
acadoWorkspace.rk_xxx[47] = + rk_eta[47]; |
||||
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); |
||||
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[0] + rk_eta[0]; |
||||
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[1] + rk_eta[1]; |
||||
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[2] + rk_eta[2]; |
||||
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[3] + rk_eta[3]; |
||||
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[4] + rk_eta[4]; |
||||
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[5] + rk_eta[5]; |
||||
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[6] + rk_eta[6]; |
||||
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[7] + rk_eta[7]; |
||||
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[8] + rk_eta[8]; |
||||
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[9] + rk_eta[9]; |
||||
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[10] + rk_eta[10]; |
||||
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[11] + rk_eta[11]; |
||||
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[12] + rk_eta[12]; |
||||
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[13] + rk_eta[13]; |
||||
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[14] + rk_eta[14]; |
||||
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[15] + rk_eta[15]; |
||||
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[16] + rk_eta[16]; |
||||
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[17] + rk_eta[17]; |
||||
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[18] + rk_eta[18]; |
||||
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[19] + rk_eta[19]; |
||||
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[20] + rk_eta[20]; |
||||
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[21] + rk_eta[21]; |
||||
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[22] + rk_eta[22]; |
||||
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[23] + rk_eta[23]; |
||||
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[24] + rk_eta[24]; |
||||
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[25] + rk_eta[25]; |
||||
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[26] + rk_eta[26]; |
||||
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[27] + rk_eta[27]; |
||||
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[28] + rk_eta[28]; |
||||
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[29] + rk_eta[29]; |
||||
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[30] + rk_eta[30]; |
||||
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[31] + rk_eta[31]; |
||||
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[32] + rk_eta[32]; |
||||
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[33] + rk_eta[33]; |
||||
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[34] + rk_eta[34]; |
||||
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[35] + rk_eta[35]; |
||||
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[36] + rk_eta[36]; |
||||
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[37] + rk_eta[37]; |
||||
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[38] + rk_eta[38]; |
||||
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[39] + rk_eta[39]; |
||||
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[40] + rk_eta[40]; |
||||
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[41] + rk_eta[41]; |
||||
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[42] + rk_eta[42]; |
||||
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[43] + rk_eta[43]; |
||||
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[44] + rk_eta[44]; |
||||
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[45] + rk_eta[45]; |
||||
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[46] + rk_eta[46]; |
||||
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[47] + rk_eta[47]; |
||||
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); |
||||
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0]; |
||||
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1]; |
||||
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2]; |
||||
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3]; |
||||
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4]; |
||||
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5]; |
||||
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6]; |
||||
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7]; |
||||
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8]; |
||||
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9]; |
||||
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10]; |
||||
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11]; |
||||
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12]; |
||||
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13]; |
||||
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14]; |
||||
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15]; |
||||
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16]; |
||||
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17]; |
||||
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18]; |
||||
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19]; |
||||
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20]; |
||||
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21]; |
||||
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22]; |
||||
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23]; |
||||
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[72] + rk_eta[24]; |
||||
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[73] + rk_eta[25]; |
||||
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[74] + rk_eta[26]; |
||||
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[75] + rk_eta[27]; |
||||
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[76] + rk_eta[28]; |
||||
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[77] + rk_eta[29]; |
||||
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[78] + rk_eta[30]; |
||||
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[79] + rk_eta[31]; |
||||
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[80] + rk_eta[32]; |
||||
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[81] + rk_eta[33]; |
||||
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[82] + rk_eta[34]; |
||||
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[83] + rk_eta[35]; |
||||
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[84] + rk_eta[36]; |
||||
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[85] + rk_eta[37]; |
||||
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[86] + rk_eta[38]; |
||||
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[87] + rk_eta[39]; |
||||
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[88] + rk_eta[40]; |
||||
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[89] + rk_eta[41]; |
||||
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[90] + rk_eta[42]; |
||||
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[91] + rk_eta[43]; |
||||
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[92] + rk_eta[44]; |
||||
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[93] + rk_eta[45]; |
||||
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[94] + rk_eta[46]; |
||||
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[95] + rk_eta[47]; |
||||
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) ); |
||||
acadoWorkspace.rk_xxx[0] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0]; |
||||
acadoWorkspace.rk_xxx[1] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1]; |
||||
acadoWorkspace.rk_xxx[2] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2]; |
||||
acadoWorkspace.rk_xxx[3] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3]; |
||||
acadoWorkspace.rk_xxx[4] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4]; |
||||
acadoWorkspace.rk_xxx[5] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5]; |
||||
acadoWorkspace.rk_xxx[6] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6]; |
||||
acadoWorkspace.rk_xxx[7] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7]; |
||||
acadoWorkspace.rk_xxx[8] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8]; |
||||
acadoWorkspace.rk_xxx[9] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9]; |
||||
acadoWorkspace.rk_xxx[10] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10]; |
||||
acadoWorkspace.rk_xxx[11] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11]; |
||||
acadoWorkspace.rk_xxx[12] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12]; |
||||
acadoWorkspace.rk_xxx[13] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13]; |
||||
acadoWorkspace.rk_xxx[14] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14]; |
||||
acadoWorkspace.rk_xxx[15] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15]; |
||||
acadoWorkspace.rk_xxx[16] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16]; |
||||
acadoWorkspace.rk_xxx[17] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17]; |
||||
acadoWorkspace.rk_xxx[18] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18]; |
||||
acadoWorkspace.rk_xxx[19] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19]; |
||||
acadoWorkspace.rk_xxx[20] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20]; |
||||
acadoWorkspace.rk_xxx[21] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21]; |
||||
acadoWorkspace.rk_xxx[22] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22]; |
||||
acadoWorkspace.rk_xxx[23] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23]; |
||||
acadoWorkspace.rk_xxx[24] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24]; |
||||
acadoWorkspace.rk_xxx[25] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25]; |
||||
acadoWorkspace.rk_xxx[26] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26]; |
||||
acadoWorkspace.rk_xxx[27] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27]; |
||||
acadoWorkspace.rk_xxx[28] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28]; |
||||
acadoWorkspace.rk_xxx[29] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29]; |
||||
acadoWorkspace.rk_xxx[30] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30]; |
||||
acadoWorkspace.rk_xxx[31] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31]; |
||||
acadoWorkspace.rk_xxx[32] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32]; |
||||
acadoWorkspace.rk_xxx[33] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33]; |
||||
acadoWorkspace.rk_xxx[34] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34]; |
||||
acadoWorkspace.rk_xxx[35] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35]; |
||||
acadoWorkspace.rk_xxx[36] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36]; |
||||
acadoWorkspace.rk_xxx[37] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37]; |
||||
acadoWorkspace.rk_xxx[38] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38]; |
||||
acadoWorkspace.rk_xxx[39] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39]; |
||||
acadoWorkspace.rk_xxx[40] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40]; |
||||
acadoWorkspace.rk_xxx[41] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41]; |
||||
acadoWorkspace.rk_xxx[42] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42]; |
||||
acadoWorkspace.rk_xxx[43] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43]; |
||||
acadoWorkspace.rk_xxx[44] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44]; |
||||
acadoWorkspace.rk_xxx[45] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45]; |
||||
acadoWorkspace.rk_xxx[46] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46]; |
||||
acadoWorkspace.rk_xxx[47] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47]; |
||||
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) ); |
||||
rk_eta[0] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[144]; |
||||
rk_eta[1] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[145]; |
||||
rk_eta[2] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[146]; |
||||
rk_eta[3] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[147]; |
||||
rk_eta[4] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[148]; |
||||
rk_eta[5] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[149]; |
||||
rk_eta[6] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[150]; |
||||
rk_eta[7] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[151]; |
||||
rk_eta[8] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[152]; |
||||
rk_eta[9] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[153]; |
||||
rk_eta[10] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[154]; |
||||
rk_eta[11] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[155]; |
||||
rk_eta[12] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[156]; |
||||
rk_eta[13] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[157]; |
||||
rk_eta[14] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[158]; |
||||
rk_eta[15] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[159]; |
||||
rk_eta[16] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[160]; |
||||
rk_eta[17] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[161]; |
||||
rk_eta[18] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[162]; |
||||
rk_eta[19] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[163]; |
||||
rk_eta[20] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[164]; |
||||
rk_eta[21] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[165]; |
||||
rk_eta[22] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[166]; |
||||
rk_eta[23] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[167]; |
||||
rk_eta[24] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[168]; |
||||
rk_eta[25] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[169]; |
||||
rk_eta[26] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[170]; |
||||
rk_eta[27] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[171]; |
||||
rk_eta[28] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[172]; |
||||
rk_eta[29] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[173]; |
||||
rk_eta[30] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[174]; |
||||
rk_eta[31] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[175]; |
||||
rk_eta[32] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[176]; |
||||
rk_eta[33] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[177]; |
||||
rk_eta[34] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[178]; |
||||
rk_eta[35] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[179]; |
||||
rk_eta[36] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[180]; |
||||
rk_eta[37] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[181]; |
||||
rk_eta[38] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[182]; |
||||
rk_eta[39] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[183]; |
||||
rk_eta[40] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[184]; |
||||
rk_eta[41] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[185]; |
||||
rk_eta[42] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[186]; |
||||
rk_eta[43] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[187]; |
||||
rk_eta[44] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[188]; |
||||
rk_eta[45] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[189]; |
||||
rk_eta[46] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[190]; |
||||
rk_eta[47] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[191]; |
||||
acadoWorkspace.rk_ttt += 1.0000000000000000e+00; |
||||
} |
||||
error = 0; |
||||
return error; |
||||
} |
||||
|
||||
@ -0,0 +1,70 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
extern "C" |
||||
{ |
||||
#include "acado_common.h" |
||||
} |
||||
|
||||
#include "INCLUDE/QProblem.hpp" |
||||
|
||||
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 |
||||
#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" |
||||
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ |
||||
|
||||
static int acado_nWSR; |
||||
|
||||
|
||||
|
||||
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 |
||||
static SolutionAnalysis acado_sa; |
||||
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ |
||||
|
||||
int acado_solve( void ) |
||||
{ |
||||
acado_nWSR = QPOASES_NWSRMAX; |
||||
|
||||
QProblem qp(56, 50); |
||||
|
||||
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); |
||||
|
||||
qp.getPrimalSolution( acadoWorkspace.x ); |
||||
qp.getDualSolution( acadoWorkspace.y ); |
||||
|
||||
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 |
||||
|
||||
if (retVal != SUCCESSFUL_RETURN) |
||||
return (int)retVal; |
||||
|
||||
retVal = acado_sa.getHessianInverse( &qp,var ); |
||||
|
||||
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ |
||||
|
||||
return (int)retVal; |
||||
} |
||||
|
||||
int acado_getNWSR( void ) |
||||
{ |
||||
return acado_nWSR; |
||||
} |
||||
|
||||
const char* acado_getErrorString( int error ) |
||||
{ |
||||
return MessageHandling::getErrorString( error ); |
||||
} |
||||
@ -0,0 +1,65 @@ |
||||
/*
|
||||
* This file was auto-generated using the ACADO Toolkit. |
||||
*
|
||||
* While ACADO Toolkit is free software released under the terms of |
||||
* the GNU Lesser General Public License (LGPL), the generated code |
||||
* as such remains the property of the user who used ACADO Toolkit |
||||
* to generate this code. In particular, user dependent data of the code |
||||
* do not inherit the GNU LGPL license. On the other hand, parts of the |
||||
* generated code that are a direct copy of source code from the |
||||
* ACADO Toolkit or the software tools it is based on, remain, as derived |
||||
* work, automatically covered by the LGPL license. |
||||
*
|
||||
* ACADO Toolkit is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
*
|
||||
*/ |
||||
|
||||
|
||||
#ifndef QPOASES_HEADER |
||||
#define QPOASES_HEADER |
||||
|
||||
#ifdef PC_DEBUG |
||||
#include <stdio.h> |
||||
#endif /* PC_DEBUG */ |
||||
|
||||
#include <math.h> |
||||
|
||||
#ifdef __cplusplus |
||||
#define EXTERNC extern "C" |
||||
#else |
||||
#define EXTERNC |
||||
#endif |
||||
|
||||
/*
|
||||
* A set of options for qpOASES |
||||
*/ |
||||
|
||||
/** Maximum number of optimization variables. */ |
||||
#define QPOASES_NVMAX 56 |
||||
/** Maximum number of constraints. */ |
||||
#define QPOASES_NCMAX 50 |
||||
/** Maximum number of working set recalculations. */ |
||||
#define QPOASES_NWSRMAX 500 |
||||
/** Print level for qpOASES. */ |
||||
#define QPOASES_PRINTLEVEL PL_NONE |
||||
/** The value of EPS */ |
||||
#define QPOASES_EPS 2.221e-16 |
||||
/** Internally used floating point type */ |
||||
typedef double real_t; |
||||
|
||||
/*
|
||||
* Forward function declarations |
||||
*/ |
||||
|
||||
/** A function that calls the QP solver */ |
||||
EXTERNC int acado_solve( void ); |
||||
|
||||
/** Get the number of active set changes */ |
||||
EXTERNC int acado_getNWSR( void ); |
||||
|
||||
/** Get the error string. */ |
||||
const char* acado_getErrorString( int error ); |
||||
|
||||
#endif /* QPOASES_HEADER */ |
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,94 @@ |
||||
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 |
||||
|
||||
# recover quickly if dV is positive and aEgo is negative or viceversa |
||||
if dV > 0. and aEgo < 0.: |
||||
jMax *= 3. |
||||
elif dV < 0. and aEgo > 0.: |
||||
jMin *= 3. |
||||
|
||||
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) |
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue