def configure_model(self):
"""
Create state and input variables, as well as describe the vehicle's physical properties by using approximative \
motion's equations.
"""
self._model = Model("continuous")
# Create state and input variables
self._position_x = self._model.set_variable(var_type="_x",
var_name="position_x",
shape=(1, 1))
self._position_y = self._model.set_variable(var_type="_x",
var_name="position_y",
shape=(1, 1))
self._heading_angle = self._model.set_variable(
var_type="_x", var_name="heading_angle", shape=(1, 1))
self._velocity = self._model.set_variable(var_type="_u",
var_name="velocity",
shape=(1, 1))
self._steering_angle = self._model.set_variable(
var_type="_u", var_name="steering_angle", shape=(1, 1))
# Next state equations - these mathematical formulas are the core of the model
slip_factor = casadi.arctan(LR * casadi.tan(self._steering_angle) /
WHEELBASE_LENGTH)
self._model.set_rhs(
"position_x",
self._velocity * casadi.cos(self._heading_angle + slip_factor))
self._model.set_rhs(
"position_y",
self._velocity * casadi.sin(self._heading_angle + slip_factor))
self._model.set_rhs(
"heading_angle",
self._velocity * casadi.tan(self._steering_angle) *
casadi.cos(slip_factor) / WHEELBASE_LENGTH)
def bicycle_robot_model(q, u, L=0.3, dt=0.01):
"""
Implements the discrete time dynamics of your robot.
i.e. this function implements F in
q_{t+1} = F(q_{t}, u_{t})
dt is the discretization timestep.
L is the axel-to-axel length of the car.
q = array of casadi MX.sym symbolic variables [x, y, theta, phi].
u = array of casadi MX.sym symbolic variables [u1, u2] (velocity and steering inputs).
Use the casadi sin, cos, tan functions.
The casadi vertcat or vcat functions may also be useful. Note that to turn a list of
casadi expressions into a column vector of those expressions, you should use vertcat or
vcat. vertcat takes as input a variable number of arguments, and vcat takes as input a list.
Example:
x = MX.sym('x')
y = MX.sym('y')
z = MX.sym('z')
q = vertcat(x + y, y, z) # makes a 3x1 matrix with entries x + y, y, and z.
# OR
q = vcat([x + y, y, z]) # makes a 3x1 matrix with entries x + y, y, and z.
"""
x, y, theta, sigma = q[0], q[1], q[2], q[3]
v, w = u[0], u[1]
x_dot = v * cos(theta)
y_dot = v * sin(theta)
theta_dot = v * tan(sigma) / L
sigma_dot = w
result = vertcat(x + x_dot * dt, y + y_dot * dt, theta + theta_dot * dt,
sigma + sigma_dot * dt)
return result
def proc_model(self):
g1 = c.MX.zeros(self.nx,self.nx)
#assigning zeros
# for i in range(self.nx):
# for j in range(self.nx):
# g1[i,j] = 0
g1[0,3] = 1; g1[1,4] = 1; g1[2,5] = 1;
g1[6,9] = 1; g1[6,10] = c.sin(self.x[6])*c.tan(self.x[7]); g1[6,11] = c.cos(self.x[6])*c.tan(self.x[7]);
g1[7,10] = c.cos(self.x[6]); g1[7,11] = -c.sin(self.x[6]);
g1[8,10] = c.sin(self.x[6])/c.cos(self.x[7]); g1[8,11] = c.cos(self.x[6])/c.cos(self.x[7]);
g2 = c.MX.zeros(self.nx,self.nu)
g2[3,0] = (c.cos(self.x[8])*c.sin(self.x[7])*c.cos(self.x[6]) + c.sin(self.x[8])*c.sin(self.x[6]))/self.m
g2[4,0] = (c.sin(self.x[8])*c.sin(self.x[7])*c.cos(self.x[6]) - c.cos(self.x[8])*c.sin(self.x[6]))/self.m
g2[5,0] = c.cos(self.x[7])*c.cos(self.x[6])/self.m
g2[9:,1:] = c.inv(self.Ic)
g3 = c.MX.zeros(self.nx,1)
f = c.Function('f',[self.x,self.u],[self.x + c.mtimes(g1,self.x)*self.dt + c.mtimes(g2,self.u)*self.dt - g3*self.g*self.dt])
A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])
B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])
return f,A, B
def exp(cls, v):
assert v.shape == (3, 1) or v.shape == (3, )
angle = ca.norm_2(v)
res = ca.SX(4, 1)
res[:3] = ca.tan(angle / 4) * v / angle
res[3] = 0
return ca.if_else(angle > eps, res, ca.SX([0, 0, 0, 0]))
def proc_model(self):
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
g = c.MX(self.nx,self.nu)
g[0,0] = c.cos(self.x[2]); g[0,1] = 0;
g[1,0] = c.sin(self.x[2]); g[1,1] = 0;
g[2,0] = c.tan(self.x[3])/self.length; g[2,1] = 0
g[3,0] = 0; g[3,1] = 1;
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
f = c.Function('f',[self.x,self.u],[self.x + c.mtimes(g,self.u)*self.dt])
# A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.x),
# c.jacobian(f(self.x,self.u)[1],self.x),
# c.jacobian(f(self.x,self.u)[2],self.x),
# c.jacobian(f(self.x,self.u)[3],self.x)]) #linearization
# B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.u),
# c.jacobian(f(self.x,self.u)[1],self.u),
# c.jacobian(f(self.x,self.u)[2],self.u),
# c.jacobian(f(self.x,self.u)[3],self.u)])
A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])
B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])
return f,A, B
def __init__(self, N=30, step=0.01):
# We construct the model as a set of differential-algebraic equations (DAE)
self.dae = casadi.DaeBuilder()
# Parameters
self.n = 4
self.m = 2 # controls
self.N = N
self.step = step
self.T = N * step # Time horizon (seconds)
self.u_upper = 2.5
self.fixed_points = dict() # None yet
# Constants
self.lr = 2.10 # distance from CG to back wheel in meters
self.lf = 2.67
# Source, page 40: https://www.diva-portal.org/smash/get/diva2:860675/FULLTEXT01.pdf
# States
z = self.dae.add_x('z', self.n)
x = z[0]
y = z[1]
v = z[2]
psi = z[3]
self.STATE_NAMES = [
'x',
'y',
'v',
'psi',
]
# Controls
u = self.dae.add_u('u', self.m) # acceleration
a = u[0]
delta_f = u[1] # front steering angle
# This is a weird "state".
beta = casadi.arctan(self.lr / (self.lr + self.lf) *
casadi.tan(delta_f))
self.CONTROL_NAMES = ['a', 'delta_f']
# Define ODEs
xdot = v * casadi.cos(psi + beta)
ydot = v * casadi.sin(psi + beta)
vdot = a
psidot = v / self.lr * casadi.sin(beta)
zdot = casadi.vertcat(xdot, ydot, vdot, psidot)
self.dae.add_ode('zdot', zdot)
# Customize Matplotlib:
mpl.rcParams['font.size'] = 18
mpl.rcParams['lines.linewidth'] = 3
mpl.rcParams['axes.grid'] = True
self.state_estimate = None
self.control_estimate = np.zeros((self.m, self.N))
def _add_constraints(self):
# State Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.V_MIN, self.v_dv, self.V_MAX))
# Initial State Constraint
self.opti.subject_to(self.x_dv[0] == self.z_curr[0])
self.opti.subject_to(self.y_dv[0] == self.z_curr[1])
self.opti.subject_to(self.psi_dv[0] == self.z_curr[2])
self.opti.subject_to(self.v_dv[0] == self.z_curr[3])
# State Dynamics Constraints
for i in range(self.N):
beta = casadi.atan(self.L_R / (self.L_F + self.L_R) *
casadi.tan(self.df_dv[i]))
self.opti.subject_to(
self.x_dv[i + 1] == self.x_dv[i] + self.DT *
(self.v_dv[i] * casadi.cos(self.psi_dv[i] + beta)))
self.opti.subject_to(
self.y_dv[i + 1] == self.y_dv[i] + self.DT *
(self.v_dv[i] * casadi.sin(self.psi_dv[i] + beta)))
self.opti.subject_to(self.psi_dv[i +
1] == self.psi_dv[i] + self.DT *
(self.v_dv[i] / self.L_R * casadi.sin(beta)))
self.opti.subject_to(self.v_dv[i + 1] == self.v_dv[i] + self.DT *
(self.acc_dv[i]))
# Input Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_MIN, self.acc_dv, self.A_MAX))
self.opti.subject_to(
self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX))
# Input Rate Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[0],
self.acc_dv[0] - self.u_prev[0],
self.A_DOT_MAX + self.sl_acc_dv[0]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[0],
self.df_dv[0] - self.u_prev[1],
self.DF_DOT_MAX + self.sl_df_dv[0]))
for i in range(self.N - 1):
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[i + 1],
self.acc_dv[i + 1] - self.acc_dv[i],
self.A_DOT_MAX + self.sl_acc_dv[i + 1]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[i + 1],
self.df_dv[i + 1] - self.df_dv[i],
self.DF_DOT_MAX + self.sl_df_dv[i + 1]))
# Other Constraints
self.opti.subject_to(0 <= self.sl_df_dv)
self.opti.subject_to(0 <= self.sl_acc_dv)
def incidence_angle_function(latitude, day_of_year, time, scattering=False):
"""
What is the fraction of insolation that a horizontal surface will receive as a function of sun position in the sky?
:param latitude: Latitude [degrees]
:param day_of_year: Julian day (1 == Jan. 1, 365 == Dec. 31)
:param time: Time since (local) solar noon [seconds]
:param scattering: Boolean: include scattering effects at very low angles?
"""
# Old description:
# To first-order, this is true. In class, Kevin Uleck claimed that you have higher-than-cosine losses at extreme angles,
# since you get reflection losses. However, an experiment by Sharma appears to not reproduce this finding, showing only a
# 0.4-percentage-point drop in cell efficiency from 0 to 60 degrees. So, for now, we'll just say it's a cosine loss.
# Sharma: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6611928/
elevation_angle = solar_elevation_angle(latitude, day_of_year, time)
theta = 90 - elevation_angle # Angle between panel normal and the sun, in degrees
cosine_factor = cosd(theta)
if not scattering:
return cosine_factor
else:
# Calculate scattering knockdown (See C:\Users\User\Google Drive\School\Grad School\2020 Spring\16-885\Solar Panel Scattering Rough Fit)
theta_rad = theta * cas.pi / 180
# Model 1
c = (0.27891510500505767300438719757949,
-0.015994330894744987481281839336589,
-19.707332432605799255043166340329,
-0.66260979582573353852126274432521)
scattering_factor = c[0] + c[3] * theta_rad + cas.exp(
c[1] * (cas.tan(theta_rad - 1e-8) + c[2] * theta_rad))
# Model 2
# c = (
# -0.04636,
# -0.3171
# )
# scattering_factor = cas.exp(
# c[0] * (
# cas.tan(theta_rad-1e-8) + c[1] * theta_rad
# )
# )
# Model 3
# p1 = -21.74
# p2 = 282.6
# p3 = -1538
# p4 = 1786
# q1 = -923.2
# q2 = 1456
# x = theta_rad
# scattering_factor = ((p1*x**3 + p2*x**2 + p3*x + p4) /
# (x**2 + q1*x + q2))
# scattering_factor = cas.fmin(cas.fmax(scattering_factor, 0), 1)
return cosine_factor * scattering_factor
def omegaToRpyDot(self, rpy, omega):
secant_pitch = (1.0 / cs.cos(rpy[1]))
c_roll = cs.cos(rpy[0])
s_roll = cs.sin(rpy[0])
tan_pitch = cs.tan(rpy[1])
c0 = cs.DM([1, 0, 0])
c1 = cs.vertcat(s_roll * tan_pitch, c_roll, s_roll * secant_pitch)
c2 = cs.vertcat(c_roll * tan_pitch, -s_roll, c_roll * secant_pitch)
Momega_to_rpydot = cs.horzcat(c0, c1, c2)
rpy_dot = cs.mtimes(Momega_to_rpydot, omega)
return rpy_dot
def apply_unary_opcode(code, p): assert code in unary_opcodes, "Opcode not recognized!" if code==5: return -p elif code==10: return casadi.sqrt(p) elif code==11: return casadi.sin(p) elif code==12: return casadi.cos(p) elif code==13: return casadi.tan(p) assert False
def scattering_factor(elevation_angle):
"""
Calculates a scattering factor (a factor that gives losses due to atmospheric scattering at low elevation angles).
Source: AeroSandbox/studies/SolarPanelScattering
:param elevation_angle: Angle between the horizon and the sun [degrees]
:return: Fraction of the light that is not lost to scattering.
"""
elevation_angle = cas.fmin(cas.fmax(elevation_angle, 0), 90)
theta = 90 - elevation_angle # Angle between panel normal and the sun, in degrees
theta_rad = theta * cas.pi / 180
# # Model 1
# c = (
# 0.27891510500505767300438719757949,
# -0.015994330894744987481281839336589,
# -19.707332432605799255043166340329,
# -0.66260979582573353852126274432521
# )
# scattering_factor = c[0] + c[3] * theta_rad + cas.exp(
# c[1] * (
# cas.tan(theta_rad) + c[2] * theta_rad
# )
# )
# Model 2
c = (
-0.04636,
-0.3171
)
scattering_factor = cas.exp(
c[0] * (
cas.tan(theta_rad * 0.999) + c[1] * theta_rad
)
)
# # Model 3
# p1 = -21.74
# p2 = 282.6
# p3 = -1538
# p4 = 1786
# q1 = -923.2
# q2 = 1456
# x = theta_rad
# scattering_factor = ((p1*x**3 + p2*x**2 + p3*x + p4) /
# (x**2 + q1*x + q2))
# Keep this:
# scattering_factor = cas.fmin(cas.fmax(scattering_factor, 0), 1)
return scattering_factor
def _add_constraints(self):
## State Bound Constraints
self.opti.subject_to( self.opti.bounded(self.EY_MIN, self.ey_dv, self.EY_MAX) )
self.opti.subject_to( self.opti.bounded(self.EPSI_MIN, self.epsi_dv, self.EPSI_MAX) )
## Initial State Constraint
self.opti.subject_to( self.s_dv[0] == self.z_curr[0] )
self.opti.subject_to( self.ey_dv[0] == self.z_curr[1] )
self.opti.subject_to( self.epsi_dv[0] == self.z_curr[2] )
self.opti.subject_to( self.v_dv[0] == self.z_curr[3] )
## State Dynamics Constraints
for i in range(self.N):
beta = casadi.atan( self.L_R / (self.L_F + self.L_R) * casadi.tan(self.df_dv[i]) )
dyawdt = self.v_dv[i] / self.L_R * casadi.sin(beta)
dsdt = self.v_dv[i] * casadi.cos(self.epsi_dv[i]+beta) / (1 - self.ey_dv[i] * self.curv_ref[i] )
ay = self.v_dv[i] * dyawdt
self.opti.subject_to( self.s_dv[i+1] == self.s_dv[i] + self.DT * (dsdt) )
self.opti.subject_to( self.ey_dv[i+1] == self.ey_dv[i] + self.DT * (self.v_dv[i] * casadi.sin(self.epsi_dv[i] + beta)) )
self.opti.subject_to( self.epsi_dv[i+1] == self.epsi_dv[i] + self.DT * (dyawdt - dsdt * self.curv_ref[i]) )
self.opti.subject_to( self.v_dv[i+1] == self.v_dv[i] + self.DT * (self.acc_dv[i]) )
self.opti.subject_to( self.opti.bounded(self.AY_MIN - self.sl_ay_dv[i],
ay,
self.AY_MAX + self.sl_ay_dv[i]) )
## Input Bound Constraints
self.opti.subject_to( self.opti.bounded(self.AX_MIN, self.acc_dv, self.AX_MAX) )
self.opti.subject_to( self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX) )
# Input Rate Bound Constraints
self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT,
self.acc_dv[0] - self.u_prev[0],
self.AX_DOT_MAX*self.DT) )
self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT,
self.df_dv[0] - self.u_prev[1],
self.DF_DOT_MAX*self.DT) )
for i in range(self.N - 1):
self.opti.subject_to( self.opti.bounded( self.AX_DOT_MIN*self.DT,
self.acc_dv[i+1] - self.acc_dv[i],
self.AX_DOT_MAX*self.DT) )
self.opti.subject_to( self.opti.bounded( self.DF_DOT_MIN*self.DT,
self.df_dv[i+1] - self.df_dv[i],
self.DF_DOT_MAX*self.DT) )
# Other Constraints
self.opti.subject_to( 0 <= self.sl_ay_dv ) # nonnegative lateral acceleration slack variable
def CL_over_Cl(AR, mach=0, sweep=0):
"""
Returns the ratio of 3D lift_force coefficient (with compressibility) to 2D lift_force (incompressible) coefficient.
:param AR: Aspect ratio
:param mach: Mach number
:param sweep: Sweep angle [deg]
:return:
"""
beta = cas.sqrt(1 - mach**2)
sweep_rad = sweep * np.pi / 180
# return AR / (AR + 2) # Equivalent to equation in Drela's FVA in incompressible, 2*pi*alpha limit.
# return AR / (2 + cas.sqrt(4 + AR ** 2)) # more theoretically sound at low AR
eta = 0.95
return AR / (2 + cas.sqrt(4 + (AR * beta / eta)**2 *
(1 + (cas.tan(sweep_rad) / beta)**2))
) # From Raymer, Sect. 12.4.1; citing DATCOM
def update(X, U):
"""
X contains [x,y,theta] in a column
U contains [v,s] in a column
"""
x = X[0]
y = X[1]
theta = X[2]
v = U[0]
s = U[1]
dt = 0.1 #update time
L, _ = robot_params()
#kinematics
x_new = x + v * casadi.cos(theta) * dt
y_new = y + v * casadi.sin(theta) * dt
theta_new = theta + ((v * casadi.tan(s)) / L) * dt
#lets make it into a casadi thing
X_new = casadi.blockcat([[x_new], [y_new], [theta_new]])
return X_new
def T_rpy(roll, pitch, yaw):
cr = cs.cos(roll)
sr = cs.sin(roll)
cp = cs.cos(pitch)
tp = cs.tan(pitch)
sp = cs.sin(pitch)
cy = cs.cos(yaw)
sy = cs.sin(yaw)
T = cs.SX.zeros(3, 3)
T[0, 0], T[0, 1], T[0, 2] = 1, sr * tp, cr * tp
T[1, 0], T[1, 1], T[1, 2] = 0, cr, -sr
T[2, 0], T[2, 1], T[2, 2] = 0, sr / cp, cr / cp
# T[0,1] = sr*tp
# T[0,2] = cr*tp
# T[1,0] = 0
# T[1,1] = cr
# T[1,2] = -sr
# T[2,0] = 0
# T[2,1] = sr/cp
# T[2,2] = cr/cp
return T
def f_ct(self, x, u, type='casadi'):
if type == 'casadi':
beta = lambda d: ca.atan2(self.L_r * ca.tan(d), self.L_r + self.L_f
)
x_dot = ca.vcat([
x[3] * ca.cos(x[2] + beta(u[0])),
x[3] * ca.sin(x[2] + beta(u[0])),
x[3] * ca.sin(beta(u[0])) / self.L_r, u[1]
])
elif type == 'numpy':
beta = lambda d: np.arctan2(self.L_r * np.tan(d), self.L_r + self.
L_f)
x_dot = np.array([
x[3] * np.cos(x[2] + beta(u[0])),
x[3] * np.sin(x[2] + beta(u[0])),
x[3] * np.sin(beta(u[0])) / self.L_r, u[1]
])
else:
raise RuntimeError('Dynamics type %s not recognized' % type)
return x_dot
def __init__(self, ntrailers):
nstates = 3 + ntrailers
x = ca.SX.sym('x')
y = ca.SX.sym('y')
phi = ca.SX.sym('phi')
thetas = ca.SX.sym('theta', ntrailers)
u = ca.SX.sym('u', 2)
self.ntrailers = ntrailers
self.state = ca.vertcat(x, y, phi, thetas)
self.x = x
self.y = y
self.phi = phi
self.thetas = thetas
self.u = u
g1 = ca.SX.zeros((nstates, 1))
g1[0] = ca.cos(thetas[0])
g1[1] = ca.sin(thetas[0])
if ntrailers > 0:
g1[3] = 1 * ca.tan(phi)
for i in range(1, ntrailers):
fun = lambda j: ca.cos(thetas[j - 1] - thetas[j])
g1[3 +
i] = product(fun, 1, i - 1) * ca.sin(thetas[i - 1] - thetas[i])
g2 = ca.SX.zeros((nstates, 1))
g2[2] = 1
self.g = ca.horzcat(g1, g2)
self.f = ca.SX.zeros((nstates, 1))
self.rhs = ca.Function('RHS',
[x, y, phi] + thetas.elements() + u.elements(),
[self.g @ u])
def MinCurvature():
track = run_map_gen()
txs = track[:, 0]
tys = track[:, 1]
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
# sls = np.sqrt(np.sum(np.power(np.diff(track[:, :2], axis=0), 2), axis=1))
n_max = 3
N = len(track)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f_a = ca.MX.sym('n_f', N)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan(im(th1_f, th2_f) / real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
real1 = ca.Function(
'real', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx', [th1_f1, th2_f1],
[ca.atan(im1(th1_f1, th2_f1) / real1(th1_f1, th2_f1))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# curvature = ca.Function('curv', [th_f_a], [th_f_a[1:] - th_f_a[:-1]])
grad = ca.Function(
'grad', [n_f_a],
[(o_y_e(n_f_a[1:]) - o_y_s(n_f_a[:-1])) /
ca.fmax(o_x_e(n_f_a[1:]) - o_x_s(n_f_a[:-1]), 0.1 * np.ones(N - 1))])
curvature = ca.Function(
'curv', [n_f_a],
[grad(n_f_a)[1:] - grad(n_f_a)[:-1]]) # changes in grad
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N)
nlp = {\
'x': ca.vertcat(n, th),
# 'f': ca.sumsqr(curvature(n)),
# 'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
'f': ca.sumsqr(sub_cmplx1(sub_cmplx1(th[2:], th[1:-1]), sub_cmplx1(th[1:-1], th[:-2]))),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th[:-1])),
# boundary constraints
n[0], th[0],
n[-1], #th[-1],
) \
}
S = ca.nlpsol('S', 'ipopt', nlp)
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0.append(0)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = [-n_max] * N + [-np.pi] * N
ubx = [n_max] * N + [np.pi] * N
print(curvature(n0))
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
print(f"Solution found")
x_opt = r['x']
n_set = np.array(x_opt[:N])
thetas = np.array(x_opt[1 * N:2 * N])
# print(sub_cmplx(thetas[1:], thetas[:-1]))
plt.figure(2)
th_data = sub_cmplx(thetas[1:], thetas[:-1])
plt.plot(th_data)
plt.plot(ca.fabs(th_data), '--')
plt.pause(0.001)
plt.figure(3)
plt.plot(abs(thetas), '--')
plt.plot(thetas)
plt.pause(0.001)
plt.figure(4)
plt.plot(sub_cmplx(th0[1:], th0[:-1]))
plt.pause(0.001)
plot_race_line(np.array(track), n_set, width=3, wait=True)
def compute_mprim(state_i, lattice, L, v, u_max, print_level):
N = 75
nx = 3
Nc = 3
mprim = []
for state_f in lattice:
x = ca.MX.sym('x', nx)
u = ca.MX.sym('u')
F = ca.Function('f', [x, u],
[v*ca.cos(x[2]), v*ca.sin(x[2]), v*ca.tan(u)/L])
opti = ca.Opti()
X = opti.variable(nx, N+1)
pos_x = X[0, :]
pos_y = X[1, :]
ang_th = X[2, :]
U = opti.variable(N, 1)
T = opti.variable(1)
dt = T/N
opti.set_initial(T, 0.1)
opti.set_initial(U, 0.0*np.ones(N))
opti.set_initial(pos_x, np.linspace(state_i[0], state_f[0], N+1))
opti.set_initial(pos_y, np.linspace(state_i[1], state_f[1], N+1))
tau = ca.collocation_points(Nc, 'radau')
C, _ = ca.collocation_interpolators(tau)
Xc_vec = []
for k in range(N):
Xc = opti.variable(nx, Nc)
Xc_vec.append(Xc)
X_kc = ca.horzcat(X[:, k], Xc)
for j in range(Nc):
fo = F(Xc[:, j], U[k])
opti.subject_to(X_kc@C[j+1] == dt*ca.vertcat(fo[0], fo[1], fo[2]))
opti.subject_to(X_kc[:, Nc] == X[:, k+1])
for k in range(N):
opti.subject_to(U[k] <= u_max)
opti.subject_to(-u_max <= U[k])
opti.subject_to(T >= 0.001)
opti.subject_to(X[:, 0] == state_i)
opti.subject_to(X[:, -1] == state_f)
alpha = 1e-2
opti.minimize(T + alpha*ca.sumsqr(U))
opti.solver('ipopt', {'expand': True},
{'tol': 10**-8, 'print_level': print_level})
sol = opti.solve()
pos_x_opt = sol.value(pos_x)
pos_y_opt = sol.value(pos_y)
ang_th_opt = sol.value(ang_th)
u_opt = sol.value(U)
T_opt = sol.value(T)
mprim.append({'x': pos_x_opt, 'y': pos_y_opt, 'th': ang_th_opt,
'u': u_opt, 'T': T_opt, 'ds': T_opt*v})
return np.array(mprim)
def quad_nl_euler_dynamics(self, x, u, *_):
"""
Pendulum nonlinear dynamics based on euler angle description.
:param x: state
:type x: casadi.DM or casadi.MX
:param u: control input
:type u: casadi.DM or casadi.MX
:return: state time derivative
:rtype: casadi.DM or casadi.MX, depending on inputs
"""
x1 = x[X] # x
x2 = x[Y] # y
x3 = x[Z] # z
x4 = x[VX] # Vx
x5 = x[VY] # Vy
x6 = x[VZ] # Vz
x7 = x[WX] # Wroll
x8 = x[WY] # Wpitch
x9 = x[WZ] # Wyaw
x10 = x[Roll] # Roll
x11 = x[Pitch] # Pitch
x12 = x[Yaw] # Yaw
u1 = u[0]
u2 = u[1]
u3 = u[2]
u4 = u[3]
Ix = self.Ix
Iy = self.Iy
Iz = self.Iz
xdot1 = x4
xdot2 = x5
xdot3 = x6
xdot4 = (ca.cos(x10) * ca.sin(x11) * ca.cos(x12) +
ca.sin(x10) * ca.sin(x12)) * u1 / self.m
xdot5 = (ca.cos(x10) * ca.sin(x11) * ca.sin(x12) -
ca.sin(x10) * ca.cos(x12)) * u1 / self.m
xdot6 = (ca.cos(x10) * ca.cos(x11)) * u1 / self.m - self.g
xdot7 = ((Iy - Iz) * x8 * x9 + u2) / Ix
xdot8 = ((Iz - Ix) * x9 * x7 + u3) / Iy
xdot9 = ((Ix - Iy) * x7 * x8 + u4) / Iz
xdot10 = x7 + x8 * ca.sin(x10) * ca.tan(x11) + x9 * ca.cos(
x10) * ca.tan(x11)
xdot11 = x8 * ca.cos(x10) - x9 * ca.sin(x10)
xdot12 = x8 * ca.sin(x10) / ca.cos(x11) + x9 * ca.cos(x10) / ca.cos(
x11)
dxdt = [
xdot1,
xdot2,
xdot3,
xdot4,
xdot5,
xdot6,
xdot7,
xdot8,
xdot9,
xdot10,
xdot11,
xdot12,
]
return ca.vertcat(*dxdt)
def quad_nl_euler_reduced_dynamics(self, x, u, *_):
"""
Pendulum nonlinear dynamics based on euler angle description.
:param x: state
:type x: casadi.DM or casadi.MX
:param u: control input
:type u: casadi.DM or casadi.MX
:return: state time derivative
:rtype: casadi.DM or casadi.MX, depending on inputs
"""
x1 = x[0] # x
x2 = x[1] # y
x3 = x[2] # z
x4 = x[3] # Vx
x5 = x[4] # Vy
x6 = x[5] # Vz
x10 = x[6] # Roll
x11 = x[7] # Pitch
x12 = x[8] # Yaw
T = u[0]
w1 = u[1]
w2 = u[2]
w3 = u[3]
Ix = self.Ix
Iy = self.Iy
Iz = self.Iz
xdot1 = x4
xdot2 = x5
xdot3 = x6
xdot4 = (ca.cos(x10) * ca.sin(x11) * ca.cos(x12) +
ca.sin(x10) * ca.sin(x12)) * T / self.m
xdot5 = (ca.cos(x10) * ca.sin(x11) * ca.sin(x12) -
ca.sin(x10) * ca.cos(x12)) * T / self.m
xdot6 = (ca.cos(x10) * ca.cos(x11)) * T / self.m - self.g
xdot10 = w1 + w2 * ca.sin(x10) * ca.tan(x11) + w3 * ca.cos(
x10) * ca.tan(x11)
xdot11 = w2 * ca.cos(x10) - w3 * ca.sin(x10)
xdot12 = w2 * ca.sin(x10) / ca.cos(x11) + w3 * ca.cos(x10) / ca.cos(
x11)
dxdt = [
xdot1,
xdot2,
xdot3,
xdot4,
xdot5,
xdot6,
xdot10,
xdot11,
xdot12,
]
return ca.vertcat(*dxdt)
def plot_heuristics(model, x_all, u_all, n_last=2):
n_interm_points = 301
n = len(x_all[:])
t_all = np.linspace(0, (n - 1) * model.dt, n)
t_all_dense = np.linspace(t_all[0], t_all[-1], n_interm_points)
fig, ax = plt.subplots(2, 2, figsize=(10, 10))
# ---------------- Optic acceleration cancellation ----------------- #
oac = []
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_bc_xy = ca.norm_2(x_b - x_c)
z_b = x_all[k, 'z_b']
tan_phi = ca.arctan2(z_b, r_bc_xy)
oac.append(tan_phi)
# Fit a line for OAC
fit_oac = np.polyfit(t_all[:-n_last], oac[:-n_last], 1)
fit_oac_fn = np.poly1d(fit_oac)
# Plot OAC
ax[0, 0].plot(t_all[:-n_last], oac[:-n_last],
label='Simulation', lw=3)
ax[0, 0].plot(t_all, fit_oac_fn(t_all), '--k', label='Linear fit')
# ------------------- Constant bearing angle ----------------------- #
cba = []
d = ca.veccat([ca.cos(model.m0['phi']),
ca.sin(model.m0['phi'])])
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_cb = x_b - x_c
cos_gamma = ca.mul(d.T, r_cb) / ca.norm_2(r_cb)
cba.append(np.rad2deg(np.float(ca.arccos(cos_gamma))))
# Fit a const for CBA
fit_cba = np.polyfit(t_all[:-n_last], cba[:-n_last], 0)
fit_cba_fn = np.poly1d(fit_cba)
# Smoothen the trajectory
t_part_dense = np.linspace(t_all[0], t_all[-n_last-1], 301)
cba_smooth = spline(t_all[:-n_last], cba[:-n_last], t_part_dense)
ax[1, 0].plot(t_part_dense, cba_smooth, lw=3, label='Simulation')
# Plot CBA
# ax[1, 0].plot(t_all[:-n_last], cba[:-n_last],
# label='$\gamma \\approx const$')
ax[1, 0].plot(t_all, fit_cba_fn(t_all), '--k', label='Constant fit')
# ---------- Generalized optic acceleration cancellation ----------- #
goac_smooth = spline(t_all,
model.m0['phi'] - x_all[:, 'phi'],
t_all_dense)
n_many_last = n_last *\
n_interm_points / (t_all[-1] - t_all[0]) * model.dt
# Delta
ax[0, 1].plot(t_all_dense[:-n_many_last],
np.rad2deg(goac_smooth[:-n_many_last]), lw=3,
label=r'Rotation angle $\delta$')
# Gamma
ax[0, 1].plot(t_all[:-n_last], cba[:-n_last], '--', lw=2,
label=r'Bearing angle $\gamma$')
# ax[0, 1].plot([t_all[0], t_all[-1]], [30, 30], 'k--',
# label='experimental bound')
# ax[0, 1].plot([t_all[0], t_all[-1]], [-30, -30], 'k--')
# ax[0, 1].yaxis.set_ticks(range(-60, 70, 30))
# Derivative of delta
# ax0_twin = ax[0, 1].twinx()
# ax0_twin.step(t_all,
# np.rad2deg(np.array(ca.veccat([0, u_all[:, 'w_phi']]))),
# 'g-', label='derivative $\mathrm{d}\delta/\mathrm{d}t$')
# ax0_twin.set_ylim(-90, 90)
# ax0_twin.yaxis.set_ticks(range(-90, 100, 30))
# ax0_twin.set_ylabel('$\mathrm{d}\delta/\mathrm{d}t$, deg/s')
# ax0_twin.yaxis.label.set_color('g')
# ax0_twin.legend(loc='lower right')
# -------------------- Linear optic trajectory --------------------- #
lot_beta = []
x_b = model.m0[ca.veccat, ['x_b', 'y_b']]
for k in range(n):
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
d = ca.veccat([ca.cos(x_all[k, 'phi']),
ca.sin(x_all[k, 'phi'])])
r = x_b - x_c
cos_beta = ca.mul(d.T, r) / ca.norm_2(r)
beta = ca.arccos(cos_beta)
tan_beta = ca.tan(beta)
lot_beta.append(tan_beta)
# lot_beta.append(np.rad2deg(np.float(ca.arccos(cos_beta))))
# lot_alpha = np.rad2deg(np.array(x_all[:, 'psi']))
lot_alpha = ca.tan(x_all[:, 'psi'])
# Fit a line for LOT
fit_lot = np.polyfit(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last], 1)
fit_lot_fn = np.poly1d(fit_lot)
# Plot
ax[1, 1].scatter(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last],
label='Simulation')
ax[1, 1].plot(lot_alpha[model.n_delay:-n_last],
fit_lot_fn(lot_alpha[model.n_delay:-n_last]),
'--k', label='Linear fit')
fig.tight_layout()
return fig, ax
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def skewY(self, theta):
tan = cas.tan(theta / 180 * cas.pi)
self.matrix(1, tan, 0, 1, 0, 0)
# Load the dual
if load_dual:
with open(dual_location, 'r') as infile:
dual_vals = json.load(infile)
if len(dual_vals) != opti.ng:
raise Exception(
"Couldn't load the dual, since your problem has %i cons and the cached problem has %i cons."
% (opti.ng, len(dual_vals)))
for i in tqdm(range(opti.ng), desc="Loading dual variables:"):
opti.set_initial(opti.lam_g[i], dual_vals[i])
sind = lambda theta: cas.sin(theta * cas.pi / 180)
cosd = lambda theta: cas.cos(theta * cas.pi / 180)
tand = lambda theta: cas.tan(theta * cas.pi / 180)
atan2d = lambda y_val, x_val: cas.atan2(y_val, x_val) * 180 / np.pi
clip = lambda x, min, max: cas.fmin(cas.fmax(min, x), max)
def smoothmax(value1, value2, hardness):
"""
A smooth maximum between two functions.
Useful because it's differentiable and convex!
Great writeup by John D Cook here:
https://www.johndcook.com/soft_maximum.pdf
:param value1: Value of function 1.
:param value2: Value of function 2.
:param hardness: Hardness parameter. Higher values make this closer to max(x1, x2).
:return: Soft maximum of the two supplied values.
"""
def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
opti = ca.Opti()
x = opti.variable(self.n_x*self.n_a, N+1)
u = opti.variable(self.n_u*self.n_a, N)
slack = opti.variable(self.n_x*self.n_a)
if x_guess is not None:
opti.set_initial(x, x_guess)
if u_guess is not None:
opti.set_initial(u, u_guess)
opti.set_initial(slack, np.zeros(self.n_x*self.n_a))
da_lim = self.da_lim
ddf_lim = self.ddf_lim
pairs = list(itertools.combinations(range(self.n_a), 2))
opti.subject_to(x[:,0] == np.squeeze(x_0))
for j in range(self.n_a):
opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,0]-last_u[j*self.n_u+0], ddf_lim[1]*self.dt))
opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,0]-last_u[j*self.n_u+1], da_lim[1]*self.dt))
stage_cost = 0
for i in range(N):
stage_cost = stage_cost + 1
for j in range(self.n_a):
opti.subject_to(x[j*self.n_x+0,i+1] == x[j*self.n_x+0,i] + self.dt*x[j*self.n_x+3,i]*ca.cos(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
opti.subject_to(x[j*self.n_x+1,i+1] == x[j*self.n_x+1,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(x[j*self.n_x+2,i] + ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
opti.subject_to(x[j*self.n_x+2,i+1] == x[j*self.n_x+2,i] + self.dt*x[j*self.n_x+3,i]*ca.sin(ca.atan2(self.l_r*ca.tan(u[j*self.n_u+0,i]), self.l_f + self.l_r)))
opti.subject_to(x[j*self.n_x+3,i+1] == x[j*self.n_x+3,i] + self.dt*u[j*self.n_u+1,i])
if i < N-1:
opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[j*self.n_u+0,i+1]-u[j*self.n_u+0,i], ddf_lim[1]*self.dt))
opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[j*self.n_u+1,i+1]-u[j*self.n_u+1,i], da_lim[1]*self.dt))
if self.F is not None:
opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b)
if self.H is not None:
opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g)
if expl_constraints is not None:
V = expl_constraints[i][0]
w = expl_constraints[i][1]
opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w)))
# Collision avoidance constraint
for p in pairs:
opti.subject_to(ca.norm_2(x[p[0]*self.n_x:p[0]*self.n_x+2,i] - x[p[1]*self.n_x:p[1]*self.n_x+2,i]) >= 2*self.r)
opti.subject_to(x[:,N] - x_ss == slack)
slack_cost = 1e6*ca.sumsqr(slack)
total_cost = stage_cost + slack_cost
opti.minimize(total_cost)
solver_opts = {
"mu_strategy" : "adaptive",
"mu_init" : 1e-5,
"mu_min" : 1e-15,
"barrier_tol_factor" : 1,
"print_level" : 0,
"linear_solver" : "ma27"
}
# solver_opts = {}
plugin_opts = {"verbose" : False, "print_time" : False, "print_out" : False}
opti.solver('ipopt', plugin_opts, solver_opts)
sol = opti.solve()
slack_val = sol.value(slack)
slack_valid = True
for j in range(self.n_a):
if la.norm(slack_val[j*self.n_x:(j+1)*self.n_x]) > 2e-8:
slack_valid = False
print('Warning! Solved, but with agent %i slack norm of %g is greater than 2e-8!' % (j+1, la.norm(slack_val[j*self.n_x:(j+1)*self.n_x])))
break
if sol.stats()['success'] and slack_valid:
feasible = True
x_pred = sol.value(x)
u_pred = sol.value(u)
sol_cost = sol.value(stage_cost)
else:
# print(sol.stats()['return_status'])
# print(opti.debug.show_infeasibilities())
feasible = False
x_pred = None
u_pred = None
sol_cost = None
# pdb.set_trace()
return x_pred, u_pred, sol_cost
def solve_opti(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
opti = ca.Opti()
x = opti.variable(self.n_x, N+1)
u = opti.variable(self.n_u, N)
slack = opti.variable(self.n_x)
if x_guess is not None:
opti.set_initial(x, x_guess)
if u_guess is not None:
opti.set_initial(u, u_guess)
opti.set_initial(slack, np.zeros(self.n_x))
da_lim = self.agent.da_lim
ddf_lim = self.agent.ddf_lim
opti.subject_to(x[:,0] == np.squeeze(x_0))
opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,0]-last_u[0], ddf_lim[1]*self.dt))
opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,0]-last_u[1], da_lim[1]*self.dt))
stage_cost = 0
for i in range(N):
stage_cost = stage_cost + 1
beta = ca.atan2(self.l_r*ca.tan(u[0,i]), self.l_f+self.l_r)
opti.subject_to(x[0,i+1] == x[0,i] + self.dt*x[3,i]*ca.cos(x[2,i] + beta))
opti.subject_to(x[1,i+1] == x[1,i] + self.dt*x[3,i]*ca.sin(x[2,i] + beta))
opti.subject_to(x[2,i+1] == x[2,i] + self.dt*x[3,i]*ca.sin(beta))
opti.subject_to(x[3,i+1] == x[3,i] + self.dt*u[1,i])
if self.F is not None:
opti.subject_to(ca.mtimes(self.F, x[:,i]) <= self.b)
if self.H is not None:
opti.subject_to(ca.mtimes(self.H, u[:,i]) <= self.g)
if expl_constraints is not None:
V = expl_constraints[i][0]
w = expl_constraints[i][1]
opti.subject_to(ca.mtimes(V, x[:2,i]) + w <= np.zeros(len(w)))
if i < N-1:
opti.subject_to(opti.bounded(ddf_lim[0]*self.dt, u[0,i+1]-u[0,i], ddf_lim[1]*self.dt))
opti.subject_to(opti.bounded(da_lim[0]*self.dt, u[1,i+1]-u[1,i], da_lim[1]*self.dt))
opti.subject_to(x[:,N] - x_ss == slack)
slack_cost = 1e6*ca.sumsqr(slack)
total_cost = stage_cost + slack_cost
opti.minimize(total_cost)
solver_opts = {
"mu_strategy" : "adaptive",
"mu_init" : 1e-5,
"mu_min" : 1e-15,
"barrier_tol_factor" : 1,
"print_level" : 0,
"linear_solver" : "ma27"
}
# solver_opts = {}
plugin_opts = {"verbose" : False, "print_time" : False, "print_out" : False}
opti.solver('ipopt', plugin_opts, solver_opts)
sol = opti.solve()
slack_val = sol.value(slack)
if la.norm(slack_val) > 1e-6:
print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val))
if sol.stats()['success'] and la.norm(slack_val) <= 1e-6:
print('Solve success, with slack norm of %g!' % la.norm(slack_val))
feasible = True
x_pred = sol.value(x)
u_pred = sol.value(u)
sol_cost = sol.value(stage_cost)
else:
# print(sol.stats()['return_status'])
# print(opti.debug.show_infeasibilities())
feasible = False
x_pred = None
u_pred = None
sol_cost = None
# pdb.set_trace()
return x_pred, u_pred, sol_cost
v_f = ca.MX.sym('v_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
o_x_s = ca.Function('o_x', [n_f], [track[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [track[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [track[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [track[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
d_n = ca.Function('d_n', [n_f, th_f], [sls / ca.tan(th_ns[:-1] - th_f)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]), o_y_e(
n_f_a[1:]))
])
d_th = ca.Function('d_th', [v_f, d_f], [v_f / l * ca.tan(d_f)])
# d_t = ca.Function('d_t', [n_f_a, v_f], [])
d_t = ca.Function('d_t', [n_f_a, v_f], [
dis(o_x_e(n_f_a[1:]), o_x_s(n_f_a[:-1]), o_y_e(n_f_a[1:]), o_y_s(
n_f_a[:-1])) / v_f
])
# d_t = ca.Function('d_t', [n_f_a, v_f], [ca.sqrt(ca.sum1(ca.power(ca.diff(, axis=0), 2), axis=1))])
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def MinCurvatureTrajectory(pts, nvecs, ws):
"""
This function uses optimisation to minimise the curvature of the path
"""
w_min = - ws[:, 0] * 0.9
w_max = ws[:, 1] * 0.9
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
N = len(pts)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N-1)
th_f = ca.MX.sym('n_f', N-1)
x0_f = ca.MX.sym('x0_f', N-1)
x1_f = ca.MX.sym('x1_f', N-1)
y0_f = ca.MX.sym('y0_f', N-1)
y1_f = ca.MX.sym('y1_f', N-1)
th1_f = ca.MX.sym('y1_f', N-1)
th2_f = ca.MX.sym('y1_f', N-1)
th1_f1 = ca.MX.sym('y1_f', N-2)
th2_f1 = ca.MX.sym('y1_f', N-2)
o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f], [ca.sqrt((x1_f-x0_f)**2 + (y1_f-y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]),
o_y_s(n_f_a[:-1]), o_y_e(n_f_a[1:]))])
real = ca.Function('real', [th1_f, th2_f], [ca.cos(th1_f)*ca.cos(th2_f) + ca.sin(th1_f)*ca.sin(th2_f)])
im = ca.Function('im', [th1_f, th2_f], [-ca.cos(th1_f)*ca.sin(th2_f) + ca.sin(th1_f)*ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f], [ca.atan2(im(th1_f, th2_f),real(th1_f, th2_f))])
get_th_n = ca.Function('gth', [th_f], [sub_cmplx(ca.pi*np.ones(N-1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f], [track_length(n_f_a)/ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function('real1', [th1_f1, th2_f1], [ca.cos(th1_f1)*ca.cos(th2_f1) + ca.sin(th1_f1)*ca.sin(th2_f1)])
im1 = ca.Function('im1', [th1_f1, th2_f1], [-ca.cos(th1_f1)*ca.sin(th2_f1) + ca.sin(th1_f1)*ca.cos(th2_f1)])
sub_cmplx1 = ca.Function('a_cpx1', [th1_f1, th2_f1], [ca.atan2(im1(th1_f1, th2_f1),real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N-1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
ones = np.ones(N)
n0 = ones*0
th0 = []
for i in range(N-1):
th_00 = lib.get_bearing(pts[i, 0:2], pts[i+1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = list(w_min) + [-np.pi]*(N-1)
ubx = list(w_max) + [np.pi]*(N-1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
# thetas = np.array(x_opt[1*N:2*(N-1)])
return n_set
def example_loop_v_d():
pathpoints = 30
ref_path = {}
ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1))
ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10
wp = ca.horzcat(ref_path['x'], ref_path['y']).T
N = 10
N1 = N - 1
ocp = MPC(N)
x = ocp.state('x')
y = ocp.state('y')
th = ocp.state('th')
d = ocp.control('d')
v = ocp.control('v')
dt = ocp.get_time()
ocp.set_der(x, v * ca.cos(th[:-1]))
ocp.set_der(y, v * ca.sin(th[:-1]))
ocp.set_der(th, v / 0.33 * ca.tan(d))
ocp.set_objective(dt, ca.GenMX_zeros(N1), 0.01)
# set limits
max_speed = 10
ocp.set_lims(x, -ca.inf, ca.inf)
ocp.set_lims(y, -ca.inf, ca.inf)
ocp.set_lims(v, 0, max_speed)
ocp.set_lims(th, -ca.pi, ca.pi)
ocp.set_lims(d, -0.4, 0.4)
wpts = np.array(wp[:, 0:N])
# find starting vals
T = 5
dt00 = np.array([T / N1] * N1)
ocp.set_inital(dt, dt00)
x00 = wpts[0, :]
ocp.set_inital(x, x00)
y00 = wpts[1, :]
ocp.set_inital(y, y00)
th00 = [lib.get_bearing(wpts[:, i], wpts[:, i + 1]) for i in range(N1)]
th00.append(0)
ocp.set_inital(th, th00.copy())
v00 = np.array(
[lib.get_distance(wpts[:, i], wpts[:, i + 1])
for i in range(N1)]) / dt00
ocp.set_inital(v, v00)
th00.pop(-1)
d00 = np.arctan(np.array(th00) * 0.33 / v00)
ocp.set_inital(d, d00)
ocp.set_up_solve()
for i in range(20):
wpts = np.array(wp[:, i:i + N])
x0 = [wpts[0, 0], wpts[1, 0], lib.get_bearing(wpts[:, 0], wpts[:, 1])]
ocp.set_objective(x, wpts[0, :])
ocp.set_objective(y, wpts[1, :])
states, controls, times = ocp.solve(x0)
xs = states[:N]
ys = states[N:2 * N]
ths = states[2 * N:]
vs = controls[:N1]
ds = controls[N1:]
total_time = np.sum(times)
print(f"Times: {times}")
print(f"Total Time: {total_time}")
print(f"xs: {xs.T}")
print(f"ys: {ys.T}")
print(f"Thetas: {ths.T}")
print(f"Velocities: {vs.T}")
plt.figure(1)
plt.clf()
plt.plot(wpts[0, :], wpts[1, :], 'o', markersize=12)
plt.plot(xs, ys, '+', markersize=20)
plt.pause(0.5)
def solve(self, abs_t, x_0, x_ss, N, last_u, x_guess=None, u_guess=None, expl_constraints=None, verbose=False):
x = ca.SX.sym('x', self.n_x*(N+1))
u = ca.SX.sym('y', self.n_u*N)
slack = ca.SX.sym('slack', self.n_x)
z = ca.vertcat(x, u, slack)
# Flatten candidate solutions into 1-D array (- x_1 -, - x_2 -, ..., - x_N+1 -)
if x_guess is not None:
x_guess_flat = x_guess.flatten(order='F')
else:
x_guess_flat = np.zeros(self.n_x*(N+1))
if u_guess is not None:
u_guess_flat = u_guess.flatten(order='F')
else:
u_guess_flat = np.zeros(self.n_u*N)
slack_guess = np.zeros(self.n_x)
z_guess = np.concatenate((x_guess_flat, u_guess_flat, slack_guess))
lb_slack = [-1000.0]*self.n_x
ub_slack = [1000.0]*self.n_x
# Box constraints on decision variables
lb_x = x_0.tolist() + self.state_lb*(N) + self.input_lb*(N) + lb_slack
ub_x = x_0.tolist() + self.state_ub*(N) + self.input_ub*(N) + ub_slack
# Constraints on functions of decision variables
lb_g = []
ub_g = []
stage_cost = 0
constraint = []
for i in range(N):
stage_cost += 1
# Formulate dynamics equality constraints as inequalities
beta = ca.atan2(self.l_r*ca.tan(u[self.n_u*i+0]), self.l_f+self.l_r)
constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+0] - (x[self.n_x*i+0] + self.dt*x[self.n_x*i+3]*ca.cos(x[self.n_x*i+2] + beta)))
constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+1] - (x[self.n_x*i+1] + self.dt*x[self.n_x*i+3]*ca.sin(x[self.n_x*i+2] + beta)))
constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+2] - (x[self.n_x*i+2] + self.dt*x[self.n_x*i+3]*ca.sin(beta)))
constraint = ca.vertcat(constraint, x[self.n_x*(i+1)+3] - (x[self.n_x*i+3] + self.dt*u[self.n_u*i+1]))
lb_g += [0.0]*self.n_x
ub_g += [0.0]*self.n_x
# Steering rate constraints
if self.ddf_lim is not None:
if i == 0:
# Constrain steering rate with respect to previously applied input
constraint = ca.vertcat(constraint, u[self.n_u*(i)+0]-last_u[0])
if i < N-1:
# Constrain steering rate along horizon
constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+0]-u[self.n_u*(i)+0])
lb_g += [self.ddf_lim[0]*self.dt]
ub_g += [self.ddf_lim[1]*self.dt]
# Throttle rate constraints
if self.da_lim is not None:
if i == 0:
# Constrain throttle rate
constraint = ca.vertcat(constraint, u[self.n_u*i+1]-last_u[1])
if i < N-1:
constraint = ca.vertcat(constraint, u[self.n_u*(i+1)+1]-u[self.n_u*(i)+1])
lb_g += [self.da_lim[0]*self.dt]
ub_g += [self.da_lim[1]*self.dt]
# Exploration constraints on predicted positions of agent
if expl_constraints is not None:
V = expl_constraints[i][0]
w = expl_constraints[i][1]
for j in range(V.shape[0]):
constraint = ca.vertcat(constraint, V[j,0]*x[self.n_x*i+0] + V[j,1]*x[self.n_x*i+1] + w[j])
lb_g += [-1e9]
ub_g += [0]
# Formulate terminal soft equality constraint as inequalities
constraint = ca.vertcat(constraint, x[self.n_x*N:] - x_ss + slack)
lb_g += [0.0]*self.n_x
ub_g += [0.0]*self.n_x
slack_cost = 1e6*ca.sumsqr(slack)
total_cost = stage_cost + slack_cost
opts = {'verbose' : False, 'print_time' : 0,
'ipopt.print_level' : 5,
'ipopt.mu_strategy' : 'adaptive',
'ipopt.mu_init' : 1e-5,
'ipopt.mu_min' : 1e-15,
'ipopt.barrier_tol_factor' : 1,
'ipopt.linear_solver': 'ma27'}
nlp = {'x' : z, 'f' : total_cost, 'g' : constraint}
solver = ca.nlpsol('solver', 'ipopt', nlp, opts)
sol = solver(lbx=lb_x, ubx=ub_x, lbg=lb_g, ubg=ub_g, x0=z_guess)
pdb.set_trace()
if solver.stats()['success']:
if la.norm(slack_val) <= 1e-8:
print('Solve success for safe set point', x_ss)
feasible = True
z_sol = np.array(sol['x'])
x_pred = z_sol[:self.n_x*(N+1)].reshape((N+1,self.n_x)).T
u_pred = z_sol[self.n_x*(N+1):self.n_x*(N+1)+self.n_u*N].reshape((N,self.n_u)).T
slack_val = z_sol[self.n_x*(N+1)+self.n_u*N:]
cost_val = sol['f']
else:
print('Warning! Solved, but with slack norm of %g is greater than 1e-8!' % la.norm(slack_val))
feasible = False
x_pred = None
u_pred = None
cost_val = None
else:
print(solver.stats()['return_status'])
feasible = False
x_pred = None
u_pred = None
cost_val = None
# pdb.set_trace()
# pdb.set_trace()
return x_pred, u_pred, cost_val
def Max_velocity(pts, conf, show=False):
mu = conf.mu
m = conf.m
g = conf.g
l_f = conf.l_f
l_r = conf.l_r
safety_f = conf.force_f
f_max = mu * m * g #* safety_f
f_long_max = l_f / (l_r + l_f) * f_max
max_v = conf.max_v
max_a = conf.max_a
s_i, th_i = convert_pts_s_th(pts)
th_i_1 = th_i[:-1]
s_i_1 = s_i[:-1]
N = len(s_i)
N1 = len(s_i) - 1
# setup possible casadi functions
d_x = ca.MX.sym('d_x', N-1)
d_y = ca.MX.sym('d_y', N-1)
vel = ca.Function('vel', [d_x, d_y], [ca.sqrt(ca.power(d_x, 2) + ca.power(d_y, 2))])
dx = ca.MX.sym('dx', N)
dy = ca.MX.sym('dy', N)
dt = ca.MX.sym('t', N-1)
f_long = ca.MX.sym('f_long', N-1)
f_lat = ca.MX.sym('f_lat', N-1)
nlp = {\
'x': ca.vertcat(dx, dy, dt, f_long, f_lat),
# 'f': ca.sum1(dt),
'f': ca.sumsqr(dt),
'g': ca.vertcat(
# dynamic constraints
dt - s_i_1 / ((vel(dx[:-1], dy[:-1]) + vel(dx[1:], dy[1:])) / 2 ),
# ca.arctan2(dy, dx) - th_i,
dx/dy - ca.tan(th_i),
dx[1:] - (dx[:-1] + (ca.sin(th_i_1) * f_long + ca.cos(th_i_1) * f_lat) * dt / m),
dy[1:] - (dy[:-1] + (ca.cos(th_i_1) * f_long - ca.sin(th_i_1) * f_lat) * dt / m),
# path constraints
ca.sqrt(ca.power(f_long, 2) + ca.power(f_lat, 2)),
# boundary constraints
# dx[0], dy[0]
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':0}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
# make init sol
v0 = np.ones(N) * max_v/2
dx0 = v0 * np.sin(th_i)
dy0 = v0 * np.cos(th_i)
dt0 = s_i_1 / ca.sqrt(ca.power(dx0[:-1], 2) + ca.power(dy0[:-1], 2))
f_long0 = np.zeros(N-1)
ddx0 = dx0[1:] - dx0[:-1]
ddy0 = dy0[1:] - dy0[:-1]
a0 = (ddx0**2 + ddy0**2)**0.5
f_lat0 = a0 * m
x0 = ca.vertcat(dx0, dy0, dt0, f_long0, f_lat0)
# make lbx, ubx
# lbx = [-max_v] * N + [-max_v] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1
lbx = [-max_v] * N + [0] * N + [0] * N1 + [-ca.inf] * N1 + [-f_max] * N1
ubx = [max_v] * N + [max_v] * N + [10] * N1 + [ca.inf] * N1 + [f_max] * N1
# lbx = [-max_v] * N + [0] * N + [0] * N1 + [-f_long_max] * N1 + [-f_max] * N1
# ubx = [max_v] * N + [max_v] * N + [10] * N1 + [f_long_max] * N1 + [f_max] * N1
#make lbg, ubg
lbg = [0] * N1 + [0] * N + [0] * 2 * N1 + [0] * N1 #+ [0] * 2
ubg = [0] * N1 + [0] * N + [0] * 2 * N1 + [ca.inf] * N1 #+ [0] * 2
r = S(x0=x0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
x_opt = r['x']
dx = np.array(x_opt[:N])
dy = np.array(x_opt[N:N*2])
dt = np.array(x_opt[2*N:N*2 + N1])
f_long = np.array(x_opt[2*N+N1:2*N + N1*2])
f_lat = np.array(x_opt[-N1:])
f_t = (f_long**2 + f_lat**2)**0.5
# print(f"Dt: {dt.T}")
# print(f"DT0: {dt[0]}")
t = np.cumsum(dt)
t = np.insert(t, 0, 0)
print(f"Total Time: {t[-1]}")
# print(f"Dt: {dt.T}")
# print(f"Dx: {dx.T}")
# print(f"Dy: {dy.T}")
vs = (dx**2 + dy**2)**0.5
if show:
plt.figure(1)
plt.clf()
plt.title("Velocity vs dt")
plt.plot(t, vs)
plt.plot(t, th_i)
plt.legend(['vs', 'ths'])
# plt.plot(t, dx)
# plt.plot(t, dy)
# plt.legend(['v', 'dx', 'dy'])
plt.plot(t, np.ones_like(t) * max_v, '--')
plt.figure(3)
plt.clf()
plt.title("F_long, F_lat vs t")
plt.plot(t[:-1], f_long)
plt.plot(t[:-1], f_lat)
plt.plot(t[:-1], f_t, linewidth=3)
plt.ylim([-25, 25])
plt.plot(t, np.ones_like(t) * f_max, '--')
plt.plot(t, np.ones_like(t) * -f_max, '--')
plt.plot(t, np.ones_like(t) * f_long_max, '--')
plt.plot(t, np.ones_like(t) * -f_long_max, '--')
plt.legend(['Flong', "f_lat", "f_t"])
# plt.show()
plt.pause(0.001)
# plt.figure(9)
# plt.clf()
# plt.title("F_long, F_lat vs t")
# plt.plot(t[:-1], f_long)
# plt.plot(t[:-1], f_lat)
# plt.plot(t[:-1], f_t, linewidth=3)
# plt.plot(t, np.ones_like(t) * f_max, '--')
# plt.plot(t, np.ones_like(t) * -f_max, '--')
# plt.plot(t, np.ones_like(t) * f_long_max, '--')
# plt.plot(t, np.ones_like(t) * -f_long_max, '--')
# plt.legend(['Flong', "f_lat", "f_t"])
return vs