def __build_dae_variables(self, variables, parameters, param):
""" Create dae variables based on awebox model variables and parameters.
@ param variables - model variables struct
@ param parameters - parameters struct
@ return - dae states 'x', algebraic vars 'z' and parameters 'p'
"""
# differential states
x = cas.struct_SX([cas.entry('xd', expr = variables['xd'])])
# algebraic variables
if 'xl' in list(variables.keys()):
z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives
cas.entry('xa', expr = variables['xa']), # algebraic variables
cas.entry('xl', expr = variables['xl']), # lifted variables
])
else:
z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives
cas.entry('xa', expr = variables['xa']), # algebraic variables
])
# parameters
if param == 'sym':
p = cas.struct_SX([cas.entry('u', expr = variables['u']), # controls
cas.entry('theta', expr = variables['theta']), # free parameters
cas.entry('param', expr = parameters)]) # fixed parameters
elif param == 'num':
p = cas.struct_SX([cas.entry('u', expr = variables['u'])]) # controls
return x, z, p
def _dict2struct(self, var, stru):
if isinstance(var, list):
return [self._dict2struct(v, stru) for v in var]
elif 'dd' in list(var.keys())[0] or 'admm' in list(var.keys())[0]:
chck = list(list(list(var.values())[0].values())[0].values())[0]
if isinstance(chck, SX):
ret = struct_SX(stru)
elif isinstance(chck, MX):
ret = struct_MX_mutable(stru)
elif isinstance(chck, DM):
ret = stru(0)
for nghb in var.keys():
for child, q in var[nghb].items():
for name in q.keys():
ret[nghb, child, name] = var[nghb][child][name]
return ret
else:
chck = list(list(var.values())[0].values())[0]
if isinstance(chck, SX):
ret = struct_SX(stru)
elif isinstance(chck, MX):
ret = struct_MX_mutable(stru)
elif isinstance(chck, DM):
ret = stru(0)
for child, q in var.items():
for name in q.keys():
ret[child, name] = var[child][name]
return ret
def generate_generalized_coordinates(system_variables, system_gc):
try:
test = system_variables['xd', 'l_t']
struct_flag = 1
except:
struct_flag = 0
if struct_flag == 1:
generalized_coordinates = {}
generalized_coordinates['xgc'] = cas.struct_SX([
cas.entry(name, expr=system_variables['xd', name])
for name in system_gc
])
generalized_coordinates['xgcdot'] = cas.struct_SX([
cas.entry('d' + name, expr=system_variables['xd', 'd' + name])
for name in system_gc
])
# generalized_coordinates['xgcddot'] = cas.struct_SX(
# [cas.entry('dd' + name, expr=system_variables['xddot', 'dd' + name])
# for name in system_gc])
else:
generalized_coordinates = {}
generalized_coordinates['xgc'] = cas.struct_SX([
cas.entry(name, expr=system_variables['xd'][name])
for name in system_gc
])
generalized_coordinates['xgcdot'] = cas.struct_SX([
cas.entry('d' + name, expr=system_variables['xd']['d' + name])
for name in system_gc
])
# generalized_coordinates['xgcddot'] = cas.struct_SX(
# [cas.entry('dd' + name, expr=system_variables['xddot']['dd' + name])
# for name in system_gc])
return generalized_coordinates
def _create_belief_objective_function(model, V):
# Simple cost
running_cost = 0
for k in range(model.n):
[stage_cost] = model.c([V['X', k], V['U', k]])
running_cost += stage_cost
[final_cost] = model.cl([V['X', model.n]])
# Uncertainty cost
running_uncertainty_cost = 0
bk = cat.struct_SX(model.b)
bk['S'] = model.b0['S']
for k in range(model.n):
# Belief propagation
bk['m'] = V['X', k]
[bk_next] = model.BF([bk, V['U', k]])
bk_next = model.b(bk_next)
# Accumulate cost
[stage_uncertainty_cost] = model.cS([bk_next])
running_uncertainty_cost += stage_uncertainty_cost
# Advance time
bk = bk_next
[final_uncertainty_cost] = model.cSl([bk_next])
return running_cost + final_cost +\
running_uncertainty_cost + final_uncertainty_cost
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 ext_belief_dynamics(ext_belief, control,
F, dt, A_fcn, C_fcn,
Q, obs_cov_fcn):
ext_belief_next = cat.struct_SX(ext_belief)
# Predict the mean
[mu_bar] = F([ ext_belief['m'], control, dt ])
# Compute linearization
[A,_] = A_fcn([ ext_belief['m'], control, dt ])
[C,_] = C_fcn([ mu_bar ])
# Predict the covariance
S_bar = ca.mul([ A, ext_belief['S'], A.T ]) + Q
# Get the observations noise covariance
[R] = obs_cov_fcn([ mu_bar ])
# Compute the inverse
P = ca.mul([ C, S_bar, C.T ]) + R
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([ S_bar, C.T, P_inv ])
# Update equations
nx = ext_belief['m'].size()
ext_belief_next['m'] = mu_bar
ext_belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
ext_belief_next['L'] = ca.mul([ A, ext_belief['L'], A.T ]) + \
ca.mul([ K, C, S_bar ])
# (ext_belief, control) -> next_ext_belief
return ca.SXFunction('Extended-belief dynamics',
[ext_belief,control],[ext_belief_next])
def _create_belief_nonlinear_constraints(model, V):
"""Non-linear constraints for planning"""
bk = cat.struct_SX(model.b)
bk['S'] = model.b0['S']
g, lbg, ubg = [], [], []
for k in range(model.n):
# Belief propagation
bk['m'] = V['X', k]
[bk_next] = model.BF([bk, V['U', k]])
bk_next = model.b(bk_next)
# Multiple shooting
g.append(bk_next['m'] - V['X', k+1])
lbg.append(ca.DMatrix.zeros(model.nx))
ubg.append(ca.DMatrix.zeros(model.nx))
# Control constraints
constraint_k = model._set_constraint(V, k)
g.append(constraint_k)
lbg.append(-ca.inf)
ubg.append(0)
# Advance time
bk = bk_next
g = ca.veccat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
return [g, lbg, ubg]
def _create_EBF(self):
"""Extended belief dynamics"""
eb_next = cat.struct_SX(self.eb)
# Compute the mean
[mu_bar] = self.F([self.eb['m'], self.u])
# Compute linearization
[A, _] = self.Fj_x([self.eb['m'], self.u])
[C, _] = self.hj_x([mu_bar])
# Get system and observation noises, as if the state was mu_bar
[M] = self.M([self.eb['m'], self.u])
[N] = self.N([mu_bar])
# Predict the covariance
S_bar = ca.mul([A, self.eb['S'], A.T]) + M
# Compute the inverse
P = ca.mul([C, S_bar, C.T]) + N
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([S_bar, C.T, P_inv])
# Update equations
eb_next['m'] = mu_bar
eb_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar)
eb_next['L'] = ca.mul([A, self.eb['L'], A.T]) + ca.mul([K, C, S_bar])
# (eb, u) -> eb_next
op = {'input_scheme': ['eb', 'u'],
'output_scheme': ['eb_next']}
return ca.SXFunction('Extended belief dynamics',
[self.eb, self.u], [eb_next], op)
def belief_dynamics(belief, control, # current (b, u)
F, dt, A_fcn, C_fcn, # dynamics
Q, obs_cov_fcn): # covariances Q and R(x)
belief_next = cat.struct_SX(belief)
# Predict the mean
[mu_bar] = F([ belief['m'], control, dt ])
# Compute linearization
[A,_] = A_fcn([ belief['m'], control, dt ])
[C,_] = C_fcn([ mu_bar ])
# Predict the covariance
S_bar = ca.mul([ A, belief['S'], A.T ]) + Q
# Get the observations noise covariance
[R] = obs_cov_fcn([ mu_bar ])
# Compute the inverse
P = ca.mul([ C, S_bar, C.T ]) + R
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([ S_bar, C.T, P_inv ])
# Update equations
nx = belief['m'].size()
belief_next['m'] = mu_bar
belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
# (belief, control) -> next_belief
return ca.SXFunction('Belief dynamics',[belief,control],[belief_next])
def build_integrator(self, options, model):
print('Building integrator...')
# get model variables
variables = model.variables
# construct the DAE variables
x = cas.struct_SX([cas.entry('xd', expr = variables['xd'])]) # differential states
z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives
cas.entry('xa', expr = variables['xa']), # algebraic variables
cas.entry('xl', expr = variables['xl']), # lifted variables
])
p = cas.struct_SX([cas.entry('u', expr = variables['u']), # dae parameters
cas.entry('theta', expr = variables['theta']),
cas.entry('phi', expr = model.parameters)])
# scale xddot with t_f
time_scaled_variables = scale_xddot(variables)
# model equations
alg = model.dynamics(time_scaled_variables, model.parameters)
ode = variables['xddot']
# create dae
dae = {'x': x.cat, 'z': z.cat, 'p': p.cat, 'alg': alg,'ode': ode}
# system dynamics
f = cas.Function('f', [x, z, p], [ode, alg], ['x', 'z', 'p'], ['ode', 'alg'])
# create integrator and rootfinder
if cas.sprank(cas.jacobian(alg,z)) < z.cat.size()[0]: # check dae index
raise ValueError('jacobian of dynamics is structurally rank-deficient: DAE is not of index 1!')
else:
# create integrator
I = cas.integrator('I', options['integrator']['type'], dae, {'tf': 1.0 / self.__N_sim})
# create rootfinder
g = cas.Function('g',[z.cat,x.cat,p.cat],[alg])
G = cas.rootfinder('G', 'newton', g, {'linear_solver': 'csparse'})
self.__integrator = I
self.__rootfinder = G
self.__variables_dict = model.variables_dict
self.__phi = model.parameters
self.__dae = dae
self.__f = f
self.__x = x
self.__z = z
self.__p = p
def _create_observation_function(self):
# Define the observation
rhs = cat.struct_SX(self.z)
for label in self.z.keys():
rhs[label] = self.x[label]
op = {'input_scheme': ['x'],
'output_scheme': ['z']}
return ca.SXFunction('Observation function',
[self.x], [rhs], op)
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 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 gen(NX, NU, M, stateBounds=None, controlBounds=None):
"""gen Generates a casadi struct containing the symbolic optimization variables required
for direct multiple shooting. x is a <NX>x<M+1> matrix, u is a <NU>x<M> matrix.
:param NX: Number of state variables
:param NU: Number of control variables
:param M: Prediction horizon length
:returns: A casadi struct_symSX object
"""
# decision (free) variables
variables = struct_symSX(
[entry('x', shape=(NX, M + 1)),
entry('u', shape=(NU, M))])
# symbolic bounds
bx = ca.SX.sym('bx', NX)
bu = ca.SX.sym('bu', NU)
# bounds struct, must be identical to variables struct in dimensions and keys
bounds = struct_SX([
entry('x', expr=ca.repmat(bx, 1, M + 1)),
entry('u', expr=ca.repmat(bu, 1, M))
])
boundsFun = ca.Function('varBoundsFun', [bx, bu], [bounds.cat])
if stateBounds is None:
stateBounds = np.multiply(np.ones((2, NX)),
np.array((-np.inf, np.inf), ndmin=2).T)
if controlBounds is None:
controlBounds = np.multiply(np.ones((2, NU)),
np.array((-np.inf, np.inf), ndmin=2).T)
lbw = boundsFun(stateBounds[0, :], controlBounds[0, :])
ubw = boundsFun(stateBounds[1, :], controlBounds[1, :])
return variables, lbw, ubw
def _create_EKF(self):
"""Extended Kalman filter"""
b_next = cat.struct_SX(self.b)
# Compute the mean
[mu_bar] = self.F([self.b['m'], self.u])
# Compute linearization
[A, _] = self.Fj_x([self.b['m'], self.u])
[C, _] = self.hj_x([mu_bar])
# Get system and observation noises
[M] = self.M([self.b['m'], self.u])
[N] = self.N([mu_bar])
# Predict the covariance
S_bar = ca.mul([A, self.b['S'], A.T]) + M
# Compute the inverse
P = ca.mul([C, S_bar, C.T]) + N
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([S_bar, C.T, P_inv])
# Predict observation
[z_bar] = self.h([mu_bar])
# Update equations
b_next['m'] = mu_bar + ca.mul([K, self.z - z_bar])
b_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar)
# (b, u, z) -> b_next
op = {'input_scheme': ['b', 'u', 'z'],
'output_scheme': ['b_next']}
return ca.SXFunction('Extended Kalman filter',
[self.b, self.u, self.z], [b_next], op)
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(["x_b", "y_b", "z_b", "vx_b", "vy_b", "vz_b", "x_c", "y_c", "phi"])
# Control
control = cat.struct_symSX(["v", "w", "psi"])
# Observation
observation = cat.struct_SX(
[
cat.entry("x_b", expr=state["x_b"]),
cat.entry("y_b", expr=state["y_b"]),
cat.entry("z_b", expr=state["z_b"]),
cat.entry("x_c", expr=state["x_c"]),
cat.entry("y_c", expr=state["y_c"]),
cat.entry("phi", expr=state["phi"]),
]
)
# Belief state (mu, Sigma)
belief = cat.struct_symSX([cat.entry("m", struct=state), cat.entry("S", shapestruct=(state, state))])
# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX(
[
cat.entry("m", struct=state),
cat.entry("S", shapestruct=(state, state)),
cat.entry("L", shapestruct=(state, state)),
]
aeroCLaTip = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
#aeroCLaRoot = numpy.array([ -28.2136, 16.4140, 0.9568,-0.4000])
#aeroCLaTip = numpy.array([ -28.2136, 16.4140, 0.9568, -0.4000])
clPolyLoc = numpy.zeros((4,n)) #setup lift slope curves for each station
for i in range(n):
clPolyLoc[:,i] = aeroCLaRoot[:] + 2.0*(aeroCLaTip[:] - aeroCLaRoot[:])*yLoc[i]/bref
geom = geometry.Geometry(thetaLoc, chordLoc, yLoc, aIncGeometricLoc, clPolyLoc, bref, n)
(makeMeZero, alphaiLoc, CL, CDi) = setupImplicitFunction(dvs['operAlpha'], dvs['An'], geom)
outputsFcn = C.SXFunction([dvs.cat], [alphaiLoc, CL, CDi, geom.sref])
outputsFcn.init()
g = CT.struct_SX([ CT.entry("makeMeZero",expr=makeMeZero),
CT.entry('alphaiLoc',expr=alphaiLoc),
CT.entry("CL",expr=CL),
CT.entry("CDi",expr=CDi),
CT.entry("sref",expr=geom.sref)])
obj = -CL / (CDi + 0.01)
nlp = C.SXFunction(C.nlpIn(x=dvs),C.nlpOut(f=obj,g=g))
solver = C.IpoptSolver(nlp)
solver.setOption('tol',1e-11)
solver.setOption('linear_solver','ma27')
#solver.setOption('linear_solver','ma57')
solver.init()
lbg = g(solver.input("lbg"))
ubg = g(solver.input("ubg"))
lbx = dvs(solver.input("lbx"))
# Initial condition
x0 = ca.DMatrix([10, 6, ca.pi])
# 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
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(['x_b','y_b','vx_b','vy_b',
'x_c','y_c','phi'])
# Control
control = cat.struct_symSX(['v','w'])
# Observation
observation = cat.struct_SX([
cat.entry('x_b', expr = state['x_b']),
cat.entry('y_b', expr = state['y_b']),
cat.entry('x_c', expr = state['x_c']),
cat.entry('y_c', expr = state['y_c']),
cat.entry('phi', expr = state['phi'])
])
# Belief state (mu, Sigma)
belief = cat.struct_symSX([
cat.entry('m',struct=state),
cat.entry('S',shapestruct=(state,state))
])
# Extended belief (mu, Sigma, Lambda) for plotting
ext_belief = cat.struct_symSX([
cat.entry('m',struct=state),
cat.entry('S',shapestruct=(state,state)),
cat.entry('L',shapestruct=(state,state))
# Initial condition
x0 = ca.DMatrix([10, 6, ca.pi])
# 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')
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 create_optim(self):
# Initialize trajectory lists (each list item, one time-step):
self.mpc_obj_x = mpc_obj_x = struct_symSX([
entry('x', repeat=self.N_steps+1, struct=self.mpc_xk),
entry('u', repeat=self.N_steps, struct=self.mpc_uk),
entry('eps', repeat=self.N_steps+1, struct=self.mpc_eps)
])
# Note that:
# x = [x_0, x_1, ... , x_N+1] (N+1 elements)
# u = [u_0, u_1, ... , u_N] (N elements)
# For the optimization variable x_0 we introduce the simple equality constraint that it has
# to be equal to the parameter x0 (mpc_obj_p)
self.mpc_obj_p = mpc_obj_p = struct_symSX([
entry('tvp', repeat=self.N_steps, struct=self.mpc_tvpk),
entry('x0', struct=self.mpc_xk),
entry('p', struct=self.mpc_pk),
entry('pN', struct=self.mpc_pN)
])
# Dummy struct with symbolic variables
aux_struct = struct_symSX([
entry('aux', repeat=self.N_steps, struct=self.mpc_aux_expr)
])
# Create mutable symbolic expression from the struct defined above.
self.mpc_obj_aux = mpc_obj_aux = struct_SX(aux_struct)
# Initialize objective value:
obj = 0
# Initialize constraings:
cons = []
cons_ub = []
cons_lb = []
# Equality constraint for first state:
cons.append(mpc_obj_x['x', 0]-mpc_obj_p['x0'])
cons_lb.append(np.zeros(self.mpc_xk.shape))
cons_ub.append(np.zeros(self.mpc_xk.shape))
# Recursively evaluate system equation and add stage cost and stage constraints:
for k in range(self.N_steps):
mpc_xk_next = self.mpc_problem['model'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# State constraint:
cons.append(mpc_xk_next-mpc_obj_x['x', k+1])
cons_lb.append(np.zeros(self.mpc_xk.shape))
cons_ub.append(np.zeros(self.mpc_xk.shape))
# Add the "stage cost" to the objective
obj += self.mpc_problem['stage_cost'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# Constraints for the current step
cons.append(self.mpc_problem['cons'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p']))
cons_lb.append(self.mpc_problem['cons_lb'])
cons_ub.append(self.mpc_problem['cons_ub'])
# Calculate auxiliary values:
mpc_obj_aux['aux', k] = self.mpc_problem['aux'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# Terminal cost:
obj += self.mpc_problem['terminal_cost'](mpc_obj_x['x', -1], mpc_obj_x['eps', -1])
# Terminal set:
cons.append(self.mpc_problem['tcons'](mpc_obj_x['x', -1], mpc_obj_x['eps', -1], mpc_obj_p['pN']))
cons_lb.append(self.mpc_problem['tcons_lb'])
cons_ub.append(self.mpc_problem['tcons_ub'])
# Upper and lower bounds on objective x:
self.mpc_obj_x_lb = self.mpc_obj_x(0)
self.mpc_obj_x_ub = self.mpc_obj_x(0)
self.mpc_obj_x_lb['x', :] = self.mpc_xk_lb
self.mpc_obj_x_ub['x', :] = self.mpc_xk_ub
self.mpc_obj_x_lb['u', :] = self.mpc_uk_lb
self.mpc_obj_x_ub['u', :] = self.mpc_uk_ub
self.mpc_obj_x_lb['eps', :] = self.mpc_epsk_lb
self.mpc_obj_x_ub['eps', :] = self.mpc_epsk_ub
optim_dict = {'x': mpc_obj_x, # Optimization variable
'f': obj, # objective
'g': vertcat(*cons), # constraints
'p': mpc_obj_p} # parameters
self.cons_lb = vertcat(*cons_lb)
self.cons_ub = vertcat(*cons_ub)
# Use the structured data obj_x and obj_p and create identically organized copies with numerical values (initialized to zero)
self.mpc_obj_x_num = self.mpc_obj_x(0)
self.mpc_obj_p_num = self.mpc_obj_p(0)
self.mpc_obj_aux_num = self.mpc_obj_aux(0)
# TODO: Make optimization option available to user.
# Create casadi optimization object:
opts = {'ipopt.linear_solver': 'ma27', 'error_on_fail': False, 'ipopt.tol': 1e-8, 'ipopt.max_iter': 300}
self.optim = nlpsol('optim', 'ipopt', optim_dict, opts)
if self.silent:
opts['ipopt.print_level'] = 0
opts['ipopt.sb'] = "yes"
opts['print_time'] = 0
# Create function to calculate buffer memory from parameter and optimization variable trajectories
self.aux_fun = Function('aux_fun', [mpc_obj_x, mpc_obj_p], [mpc_obj_aux])
rob_diam = 0.3 # [m]
v_max = 0.6
omega_max = np.pi / 4.0
states = ca_tools.struct_symSX([(ca_tools.entry('x'), ca_tools.entry('y'),
ca_tools.entry('theta'))])
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
def problem_formulation(self):
""" MPC states for stage k"""
self.mpc_xk = struct_symSX([
entry('s_buffer', shape=(self.n_out, 1)),
entry('ds_buffer_source', shape=(self.n_in,1))
])
# States at next time-step. Same structure as mpc_xk. Will be assigned expressions later on.
self.mpc_xk_next = struct_SX(self.mpc_xk)
""" MPC control inputs for stage k"""
self.mpc_uk = struct_symSX([
entry('dv_in', shape=(self.n_in, 1)),
entry('dv_out', shape=(self.n_out, 1)),
])
""" MPC soft constraints"""
self.mpc_eps = struct_symSX([
entry('s_buffer', shape=(self.n_out, 1)),
])
eps_s_buffer = self.mpc_eps['s_buffer']
""" MPC time-varying parameters for stage k"""
self.mpc_tvpk = struct_symSX([
entry('u_prev', struct=self.mpc_uk),
entry('v_out_max', shape=(self.n_out, 1)),
entry('s_buffer_source', shape=(self.n_in, 1)),
entry('v_out_source', shape=(self.n_in, 1)),
entry('dv_out_source_fix', shape=(self.n_in,1)),
entry('time_fac'),
])
""" MPC parameters for stage k"""
self.mpc_pk = struct_symSX([
# Note: Pb is defined "transposed", as casadi will raise an error for n_out=1, since it cant handle row vectors.
entry('Pb', shape=(np.sum(self.n_in), self.n_out)),
entry('control_delta', shape=1),
])
""" MPC parameters for stage N"""
self.mpc_pN = struct_symSX([
entry('s_buffer_source_N', shape=(self.n_in,1)),
])
""" Memory """
# Buffer memory
s_buffer = self.mpc_xk['s_buffer']
""" Incoming packet stream """
# Allowed/accepted incoming packet stream:
v_in = self.v_in_max_total-self.mpc_uk['dv_in']
""" Outgoing packet stream """
# Outgoing packet stream:
v_out = self.v_out_max_total-self.mpc_uk['dv_out']
# Maximum value for v_out (determined by target server):
v_out_max = self.mpc_tvpk['v_out_max']
""" Load information """
# memory info incoming servers
s_buffer_source = self.mpc_tvpk['s_buffer_source']
""" Source Node """
# Adjusted buffer memory of source:
ds_buffer_source = self.mpc_xk['ds_buffer_source']
# Predicted outgoing packet stream:
v_out_source = self.mpc_tvpk['v_out_source']
# v_out_source adjusted:
dv_out_source = v_in - v_out_source
# Corrected buffer memory of source:
s_buffer_source_corr = s_buffer_source - ds_buffer_source
# ds_buffer_source_next:
self.mpc_xk_next['ds_buffer_source'] = ds_buffer_source + dv_out_source*self.dt
""" Circuit matching """
# Assignment Matrix: Which element of each input is assigned to which output buffer:
# Note: Pb is defined "transposed", as casadi will raise an error for n_out=1, since it cant handle row vectors.
Pb = self.mpc_pk['Pb'].T
""" System dynamics"""
# system dynamics, constraints and objective definition:
s_tilde_next = s_buffer + self.dt*Pb@(v_in)
s_buffer_next = s_tilde_next - self.dt*v_out
self.mpc_xk_next['s_buffer'] = s_buffer_next
""" Objective """
stage_cost = 0
# Objective function with fairness formulation:
#s_buffer_source_split = (s_buffer_source+eps)/(sum1(s_buffer_source+eps))
time_fac = self.mpc_tvpk['time_fac']
stage_cost += 10*sum1(time_fac/(self.n_in)*self.mpc_uk['dv_in']**2)
stage_cost += sum1(time_fac/(self.n_out)*self.mpc_uk['dv_out']**2)
stage_cost += 1e5*sum1(eps_s_buffer)
# Control delta regularization
stage_cost += self.mpc_pk['control_delta']*sum1((self.mpc_uk-self.mpc_tvpk['u_prev'])**2)
# Terminal cost:
terminal_cost = 1e5*sum1(eps_s_buffer)
""" Constraints"""
self.mpc_xk_lb = self.mpc_xk(0)
self.mpc_xk_lb['ds_buffer_source'] = -np.inf
self.mpc_xk_ub = self.mpc_xk(np.inf)
# All inputs with lower bound 0 and upper bound infinity
self.mpc_uk_lb = self.mpc_uk(0)
self.mpc_uk_ub = self.mpc_uk(np.inf)
# All eps with lower bound 0 and upper bound infinity
self.mpc_epsk_lb = self.mpc_eps(0)
self.mpc_epsk_ub = self.mpc_eps(np.inf)
# Further constraints on states and inputs:
# Note lb and ub must be lists to be concatenated lateron
dv_out_source_fix = self.mpc_tvpk['dv_out_source_fix']
cons_list = [
{'lb': [0]*self.n_in, 'eq': v_in, 'ub': [np.inf]*self.n_in}, # v_in must be greater than 0.
{'lb': [0]*self.n_out, 'eq': v_out, 'ub': [np.inf]*self.n_out}, # v_out cant be negative
{'lb': [0]*self.n_out, 'eq': v_out_max-v_out, 'ub': [np.inf]*self.n_out}, # outgoing packet stream cant be greater than what is allowed individually
{'lb': [-np.inf], 'eq': sum1(v_in), 'ub': [self.v_in_max_total]}, # sum of all incoming traffic can't exceed v_in_max_total
{'lb': [-np.inf], 'eq': sum1(v_out), 'ub': [self.v_out_max_total]}, # outgoing packet stream cant be greater than what is allowed in total.
{'lb': [0]*self.n_out, 'eq': self.s_c_max_total+eps_s_buffer-s_buffer, 'ub': [np.inf]*self.n_out},
{'lb': [0]*self.n_in, 'eq': s_buffer_source_corr, 'ub': [np.inf]*self.n_in}, # Adjusted s_buffer_source must be >0
{'lb': [0]*self.n_in, 'eq': dv_out_source_fix*dv_out_source, 'ub': [0]*self.n_in},
]
assert np.all([type(cons_list_i['lb']) == list for cons_list_i in cons_list])
assert np.all([type(cons_list_i['ub']) == list for cons_list_i in cons_list])
cons = vertcat(*[con_i['eq'] for con_i in cons_list])
cons_lb = np.concatenate([con_i['lb'] for con_i in cons_list])
cons_ub = np.concatenate([con_i['ub'] for con_i in cons_list])
# Terminal constraints:
tcons_list = [
{'lb': [0]*self.n_out, 'eq': self.s_c_max_total+eps_s_buffer-s_buffer, 'ub': [np.inf]*self.n_out},
{'lb': [0]*self.n_out, 'eq': self.mpc_pN['s_buffer_source_N']-ds_buffer_source, 'ub': [np.inf]*self.n_in}
]
tcons = vertcat(*[tcon_i['eq'] for tcon_i in tcons_list])
tcons_lb = np.concatenate([tcon_i['lb'] for tcon_i in tcons_list])
tcons_ub = np.concatenate([tcon_i['ub'] for tcon_i in tcons_list])
""" Summarize auxiliary / intermediate variables in mpc_aux with their respective expression """
bandwidth_load_in = sum1(v_in)/self.v_in_max_total
bandwidth_load_out = sum1(v_out)/self.v_out_max_total
# For debugging: Add intermediate variables to mpc_aux_expr and query them after solving the optimization problem.
self.mpc_aux_expr = struct_SX([
entry('v_in', expr=v_in),
entry('v_out', expr=v_out),
entry('bandwidth_load_in', expr=bandwidth_load_in),
entry('bandwidth_load_out', expr=bandwidth_load_out),
entry('s_buffer_source_corr', expr=s_buffer_source_corr)
])
""" Problem dictionary """
mpc_problem = {}
mpc_problem['cons'] = Function('cons', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [cons])
mpc_problem['cons_lb'] = cons_lb
mpc_problem['cons_ub'] = cons_ub
mpc_problem['tcons'] = Function('tcons', [self.mpc_xk, self.mpc_eps, self.mpc_pN], [tcons])
mpc_problem['tcons_lb'] = tcons_lb
mpc_problem['tcons_ub'] = tcons_ub
mpc_problem['stage_cost'] = Function('stage_cost', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [stage_cost])
mpc_problem['terminal_cost'] = Function('terminal_cost', [self.mpc_xk, self.mpc_eps], [terminal_cost])
mpc_problem['model'] = Function('model', [self.mpc_xk, self.mpc_uk, self.mpc_tvpk, self.mpc_pk], [self.mpc_xk_next])
mpc_problem['aux'] = Function('aux', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [self.mpc_aux_expr])
self.mpc_problem = mpc_problem
def create_plan_pc(b0, 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 means
m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
# 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,'v'] - 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(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
# 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)
# 0 <= v
lbx['U',:,'v'] = 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']
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
aeroCLaTip = numpy.array([0.0, 0.0, 2*numpy.pi, 0.0])
#aeroCLaRoot = numpy.array([ -28.2136, 16.4140, 0.9568,-0.4000])
#aeroCLaTip = numpy.array([ -28.2136, 16.4140, 0.9568, -0.4000])
clPolyLoc = numpy.zeros((4,n)) #setup lift slope curves for each station
for i in range(n):
clPolyLoc[:,i] = aeroCLaRoot[:] + 2.0*(aeroCLaTip[:] - aeroCLaRoot[:])*yLoc[i]/bref
geom = geometry.Geometry(thetaLoc, chordLoc, yLoc, aIncGeometricLoc, clPolyLoc, bref, n)
(makeMeZero, alphaiLoc, CL, CDi) = setupImplicitFunction(dvs['operAlpha'], dvs['An'], geom)
outputsFcn = C.SXFunction([dvs.cat], [alphaiLoc, CL, CDi, geom.sref])
outputsFcn.init()
g = CT.struct_SX([ CT.entry("makeMeZero",expr=makeMeZero),
CT.entry('alphaiLoc',expr=alphaiLoc),
CT.entry("CL",expr=CL),
CT.entry("CDi",expr=CDi),
CT.entry("sref",expr=geom.sref)])
obj = -CL / (CDi + 0.01)
nlp = C.SXFunction(C.nlpIn(x=dvs),C.nlpOut(f=obj,g=g))
# callback
class MyCallback:
def __init__(self):
self.iters = []
def __call__(self,f,*args):
self.iters.append(numpy.array(f.getInput("x")))
mycallback = MyCallback()
pyfun = C.PyFunction( mycallback, C.nlpSolverOut(x=C.sp_dense(dvs.size,1),
