def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
def setTrajectory(ocp, t0):
tk = t0
for k in range(N):
ocp.y[k,0] = A*C.sin(omega*tk)
ocp.y[k,1] = A*C.cos(omega*tk)*omega
tk += ts
ocp.yN[0] = A*C.sin(omega*tk)
ocp.yN[1] = A*C.cos(omega*tk)*omega
refLog.append(numpy.copy(numpy.vstack((ocp.y[:,:2], ocp.yN.T))))
def _plot_arrows_3D(name, ax, x, y, phi, psi):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
psi = ca.veccat(psi)
x_vec = ca.cos(psi) * ca.cos(phi)
y_vec = ca.cos(psi) * ca.sin(phi)
z_vec = ca.sin(psi)
ax.quiver(x + x_vec, y + y_vec, z + z_vec,
x_vec, y_vec, z_vec,
color='r', alpha=0.8)
return [Patch(color='red', label=name)]
def plot_arrows3D(name, ax, x, y, phi, theta):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
theta = ca.veccat(theta)
x_vec = ca.cos(theta)*ca.cos(phi)
y_vec = ca.cos(theta)*ca.sin(phi)
z_vec = ca.sin(theta)
length = 2.0
ax.quiver(x+length*x_vec, y+length*y_vec, z+length*z_vec,
x_vec, y_vec, z_vec,
length=length, arrow_length_ratio=0.5)
return [Patch(color='red', label=name)]
def _create_continuous_dynamics(self):
# Unpack arguments
[x_b, y_b, z_b, vx_b, vy_b, vz_b,
x_c, y_c, vx_c, vy_c, phi, psi] = self.x[...]
[F_c, w_phi, w_psi, theta] = self.u[...]
# Define the governing ordinary differential equation (ODE)
rhs = cat.struct_SX(self.x)
rhs['x_b'] = vx_b
rhs['y_b'] = vy_b
rhs['z_b'] = vz_b
rhs['vx_b'] = 0
rhs['vy_b'] = 0
rhs['vz_b'] = -self.g
rhs['x_c'] = vx_c
rhs['y_c'] = vy_c
rhs['vx_c'] = F_c * ca.cos(phi + theta) - self.mu * vx_c
rhs['vy_c'] = F_c * ca.sin(phi + theta) - self.mu * vy_c
rhs['phi'] = w_phi
rhs['psi'] = w_psi
op = {'input_scheme': ['x', 'u'],
'output_scheme': ['x_dot']}
return ca.SXFunction('Continuous dynamics',
[self.x, self.u], [rhs], op)
def continuous_dynamics(state, control):
# Unpack arguments
[x_b, y_b, z_b, vx_b, vy_b, vz_b, x_c, y_c, phi] = state[...]
[v, w, psi] = control[...]
# Define right-hand side
rhs = cat.struct_SX(state)
rhs["x_b"] = vx_b
rhs["y_b"] = vy_b
rhs["z_b"] = vz_b
rhs["vx_b"] = 0
rhs["vy_b"] = 0
rhs["vz_b"] = -g0
rhs["x_c"] = v * (ca.cos(psi) * ca.cos(phi) - ca.sin(psi) * ca.sin(phi))
rhs["y_c"] = v * (ca.cos(psi) * ca.sin(phi) + ca.sin(psi) * ca.cos(phi))
rhs["phi"] = w
return ca.SXFunction("Continuous dynamics", [state, control], [rhs])
def continuous_dynamics(state, control):
# Unpack arguments
[x_b,y_b,vx_b,vy_b,x_c,y_c,phi] = state[...]
[v,w,psi] = control[...]
# Define right-hand side
rhs = cat.struct_SX(state)
rhs['x_b'] = vx_b
rhs['y_b'] = vy_b
rhs['vx_b'] = 0
rhs['vy_b'] = 0
rhs['x_c'] = v * (ca.cos(psi) * ca.cos(phi) - \
ca.sin(psi) * ca.sin(phi))
rhs['y_c'] = v * (ca.cos(psi) * ca.sin(phi) + \
ca.sin(psi) * ca.cos(phi))
rhs['phi'] = w
return ca.SXFunction('Continuous dynamics',[state,control],[rhs])
def plot_arrows(name, ax, x, y, phi, theta):
x_vec = ca.cos(theta)*ca.cos(phi)
y_vec = ca.cos(theta)*ca.sin(phi)
ax.quiver(x, y, x_vec, y_vec,
units='xy', angles='xy', scale=1, width=0.1,
headwidth=4, headlength=6, headaxislength=4,
color='r', lw='0.1')
return [Patch(color='red', label=name)]
def _plot_arrows(name, ax, x, y, phi):
x_vec = ca.cos(phi)
y_vec = ca.sin(phi)
ax.quiver(x, y, x_vec, y_vec,
units='xy', angles='xy', scale=1, width=0.08,
headwidth=4, headlength=6, headaxislength=5,
color='r', alpha=0.8, lw=0.1)
return [Patch(color='red', label=name)]
def init(self, horizon_times=None):
ObstaclexD.init(self, horizon_times=horizon_times)
if self.signals['angular_velocity'][:, -1] == 0.:
self.cos = cos(self.signals['orientation'][:, -1][0])
self.sin = sin(self.signals['orientation'][:, -1][0])
self.gon_weight = 1.
return
# theta, omega
theta = self.define_parameter('theta', 1)
omega = self.signals['angular_velocity'][:, -1][0]
# theta, omega at time zero of time horizon
theta0 = theta - self.t*omega
# cos, sin, weight spline over time horizon
Ts = 2.*np.pi/abs(omega)
if self.options['horizon_time'] is None:
raise ValueError(
'You need to provide a horizon time when using rotating obstacles!')
else:
T = self.options['horizon_time']
n_quarters = int(np.ceil(4*T/Ts))
knots_theta = np.r_[np.zeros(3), np.hstack(
[0.25*k*np.ones(2) for k in range(1, n_quarters+1)]), 0.25*n_quarters]*(Ts/T)
Tf, knots = get_interval_T(BSplineBasis(knots_theta, 2), 0, 1.)
basis = BSplineBasis(knots, 2)
# coefficients based on nurbs representation of circle
cos_cfs = np.r_[1., np.sqrt(
2.)/2., 0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2., 0., np.sqrt(2.)/2., 1.]
sin_cfs = np.r_[0., np.sqrt(
2.)/2., 1., np.sqrt(2.)/2., 0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2., 0.]
weight_cfs = np.r_[1., np.sqrt(
2.)/2., 1., np.sqrt(2.)/2., 1., np.sqrt(2.)/2., 1., np.sqrt(2.)/2., 1.]
cos_cfs = Tf.dot(np.array([cos_cfs[k % 8] for k in range(len(basis))]))
sin_cfs = Tf.dot(np.array([sin_cfs[k % 8] for k in range(len(basis))]))
weight_cfs = Tf.dot(
np.array([weight_cfs[k % 8] for k in range(len(basis))]))
cos_wt = BSpline(basis, cos_cfs)
sin_wt = BSpline(basis, sin_cfs)*np.sign(omega)
self.cos = cos_wt*cos(theta0) - sin_wt*sin(theta0)
self.sin = cos_wt*sin(theta0) + sin_wt*cos(theta0)
self.gon_weight = BSpline(basis, weight_cfs)
def _create_objective_function(model, V, warm_start):
[final_cost] = model.cl([V['X', model.n]])
running_cost = 0
for k in range(model.n):
[stage_cost] = model.c([V['X', k], V['U', k]])
# Encourage looking at the ball
d = ca.veccat([ca.cos(V['X', k, 'psi'])*ca.cos(V['X', k, 'phi']),
ca.cos(V['X', k, 'psi'])*ca.sin(V['X', k, 'phi']),
ca.sin(V['X', k, 'psi'])])
r = ca.veccat([V['X', k, 'x_b'] - V['X', k, 'x_c'],
V['X', k, 'y_b'] - V['X', k, 'y_c'],
V['X', k, 'z_b']])
r_cos_omega = ca.mul(d.T, r)
if warm_start:
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
stage_cost += 1e-1 * (1 - cos_omega)
else:
stage_cost -= 1e-1 * r_cos_omega * model.dt
running_cost += stage_cost
return final_cost + running_cost
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 observation_covariance(state, observation):
d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])])
r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]])
r_cos_theta = ca.mul(d.T, r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz, nz))
R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction("Observation covariance", [state], [R])
def R_from_roll_pitch_yaw(roll, pitch, yaw): ca1 = cos(yaw) sa1 = sin(yaw) cb1 = cos(pitch) sb1 = sin(pitch) cc1 = cos(roll) sc1 = sin(roll) ca1sb1 = ca1*sb1 sa1sb1 = sa1*sb1 if casadi.SXMatrix in map(type,[roll,pitch,yaw]): R = SXMatrix(3,3) else: R = numpy.zeros((3,3)) R[0,0] = ca1*cb1 R[0,1] = ca1sb1*sc1-sa1*cc1 R[0,2] = ca1sb1*cc1+sa1*sc1 R[1,0] = sa1*cb1 R[1,1] = sa1sb1*sc1+ca1*cc1 R[1,2] = sa1sb1*cc1-ca1*sc1 R[2,0] = -sb1 R[2,1] = cb1*sc1 R[2,2] = cb1*cc1 return R
def Rz(theta): if type(theta)==SXMatrix: R = SXMatrix(4,4) R[0,0] = 1 R[1,1] = 1 R[2,2] = 1 R[3,3] = 1 c = cos(theta) s = sin(theta) R[0,0] = c R[1,0] = s R[0,1] = -s R[1,1] = c return R
def _create_observation_covariance_function(self, N_min, N_max):
d = ca.veccat([ca.cos(self.x['psi']) * ca.cos(self.x['phi']),
ca.cos(self.x['psi']) * ca.sin(self.x['phi']),
ca.sin(self.x['psi'])])
r = ca.veccat([self.x['x_b'] - self.x['x_c'],
self.x['y_b'] - self.x['y_c'],
self.x['z_b']])
r_cos_omega = ca.mul(d.T, r)
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
# Look at the ball
N = self.z.squared(ca.SX.zeros(self.nz, self.nz))
# variance = N_max * (1 - cos_omega) + N_min
# variance = ca.mul(r.T, r) * (N_max * (1 - cos_omega) + N_min)
variance = ca.norm_2(r) * (N_max * (1 - cos_omega) + N_min)
N['x_b', 'x_b'] = variance
N['y_b', 'y_b'] = variance
N['z_b', 'z_b'] = variance
op = {'input_scheme': ['x'],
'output_scheme': ['N']}
return ca.SXFunction('Observation covariance',
[self.x], [N], op)
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])])
r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']])
r_cos_theta = ca.mul(d.T,r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz,nz))
R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction('Observation covariance',[state],[R])
def create_simple_plan(x0, F, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final position
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
def setPhase(self,phase):
maxPoly = max([max(self.fits[name].polyOrder) for name in self.fits])
maxCos = max([max(self.fits[name].cosOrder) for name in self.fits])
maxSin = max([max(self.fits[name].sinOrder) for name in self.fits])
# all bases
polyBases = [phase**po for po in range(maxPoly+1)]
cosBases = [C.cos(co*phase) for co in range(maxCos+1)]
sinBases = [C.sin(co*phase) for si in range(maxSin+1)]
self.fitsWithPhase = {}
for name in self.fits:
fit = self.fits[name]
polys = [coeff*polyBases[order] for coeff,order in zip(fit.polyCoeffs, fit.polyOrder)]
coses = [coeff*cosBases[order] for coeff,order in zip(fit.cosCoeffs, fit.cosOrder)]
sins = [coeff*sinBases[order] for coeff,order in zip(fit.sinCoeffs, fit.sinOrder)]
self.fitsWithPhase[name] = sum(polys+coses+sins)
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "dmotor_torque"
, 'dddr'
] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def build_opti0_solver(self, agt_idx):
self.opti0 = ca.Opti()
self.x0 = self.opti0.variable(self.n_x, self.N + 1)
self.u0 = self.opti0.variable(self.n_u, self.N)
self.x_s0 = self.opti0.parameter(self.n_x)
self.x_f0 = self.opti0.parameter(self.n_x)
self.agt_x0 = []
for i in range(agt_idx):
self.agt_x0.append(self.opti0.parameter(self.n_x, self.N + 1))
self.opti0.set_value(self.x_f0, self.x_refs[self.x_refs_idx])
da_lim = self.agent.da_lim
ddf_lim = self.agent.ddf_lim
r = self.agent.r
self.opti0.subject_to(self.x0[0, 0] == self.x_s0[0])
self.opti0.subject_to(self.x0[1, 0] == self.x_s0[1])
# self.opti0.subject_to(self.opti0.bounded(-2*np.pi, self.x0[2,0], 2*np.pi))
self.opti0.subject_to(self.x0[2, 0] == self.x_s0[2])
self.opti0.subject_to(self.x0[3, 0] == self.x_s0[3])
stage_cost = 0
for i in range(self.N):
stage_cost += ca.bilin(self.Q, self.x0[:, i + 1] - self.x_f0,
self.x0[:, i + 1] - self.x_f0) + ca.bilin(
self.R, self.u0[:, i], self.u0[:, i])
beta = ca.atan2(self.l_r * ca.tan(self.u0[0, i]),
self.l_f + self.l_r)
self.opti0.subject_to(
self.x0[0, i + 1] == self.x0[0, i] +
self.dt * self.x0[3, i] * ca.cos(self.x0[2, i] + beta))
self.opti0.subject_to(
self.x0[1, i + 1] == self.x0[1, i] +
self.dt * self.x0[3, i] * ca.sin(self.x0[2, i] + beta))
self.opti0.subject_to(self.x0[2, i + 1] == self.x0[2, i] +
self.dt * self.x0[3, i] * ca.sin(beta))
self.opti0.subject_to(self.x0[3, i + 1] == self.x0[3, i] +
self.dt * self.u0[1, i])
if self.F is not None:
self.opti0.subject_to(
ca.mtimes(self.F, self.x0[:, i + 1]) <= self.b)
if self.H is not None:
self.opti0.subject_to(
ca.mtimes(self.H, self.u0[:, i]) <= self.g)
# Treat init and final states of agents before as obstacles
for j in range(agt_idx):
self.opti0.subject_to(
ca.bilin(np.eye(2), self.x0[:2, i + 1] -
self.agt_x0[j][:2, i + 1], self.x0[:2, i + 1] -
self.agt_x0[j][:2, i + 1]) >= (1.5 * 2 * r)**2)
if i < self.N - 1:
stage_cost += ca.bilin(self.Rd,
self.u0[:, i + 1] - self.u0[:, i],
self.u0[:, i + 1] - self.u0[:, i])
self.opti0.subject_to(
self.opti0.bounded(ddf_lim[0] * self.dt,
self.u0[0, i + 1] - self.u0[0, i],
ddf_lim[1] * self.dt))
self.opti0.subject_to(
self.opti0.bounded(da_lim[0] * self.dt,
self.u0[1, i + 1] - self.u0[1, i],
da_lim[1] * self.dt))
self.cost0 = stage_cost
self.opti0.minimize(self.cost0)
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
}
self.opti0.solver('ipopt', plugin_opts, solver_opts)
def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf["kite"]["mass"] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf["env"]["g"] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
# INERTIA MATRIX (Kurt's direct measurements)
j1 = conf["kite"]["j1"]
j31 = conf["kite"]["j31"]
j2 = conf["kite"]["j2"]
j3 = conf["kite"]["j3"]
# Carousel Friction & inertia
jCarousel = conf["carousel"]["jCarousel"]
cfric = conf["carousel"]["cfric"]
zt = conf["kite"]["zt"]
rA = conf["carousel"]["rArm"]
########### model integ ###################
e11 = dae.x("e11")
e12 = dae.x("e12")
e13 = dae.x("e13")
e21 = dae.x("e21")
e22 = dae.x("e22")
e23 = dae.x("e23")
e31 = dae.x("e31")
e32 = dae.x("e32")
e33 = dae.x("e33")
x = dae.x("x")
y = dae.x("y")
z = dae.x("z")
dx = dae.x("dx")
dy = dae.x("dy")
dz = dae.x("dz")
w1 = dae.x("w1")
w2 = dae.x("w2")
w3 = dae.x("w3")
delta = 0
ddelta = 0
r = dae.x("r")
dr = dae.x("dr")
ddr = dae.u("ddr")
# wind
z0 = conf["wind shear"]["z0"]
zt_roughness = conf["wind shear"]["zt_roughness"]
zsat = 0.5 * (z + C.sqrt(z * z))
wind_x = dae.p("w0") * C.log((zsat + zt_roughness + 2) / zt_roughness) / C.log(z0 / zt_roughness)
dae.addOutput("wind at altitude", wind_x)
dae.addOutput("w0", dae.p("w0"))
dp_carousel_frame = C.veccat([dx - ddelta * y, dy + ddelta * (rA + x), dz]) - C.veccat(
[C.cos(delta) * wind_x, C.sin(delta) * wind_x, 0]
)
R_c2b = C.veccat(dae.x(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"])).reshape((3, 3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul(R_c2b, dp_carousel_frame)
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(
dae,
conf,
dp_carousel_frame,
dpE,
dae.x(("w1", "w2", "w3")),
dae.x(("e21", "e22", "e23")),
dae.u(("aileron", "elevator")),
)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0, 0] = m
mm[0, 1] = 0
mm[0, 2] = 0
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = x + zt * e31
mm[1, 0] = 0
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = y + zt * e32
mm[2, 0] = 0
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = z + zt * e33
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = j1
mm[3, 4] = 0
mm[3, 5] = j31
mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j2
mm[4, 5] = 0
mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = j31
mm[5, 4] = 0
mm[5, 5] = j3
mm[5, 6] = 0
mm[6, 0] = x + zt * e31
mm[6, 1] = y + zt * e32
mm[6, 2] = z + zt * e33
mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[6, 5] = 0
mm[6, 6] = 0
# right hand side
zt2 = zt * zt
rhs = C.veccat(
[
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m,
f3 - g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r
- (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
- dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 * (w2 - ddelta * e23))
- dy * (dy - zt * e22 * (w1 - ddelta * e13) + zt * e12 * (w2 - ddelta * e23))
- dz * (dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 * (w2 - ddelta * e23))
+ dr * dr
+ (w1 - ddelta * e13)
* (
e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e22 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
- (w2 - ddelta * e23)
* (
e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e12 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
),
]
)
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31) ** 2 / 2 + (y + zt * e32) ** 2 / 2 + (z + zt * e33) ** 2 / 2 - r ** 2 / 2
cdot = (
dx * (x + zt * e31)
+ dy * (y + zt * e32)
+ dz * (z + zt * e33)
+ zt * (w2 - ddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (w1 - ddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- r * dr
)
ddx = dae.z("ddx")
ddy = dae.z("ddy")
ddz = dae.z("ddz")
dw1 = dae.z("dw1")
dw2 = dae.z("dw2")
dddelta = 0
cddot = (
-(w1 - ddelta * e13)
* (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e21 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e22 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
+ (w2 - ddelta * e23)
* (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
+ zt * e11 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e12 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
)
- ddr * r
+ (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
+ dx * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ dy * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ dz * (dz + zt * e13 * w2 - zt * e23 * w1)
+ ddx * (x + zt * e31)
+ ddy * (y + zt * e32)
+ ddz * (z + zt * e33)
- dr * dr
+ zt * (dw2 - dddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (dw1 - dddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- zt
* dddelta
* (
e11 * e23 * x
- e13 * e21 * x
+ e12 * e23 * y
- e13 * e22 * y
+ zt * e11 * e23 * e31
- zt * e13 * e21 * e31
+ zt * e12 * e23 * e32
- zt * e13 * e22 * e32
)
)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae.addOutput("c", c)
dae.addOutput("cdot", cdot)
dae.addOutput("cddot", cddot)
return (mm, rhs, dRexp)
# Horizontal theta angle of the tether behind the arm
# a.k.a. theta
theta = ssym('theta')
dtheta = ssym('dtheta')
ddtheta = ssym('ddtheta')
# Our generalized coordinate is theta, i.e. the azimuth angle referenced
# to world frame x
q = casadi.vertcat([theta])
dq = casadi.vertcat([dtheta])
ddq = casadi.vertcat([ddtheta])
nq = q.size()
# some subexpressions
s_delta = sin(delta)
c_delta = cos(delta)
s_theta = sin(theta)
c_theta = cos(theta)
# stone position in world coordinates. Maps theta,t -> x
x_w = R*casadi.vertcat([c_delta, s_delta]) \
+ r*casadi.vertcat([c_theta, s_theta])
v_w = R*ddelta*casadi.vertcat([-s_delta,c_delta]) \
+ r*dtheta*casadi.vertcat([-s_theta,c_theta])
# Assume we are spinning a 7kg sphere of water
m = 7.0 # kg
Cd = 0.47 # Drag coefficient for a sphere
rho_water = 1000. # kg / m^2
print '='*80
# set the mpc weights
xRms = 0.1
vRms = 1.5
fRms = 5.0
mpc.S[0,0] = (1.0/xRms)**2/N
mpc.S[1,1] = (1.0/vRms)**2/N
mpc.S[2,2] = (1.0/fRms)**2/N
mpc.SN[0,0] = (1.0/0.01)**2
mpc.SN[1,1] = (1.0/0.01)**2
# initial guess
tk = 0
for k in range(N+1):
mpc.x[k,0] = A*C.sin(omega*tk)
mpc.x[k,1] = A*C.cos(omega*tk)*omega
tk += ts
# set tracking trajectory
t = 0
refLog = []
def setTrajectory(ocp, t0):
tk = t0
for k in range(N):
ocp.y[k,0] = A*C.sin(omega*tk)
ocp.y[k,1] = A*C.cos(omega*tk)*omega
tk += ts
ocp.yN[0] = A*C.sin(omega*tk)
ocp.yN[1] = A*C.cos(omega*tk)*omega
x, y, theta = states[...]
n_states = states.size
controls = ca_tools.struct_symSX([
(
ca_tools.entry('v'),
ca_tools.entry('omega')
)
])
v, omega = controls[...]
n_controls = controls.size
## rhs
rhs = ca_tools.struct_SX(states)
rhs['x'] = v*ca.cos(theta)
rhs['y'] = v*ca.sin(theta)
rhs['theta'] = omega
## function
f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])
## for MPC
optimizing_target = ca_tools.struct_symSX([
(
ca_tools.entry('U', repeat=N, struct=controls),
ca_tools.entry('X', repeat=N+1, struct=states)
)
])
U, X, = optimizing_target[...] # data are stored in list [], notice that ',' cannot be missed
# X = ca.SX.sym('X', n_states, N+1)
def create_plan_pc(b0, BF, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final coordinate
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
J += 1e1 * ca.trace(bk_next['S']) # uncertainty
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# State constraints
# catcher can look within the upper hemisphere
lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
# Control constraints
# 0 <= F
lbx['U',:,'F'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Take care of the time
#lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
# Simulation ends when the ball touches the ground
#lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
def dynamic_model(modelparams):
# define casadi struct
model = casadi.types.SimpleNamespace()
constraints = casadi.types.SimpleNamespace()
model_name = "f110_dynamic_model"
model.name = model_name
#loadparameters
with open(modelparams) as file:
params = yaml.load(file, Loader=yaml.FullLoader)
m = params['m'] #[kg]
lf = params['lf'] #[m]
lr = params['lr'] #[m]
Iz = params['Iz'] #[kg*m^3]
#pajecka and motor coefficients
Bf = params['Bf']
Br = params['Br']
Cf = params['Cf']
Cr = params['Cr']
Cm1 = params['Cm1']
Cm2 = params['Cm2']
Croll = params['Croll']
Cd = params['Cd']
Df = params['Df']
Dr = params['Dr']
print("CasADi model created with the following parameters: filename: ",
modelparams, "\n values:", params)
xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
uvars = ['ddot', 'deltadot', 'thetadot']
pvars = [
'xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql',
'Q_theta', 'R_d', 'R_delta', 'r'
]
#parameter vector, contains linearization poitns
xt = casadi.SX.sym("xt")
yt = casadi.SX.sym("yt")
phit = casadi.SX.sym("phit")
sin_phit = casadi.SX.sym("sin_phit")
cos_phit = casadi.SX.sym("cos_phit")
#stores linearization point
theta_hat = casadi.SX.sym("theta_hat")
Qc = casadi.SX.sym("Qc")
Ql = casadi.SX.sym("Ql")
Q_theta = casadi.SX.sym("Q_theta")
#cost on smoothnes of motorinput
R_d = casadi.SX.sym("R_d")
#cost on smoothness of steering_angle
R_delta = casadi.SX.sym("R_delta")
#trackwidth
r = casadi.SX.sym("r")
#pvars = ['xt', 'yt', 'phit', 'sin_phit', 'cos_phit', 'theta_hat', 'Qc', 'Ql', 'Q_theta', 'R_d', 'R_delta', 'r']
p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql,
Q_theta, R_d, R_delta, r)
#single track model with pajecka tireforces as in Optimization-Based Autonomous Racing of 1:43 Scale RC Cars Alexander Liniger, Alexander Domahidi and Manfred Morari
#pose
posx = casadi.SX.sym("posx")
posy = casadi.SX.sym("posy")
#vel (long and lateral)
vx = casadi.SX.sym("vx")
vy = casadi.SX.sym("vy")
#body angular Rate
omega = casadi.SX.sym("omega")
#heading
phi = casadi.SX.sym("phi")
#steering_angle
delta = casadi.SX.sym("delta")
#motorinput
d = casadi.SX.sym("d")
#dynamic forces
Frx = casadi.SX.sym("Frx")
Fry = casadi.SX.sym("Fry")
Ffx = casadi.SX.sym("Ffx")
Ffy = casadi.SX.sym("Ffy")
#arclength progress
theta = casadi.SX.sym("theta")
#temporal derivatives
posxdot = casadi.SX.sym("xdot")
posydot = casadi.SX.sym("ydot")
vxdot = casadi.SX.sym("vxdot")
vydot = casadi.SX.sym("vydot")
phidot = casadi.SX.sym("phidot")
omegadot = casadi.SX.sym("omegadot")
deltadot = casadi.SX.sym("deltadot")
thetadot = casadi.SX.sym("thetadot")
ddot = casadi.SX.sym("ddot")
#inputvector
#uvars = ['ddot', 'deltadot', 'thetadot']
u = casadi.vertcat(ddot, deltadot, thetadot)
#car state Dynamics
#xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
x = casadi.vertcat(posx, posy, phi, vx, vy, omega, d, delta, theta)
xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, vydot, omegadot,
ddot, deltadot, thetadot)
#build CasADi expressions for dynamic model
#front lateral tireforce
alphaf = -casadi.atan2((omega * lf + vy), vx) + delta
Ffy = Df * casadi.sin(Cf * casadi.atan(Bf * alphaf))
#rear lateral tireforce
alphar = casadi.atan2((omega * lr - vy), vx)
Fry = Dr * casadi.sin(Cr * casadi.atan(Br * alphar))
#rear longitudinal forces
Frx = (Cm1 - Cm2 * vx) * d - Croll - Cd * vx * vx
f_expl = casadi.vertcat(
vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot
vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot
omega, #phidot
1 / m * (Frx - Ffy * casadi.sin(delta) + m * vy * omega), #vxdot
1 / m * (Fry + Ffy * casadi.cos(delta) - m * vx * omega), #vydot
1 / Iz * (Ffy * lf * casadi.cos(delta) - Fry * lr), #omegadot
ddot,
deltadot,
thetadot)
# algebraic variables
z = casadi.vertcat([])
model.f_expl_expr = f_expl
model.f_impl_expr = xdot - f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.z = z
#boxconstraints
model.ddot_min = -10.0 #min change in d [-]
model.ddot_max = 10.0 #max change in d [-]
model.d_min = -0.1 #min d [-]
model.d_max = 1 #max d [-]
model.delta_min = -0.40 # minimum steering angle [rad]
model.delta_max = 0.40 # maximum steering angle [rad]
model.deltadot_min = -2 # minimum steering angle cahgne[rad/s]
model.deltadot_max = 2 # maximum steering angle cahgne[rad/s]
model.omega_min = -100 # minimum yawrate [rad/sec]
model.omega_max = 100 # maximum yawrate [rad/sec]
model.thetadot_min = 0.05 # minimum adv param speed [m/s]
model.thetadot_max = 5 # maximum adv param speed [m/s]
model.theta_min = 0.00 # minimum adv param [m]
model.theta_max = 1000 # maximum adv param [m]
model.vx_max = 3.5 # max long vel [m/s]
model.vx_min = -0.5 #0.05 # min long vel [m/s]
model.vy_max = 3 # max lat vel [m/s]
model.vy_min = -3 # min lat vel [m/s]
model.x0 = np.array([0, 0, 0, 1, 0.01, 0, 0, 0, 0])
#compute approximate linearized contouring and lag error
xt_hat = xt + cos_phit * (theta - theta_hat)
yt_hat = yt + sin_phit * (theta - theta_hat)
e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy)
e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy)
cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
#error = casadi.vertcat(e_cont, e_lag)
#set up stage cost
#Q = diag(vertcat(Qc, Ql))
model.con_h_expr = (xt_hat - posx)**2 + (yt_hat - posy)**2 - r**2
model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
return model, constraints
def kinematic_model(modelparams):
# define casadi struct
# define casadi struct
model = casadi.types.SimpleNamespace()
constraints = casadi.types.SimpleNamespace()
model_name = "f110_kinematic_model"
model.name = model_name
#loadparameters
with open(modelparams) as file:
params = yaml.load(file, Loader=yaml.FullLoader)
lf = params['lf'] #[m]
lr = params['lr'] #[m]
lwb = lf + lr
print("CasADi model created with the following parameters: filename: ",
modelparams, "\n values:", params)
#parameter vector, contains linearization poitns
xt = casadi.SX.sym("xt")
yt = casadi.SX.sym("yt")
phit = casadi.SX.sym("phit")
sin_phit = casadi.SX.sym("sin_phit")
cos_phit = casadi.SX.sym("cos_phit")
gt_upper = casadi.SX.sym("gt_upper")
gt_lower = casadi.SX.sym("gt_lower")
#stores linearization point
theta_hat = casadi.SX.sym("theta_hat")
Qc = casadi.SX.sym("Qc")
Ql = casadi.SX.sym("Ql")
Q_theta = casadi.SX.sym("Q_theta")
#cost on smoothnes of motorinput
R_d = casadi.SX.sym("R_d")
#cost on smoothness of steering_angle
R_delta = casadi.SX.sym("R_delta")
r = casadi.SX.sym("r")
p = casadi.vertcat(xt, yt, phit, sin_phit, cos_phit, theta_hat, Qc, Ql,
Q_theta, R_d, R_delta, r)
#single track model
#pose
posx = casadi.SX.sym("posx")
posy = casadi.SX.sym("posy")
#longitudinal velocity
vx = casadi.SX.sym("vx")
#heading
phi = casadi.SX.sym("phi")
#steering_angle
delta = casadi.SX.sym("delta")
#motorinput
d = casadi.SX.sym("d")
#arclength progress
theta = casadi.SX.sym("theta")
#temporal derivatives
posxdot = casadi.SX.sym("xdot")
posydot = casadi.SX.sym("ydot")
vxdot = casadi.SX.sym("vxdot")
phidot = casadi.SX.sym("phidot")
deltadot = casadi.SX.sym("deltadot")
thetadot = casadi.SX.sym("thetadot")
ddot = casadi.SX.sym("ddot")
#inputvector
u = casadi.vertcat(ddot, deltadot, thetadot)
#car state Dynamics
x = casadi.vertcat(posx, posy, phi, vx, theta, d, delta)
xdot = casadi.vertcat(posxdot, posydot, phidot, vxdot, thetadot, ddot,
deltadot)
f_expl = casadi.vertcat(vx * casadi.cos(phi), vx * casadi.sin(phi),
vx / lwb * casadi.tan(delta), d, thetadot, ddot,
deltadot)
# algebraic variables
z = casadi.vertcat([])
model.f_expl_expr = f_expl
model.f_impl_expr = xdot - f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.z = z
#boxconstraints
model.d_min = -3.0
model.d_max = 5.0
model.ddot_min = -10.0
model.ddot_max = 10.0
model.delta_min = -0.40 # minimum steering angle [rad]
model.delta_max = 0.40 # maximum steering angle [rad]
model.deltadot_min = -2 # minimum steering angle cahgne[rad/s]
model.deltadot_max = 2 # maximum steering angle cahgne[rad/s]
model.thetadot_min = -0.1 # minimum adv param speed [m/s]
model.thetadot_max = 5 # maximum adv param speed [m/s]
model.theta_min = 0.00 # minimum adv param [m]
model.theta_max = 100 # maximum adv param [m]
model.vx_max = 2 # max long vel [m/s]
model.vx_min = -1 # min long vel [m/s]
model.x0 = np.array([0, 0, 0, 1, 0, 0, 0])
#halfspace constraints on x capturing the track at each stage
#n = casadi.vertcat(-sin_phit, cos_phit)
#constraints.h_upper = casadi.vertcat(0,0)
#g_upper = casadi.vertcat(gt_upper, -gt_lower)
#halfspace constriants for track boundaries, con_expr <= 0
#model.con_h_expr = casadi.vertcat(n[0]*x[0]+n[1]*x[1]-g_upper[0], -n[0]*x[0]-n[1]*x[1]-g_upper[1])
#compute approximate linearized contouring and lag error
xt_hat = xt + cos_phit * (theta - theta_hat)
yt_hat = yt + sin_phit * (theta - theta_hat)
e_cont = sin_phit * (xt_hat - posx) - cos_phit * (yt_hat - posy)
e_lag = cos_phit * (xt_hat - posx) + sin_phit * (yt_hat - posy)
model.con_h_expr = e_cont**2 + e_lag**2 - (r)**2
#virtual turn radius
#error = casadi.vertcat(e_cont, e_lag)
#set up stage cost
#Q = diag(vertcat(Qc, Ql))
model.stage_cost = e_cont * Qc * e_cont + e_lag * Qc * e_lag - Q_theta * thetadot + ddot * R_d * ddot + deltadot * R_delta * deltadot
return model, constraints
def firefly_CLA_and_CDA_fuse_hybrid( # TODO remove
fuse_fineness_ratio,
fuse_boattail_angle,
fuse_TE_diameter,
fuse_length,
fuse_diameter,
alpha,
V,
mach,
rho,
mu,
):
"""
Estimated equiv. lift area and equiv. drag area of the Firefly fuselage, component buildup.
:param fuse_fineness_ratio: Fineness ratio of the fuselage nose (length / diameter)
:param fuse_boattail_angle: Boattail half-angle [deg]
:param fuse_TE_diameter: Diameter of the fuselage's base at the "trailing edge" [m]
:param fuse_length: Length of the fuselage [m]
:param fuse_diameter: Diameter of the fuselage [m]
:param alpha: Angle of attack [deg]
:param V: Airspeed [m/s]
:param mach: Mach number [unitless]
:param rho: Air density [kg/m^3]
:param mu: Dynamic viscosity of air [Pa*s]
:return: A tuple of (CLA, CDA) [m^2]
"""
alpha_rad = alpha * np.pi / 180
sin_alpha = cas.sin(alpha_rad)
cos_alpha = cas.cos(alpha_rad)
"""
Lift of a truncated fuselage, following slender body theory.
"""
separation_location = 0.3 # 0 for separation at trailing edge, 1 for separation at start of boat-tail. Calibrate this.
diameter_at_separation = (
1 - separation_location
) * fuse_TE_diameter + separation_location * fuse_diameter
fuse_TE_area = np.pi / 4 * diameter_at_separation**2
CLA_slender_body = 2 * fuse_TE_area * alpha_rad # Derived from FVA 6.6.5, Eq. 6.75
"""
Crossflow force, following the method of Jorgensen, 1977: "Prediction of Static Aero. Chars. for Slender..."
"""
V_crossflow = V * sin_alpha
Re_crossflow = rho * cas.fabs(V_crossflow) * fuse_diameter / mu
mach_crossflow = mach * cas.fabs(sin_alpha)
eta = 1 # TODO make this a function of overall fineness ratio
Cdn = 0.2 # Taken from suggestion for supercritical cylinders in Hoerner's Fluid Dynamic Drag, pg. 3-11
S_planform = fuse_diameter * fuse_length
CNA = eta * Cdn * S_planform * sin_alpha * cas.fabs(sin_alpha)
CLA_crossflow = CNA * cos_alpha
CDA_crossflow = CNA * sin_alpha
CLA = CLA_slender_body + CLA_crossflow
r"""
Zero-lift_force drag_force
Model derived from high-fidelity data at: C:\Projects\GitHub\firefly_aerodynamics\Design_Opt\studies\Circular Firefly Fuse CFD\Zero-Lift Drag
"""
c = np.array([
112.57153128951402720758778741583,
-7.1720570832587240417410612280946,
-0.01765596807595304351679033061373,
0.0026135564778264172743071913629365,
550.8775012129947299399645999074,
3.3166868391027000129156476759817,
11774.081980549422951298765838146,
3073258.204571904614567756652832,
0.0299,
]) # coefficients
fuse_Re = rho * cas.fabs(V) * fuse_length / mu
CDA_zero_lift = (
(c[0] * cas.exp(c[1] * fuse_fineness_ratio) +
c[2] * fuse_boattail_angle + c[3] * fuse_boattail_angle**2 +
c[4] * fuse_TE_diameter**2 + c[5]) / c[6] *
(fuse_Re / c[7])**(-1 / 7) * (fuse_length * fuse_diameter) / c[8])
r"""
Assumes a ring-shaped wake projection into the Trefftz plane with a sinusoidal circulation distribution.
This results in a uniform grad(phi) within the ring in the Trefftz plane.
Derivation at C:\Projects\GitHub\firefly_aerodynamics\Gists and Ideas\Ring Wing Potential Flow
"""
fuse_oswalds_efficiency = 0.5
CDiA_slender_body = CLA**2 / (
diameter_at_separation**2 * np.pi *
fuse_oswalds_efficiency) / 2 # or equivalently, use the next line
# CDiA_slender_body = fuse_TE_area * alpha_rad ** 2 / 2
CDA = CDA_crossflow + CDiA_slender_body + CDA_zero_lift
return CLA, CDA
def solveProblem():
print_time = rospy.Time().now().to_sec()
N = 51
U = ca.SX.sym('U', N)
x0 = ca.SX.sym('x0', 5) #初始条件
x0[0] = -1
x0[1] = 1
x0[2] = 0
x0[3] = 0
x0[4] = 0
xf = ca.SX.sym('xf', 5) #终端状态
xf[0] = x0[0]
xf[1] = x0[1]
xf[2] = x0[2]
xf[3] = x0[3]
xf[4] = x0[4]
obj = U[-1]
g = []
det_t = ca.SX.sym('det_t', 1)
det_t = U[-1] / ((N - 1) / 2)
for i in range((N - 1) / 2):
xf[4] += det_t * U[2 * i + 1]
xf[3] += det_t * U[2 * i]
xf[2] += det_t * xf[4]
xf[1] += det_t * xf[3] * ca.sin(xf[2])
xf[0] += det_t * xf[3] * ca.cos(xf[2])
g.append(xf[0])
g.append(xf[1])
g.append(xf[2])
g.append(xf[3])
g.append(xf[4])
nlp_prob = {'f': obj, 'x': U, 'g': ca.vertcat(*g)}
opts_setting = {
'ipopt.max_iter': 100,
'ipopt.print_level': 0,
'print_time': 0,
'ipopt.acceptable_tol': 1e-8,
'ipopt.acceptable_obj_change_tol': 1e-6
}
solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
lbx = []
ubx = []
lbg = []
ubg = []
for i in range((N - 1) / 2):
lbx.append(-1)
ubx.append(1)
lbx.append(-3)
ubx.append(3)
lbx.append(0)
ubx.append(100)
for i in range(5):
lbg.append(0) #终端约束
ubg.append(0) #终端约束
res = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
print('cost time of casadi opt problem: ',
rospy.Time().now().to_sec() - print_time)
print(res['f'], res['x'])
#print(xf)
UU = res['x']
points = PointCloud()
points.header.frame_id = 'map'
xf[0] = x0[0]
xf[1] = x0[1]
xf[2] = x0[2]
xf[3] = x0[3]
xf[4] = x0[4]
points.points.append(Point32(xf[0], xf[1], 0))
det_t = ca.SX.sym('det_t', 1)
det_t = UU[-1] / ((N - 1) / 2)
for i in range((N - 1) / 2):
xf[4] += det_t * UU[2 * i + 1]
xf[3] += det_t * UU[2 * i]
xf[2] += det_t * xf[4]
xf[1] += det_t * xf[3] * ca.sin(xf[2])
xf[0] += det_t * xf[3] * ca.cos(xf[2])
points.points.append(Point32(xf[0], xf[1], 0))
g_pub_clothoid.publish(points)
# #while not rospy.is_shutdown():
for i in range(100000):
g_pub_clothoid.publish(points)
#rospy.sleep(0.1)
print('end pub')
def _convert(self, symbol, t, y, y_dot, inputs):
""" See :meth:`CasadiConverter.convert()`. """
if isinstance(
symbol,
(
pybamm.Scalar,
pybamm.Array,
pybamm.Time,
pybamm.InputParameter,
pybamm.ExternalVariable,
),
):
return casadi.MX(symbol.evaluate(t, y, y_dot, inputs))
elif isinstance(symbol, pybamm.StateVector):
if y is None:
raise ValueError(
"Must provide a 'y' for converting state vectors")
return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.StateVectorDot):
if y_dot is None:
raise ValueError(
"Must provide a 'y_dot' for converting state vectors")
return casadi.vertcat(
*[y_dot[y_slice] for y_slice in symbol.y_slices])
elif isinstance(symbol, pybamm.BinaryOperator):
left, right = symbol.children
# process children
converted_left = self.convert(left, t, y, y_dot, inputs)
converted_right = self.convert(right, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.Modulo):
return casadi.fmod(converted_left, converted_right)
if isinstance(symbol, pybamm.Minimum):
return casadi.fmin(converted_left, converted_right)
if isinstance(symbol, pybamm.Maximum):
return casadi.fmax(converted_left, converted_right)
# _binary_evaluate defined in derived classes for specific rules
return symbol._binary_evaluate(converted_left, converted_right)
elif isinstance(symbol, pybamm.UnaryOperator):
converted_child = self.convert(symbol.child, t, y, y_dot, inputs)
if isinstance(symbol, pybamm.AbsoluteValue):
return casadi.fabs(converted_child)
if isinstance(symbol, pybamm.Floor):
return casadi.floor(converted_child)
if isinstance(symbol, pybamm.Ceiling):
return casadi.ceil(converted_child)
return symbol._unary_evaluate(converted_child)
elif isinstance(symbol, pybamm.Function):
converted_children = [
self.convert(child, t, y, y_dot, inputs)
for child in symbol.children
]
# Special functions
if symbol.function == np.min:
return casadi.mmin(*converted_children)
elif symbol.function == np.max:
return casadi.mmax(*converted_children)
elif symbol.function == np.abs:
return casadi.fabs(*converted_children)
elif symbol.function == np.sqrt:
return casadi.sqrt(*converted_children)
elif symbol.function == np.sin:
return casadi.sin(*converted_children)
elif symbol.function == np.arcsinh:
return casadi.arcsinh(*converted_children)
elif symbol.function == np.arccosh:
return casadi.arccosh(*converted_children)
elif symbol.function == np.tanh:
return casadi.tanh(*converted_children)
elif symbol.function == np.cosh:
return casadi.cosh(*converted_children)
elif symbol.function == np.sinh:
return casadi.sinh(*converted_children)
elif symbol.function == np.cos:
return casadi.cos(*converted_children)
elif symbol.function == np.exp:
return casadi.exp(*converted_children)
elif symbol.function == np.log:
return casadi.log(*converted_children)
elif symbol.function == np.sign:
return casadi.sign(*converted_children)
elif symbol.function == special.erf:
return casadi.erf(*converted_children)
elif isinstance(symbol, pybamm.Interpolant):
return casadi.interpolant(
"LUT", "bspline", symbol.x,
symbol.y.flatten())(*converted_children)
elif symbol.function.__name__.startswith("elementwise_grad_of_"):
differentiating_child_idx = int(symbol.function.__name__[-1])
# Create dummy symbolic variables in order to differentiate using CasADi
dummy_vars = [
casadi.MX.sym("y_" + str(i))
for i in range(len(converted_children))
]
func_diff = casadi.gradient(
symbol.differentiated_function(*dummy_vars),
dummy_vars[differentiating_child_idx],
)
# Create function and evaluate it using the children
casadi_func_diff = casadi.Function("func_diff", dummy_vars,
[func_diff])
return casadi_func_diff(*converted_children)
# Other functions
else:
return symbol._function_evaluate(converted_children)
elif isinstance(symbol, pybamm.Concatenation):
converted_children = [
self.convert(child, t, y, y_dot, inputs)
for child in symbol.children
]
if isinstance(symbol,
(pybamm.NumpyConcatenation, pybamm.SparseStack)):
return casadi.vertcat(*converted_children)
# DomainConcatenation specifies a particular ordering for the concatenation,
# which we must follow
elif isinstance(symbol, pybamm.DomainConcatenation):
slice_starts = []
all_child_vectors = []
for i in range(symbol.secondary_dimensions_npts):
child_vectors = []
for child_var, slices in zip(converted_children,
symbol._children_slices):
for child_dom, child_slice in slices.items():
slice_starts.append(
symbol._slices[child_dom][i].start)
child_vectors.append(
child_var[child_slice[i].start:child_slice[i].
stop])
all_child_vectors.extend([
v for _, v in sorted(zip(slice_starts, child_vectors))
])
return casadi.vertcat(*all_child_vectors)
else:
raise TypeError("""
Cannot convert symbol of type '{}' to CasADi. Symbols must all be
'linear algebra' at this stage.
""".format(type(symbol)))
def opt_mintime(reftrack: np.ndarray,
coeffs_x: np.ndarray,
coeffs_y: np.ndarray,
normvectors: np.ndarray,
pars: dict,
tpamap_path: str,
tpadata_path: str,
export_path: str,
print_debug: bool = False,
plot_debug: bool = False) -> tuple:
"""
Created by:
Fabian Christ
Documentation:
The minimum lap time problem is described as an optimal control problem, converted to a nonlinear program using
direct orthogonal Gauss-Legendre collocation and then solved by the interior-point method IPOPT. Reduced computing
times are achieved using a curvilinear abscissa approach for track description, algorithmic differentiation using
the software framework CasADi, and a smoothing of the track input data by approximate spline regression. The
vehicles behavior is approximated as a double track model with quasi-steady state tire load simplification and
nonlinear tire model.
Please refer to our paper for further information:
Christ, Wischnewski, Heilmeier, Lohmann
Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients
Inputs:
reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m]
coeffs_x: coefficient matrix of the x splines with size (no_splines x 4)
coeffs_y: coefficient matrix of the y splines with size (no_splines x 4)
normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component]
pars: parameters dictionary
tpamap_path: file path to tpa map (required for friction map loading)
tpadata_path: file path to tpa data (required for friction map loading)
export_path: path to output folder for warm start files and solution files
print_debug: determines if debug messages are printed
plot_debug: determines if debug plots are shown
Outputs:
alpha_opt: solution vector of the optimization problem containing the lateral shift in m for every point
v_opt: velocity profile for the raceline
"""
# ------------------------------------------------------------------------------------------------------------------
# PREPARE TRACK INFORMATION ----------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# spline lengths
spline_lengths_refline = tph.calc_spline_lengths.calc_spline_lengths(coeffs_x=coeffs_x,
coeffs_y=coeffs_y)
# calculate heading and curvature (numerically)
kappa_refline = tph.calc_head_curv_num. \
calc_head_curv_num(path=reftrack[:, :2],
el_lengths=spline_lengths_refline,
is_closed=True,
stepsize_curv_preview=pars["curv_calc_opts"]["d_preview_curv"],
stepsize_curv_review=pars["curv_calc_opts"]["d_review_curv"],
stepsize_psi_preview=pars["curv_calc_opts"]["d_preview_head"],
stepsize_psi_review=pars["curv_calc_opts"]["d_review_head"])[1]
# close track
kappa_refline_cl = np.append(kappa_refline, kappa_refline[0])
w_tr_left_cl = np.append(reftrack[:, 3], reftrack[0, 3])
w_tr_right_cl = np.append(reftrack[:, 2], reftrack[0, 2])
# step size along the reference line
h = pars["stepsize_opts"]["stepsize_reg"]
# optimization steps (0, 1, 2 ... end point/start point)
steps = [i for i in range(kappa_refline_cl.size)]
# number of control intervals
N = steps[-1]
# station along the reference line
s_opt = np.linspace(0.0, N * h, N + 1)
# interpolate curvature of reference line in terms of steps
kappa_interp = ca.interpolant('kappa_interp', 'linear', [steps], kappa_refline_cl)
# interpolate track width (left and right to reference line) in terms of steps
w_tr_left_interp = ca.interpolant('w_tr_left_interp', 'linear', [steps], w_tr_left_cl)
w_tr_right_interp = ca.interpolant('w_tr_right_interp', 'linear', [steps], w_tr_right_cl)
# describe friction coefficients from friction map with linear equations or gaussian basis functions
if pars["optim_opts"]["var_friction"] is not None:
w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist = opt_mintime_traj.src. \
approx_friction_map.approx_friction_map(reftrack=reftrack,
normvectors=normvectors,
tpamap_path=tpamap_path,
tpadata_path=tpadata_path,
pars=pars,
dn=pars["optim_opts"]["dn"],
n_gauss=pars["optim_opts"]["n_gauss"],
print_debug=print_debug,
plot_debug=plot_debug)
# ------------------------------------------------------------------------------------------------------------------
# DIRECT GAUSS-LEGENDRE COLLOCATION --------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# degree of interpolating polynomial
d = 3
# legendre collocation points
tau = np.append(0, ca.collocation_points(d, 'legendre'))
# coefficient matrix for formulating the collocation equation
C = np.zeros((d + 1, d + 1))
# coefficient matrix for formulating the collocation equation
D = np.zeros(d + 1)
# coefficient matrix for formulating the collocation equation
B = np.zeros(d + 1)
# construct polynomial basis
for j in range(d + 1):
# construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# evaluate polynomial at the final time to get the coefficients of the continuity equation
D[j] = p(1.0)
# evaluate time derivative of polynomial at collocation points to get the coefficients of continuity equation
p_der = np.polyder(p)
for r in range(d + 1):
C[j, r] = p_der(tau[r])
# evaluate integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
B[j] = pint(1.0)
# ------------------------------------------------------------------------------------------------------------------
# STATE VARIABLES --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of state variables
nx = 5
# velocity [m/s]
v_n = ca.SX.sym('v_n')
v_s = 50
v = v_s * v_n
# side slip angle [rad]
beta_n = ca.SX.sym('beta_n')
beta_s = 0.5
beta = beta_s * beta_n
# yaw rate [rad/s]
omega_z_n = ca.SX.sym('omega_z_n')
omega_z_s = 1
omega_z = omega_z_s * omega_z_n
# lateral distance to reference line (positive = left) [m]
n_n = ca.SX.sym('n_n')
n_s = 5.0
n = n_s * n_n
# relative angle to tangent on reference line [rad]
xi_n = ca.SX.sym('xi_n')
xi_s = 1.0
xi = xi_s * xi_n
# scaling factors for state variables
x_s = np.array([v_s, beta_s, omega_z_s, n_s, xi_s])
# put all states together
x = ca.vertcat(v_n, beta_n, omega_z_n, n_n, xi_n)
# ------------------------------------------------------------------------------------------------------------------
# CONTROL VARIABLES ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# number of control variables
nu = 4
# steer angle [rad]
delta_n = ca.SX.sym('delta_n')
delta_s = 0.5
delta = delta_s * delta_n
# positive longitudinal force (drive) [N]
f_drive_n = ca.SX.sym('f_drive_n')
f_drive_s = 7500.0
f_drive = f_drive_s * f_drive_n
# negative longitudinal force (brake) [N]
f_brake_n = ca.SX.sym('f_brake_n')
f_brake_s = 20000.0
f_brake = f_brake_s * f_brake_n
# lateral wheel load transfer [N]
gamma_y_n = ca.SX.sym('gamma_y_n')
gamma_y_s = 5000.0
gamma_y = gamma_y_s * gamma_y_n
# scaling factors for control variables
u_s = np.array([delta_s, f_drive_s, f_brake_s, gamma_y_s])
# put all controls together
u = ca.vertcat(delta_n, f_drive_n, f_brake_n, gamma_y_n)
# ------------------------------------------------------------------------------------------------------------------
# MODEL EQUATIONS --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# extract vehicle and tire parameters
veh = pars["vehicle_params_mintime"]
tire = pars["tire_params_mintime"]
# general constants
g = pars["veh_params"]["g"]
mass = pars["veh_params"]["mass"]
# curvature of reference line [rad/m]
kappa = ca.SX.sym('kappa')
# drag force [N]
f_xdrag = pars["veh_params"]["dragcoeff"] * v ** 2
# rolling resistance forces [N]
f_xroll_fl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_xroll_fr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_xroll_rl = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_xroll_rr = 0.5 * tire["c_roll"] * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_xroll = tire["c_roll"] * mass * g
# static normal tire forces [N]
f_zstat_fl = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_fr = 0.5 * mass * g * veh["wheelbase_rear"] / veh["wheelbase"]
f_zstat_rl = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
f_zstat_rr = 0.5 * mass * g * veh["wheelbase_front"] / veh["wheelbase"]
# dynamic normal tire forces (aerodynamic downforces) [N]
f_zlift_fl = 0.5 * veh["liftcoeff_front"] * v ** 2
f_zlift_fr = 0.5 * veh["liftcoeff_front"] * v ** 2
f_zlift_rl = 0.5 * veh["liftcoeff_rear"] * v ** 2
f_zlift_rr = 0.5 * veh["liftcoeff_rear"] * v ** 2
# dynamic normal tire forces (load transfers) [N]
f_zdyn_fl = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
- veh["k_roll"] * gamma_y)
f_zdyn_fr = (-0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
+ veh["k_roll"] * gamma_y)
f_zdyn_rl = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
- (1.0 - veh["k_roll"]) * gamma_y)
f_zdyn_rr = (0.5 * veh["cog_z"] / veh["wheelbase"] * (f_drive + f_brake - f_xdrag - f_xroll)
+ (1.0 - veh["k_roll"]) * gamma_y)
# sum of all normal tire forces [N]
f_z_fl = f_zstat_fl + f_zlift_fl + f_zdyn_fl
f_z_fr = f_zstat_fr + f_zlift_fr + f_zdyn_fr
f_z_rl = f_zstat_rl + f_zlift_rl + f_zdyn_rl
f_z_rr = f_zstat_rr + f_zlift_rr + f_zdyn_rr
# slip angles [rad]
alpha_fl = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_front"] * omega_z))
alpha_fr = delta - ca.atan((v * ca.sin(beta) + veh["wheelbase_front"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_front"] * omega_z))
alpha_rl = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) - 0.5 * veh["track_width_rear"] * omega_z))
alpha_rr = ca.atan((-v * ca.sin(beta) + veh["wheelbase_rear"] * omega_z) /
(v * ca.cos(beta) + 0.5 * veh["track_width_rear"] * omega_z))
# lateral tire forces [N]
f_y_fl = (pars["optim_opts"]["mue"] * f_z_fl * (1 + tire["eps_front"] * f_z_fl / tire["f_z0"])
* ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fl - tire["E_front"]
* (tire["B_front"] * alpha_fl - ca.atan(tire["B_front"] * alpha_fl)))))
f_y_fr = (pars["optim_opts"]["mue"] * f_z_fr * (1 + tire["eps_front"] * f_z_fr / tire["f_z0"])
* ca.sin(tire["C_front"] * ca.atan(tire["B_front"] * alpha_fr - tire["E_front"]
* (tire["B_front"] * alpha_fr - ca.atan(tire["B_front"] * alpha_fr)))))
f_y_rl = (pars["optim_opts"]["mue"] * f_z_rl * (1 + tire["eps_rear"] * f_z_rl / tire["f_z0"])
* ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rl - tire["E_rear"]
* (tire["B_rear"] * alpha_rl - ca.atan(tire["B_rear"] * alpha_rl)))))
f_y_rr = (pars["optim_opts"]["mue"] * f_z_rr * (1 + tire["eps_rear"] * f_z_rr / tire["f_z0"])
* ca.sin(tire["C_rear"] * ca.atan(tire["B_rear"] * alpha_rr - tire["E_rear"]
* (tire["B_rear"] * alpha_rr - ca.atan(tire["B_rear"] * alpha_rr)))))
# longitudinal tire forces [N]
f_x_fl = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fl
f_x_fr = 0.5 * f_drive * veh["k_drive_front"] + 0.5 * f_brake * veh["k_brake_front"] - f_xroll_fr
f_x_rl = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rl
f_x_rr = 0.5 * f_drive * (1 - veh["k_drive_front"]) + 0.5 * f_brake * (1 - veh["k_brake_front"]) - f_xroll_rr
# longitudinal acceleration [m/s²]
ax = (f_x_rl + f_x_rr + (f_x_fl + f_x_fr) * ca.cos(delta) - (f_y_fl + f_y_fr) * ca.sin(delta)
- pars["veh_params"]["dragcoeff"] * v ** 2) / mass
# lateral acceleration [m/s²]
ay = ((f_x_fl + f_x_fr) * ca.sin(delta) + f_y_rl + f_y_rr + (f_y_fl + f_y_fr) * ca.cos(delta)) / mass
# time-distance scaling factor (dt/ds)
sf = (1.0 - n * kappa) / (v * (ca.cos(xi + beta)))
# model equations for two track model (ordinary differential equations)
dv = (sf / mass) * ((f_x_rl + f_x_rr) * ca.cos(beta) + (f_x_fl + f_x_fr) * ca.cos(delta - beta)
+ (f_y_rl + f_y_rr) * ca.sin(beta) - (f_y_fl + f_y_fr) * ca.sin(delta - beta)
- f_xdrag * ca.cos(beta))
dbeta = sf * (-omega_z + (-(f_x_rl + f_x_rr) * ca.sin(beta) + (f_x_fl + f_x_fr) * ca.sin(delta - beta)
+ (f_y_rl + f_y_rr) * ca.cos(beta) + (f_y_fl + f_y_fr) * ca.cos(delta - beta)
+ f_xdrag * ca.sin(beta)) / (mass * v))
domega_z = (sf / veh["I_z"]) * ((f_x_rr - f_x_rl) * veh["track_width_rear"] / 2
- (f_y_rl + f_y_rr) * veh["wheelbase_rear"]
+ ((f_x_fr - f_x_fl) * ca.cos(delta)
+ (f_y_fl - f_y_fr) * ca.sin(delta)) * veh["track_width_front"] / 2
+ ((f_y_fl + f_y_fr) * ca.cos(delta)
+ (f_x_fl + f_x_fr) * ca.sin(delta)) * veh["track_width_front"])
dn = sf * v * ca.sin(xi + beta)
dxi = sf * omega_z - kappa
# put all ODEs together
dx = ca.vertcat(dv, dbeta, domega_z, dn, dxi) / x_s
# ------------------------------------------------------------------------------------------------------------------
# CONTROL BOUNDARIES -----------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
delta_min = -veh["delta_max"] / delta_s # min. steer angle [rad]
delta_max = veh["delta_max"] / delta_s # max. steer angle [rad]
f_drive_min = 0.0 # min. longitudinal drive force [N]
f_drive_max = veh["f_drive_max"] / f_drive_s # max. longitudinal drive force [N]
f_brake_min = -veh["f_brake_max"] / f_brake_s # min. longitudinal brake force [N]
f_brake_max = 0.0 # max. longitudinal brake force [N]
gamma_y_min = -np.inf # min. lateral wheel load transfer [N]
gamma_y_max = np.inf # max. lateral wheel load transfer [N]
# ------------------------------------------------------------------------------------------------------------------
# STATE BOUNDARIES -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_min = 1.0 / v_s # min. velocity [m/s]
v_max = pars["veh_params"]["v_max"] / v_s # max. velocity [m/s]
beta_min = -0.5 * np.pi / beta_s # min. side slip angle [rad]
beta_max = 0.5 * np.pi / beta_s # max. side slip angle [rad]
omega_z_min = - 0.5 * np.pi / omega_z_s # min. yaw rate [rad/s]
omega_z_max = 0.5 * np.pi / omega_z_s # max. yaw rate [rad/s]
xi_min = - 0.5 * np.pi / xi_s # min. relative angle to tangent on reference line [rad]
xi_max = 0.5 * np.pi / xi_s # max. relative angle to tangent on reference line [rad]
# ------------------------------------------------------------------------------------------------------------------
# INITIAL GUESS FOR DECISION VARIABLES -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
v_guess = 20.0 / v_s
# ------------------------------------------------------------------------------------------------------------------
# HELPER FUNCTIONS -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# continuous time dynamics
f_dyn = ca.Function('f_dyn', [x, u, kappa], [dx, sf], ['x', 'u', 'kappa'], ['dx', 'sf'])
# longitudinal tire forces [N]
f_fx = ca.Function('f_fx', [x, u], [f_x_fl, f_x_fr, f_x_rl, f_x_rr],
['x', 'u'], ['f_x_fl', 'f_x_fr', 'f_x_rl', 'f_x_rr'])
# lateral tire forces [N]
f_fy = ca.Function('f_fy', [x, u], [f_y_fl, f_y_fr, f_y_rl, f_y_rr],
['x', 'u'], ['f_y_fl', 'f_y_fr', 'f_y_rl', 'f_y_rr'])
# vertical tire forces [N]
f_fz = ca.Function('f_fz', [x, u], [f_z_fl, f_z_fr, f_z_rl, f_z_rr],
['x', 'u'], ['f_z_fl', 'f_z_fr', 'f_z_rl', 'f_z_rr'])
# longitudinal and lateral acceleration [m/s²]
f_a = ca.Function('f_a', [x, u], [ax, ay], ['x', 'u'], ['ax', 'ay'])
# ------------------------------------------------------------------------------------------------------------------
# FORMULATE NONLINEAR PROGRAM --------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# initialize NLP vectors
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# initialize ouput vectors
x_opt = []
u_opt = []
dt_opt = []
tf_opt = []
ax_opt = []
ay_opt = []
ec_opt = []
# initialize control vectors (for regularization)
delta_p = []
F_p = []
# boundary constraint: lift initial conditions
Xk = ca.MX.sym('X0', nx)
w.append(Xk)
n_min = (-w_tr_right_interp(0) + pars["optim_opts"]["width_opt"] / 2) / n_s
n_max = (w_tr_left_interp(0) - pars["optim_opts"]["width_opt"] / 2) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
x_opt.append(Xk * x_s)
# loop along the racetrack and formulate path constraints & system dynamic
for k in range(N):
# add decision variables for the control
Uk = ca.MX.sym('U_' + str(k), nu)
w.append(Uk)
lbw.append([delta_min, f_drive_min, f_brake_min, gamma_y_min])
ubw.append([delta_max, f_drive_max, f_brake_max, gamma_y_max])
w0.append([0.0] * nu)
# add decision variables for the state at collocation points
Xc = []
for j in range(d):
Xkj = ca.MX.sym('X_' + str(k) + '_' + str(j), nx)
Xc.append(Xkj)
w.append(Xkj)
lbw.append([-np.inf] * nx)
ubw.append([np.inf] * nx)
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# loop over all collocation points
Xk_end = D[0] * Xk
sf_opt = []
for j in range(1, d + 1):
# calculate the state derivative at the collocation point
xp = C[0, j] * Xk
for r in range(d):
xp = xp + C[r + 1, j] * Xc[r]
# interpolate kappa at the collocation point
kappa_col = kappa_interp(k + tau[j])
# append collocation equations (system dynamic)
fj, qj = f_dyn(Xc[j - 1], Uk, kappa_col)
g.append(h * fj - xp)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# add contribution to the end state
Xk_end = Xk_end + D[j] * Xc[j - 1]
# add contribution to quadrature function
J = J + B[j] * qj * h
# add contribution to scaling factor (for calculating lap time)
sf_opt.append(B[j] * qj * h)
# calculate used energy
dt_opt.append(sf_opt[0] + sf_opt[1] + sf_opt[2])
ec_opt.append(Xk[0] * v_s * Uk[1] * f_drive_s * dt_opt[-1])
# add new decision variables for state at end of the collocation interval
Xk = ca.MX.sym('X_' + str(k + 1), nx)
w.append(Xk)
n_min = (-w_tr_right_interp(k + 1) + pars["optim_opts"]["width_opt"] / 2.0) / n_s
n_max = (w_tr_left_interp(k + 1) - pars["optim_opts"]["width_opt"] / 2.0) / n_s
lbw.append([v_min, beta_min, omega_z_min, n_min, xi_min])
ubw.append([v_max, beta_max, omega_z_max, n_max, xi_max])
w0.append([v_guess, 0.0, 0.0, 0.0, 0.0])
# add equality constraint
g.append(Xk_end - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# get tire forces
f_x_flk, f_x_frk, f_x_rlk, f_x_rrk = f_fx(Xk, Uk)
f_y_flk, f_y_frk, f_y_rlk, f_y_rrk = f_fy(Xk, Uk)
f_z_flk, f_z_frk, f_z_rlk, f_z_rrk = f_fz(Xk, Uk)
# get accelerations (longitudinal + lateral)
axk, ayk = f_a(Xk, Uk)
# path constraint: limitied engine power
g.append(Xk[0] * Uk[1])
lbg.append([-np.inf])
ubg.append([veh["power_max"] / (f_drive_s * v_s)])
# get constant friction coefficient
if pars["optim_opts"]["var_friction"] is None:
mue_fl = pars["optim_opts"]["mue"]
mue_fr = pars["optim_opts"]["mue"]
mue_rl = pars["optim_opts"]["mue"]
mue_rr = pars["optim_opts"]["mue"]
# calculate variable friction coefficients along the reference line (regression with linear equations)
elif pars["optim_opts"]["var_friction"] == "linear":
# friction coefficient for each tire
mue_fl = w_mue_fl[k + 1, 0] * Xk[3] * n_s + w_mue_fl[k + 1, 1]
mue_fr = w_mue_fr[k + 1, 0] * Xk[3] * n_s + w_mue_fr[k + 1, 1]
mue_rl = w_mue_rl[k + 1, 0] * Xk[3] * n_s + w_mue_rl[k + 1, 1]
mue_rr = w_mue_rr[k + 1, 0] * Xk[3] * n_s + w_mue_rr[k + 1, 1]
# calculate variable friction coefficients along the reference line (regression with gaussian basis functions)
elif pars["optim_opts"]["var_friction"] == "gauss":
# gaussian basis functions
sigma = 2.0 * center_dist[k + 1, 0]
n_gauss = pars["optim_opts"]["n_gauss"]
n_q = np.linspace(-n_gauss, n_gauss, 2 * n_gauss + 1) * center_dist[k + 1, 0]
gauss_basis = []
for i in range(2 * n_gauss + 1):
gauss_basis.append(ca.exp(-(Xk[3] * n_s - n_q[i]) ** 2 / (2 * (sigma ** 2))))
gauss_basis = ca.vertcat(*gauss_basis)
mue_fl = ca.dot(w_mue_fl[k + 1, :-1], gauss_basis) + w_mue_fl[k + 1, -1]
mue_fr = ca.dot(w_mue_fr[k + 1, :-1], gauss_basis) + w_mue_fr[k + 1, -1]
mue_rl = ca.dot(w_mue_rl[k + 1, :-1], gauss_basis) + w_mue_rl[k + 1, -1]
mue_rr = ca.dot(w_mue_rr[k + 1, :-1], gauss_basis) + w_mue_rr[k + 1, -1]
else:
raise ValueError("No friction coefficients are available!")
# path constraint: Kamm's Circle for each wheel
g.append(((f_x_flk / (mue_fl * f_z_flk)) ** 2 + (f_y_flk / (mue_fl * f_z_flk)) ** 2))
g.append(((f_x_frk / (mue_fr * f_z_frk)) ** 2 + (f_y_frk / (mue_fr * f_z_frk)) ** 2))
g.append(((f_x_rlk / (mue_rl * f_z_rlk)) ** 2 + (f_y_rlk / (mue_rl * f_z_rlk)) ** 2))
g.append(((f_x_rrk / (mue_rr * f_z_rrk)) ** 2 + (f_y_rrk / (mue_rr * f_z_rrk)) ** 2))
lbg.append([0.0] * 4)
ubg.append([1.0] * 4)
# path constraint: lateral wheel load transfer
g.append(((f_y_flk + f_y_frk) * ca.cos(Uk[0] * delta_s) + f_y_rlk + f_y_rrk
+ (f_x_flk + f_x_frk) * ca.sin(Uk[0] * delta_s))
* veh["cog_z"] / ((veh["track_width_front"] + veh["track_width_rear"]) / 2) - Uk[3] * gamma_y_s)
lbg.append([0.0])
ubg.append([0.0])
# path constraint: f_drive * f_brake == 0 (no simultaneous operation of brake and accelerator pedal)
g.append(Uk[1] * Uk[2])
lbg.append([-20000.0 / (f_drive_s * f_brake_s)])
ubg.append([0.0])
# path constraint: actor dynamic
if k > 0:
sigma = (1 - kappa_interp(k) * Xk[3] * n_s) / (Xk[0] * v_s)
g.append((Uk - w[1 + (k - 1) * nx]) / (pars["stepsize_opts"]["stepsize_reg"] * sigma))
lbg.append([delta_min / (veh["t_delta"]), -np.inf, f_brake_min / (veh["t_brake"]), -np.inf])
ubg.append([delta_max / (veh["t_delta"]), f_drive_max / (veh["t_drive"]), np.inf, np.inf])
# path constraint: safe trajectories with acceleration ellipse
if pars["optim_opts"]["safe_traj"]:
g.append((ca.fmax(axk, 0) / pars["optim_opts"]["ax_pos_safe"]) ** 2
+ (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
g.append((ca.fmin(axk, 0) / pars["optim_opts"]["ax_neg_safe"]) ** 2
+ (ayk / pars["optim_opts"]["ay_safe"]) ** 2)
lbg.append([0.0] * 2)
ubg.append([1.0] * 2)
# append controls (for regularization)
delta_p.append(Uk[0] * delta_s)
F_p.append(Uk[1] * f_drive_s / 10000.0 + Uk[2] * f_brake_s / 10000.0)
# append outputs
x_opt.append(Xk * x_s)
u_opt.append(Uk * u_s)
tf_opt.extend([f_x_flk, f_y_flk, f_z_flk, f_x_frk, f_y_frk, f_z_frk])
tf_opt.extend([f_x_rlk, f_y_rlk, f_z_rlk, f_x_rrk, f_y_rrk, f_z_rrk])
ax_opt.append(axk)
ay_opt.append(ayk)
# boundary constraint: start states = final states
g.append(w[0] - Xk)
lbg.append([0.0] * nx)
ubg.append([0.0] * nx)
# path constraint: limited energy consumption
if pars["optim_opts"]["limit_energy"]:
g.append(ca.sum1(ca.vertcat(*ec_opt)) / 3600000.0)
lbg.append([0])
ubg.append([pars["optim_opts"]["energy_limit"]])
# formulate differentiation matrix (for regularization)
diff_matrix = np.eye(N)
for i in range(N - 1):
diff_matrix[i, i + 1] = -1.0
diff_matrix[N - 1, 0] = -1.0
# regularization (delta)
delta_p = ca.vertcat(*delta_p)
Jp_delta = ca.mtimes(ca.MX(diff_matrix), delta_p)
Jp_delta = ca.dot(Jp_delta, Jp_delta)
# regularization (f_drive + f_brake)
F_p = ca.vertcat(*F_p)
Jp_f = ca.mtimes(ca.MX(diff_matrix), F_p)
Jp_f = ca.dot(Jp_f, Jp_f)
# formulate objective
J = J + pars["optim_opts"]["penalty_F"] * Jp_f + pars["optim_opts"]["penalty_delta"] * Jp_delta
# concatenate NLP vectors
w = ca.vertcat(*w)
g = ca.vertcat(*g)
w0 = np.concatenate(w0)
lbw = np.concatenate(lbw)
ubw = np.concatenate(ubw)
lbg = np.concatenate(lbg)
ubg = np.concatenate(ubg)
# concatenate output vectors
x_opt = ca.vertcat(*x_opt)
u_opt = ca.vertcat(*u_opt)
tf_opt = ca.vertcat(*tf_opt)
dt_opt = ca.vertcat(*dt_opt)
ax_opt = ca.vertcat(*ax_opt)
ay_opt = ca.vertcat(*ay_opt)
ec_opt = ca.vertcat(*ec_opt)
# ------------------------------------------------------------------------------------------------------------------
# CREATE NLP SOLVER ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# fill nlp structure
nlp = {'f': J, 'x': w, 'g': g}
# solver options
opts = {"expand": True,
"verbose": print_debug,
"ipopt.max_iter": 2000,
"ipopt.tol": 1e-7}
# solver options for warm start
if pars["optim_opts"]["warm_start"]:
opts_warm_start = {"ipopt.warm_start_init_point": "yes",
"ipopt.warm_start_bound_push": 1e-9,
"ipopt.warm_start_mult_bound_push": 1e-9,
"ipopt.warm_start_slack_bound_push": 1e-9,
"ipopt.mu_init": 1e-9}
opts.update(opts_warm_start)
# load warm start files
if pars["optim_opts"]["warm_start"]:
try:
w0 = np.loadtxt(os.path.join(export_path, 'w0.csv'))
lam_x0 = np.loadtxt(os.path.join(export_path, 'lam_x0.csv'))
lam_g0 = np.loadtxt(os.path.join(export_path, 'lam_g0.csv'))
except IOError:
print('\033[91m' + 'WARNING: Failed to load warm start files!' + '\033[0m')
sys.exit(1)
# check warm start files
if pars["optim_opts"]["warm_start"] and not len(w0) == len(lbw):
print('\033[91m' + 'WARNING: Warm start files do not fit to the dimension of the NLP!' + '\033[0m')
sys.exit(1)
# create solver instance
solver = ca.nlpsol("solver", "ipopt", nlp, opts)
# ------------------------------------------------------------------------------------------------------------------
# SOLVE NLP --------------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# start time measure
t0 = time.perf_counter()
# solve NLP
if pars["optim_opts"]["warm_start"]:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
# end time measure
tend = time.perf_counter()
# ------------------------------------------------------------------------------------------------------------------
# EXTRACT SOLUTION -------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# helper function to extract solution for state variables, control variables, tire forces, time
f_sol = ca.Function('f_sol', [w], [x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt],
['w'], ['x_opt', 'u_opt', 'tf_opt', 'dt_opt', 'ax_opt', 'ay_opt', 'ec_opt'])
# extract solution
x_opt, u_opt, tf_opt, dt_opt, ax_opt, ay_opt, ec_opt = f_sol(sol['x'])
# solution for state variables
x_opt = np.reshape(x_opt, (N + 1, nx))
# solution for control variables
u_opt = np.reshape(u_opt, (N, nu))
# solution for tire forces
tf_opt = np.append(tf_opt[-12:], tf_opt[:])
tf_opt = np.reshape(tf_opt, (N + 1, 12))
# solution for time
t_opt = np.hstack((0.0, np.cumsum(dt_opt)))
# solution for acceleration
ax_opt = np.append(ax_opt[-1], ax_opt)
ay_opt = np.append(ay_opt[-1], ay_opt)
atot_opt = np.sqrt(np.power(ax_opt, 2) + np.power(ay_opt, 2))
# solution for energy consumption
ec_opt_cum = np.hstack((0.0, np.cumsum(ec_opt))) / 3600.0
# ------------------------------------------------------------------------------------------------------------------
# EXPORT SOLUTION --------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
# export data to CSVs
opt_mintime_traj.src.export_mintime_solution.export_mintime_solution(file_path=export_path,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
tf=tf_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
w0=sol["x"],
lam_x0=sol["lam_x"],
lam_g0=sol["lam_g"])
# ------------------------------------------------------------------------------------------------------------------
# PLOT & PRINT RESULTS ---------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------------------------
if plot_debug:
opt_mintime_traj.src.result_plots_mintime.result_plots_mintime(pars=pars,
reftrack=reftrack,
s=s_opt,
t=t_opt,
x=x_opt,
u=u_opt,
ax=ax_opt,
ay=ay_opt,
atot=atot_opt,
tf=tf_opt,
ec=ec_opt_cum)
if print_debug:
print("INFO: Laptime: %.3fs" % t_opt[-1])
print("INFO: NLP solving time: %.3fs" % (tend - t0))
print("INFO: Maximum abs(ay): %.2fm/s2" % np.amax(ay_opt))
print("INFO: Maximum ax: %.2fm/s2" % np.amax(ax_opt))
print("INFO: Minimum ax: %.2fm/s2" % np.amin(ax_opt))
print("INFO: Maximum total acc: %.2fm/s2" % np.amax(atot_opt))
print('INFO: Energy consumption: %.3fWh' % ec_opt_cum[-1])
return -x_opt[:-1, 3], x_opt[:-1, 0]
u = casadi.MX.sym('u', nu)
# p = casadi.MX.sym('p', np.size(parameter))
# ode = casadi.vertcat(
# x[3] ,\
# x[4] ,\
# x[5] ,\
# -1/p[0]*(u[0] + u[1])*casadi.sin(x[2]) ,\
# 1/p[0]*(u[0] + u[1])*casadi.cos(x[2]) - p[3],\
# p[2]/p[1]*(u[0] - u[1])
# )
ode = casadi.vertcat(
x[3] ,\
x[4] ,\
x[5] ,\
-1/m*(u[0] + u[1])*casadi.sin(x[2]) ,\
1/m*(u[0] + u[1])*casadi.cos(x[2]) - g,\
l/I*(u[0] - u[1])
)
f = casadi.Function('f', [x,u], [ode], ['x','u'], ['ode'])
# === create integrator expression ===
intg_options = dict(tf = dt) # define integration step
dae = dict(x = x, p = u, ode = f(x,u)) # define the dae to integrate (ZOH)
# define the integrator
intg = casadi.integrator('intg', 'rk', dae, intg_options)
res = intg(x0 = x, p = u)
# define symbolic expression of integrator
x_next = res['xf']
def circle_path(s, centre, radius, rot_mat=np.eye(3)):
T_goal = cs.vertcat(centre[0] + radius * (cos(s) - 1),
centre[1] + radius * sin(s), centre[2])
return T_goal
def compute_vel_cmds(self):
if self.goal is not None and self.MO_obs is not None and self.SO_obs is not None:
x0 = self.pose
x_goal = self.goal
self.p[0:6] = np.append(x0, x_goal)
for k in range(N + 1):
for i in range(n_MO):
i_pos = n_MOst * n_MO * (k + 1) + 6 - (n_MO - i) * n_MOst
self.p[i_pos + 2:i_pos + 5] = np.array([self.MO_obs[i].velocities.twist.linear.x, self.MO_obs[i].orientation.z, self.MO_obs[i].radius])
t_predicted = k * Ts
obs_x = self.MO_obs[i].polygon.points[0].x + t_predicted * self.MO_obs[i].velocities.twist.linear.x * ca.cos(self.MO_obs[i].orientation.z)
obs_y = self.MO_obs[i].polygon.points[0].y + t_predicted * self.MO_obs[i].velocities.twist.linear.x * ca.sin(self.MO_obs[i].orientation.z)
self.p[i_pos:i_pos + 2] = [obs_x, obs_y]
i_pos += 5
self.p[i_pos:i_pos+n_SO*3] = closest_n_obs(self.SO_obs, x0, n_SO)
x0k = np.append(self.x_st_0.reshape(3 * (N + 1), 1), self.u0.reshape(2 * N, 1))
x0k = x0k.reshape(x0k.shape[0], 1)
sol = solver(x0=x0k, lbx=self.lbw, ubx=self.ubw, lbg=self.lbg, ubg=self.ubg, p=self.p)
u_sol = sol.get('x')[3 * (N + 1):].reshape((N, 2))
self.u0 = shift(u_sol)
self.x_st_0 = np.reshape(sol.get('x')[0:3 * (N + 1)], (N + 1, 3))
self.x_st_0 = np.append(self.x_st_0[1:, :], self.x_st_0[-1, :].reshape((1, 3)), axis=0)
print(u_sol)
cmd_vel = Twist()
cmd_vel.linear.x = u_sol[0]
cmd_vel.angular.z = u_sol[1]
self.pub.publish(cmd_vel)
self.mpc_i = self.mpc_i + 1
print(self.mpc_i)
print('This is the set of control solutions: ', u_sol)
else:
print("Goal has not been received yet. Waiting.")
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))]
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 = 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.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))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx(th[1:], th[:-1])),
'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
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])
plot_race_line(np.array(track), n_set, wait=True)
return n_set
def plt_fnc(state, predict, goal, t, u_cl, SO_init, MO_init):
plt.figure(1)
plt.grid()
plt.text(goal[0] - 0.15, goal[1] - 0.2, 'Goal', style='oblique', fontsize=10)
plt.text(-0.1, 0.15, 'Start', style='oblique', fontsize=10)
plt.plot(state.T[:, 0], state.T[:, 1])
plt.plot(0, 0, 'bo')
# plt.plot(predict[:, 0], predict[:, 1])
plt.plot(goal[0], goal[1], 'ro')
plt.xlabel('X-position [Meters]')
plt.ylabel('Y-position [Meters]')
plt.title('MPC in python')
for obs in range(len(SO_init[:])):
c1 = plt.Circle((SO_init[obs][0], SO_init[obs][1]), radius=SO_init[obs][2], alpha=0.5)
plt.gcf().gca().add_artist(c1)
for obs in range(len(MO_init[:])):
plt.quiver(MO_init[obs][0], MO_init[obs][1], 1 * ca.cos(MO_init[obs][2]), 1 * ca.sin(MO_init[obs][2]))
c2 = plt.Circle((MO_init[obs][0], MO_init[obs][1]), radius=MO_init[obs][4], alpha=0.5, color='r')
plt.gcf().gca().add_artist(c2)
# plt.xlim(-10, 10)
# plt.ylim(-10, 10)
fig, (ax1, ax2) = plt.subplots(2)
fig.suptitle('Control Signals From MPC Solution')
ax1.plot(t, u_cl[:, 0])
ax1.grid()
ax2.plot(t, u_cl[:, 1])
ax2.grid()
ax2.set_ylabel('Angular Velocity [m/s]')
ax2.set_xlabel('Time [s]')
ax1.set_ylabel('Linear Velocity [rad/s]')
ax1.set_xlabel('Time [s]')
plt.show()
return state, predict, goal, t
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)))
n_SO = 1
# System Model
x = ca.SX.sym('x')
y = ca.SX.sym('y')
theta = ca.SX.sym('theta')
states = ca.vertcat(x, y, theta)
n_states = 3 # len([states])
# Control system
v = ca.SX.sym('v')
omega = ca.SX.sym('omega')
controls = ca.vertcat(v, omega)
n_controls = 2 # len([controls])
rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta), omega)
# Obstacle states in each predictions
MOx = ca.SX.sym('MOx')
MOy = ca.SX.sym('MOy')
MOth = ca.SX.sym('MOth')
MOv = ca.SX.sym('MOv')
MOr = ca.SX.sym('MOr')
Ost = [MOx, MOy, MOth, MOv, MOr]
n_MOst = len(Ost)
# System setup for casadi
mapping_func = ca.Function('f', [states, controls], [rhs])
# Declare empty system matrices
n_controls = 2
opt_controls = opti.variable(N, n_controls)
v = opt_controls[:, 0]
omega = opt_controls[:, 1]
n_states = 3
opt_states = opti.variable(N + 1, n_states)
x = opt_states[:, 0]
y = opt_states[:, 1]
theta = opt_states[:, 2]
# parameters
opt_x0 = opti.parameter(3)
opt_xs = opti.parameter(3)
# create model
f = lambda x_, u_: ca.vertcat(
*[u_[0] * ca.cos(x_[2]), u_[0] * ca.sin(x_[2]), u_[1]])
f_np = lambda x_, u_: np.array(
[u_[0] * np.cos(x_[2]), u_[0] * np.sin(x_[2]), u_[1]])
## init_condition
opti.subject_to(opt_states[0, :] == opt_x0.T)
for i in range(N):
x_next = opt_states[i, :] + f(opt_states[i, :],
opt_controls[i, :]).T * T
opti.subject_to(opt_states[i + 1, :] == x_next)
## define the cost function
### some addition parameters
Q = np.array([[1.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, .1]])
R = np.array([[0.5, 0.0], [0.0, 0.05]])
#### cost function
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
deg = 4
newton = Newton(LagrangePoly,dae,nk,nicp,deg,'RADAU')
dt = 0.025
endTime = dt*nk
newton.setupStuff(endTime)
r = 0.3
x0 = [r,0,0,0]
xTraj = [np.array(x0)]
uTraj = []
p = np.array([0.3])
# make a trajectory
for k in range(nk):
uTraj.append(np.array([5*C.sin(k/float(nk))]))
# uTraj.append(np.array([0]))
newton.isolver.setInput(xTraj[-1],0)
newton.isolver.setInput(uTraj[-1],1)
newton.isolver.setInput(p[-1],2)
newton.isolver.output().set(1)
newton.isolver.evaluate()
xTraj.append(np.array(newton.isolver.output(1)).squeeze())
# for k in range(nk):
# print xTraj[k],uTraj[k]
# print xTraj[-1]
# setup MHE
nmhe = Nmhe(dae,nk)
# Identity matrix
Inx = ca.DMatrix.eye(nx)
# ----------------------------------------------------------------------------
# Dynamics
# ----------------------------------------------------------------------------
# Continuous dynamics
dt_sym = ca.SX.sym('dt')
state = cat.struct_symSX(['x', 'y', 'phi'])
control = cat.struct_symSX(['v', 'w'])
rhs = cat.struct_SX(state)
rhs['x'] = control['v'] * ca.cos(state['phi'])
rhs['y'] = control['v'] * ca.sin(state['phi'])
rhs['phi'] = control['w']
f = ca.SXFunction('Continuous dynamics', [state, control], [rhs])
# Discrete dynamics
state_next = state + dt_sym * f([state, control])[0]
op = {'input_scheme': ['state', 'control', 'dt'],
'output_scheme': ['state_next']}
F = ca.SXFunction('Discrete dynamics',
[state, control, dt_sym], [state_next], op)
Fj_x = F.jacobian('state')
Fj_u = F.jacobian('control')
F_xx = ca.SXFunction('F_xx', [state, control, dt_sym],
[ca.jacobian(F.jac('state')[i, :].T, state) for i in
range(nx)])
F_uu = ca.SXFunction('F_uu', [state, control, dt_sym],
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 forcesTorques(state, u, p, outputs):
rho = 1.23 # density of the air # [ kg/m^3]
rA = 1.085 #(dixit Kurt)
alpha0 = -0*C.pi/180
#TAIL LENGTH
lT = 0.4
#ROLL DAMPING
rD = 1e-2
pD = 0*1e-3
yD = 0*1e-3
#WIND-TUNNEL PARAMETERS
#Lift (report p. 67)
cLA = 5.064
cLe = -1.924
cL0 = 0.239
#Drag (report p. 70)
cDA = -0.195
cDA2 = 4.268
cDB2 = 5
# cDe = 0.044
# cDr = 0.111
cD0 = 0.026
#Roll (report p. 72)
cRB = -0.062
cRAB = -0.271
cRr = -5.637e-1
#Pitch (report p. 74)
cPA = 0.293
cPe = -4.9766e-1
cP0 = 0.03
#Yaw (report p. 76)
cYB = 0.05
cYAB = 0.229
# rudder (fudged by Greg)
cYr = 0.02
span = 0.96
chord = 0.1
########### model integ ###################
x = state['x']
y = state['y']
e11 = state['e11']
e12 = state['e12']
e13 = state['e13']
e21 = state['e21']
e22 = state['e22']
e23 = state['e23']
e31 = state['e31']
e32 = state['e32']
e33 = state['e33']
dx = state['dx']
dy = state['dy']
dz = state['dz']
w1 = state['w1']
w2 = state['w2']
w3 = state['w3']
delta = 0
ddelta = 0
aileron = u['aileron']
elevator = u['elevator']
rudder = u['rudder']
####### kinfile ######
dpE = C.veccat( [ dx*e11 + dy*e12 + dz*e13 + ddelta*e12*rA + ddelta*e12*x - ddelta*e11*y
, dx*e21 + dy*e22 + dz*e23 + ddelta*e22*rA + ddelta*e22*x - ddelta*e21*y
, dx*e31 + dy*e32 + dz*e33 + ddelta*e32*rA + ddelta*e32*x - ddelta*e31*y
])
dp_carousel_frame = C.veccat( [ dx - ddelta*y
, dy + ddelta*rA + ddelta*x
, dz
]) - C.veccat([C.cos(delta)*p['wind_x'],C.sin(delta)*p['wind_x'],0])
##### more model_integ ###########
# EFFECTIVE WIND IN THE KITE`S SYSTEM :
# ###############################
#Airfoil speed in carousel frame
we1 = dp_carousel_frame[0]
we2 = dp_carousel_frame[1]
we3 = dp_carousel_frame[2]
vKite2 = C.mul(dp_carousel_frame.trans(), dp_carousel_frame) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
outputs['airspeed'] = vKite
# CALCULATION OF THE FORCES :
# ###############################
#
# FORCE ARE COMPUTED IN THE CAROUSEL FRAME @>@>@>
#Aero coeff.
# LIFT DIRECTION VECTOR
# ############
# Relative wind speed in Airfoil's referential 'E'
wE1 = dpE[0]
wE2 = dpE[1]
wE3 = dpE[2]
# Airfoil's transversal axis in carousel referential 'e'
eTe1 = e21
eTe2 = e22
eTe3 = e23
# Lift axis ** Normed to we @>@> **
eLe1 = - eTe2*we3 + eTe3*we2
eLe2 = - eTe3*we1 + eTe1*we3
eLe3 = - eTe1*we2 + eTe2*we1
# AERODYNAMIC COEEFICIENTS
# #################
vT1 = wE1
vT2 = -lT*w3 + wE2
vT3 = lT*w2 + wE3
alpha = alpha0-wE3/wE1
#NOTE: beta & alphaTail are compensated for the tail motion induced by
#omega @>@>
betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3)
beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3)
alphaTail = alpha0-vT3/vT1
outputs['alpha(deg)']=alpha*180/C.pi
outputs['alphaTail(deg)']=alphaTail*180/C.pi
outputs['beta(deg)']=beta*180/C.pi
outputs['betaTail(deg)']=betaTail*180/C.pi
# cL = cLA*alpha + cLe*elevator + cL0
# cD = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cDe*elevator + cDr*aileron + cD0
# cR = -rD*w1 + cRB*beta + cRAB*alphaTail*beta + cRr*aileron
# cP = cPA*alphaTail + cPe*elevator + cP0
# cY = cYB*beta + cYAB*alphaTail*beta
cL_ = cLA*alpha + cLe*elevator + cL0
cD_ = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cD0
cR = -rD*w1 + cRB*betaTail + cRr*aileron + cRAB*alphaTail*betaTail
cP = -pD*w2 + cPA*alphaTail + cPe*elevator + cP0
cY = -yD*w3 + cYB*betaTail + cYAB*alphaTail*betaTail + cYr*rudder
# LIFT :
# ###############################
cL = 0.2*cL_
cD = 0.5*cD_
outputs['cL'] = cL
outputs['cD'] = cD
fL1 = rho*cL*eLe1*vKite/2.0
fL2 = rho*cL*eLe2*vKite/2.0
fL3 = rho*cL*eLe3*vKite/2.0
# DRAG :
# #############################
fD1 = -rho*vKite*cD*we1/2.0
fD2 = -rho*vKite*cD*we2/2.0
fD3 = -rho*vKite*cD*we3/2.0
# FORCES (AERO)
# ###############################
f1 = fL1 + fD1
f2 = fL2 + fD2
f3 = fL3 + fD3
#f = f-fT
# TORQUES (AERO)
# ###############################
t1 = 0.5*rho*vKite2*span*cR
t2 = 0.5*rho*vKite2*chord*cP
t3 = 0.5*rho*vKite2*span*cY
return (f1, f2, f3, t1, t2, t3)
def formulateMPC(self):
X = ca.MX.sym('X')
Y = ca.MX.sym('Y')
th = ca.MX.sym('th')
self.e_c = ca.Function('e_c', [X, Y, th], [
ca.sin(self.Phi(th)) * (X - self.cs_x(th)) - ca.cos(self.Phi(th)) *
(Y - self.cs_y(th))
])
self.e_l = ca.Function('e_l', [X, Y, th], [
-ca.cos(self.Phi(th)) *
(X - self.cs_x(th)) - ca.sin(self.Phi(th)) * (Y - self.cs_y(th))
])
gp_in = ca.SX.sym('gp_in', 4, 1)
mean = self.gp_dx.predict(gp_in)[0]
f_mean_dx = ca.Function('f_mean_dx', [gp_in], [mean])
mean = self.gp_dy.predict(gp_in)[0]
f_mean_dy = ca.Function('f_mean_dy', [gp_in], [mean])
gp_in2 = ca.SX.sym('gp_in2', 2, 1)
mean = self.gp_dth.predict(gp_in2)[0]
f_mean_dth = ca.Function('f_mean_dth', [gp_in2], [mean])
x = ca.SX.sym('x')
y = ca.SX.sym('y')
th = ca.SX.sym('th')
v = ca.SX.sym('v')
delta = ca.SX.sym('delta')
state = np.array([x, y, th])
control = np.array([v, delta])
rhs = np.array([
f_mean_dx(np.array([np.cos(th), np.sin(th), v, delta])),
f_mean_dy(np.array([np.cos(th), np.sin(th), v, delta])),
f_mean_dth(np.array([v, delta]))
]) + state
self.f_dyn = ca.Function('f_dyn', [state, control], [rhs])
self.mpc_opti = ca.casadi.Opti()
self.U = self.mpc_opti.variable(2, self.H)
self.X = self.mpc_opti.variable(3, self.H + 1)
self.TH = self.mpc_opti.variable(self.H + 1)
self.NU = self.mpc_opti.variable(self.H)
self.P_1 = self.mpc_opti.parameter(3)
self.P_2 = self.mpc_opti.parameter(2)
J = 0
for k in range(self.H + 1):
J += self.qc * self.e_c(self.X[0, k], self.X[
1, k], self.TH[k])**2 + self.ql * self.e_l(
self.X[0, k], self.X[1, k], self.TH[k])**2
for k in range(self.H - 1):
J += self.Ru[0] * (self.U[0, k + 1] - self.U[0, k])**2 + self.Ru[
1] * (self.U[1, k + 1] - self.U[1, k])**2 + self.Rv * (
self.NU[k + 1] - self.NU[k])**2
J += -self.gamma * self.TH[-1]
self.mpc_opti.minimize(J)
for k in range(self.H):
self.mpc_opti.subject_to(
self.X[:, k + 1] == self.f_dyn(self.X[:, k], self.U[:, k]))
self.mpc_opti.subject_to(self.TH[k + 1] == self.TH[k] +
self.T * self.NU[k])
self.mpc_opti.subject_to(0 <= self.TH)
self.mpc_opti.subject_to(self.TH <= self.L)
self.mpc_opti.subject_to(self.nu_min <= self.NU)
self.mpc_opti.subject_to(self.NU <= self.nu_max)
self.mpc_opti.subject_to(self.v_min <= self.U[0, :])
self.mpc_opti.subject_to(self.U[0, :] <= self.v_max)
self.mpc_opti.subject_to(self.delta_min <= self.U[1, :])
self.mpc_opti.subject_to(self.U[1, :] <= self.delta_max)
self.mpc_opti.subject_to(self.ramp_v_min <= self.U[0, 0] - self.P_2[0])
self.mpc_opti.subject_to(self.U[0, 0] - self.P_2[0] <= self.ramp_v_max)
self.mpc_opti.subject_to(
self.ramp_delta_min <= self.U[1, 0] - self.P_2[1])
self.mpc_opti.subject_to(
self.U[1, 0] - self.P_2[1] <= self.ramp_delta_max)
self.mpc_opti.subject_to(
self.ramp_v_min <= self.U[0, 1:] - self.U[0, 0:-1])
self.mpc_opti.subject_to(
self.U[0, 1:] - self.U[0, 0:-1] <= self.ramp_v_max)
self.mpc_opti.subject_to(
self.ramp_delta_min <= self.U[1, 1:] - self.U[1, 0:-1])
self.mpc_opti.subject_to(
self.U[1, 1:] - self.U[1, 0:-1] <= self.ramp_delta_max)
self.mpc_opti.subject_to(self.X[:, 0] == self.P_1[0:3])
p_opts = {'verbose_init': False}
s_opts = {'tol': 0.01, 'print_level': 0, 'max_iter': 100}
self.mpc_opti.solver('ipopt', p_opts, s_opts)
# Warm up
self.mpc_opti.set_value(self.P_1, self.state)
self.mpc_opti.set_value(self.P_2, self.input)
sol = self.mpc_opti.solve()
self.mpc_opti.set_initial(self.U, sol.value(self.U))
self.mpc_opti.set_initial(self.X, sol.value(self.X))
self.mpc_opti.set_initial(self.NU, sol.value(self.NU))
self.mpc_opti.set_initial(self.TH, sol.value(self.TH))
def fc(u,t):
assert u.numel()==1
return u*sin(.3*t) - cos(.4*t)
l = 1. mip = 1. mic = 5. # Declare variables (use scalar graph) t = ca.SX.sym( 't' ) # time u = ca.SX.sym( 'u' ) # control x = ca.SX.sym( 'x' , 4 ) # state # ODE rhs function # x[0] = dtheta # x[1] = theta # x[2] = dx # x[3] = x sint = ca.sin( x[1] ) cost = ca.cos( x[1] ) fric = mic * ca.sign( x[2] ) num = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l ) denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) ) ddtheta = num / denom ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp) x_err = x-x_target cost_mat = np.diag( [1,1,1,1] ) ode = ca.vertcat([ ddtheta, \ x[0], \ ddx, \ x[2]
def Max_velocity(pts, config, show=False):
mu = config['car']['mu']
m = config['car']['m']
g = config['car']['g']
l_f = config['car']['l_f']
l_r = config['car']['l_r']
safety_f = config['pp']['force_f']
f_max = mu * m * g * safety_f
f_long_max = l_f / (l_r + l_f) * f_max
max_v = config['lims'][
'max_v'] # parameter to be adapted so that optimiser isnt too fast
max_a = config['lims']['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),
'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 + [-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 + [f_max] * 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"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.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(2)
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"])
# plt.figure(3)
# plt.title("Theta vs t")
# plt.plot(t, th_i)
# plt.plot(t, np.abs(th_i))
# plt.figure(5)
# plt.title(f"t vs dt")
# plt.plot(t[1:], dt)
# plt.plot(t[1:], dt, '+')
# 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
def forcesTorques(state, u, p, outputs):
rho = 1.23 # density of the air # [ kg/m^3]
rA = 1.085 #(dixit Kurt)
alpha0 = -0*C.pi/180
#TAIL LENGTH
lT = 0.4
#ROLL DAMPING
rD = 1e-2
pD = 0*1e-3
yD = 0*1e-3
#WIND-TUNNEL PARAMETERS
#Lift (report p. 67)
cLA = 5.064
cLe = -1.924
cL0 = 0.239
#Drag (report p. 70)
cDA = -0.195
cDA2 = 4.268
cDB2 = 5
# cDe = 0.044
# cDr = 0.111
cD0 = 0.026
#Roll (report p. 72)
cRB = -0.062
cRAB = -0.271
cRr = -5.637e-1
#Pitch (report p. 74)
cPA = 0.293
cPe = -4.9766e-1
cP0 = 0.03
#Yaw (report p. 76)
cYB = 0.05
cYAB = 0.229
# rudder (fudged by Greg)
cYr = 0.02
span = 0.96
chord = 0.1
########### model integ ###################
x = state['x']
y = state['y']
e11 = state['e11']
e12 = state['e12']
e13 = state['e13']
e21 = state['e21']
e22 = state['e22']
e23 = state['e23']
e31 = state['e31']
e32 = state['e32']
e33 = state['e33']
dx = state['dx']
dy = state['dy']
dz = state['dz']
w1 = state['w1']
w2 = state['w2']
w3 = state['w3']
delta = 0
ddelta = 0
aileron = u['aileron']
elevator = u['elevator']
rudder = u['rudder']
####### kinfile ######
dpE = C.veccat( [ dx*e11 + dy*e12 + dz*e13 + ddelta*e12*rA + ddelta*e12*x - ddelta*e11*y
, dx*e21 + dy*e22 + dz*e23 + ddelta*e22*rA + ddelta*e22*x - ddelta*e21*y
, dx*e31 + dy*e32 + dz*e33 + ddelta*e32*rA + ddelta*e32*x - ddelta*e31*y
])
dp_carousel_frame = C.veccat( [ dx - ddelta*y
, dy + ddelta*rA + ddelta*x
, dz
]) - C.veccat([C.cos(delta)*p['wind_x'],C.sin(delta)*p['wind_x'],0])
##### more model_integ ###########
# EFFECTIVE WIND IN THE KITE`S SYSTEM :
# ###############################
#Airfoil speed in carousel frame
we1 = dp_carousel_frame[0]
we2 = dp_carousel_frame[1]
we3 = dp_carousel_frame[2]
vKite2 = C.mul(dp_carousel_frame.trans(), dp_carousel_frame) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
outputs['airspeed'] = vKite
# CALCULATION OF THE FORCES :
# ###############################
#
# FORCE ARE COMPUTED IN THE CAROUSEL FRAME @>@>@>
#Aero coeff.
# LIFT DIRECTION VECTOR
# ############
# Relative wind speed in Airfoil's referential 'E'
wE1 = dpE[0]
wE2 = dpE[1]
wE3 = dpE[2]
# Airfoil's transversal axis in carousel referential 'e'
eTe1 = e21
eTe2 = e22
eTe3 = e23
# Lift axis ** Normed to we @>@> **
eLe1 = - eTe2*we3 + eTe3*we2
eLe2 = - eTe3*we1 + eTe1*we3
eLe3 = - eTe1*we2 + eTe2*we1
# AERODYNAMIC COEEFICIENTS
# #################
vT1 = wE1
vT2 = -lT*w3 + wE2
vT3 = lT*w2 + wE3
alpha = alpha0-wE3/wE1
#NOTE: beta & alphaTail are compensated for the tail motion induced by
#omega @>@>
betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3)
beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3)
alphaTail = alpha0-vT3/vT1
outputs['alpha(deg)']=alpha*180/C.pi
outputs['alphaTail(deg)']=alphaTail*180/C.pi
outputs['beta(deg)']=beta*180/C.pi
outputs['betaTail(deg)']=betaTail*180/C.pi
# cL = cLA*alpha + cLe*elevator + cL0
# cD = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cDe*elevator + cDr*aileron + cD0
# cR = -rD*w1 + cRB*beta + cRAB*alphaTail*beta + cRr*aileron
# cP = cPA*alphaTail + cPe*elevator + cP0
# cY = cYB*beta + cYAB*alphaTail*beta
cL_ = cLA*alpha + cLe*elevator + cL0
cD_ = cDA*alpha + cDA2*alpha*alpha + cDB2*beta*beta + cD0
cR = -rD*w1 + cRB*betaTail + cRr*aileron + cRAB*alphaTail*betaTail
cP = -pD*w2 + cPA*alphaTail + cPe*elevator + cP0
cY = -yD*w3 + cYB*betaTail + cYAB*alphaTail*betaTail + cYr*rudder
# LIFT :
# ###############################
cL = 0.2*cL_
cD = 0.5*cD_
outputs['cL'] = cL
outputs['cD'] = cD
fL1 = rho*cL*eLe1*vKite/2.0
fL2 = rho*cL*eLe2*vKite/2.0
fL3 = rho*cL*eLe3*vKite/2.0
# DRAG :
# #############################
fD1 = -rho*vKite*cD*we1/2.0
fD2 = -rho*vKite*cD*we2/2.0
fD3 = -rho*vKite*cD*we3/2.0
# FORCES (AERO)
# ###############################
f1 = fL1 + fD1
f2 = fL2 + fD2
f3 = fL3 + fD3
#f = f-fT
# TORQUES (AERO)
# ###############################
t1 = 0.5*rho*vKite2*span*cR
t2 = 0.5*rho*vKite2*chord*cP
t3 = 0.5*rho*vKite2*span*cY
return (f1, f2, f3, t1, t2, t3)
def MinCurvatureTrajectory(track, obs_map=None):
w_min = -track[:, 4] * 0.9
w_max = track[:, 5] * 0.9
nvecs = track[:, 2:4]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
xgrid = np.arange(0, obs_map.shape[1])
ygrid = np.arange(0, obs_map.shape[0])
data_flat = np.array(obs_map).ravel(order='F')
lut = ca.interpolant('lut', 'bspline', [xgrid, ygrid], data_flat)
print(lut([10, 20]))
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 = 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.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)),
# lut(ca.horzcat(o_x_s(n[:-1]), o_y_s(n[:-1])).T).T,
# 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(track[i, 0:2], track[i + 1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
# lbx = [-n_max] * N + [-np.pi]*(N-1)
# ubx = [n_max] * N + [np.pi]*(N-1)
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)])
# lib.plot_race_line(np.array(track), n_set, wait=True)
return n_set
