def create_nlp(self, var, par, obj, con, options):
jit = options['codegen']
if options['verbose'] >= 2:
print 'Building nlp ... ',
if jit['jit']:
print('[jit compilation with flags %s]' %
(','.join(jit['jit_options']['flags']))),
t0 = time.time()
nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con))
solver = NlpSolver('solver', 'ipopt', nlp, options['solver'])
grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g')
hess_lag = solver.hessLag()
var, par = struct_symSX(var), struct_symSX(par)
nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit)
grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit)
jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit)
lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size)
hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g],
hess_lag([var, par, lam_f, lam_g]), jit)
options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g,
'hess_lag': hess_lag})
problem = NlpSolver('solver', 'ipopt', nlp, options['solver'])
t1 = time.time()
if options['verbose'] >= 2:
print 'in %5f s' % (t1-t0)
return problem, (t1-t0)
def setUp(self):
self.x = ca.MX.sym("x")
self.f = ca.MX.sym("f")
self.g = ca.MX.sym("g")
self.nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.x), ca.nlpOut(f=self.f, g=self.g))
def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# 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 <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def create_belief_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Non-linear constraints
[g, lbg, ubg] = cls._create_belief_nonlinear_constraints(model, V)
# Objective function
J = cls._create_belief_objective_function(model, V)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Warm start
# 'warm_start_init_point': 'yes',
# Termination
'max_iter': 1500,
'tol': 1e-6,
'constr_viol_tol': 1e-5,
'compl_inf_tol': 1e-4,
# Acceptable termination
'acceptable_tol': 1e-3,
'acceptable_iter': 5,
'acceptable_obj_change_tol': 1e-2,
# NLP
# 'fixed_variable_treatment': 'make_constraint',
# Quasi-Newton
'hessian_approximation': 'limited-memory',
'limited_memory_max_history': 5,
'limited_memory_max_skipping': 1}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def setupSolver(self, solverOpts=[], constraintFunOpts=[], callback=None):
if not self.collocationIsSetup:
raise ValueError("you forgot to call setupCollocation")
g = self._constraints.getG()
lbg = self._constraints.getLb()
ubg = self._constraints.getUb()
# Objective function/constraints of the NLP
if not hasattr(self, '_objective'):
raise ValueError('need to set objective function')
nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()),
CS.nlpOut(f=self._objective, g=g))
setFXOptions(nlp, constraintFunOpts)
nlp.init()
# solver callback (optional)
if callback is not None:
nd = self._dvMap.vectorize().size()
nc = self._constraints.getG().size()
c = CS.PyFunction(
callback,
CS.nlpSolverOut(x=CS.sp_dense(nd, 1),
f=CS.sp_dense(1, 1),
lam_x=CS.sp_dense(nd, 1),
lam_g=CS.sp_dense(nc, 1),
lam_p=CS.sp_dense(0, 1),
g=CS.sp_dense(nc, 1)), [CS.sp_dense(1, 1)])
c.init()
solverOpts.append(("iteration_callback", c))
# Allocate an NLP solver
self.solver = CS.IpoptSolver(nlp)
# self.solver = CS.WorhpSolver(nlp)
# self.solver = CS.SQPMethod(nlp)
# Set options
setFXOptions(self.solver, solverOpts)
# initialize the solver
self.solver.init()
# Bounds on g
self.solver.setInput(lbg, 'lbg')
self.solver.setInput(ubg, 'ubg')
## Nonlinear constraint function, for debugging
gfcn = CS.MXFunction([self._dvMap.vectorize()], [g])
gfcn.init()
setFXOptions(gfcn, constraintFunOpts)
self._gfcn = gfcn
def setupSolver(self,solverOpts=[],constraintFunOpts=[],callback=None):
if not self.collocationIsSetup:
raise ValueError("you forgot to call setupCollocation")
g = self._constraints.getG()
lbg = self._constraints.getLb()
ubg = self._constraints.getUb()
# Objective function/constraints of the NLP
if not hasattr(self,'_objective'):
raise ValueError('need to set objective function')
nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()),CS.nlpOut(f=self._objective, g=g))
setFXOptions(nlp,constraintFunOpts)
nlp.init()
# solver callback (optional)
if callback is not None:
nd = self._dvMap.vectorize().size()
nc = self._constraints.getG().size()
c = CS.PyFunction( callback,
CS.nlpSolverOut(x = CS.sp_dense(nd,1),
f = CS.sp_dense(1,1),
lam_x = CS.sp_dense(nd,1),
lam_g = CS.sp_dense(nc,1),
lam_p = CS.sp_dense(0,1),
g = CS.sp_dense(nc,1) ),
[CS.sp_dense(1,1)] )
c.init()
solverOpts.append( ("iteration_callback", c) )
# Allocate an NLP solver
self.solver = CS.IpoptSolver(nlp)
# self.solver = CS.WorhpSolver(nlp)
# self.solver = CS.SQPMethod(nlp)
# Set options
setFXOptions(self.solver, solverOpts)
# initialize the solver
self.solver.init()
# Bounds on g
self.solver.setInput(lbg,'lbg')
self.solver.setInput(ubg,'ubg')
## Nonlinear constraint function, for debugging
gfcn = CS.MXFunction([self._dvMap.vectorize()],[g])
gfcn.init()
setFXOptions(gfcn,constraintFunOpts)
self._gfcn = gfcn
def create_simple_plan(x0, 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)
)
])
# Objective function
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
J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
# Regularize controls
J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and non-linear control constraints
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
g = ca.vertcat(g)
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
opts = {}
#opts['hessian_approximation'] = 'limited-memory'
opts['linear_solver'] = 'ma57'
# Create a solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = x0
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def findZ(self, u, x0=None, z0=None):
if z0 is None:
z0 = self.z0
else:
z0 = z0 / self.algStateScaling
if x0 is None:
x0 = self.x0
else:
x0 = x0 / self.stateScaling
u = u / self.controlScaling
ocp = self.ocp
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
nlp = C.SXFunction(C.nlpIn(x=ocp.z, p=C.vertcat([ocp.x, ocp.u])),
C.nlpOut(f=C.SX(0), g=algS))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('tol', 1e-10)
solver.setOption('print_level', 0)
solver.setOption('file_print_level', 0)
solver.setOption("max_iter", 200) # IPOPT maximum iterations
solver.init()
solver.setInput(NP.zeros(ocp.z.size()), 'lbg') # g_L
solver.setInput(NP.zeros(ocp.z.size()), 'ubg') # g_U
solver.setInput(z0, 'x0')
solver.setInput(C.vertcat([x0, u]), 'p')
solver.evaluate()
self.z0 = solver.output('x')
return solver.output('x') * self.algStateScaling
def _initialize_solver(self, **kwargs):
# Initialize NLP object
self._nlp = cs.SXFunction(
'nlp',
cs.nlpIn(x = self.var.vars_sx,
p = self.col_vars['alpha']),
cs.nlpOut(f = self.objective_sx,
g = cs.vertcat(self.constraints_sx)))
opts = {
'max_iter' : 10000,
'linear_solver' : 'ma27'
}
if kwargs is not None: opts.update(kwargs)
self._solver = cs.NlpSolver("solver", "ipopt", self._nlp, opts)
def create_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Force the catcher to always look forward
# lbx['U', :, 'theta'] = ubx['U', :, 'theta'] = 0
# Non-linear constraints
[g, lbg, ubg] = cls._create_nonlinear_constraints(model, V)
# Objective function
J = cls._create_objective_function(model, V, warm_start)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Acceptable termination
'acceptable_iter': 5}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def set_data(self, data):
""" Attach experimental measurement data.
data : a pd.DataFrame object
Data should have columns corresponding to the state labels in
self.state_names, with an index corresponding to the measurement
times.
"""
# Should raise an error if no state name is present
df = data.loc[:, self.state_names]
# Rename columns with state indicies
df.columns = np.arange(self.NEQ)
# Remove empty (nonmeasured) states
self.data = df.loc[:, ~pd.isnull(df).all(0)]
obj_list = []
for ((ti, state), xi) in self.data.stack().iteritems():
obj_list += [
(self._get_interp(ti, [state]) - xi) / self.data[state].max()
]
obj_resid = cs.sum_square(cs.vertcat(obj_list))
# ts = data.index
# Define objective function
# obj_resid = cs.sum_square(cs.vertcat(
# [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0)
# for ti, xi in zip(ts, xs)]))
alpha = cs.SX.sym('alpha')
obj_lasso = alpha * cs.sumRows(cs.fabs(self._P))
self._obj = obj_resid + obj_lasso
# Create the solver object
self._nlp = cs.SXFunction('nlp', cs.nlpIn(x=self._V, p=alpha),
cs.nlpOut(f=self._obj, g=self._g))
def estimate_BtoAng(theta_0,phal,joint,s,b):
theta_x = cs.SX.sym('theta',3)
print "theta_0", theta_0
print "phal",phal
measB = [float(i) for i in b.tolist()]
print "b",measB
f = cs.norm_2(angToB(theta_x,phal,joint,s)-measB)
# f = cs.norm_2(testFun(theta_x)-b)
nlp = cs.SXFunction( cs.nlpIn(x=theta_x), cs.nlpOut(f=f))
solver = cs.NlpSolver("ipopt", nlp)
solver.init()
solver.setInput(theta_0,'x0')
solver.setInput(0.,'lbx')
solver.setInput([np.pi/2,np.pi/(180/110),np.pi/2],'ubx')
solver.evaluate()
x = solver.getOutput('x')
# val = solver.getOutput('f')
return x
def setupSolver(self,solverOpts=[],constraintFunOpts=[],callback=None,solver='ipopt'):
if not self.collocationIsSetup:
raise ValueError("you forgot to call setupCollocation")
g = self._constraints.getG()
lbg = self._constraints.getLb()
ubg = self._constraints.getUb()
# Objective function/constraints of the NLP
if not hasattr(self,'_objective'):
raise ValueError('need to set objective function')
nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()),CS.nlpOut(f=self._objective, g=g))
setFXOptions(nlp,constraintFunOpts)
nlp.init()
# solver callback (optional)
if callback is not None:
@pycallback
def c(f):
return callback(f)
solverOpts.append( ("iteration_callback", c) )
# Allocate an NLP solver
self.solver = CS.NlpSolver(solver,nlp)
# Set options
setFXOptions(self.solver, solverOpts)
# initialize the solver
self.solver.init()
# Bounds on g
self.solver.setInput(lbg,'lbg')
self.solver.setInput(ubg,'ubg')
## Nonlinear constraint function, for debugging
gfcn = CS.MXFunction([self._dvMap.vectorize()],[g])
gfcn.init()
setFXOptions(gfcn,constraintFunOpts)
self._gfcn = gfcn
def set_data(self, data):
""" Attach experimental measurement data.
data : a pd.DataFrame object
Data should have columns corresponding to the state labels in
self.state_names, with an index corresponding to the measurement
times.
"""
# Should raise an error if no state name is present
df = data.loc[:, self.state_names]
# Rename columns with state indicies
df.columns = np.arange(self.NEQ)
# Remove empty (nonmeasured) states
self.data = df.loc[:, ~pd.isnull(df).all(0)]
obj_list = []
for ((ti, state), xi) in self.data.stack().iteritems():
obj_list += [(self._get_interp(ti, [state]) - xi) / self.data[state].max()]
obj_resid = cs.sum_square(cs.vertcat(obj_list))
# ts = data.index
# Define objective function
# obj_resid = cs.sum_square(cs.vertcat(
# [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0)
# for ti, xi in zip(ts, xs)]))
alpha = cs.SX.sym("alpha")
obj_lasso = alpha * cs.sumRows(cs.fabs(self._P))
self._obj = obj_resid + obj_lasso
# Create the solver object
self._nlp = cs.SXFunction("nlp", cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
def _initialize_solver(self, **kwargs):
# Initialize NLP object
self._nlp = cs.SXFunction(
'nlp',
cs.nlpIn(x = self.var.vars_sx,
p = self.pvar.vars_sx),
cs.nlpOut(f = self.objective_sx,
g = cs.vertcat(self._constraints_sx)))
opts = {
'max_iter' : 10000,
'linear_solver' : 'ma27'
}
if kwargs is not None: opts.update(kwargs)
self._solver_opts = opts
self._solver = cs.NlpSolver("solver", "ipopt", self._nlp,
self._solver_opts)
self.col_vars['lbg'] = np.concatenate(self._constraints_lb)
self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
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'])
Xk_end = integrator({"x0": Xk, "p": U[k]})["xf"]
# append continuity constraints
g.append(Xk_next - Xk_end)
g_min.append(np.zeros(Xk.nnz()))
g_max.append(np.zeros(Xk.nnz()))
# Objective function: L(T)
f = X2[nk]
# Continuity constraints: 0<= x(T(k+1)) - X(T(k)) <=0
g = ca.vertcat(g)
# Create NLP solver instance
opts = {"linear_solver": "ma27"}
nlp = ca.MXFunction("nlp", ca.nlpIn(x=V), ca.nlpOut(f=f, g=g))
solver = ca.NlpSolver("solver", "ipopt", nlp, opts)
# Solve the problem
sol = solver({"lbx": VMIN, "ubx": VMAX, "x0": VINIT, "lbg": np.concatenate(g_min), "ubg": np.concatenate(g_max)})
# Retrieve the solution
v_opt = sol["x"]
u_opt = np.array(v_opt[0:nk])
x0_opt = np.array(v_opt[nk + 0 :: 5])
x1_opt = np.array(v_opt[nk + 1 :: 5])
x2_opt = np.array(v_opt[nk + 2 :: 5])
x3_opt = np.array(v_opt[nk + 3 :: 5])
# Get values at the beginning of each finite element
tgrid_x = np.linspace(0, tf, nk + 1)
X = ca.MX([0, 1])
# Objective function
f = 0
# Build a graph of integrator calls
for k in range(nk):
X, QF = itemgetter('xf', 'qf')(integrator({'x0': X, 'p': U[k]}))
f += QF
# Terminal constraints: x_0(T)=x_1(T)=0
g = X
# Allocate an NLP solver
opts = {'linear_solver': 'ma27'}
nlp = ca.MXFunction("nlp", ca.nlpIn(x=x), ca.nlpOut(f=f, g=g))
solver = ca.NlpSolver("solver", "ipopt", nlp, opts)
# Solve the problem
sol = solver({"lbx": -0.75, "ubx": 1, "x0": 0, "lbg": 0, "ubg": 0})
# Retrieve the solution
u_opt = NP.array(sol["x"])
print(sol)
# Time grid
tgrid_x = NP.linspace(0, 10, nk + 1)
tgrid_u = NP.linspace(0, 10, nk)
# Plot the results
plt.figure(1)
# 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]))
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Objective function
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # v_cN -> v_bN
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
opts = {}
#opts['hessian_approximation'] = 'limited-memory'
opts['linear_solver'] = 'ma57'
# Create a solver
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
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),
f=C.sp_dense(1,1),
lam_x=C.sp_dense(dvs.size,1),
lam_g = C.sp_dense(g.size,1),
lam_p = C.sp_dense(0,1),
g = C.sp_dense(g.size,1) ),
[C.sp_dense(1,1)] )
def defineOCP(self,
ocp,
DT=20,
controlCost=0,
xOpt=[],
uOpt=[],
finalStateCost=1,
deltaUCons=[]):
self.ocp = ocp
ocp = self.ocp
self.DT = DT
self.n_k = int(self.ocp.tf / self.DT)
self.controlCost = controlCost
stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
xOpt = xOpt / stateScaling
uOpt = uOpt / controlScaling
self.xOpt = xOpt
self.uOpt = uOpt
self.stateScaling = C.vertcat([
ocp.variable(ocp.x[k].getName()).nominal
for k in range(ocp.x.size())
])
self.algStateScaling = C.vertcat([
ocp.variable(ocp.z[k].getName()).nominal
for k in range(ocp.z.size())
])
self.controlScaling = C.vertcat([
ocp.variable(ocp.u[k].getName()).nominal
for k in range(ocp.u.size())
])
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
ltermS = C.substitute(
ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
sysIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
sysOut = C.daeOut(ode=odeS, alg=algS, quad=ltermS)
odeF = C.SXFunction(sysIn, sysOut)
odeF.init()
C.Integrator.loadPlugin("idas")
G = C.Integrator("idas", odeF)
G.setOption("reltol", self.INTG_REL_TOL) #for CVODES and IDAS
G.setOption("abstol", self.INTG_ABS_TOL) #for CVODES and IDAS
G.setOption("max_multistep_order", 5) #for CVODES and IDAS
G.setOption("max_step_size", self.IDAS_MAX_STEP_SIZE) #for IDAS only
G.setOption("tf", self.DT)
self.G = G
#==============================================================================
# G.setOption('verbose',True)
# G.addMonitor('res')
# G.addMonitor('inputs')
# G.addMonitor('outputs')
#G.addMonitor('djacB')
# G.addMonitor('bjacB')
# G.addMonitor('jtimesB')
# G.addMonitor('psetup')
# G.addMonitor('psetupB')
# G.addMonitor('psolveB')
# G.addMonitor('resB')
# G.addMonitor('resS')
# G.addMonitor('rhsQB')
#==============================================================================
G.init()
self.n_u = self.ocp.u.size()
self.n_x = self.ocp.x.size()
self.n_v = self.n_u * self.n_k + self.n_x * self.n_k
self.V = C.MX.sym("V", int(self.n_v), 1)
self.U, self.X = self.splitVariables(self.V)
uMin = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).min.getValue()
for i in range(self.n_u)
]) / controlScaling
uMax = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).max.getValue()
for i in range(self.n_u)
]) / controlScaling
UMIN = C.vertcat([uMin for k in range(self.n_k)])
UMAX = C.vertcat([uMax for k in range(self.n_k)])
xMin = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).min.getValue()
for i in range(self.n_x)
]) / stateScaling
xMax = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).max.getValue()
for i in range(self.n_x)
]) / stateScaling
XMIN = C.vertcat([xMin for k in range(self.n_k)])
XMAX = C.vertcat([xMax for k in range(self.n_k)])
if len(deltaUCons) > 0:
addDeltaUCons = True
deltaUCons = deltaUCons / self.controlScaling
else:
addDeltaUCons = False
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)]
pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames])
pathS = C.substitute(
ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / pathScaling
pathConstraints = C.SXFunction(pathIn, [pathS])
pathMax = C.vertcat([
ocp.variable(pathVarNames[i]).max.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathMin = C.vertcat([
ocp.variable(pathVarNames[i]).min.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathConstraints.setOption("name", "PATH")
pathConstraints.init()
pathConstraints.setInput(xOpt, 'x')
pathConstraints.setInput([], 'z')
pathConstraints.setInput(uOpt, 'p')
pathConstraints.setInput(0, 't')
pathConstraints.evaluate()
pathOpt = pathConstraints.getOutput()
optimalValues = {}
print 'min <= (name,optimal,nominal) <= max'
for i in range(self.n_x):
print ocp.variable(
ocp.x[i].getName()).min.getValue(), ' <= (', ocp.x[i].getName(
), ',', xOpt[i] * stateScaling[i], ',', stateScaling[
i], ') <= ', ocp.variable(
ocp.x[i].getName()).max.getValue()
optimalValues[ocp.x[i].getName()] = xOpt[i] * stateScaling[i]
for i in range(self.n_u):
print ocp.variable(
ocp.u[i].getName()).min.getValue(), ' <= (', ocp.u[i].getName(
), ',', uOpt[i] * controlScaling[i], ',', controlScaling[
i], ') <= ', ocp.variable(
ocp.u[i].getName()).max.getValue()
if addDeltaUCons:
print -deltaUCons[i] * controlScaling[i], ' <= (Delta(', ocp.u[
i].getName(), ')/DTMPC,', 0, ',', controlScaling[
i], ') <= ', deltaUCons[i] * controlScaling[i]
optimalValues[ocp.u[i].getName()] = uOpt[i] * controlScaling[i]
for i in range(len(pathVarNames)):
print ocp.variable(pathVarNames[i]).min.getValue(
), ' <= (', pathVarNames[i], ',', pathOpt[i] * pathScaling[
i], ',', pathScaling[i], ') <= ', ocp.variable(
pathVarNames[i]).max.getValue()
optimalValues[pathVarNames[i]] = pathOpt[i] * pathScaling[i]
plotTags = [ocp.x[i].getName() for i in range(ocp.x.size())]
plotTags = plotTags + [ocp.u[i].getName() for i in range(ocp.u.size())]
plotTags = plotTags + [sv.getName() for sv in ocp.beq(ocp.path)]
self.plotTags = plotTags
self.optimalValues = optimalValues
# Constraint functions
g = []
g_min = []
g_max = []
self.XU0 = C.MX.sym("XU0", self.n_x + self.n_u, 1)
Z = self.XU0[0:self.n_x]
U0 = self.XU0[self.n_x:self.n_x + self.n_u]
# Build up a graph of integrator calls
obj = 0
zf = C.vertcat([
ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size())
]) / algStateScaling
for k in range(self.n_k):
Z, QF, zf = C.integratorOut(
G(C.integratorIn(x0=Z, p=self.U[k], z0=zf)), "xf", "qf", "zf")
errU = self.U[k] - U0
obj = obj + QF + C.mul(C.mul(errU.T, controlCost), errU)
U0 = self.U[k]
# include MS constraints!
g.append(Z - self.X[k])
g_min.append(NP.zeros(self.n_x))
g_max.append(NP.zeros(self.n_x))
Z = self.X[k]
[pathCons] = pathConstraints.call(
C.daeIn(t=[], x=self.X[k], z=zf, p=self.U[k]))
g.append(pathCons) ## be carefull on giving all inputs
g_max.append(pathMax)
g_min.append(pathMin)
if addDeltaUCons:
g.append(errU)
g_max.append(deltaUCons * DT)
g_min.append(-deltaUCons * DT)
#errU = (self.U[-1]-uOpt)
#errX = self.X[-1]-xOpt
#obj = obj + finalStateCost*C.mul((errX).trans(),(errX))+C.mul(C.mul(errU.T,controlCost),errU)
self.obj = obj
### Constrains
g = C.vertcat(g)
nlp = C.MXFunction(C.nlpIn(x=self.V, p=self.XU0), C.nlpOut(f=obj, g=g))
nlp.init()
self.odeF = odeF
self.nlp = nlp
solver = C.NlpSolver('ipopt', nlp)
# remove the comment to implement the hessian
solver.setOption('hessian_approximation',
'limited-memory') # comment for exact hessian
solver.setOption('print_user_options', 'no')
solver.setOption("tol", self.IPOPT_tol) # IPOPT tolerance
solver.setOption("dual_inf_tol",
self.IPOPT_dual_inf_tol) # dual infeasibility
solver.setOption("constr_viol_tol",
self.IPOPT_constr_viol_tol) # primal infeasibility
solver.setOption("compl_inf_tol",
self.IPOPT_compl_inf_tol) # complementarity
# solver.setOption("acceptable_tol",0.01)
# solver.setOption("acceptable_obj_change_tol",1e-6)
# solver.setOption("acceptable_constr_viol_tol",1e-6)
solver.setOption("max_iter",
self.IPOPT_max_iter) # IPOPT maximum iterations
solver.setOption("print_level", self.IPOPT_print_level)
solver.setOption("max_cpu_time",
self.IPOPT_max_cpu_time) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat([UMIN, XMIN]), 'lbx') # u_L
solver.setInput(C.vertcat([UMAX, XMAX]), 'ubx') # u_U
solver.setInput(C.vertcat(g_min), 'lbg') # g_L
solver.setInput(C.vertcat(g_max), 'ubg') # g_U
self.solver = solver
u0N = C.vertcat([
self.ocp.variable(self.ocp.u[i].getName()).initialGuess.getValue()
for i in range(self.n_u)
]) / controlScaling
x0N = C.vertcat([
self.ocp.variable(self.ocp.x[i].getName()).initialGuess.getValue()
for i in range(self.n_x)
]) / stateScaling
USOL, XSOL = self.forwardSimulation(x0N, u0N)
self.USOL = USOL
self.XSOL = XSOL
def getSteadyState(dae,
conf,
omega0,
r0,
ref_dict={},
bounds=None,
dotBounds=None,
guess=None,
dotGuess=None,
verbose=True):
dvs, ffcn, gfcn, lbg, ubg = getSteadyStateNlpFunctions(dae, ref_dict)
if guess is None:
guess = {
'x': r0,
'y': 0,
'z': 0,
'r': r0,
'dr': 0,
'e11': 0,
'e12': -1,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': 1,
'e31': -1,
'e32': 0,
'e33': 0,
'dx': 0,
'dy': 0,
'dz': 0,
'w_bn_b_x': 0,
'w_bn_b_y': omega0,
'w_bn_b_z': 0,
'ddelta': omega0,
'cos_delta': 1,
'sin_delta': 0,
'aileron': 0,
'elevator': 0,
'rudder': 0,
'flaps': 0,
'daileron': 0,
'delevator': 0,
'drudder': 0,
'dflaps': 0,
'nu': 100,
'motor_torque': 0,
'dmotor_torque': 0,
'ddr': 0,
'dddr': 0.0,
'w0': 0.0,
'dt1_disturbance': 0.0,
'dt2_disturbance': 0.0,
'dt3_disturbance': 0.0,
't1_disturbance': 0.0,
't2_disturbance': 0.0,
't3_disturbance': 0.0,
'df1_disturbance': 0.0,
'df2_disturbance': 0.0,
'df3_disturbance': 0.0,
'f1_disturbance': 0.0,
'f2_disturbance': 0.0,
'f3_disturbance': 0.0,
'wind_x': 0.0,
'wind_y': 0.0,
'wind_z': 0.0,
'delta_wind_x': 0.0,
'delta_wind_y': 0.0,
'delta_wind_z': 0.0,
'alpha0': 0.0
}
if dotGuess is None:
dotGuess = {
'x': 0,
'y': 0,
'z': 0,
'dx': 0,
'dy': 0,
'dz': 0,
'r': 0,
'dr': 0,
'e11': 0,
'e12': 0,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': 0,
'e31': 0,
'e32': 0,
'e33': 0,
'w_bn_b_x': 0,
'w_bn_b_y': 0,
'w_bn_b_z': 0,
'ddelta': 0,
'cos_delta': 0,
'sin_delta': omega0,
'aileron': 0,
'elevator': 0,
'rudder': 0,
'flaps': 0,
'motor_torque': 0,
'ddr': 0,
'dt1_disturbance': 0.0,
'dt2_disturbance': 0.0,
'dt3_disturbance': 0.0,
't1_disturbance': 0.0,
't2_disturbance': 0.0,
't3_disturbance': 0.0,
'df1_disturbance': 0.0,
'df2_disturbance': 0.0,
'df3_disturbance': 0.0,
'f1_disturbance': 0.0,
'f2_disturbance': 0.0,
'f3_disturbance': 0.0,
'wind_x': 0.0,
'wind_y': 0.0,
'wind_z': 0.0,
'delta_wind_x': 0.0,
'delta_wind_y': 0.0,
'delta_wind_z': 0.0,
'alpha0': 0.0
}
guessVec = C.DMatrix([
guess[n]
for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
] + [dotGuess[n] for n in dae.xNames()])
if bounds is None:
bounds = {
'x': (-0.1 * r0, r0 * 2),
'y': (-1.1 * r0, 1.1 * r0),
'z': (-1.1 * r0, 1.1 * r0),
'dx': (0, 0),
'dy': (0, 0),
'dz': (0, 0),
'r': (r0, r0),
'dr': (-100, 100),
'e11': (-2, 2),
'e12': (-2, 2),
'e13': (-2, 2),
'e21': (-2, 2),
'e22': (-2, 2),
'e23': (-2, 2),
'e31': (-2, 2),
'e32': (-2, 2),
'e33': (-2, 2),
'w_bn_b_x': (-50, 50),
'w_bn_b_y': (-50, 50),
'w_bn_b_z': (-50, 50),
'ddelta': (omega0, omega0),
'cos_delta': (1, 1),
'sin_delta': (0, 0),
'aileron': (-0.17, 0.17),
'elevator': (-0.17, 0.17),
'rudder': (-0.2, 0.2),
'flaps': (-0.2, 0.2),
'daileron': (0, 0),
'delevator': (0, 0),
'drudder': (0, 0),
'dflaps': (0, 0),
'nu': (0, 3000),
'motor_torque': (-1000, 1000),
'ddr': (0, 0),
'dmotor_torque': (0, 0),
'dddr': (0, 0),
'w0': (0, 0),
'dt1_disturbance': (0, 0),
'dt2_disturbance': (0, 0),
'dt3_disturbance': (0, 0),
't1_disturbance': (0, 0),
't2_disturbance': (0, 0),
't3_disturbance': (0, 0),
'df1_disturbance': (0, 0),
'df2_disturbance': (0, 0),
'df3_disturbance': (0, 0),
'f1_disturbance': (0, 0),
'f2_disturbance': (0, 0),
'f3_disturbance': (0, 0),
'wind_x': (0, 0),
'wind_y': (0, 0),
'wind_z': (0, 0),
'delta_wind_x': (0, 0),
'delta_wind_y': (0, 0),
'delta_wind_z': (0, 0),
'alpha0': (0.0, 0.0)
}
if ref_dict is not None:
for name in ref_dict:
bounds[name] = ref_dict[name]
# print bounds
if dotBounds is None:
dotBounds = {
'x': (-1, 1),
'y': (-1, 1),
'z': (-1, 1),
'dx': (0, 0),
'dy': (0, 0),
'dz': (0, 0),
'r': (-100, 100),
'dr': (-1, 1),
'e11': (-50, 50),
'e12': (-50, 50),
'e13': (-50, 50),
'e21': (-50, 50),
'e22': (-50, 50),
'e23': (-50, 50),
'e31': (-50, 50),
'e32': (-50, 50),
'e33': (-50, 50),
'w_bn_b_x': (0, 0),
'w_bn_b_y': (0, 0),
'w_bn_b_z': (0, 0),
'ddelta': (0, 0),
'cos_delta': (-1, 1),
'sin_delta': (omega0 - 1, omega0 + 1),
'aileron': (-1, 1),
'elevator': (-1, 1),
'rudder': (-1, 1),
'flaps': (-1, 1),
'motor_torque': (-1000, 1000),
'ddr': (-100, 100),
'dt1_disturbance': (0, 0),
'dt2_disturbance': (0, 0),
'dt3_disturbance': (0, 0),
't1_disturbance': (0, 0),
't2_disturbance': (0, 0),
't3_disturbance': (0, 0),
'df1_disturbance': (0, 0),
'df2_disturbance': (0, 0),
'df3_disturbance': (0, 0),
'f1_disturbance': (0, 0),
'f2_disturbance': (0, 0),
'f3_disturbance': (0, 0),
'wind_x': (0, 0),
'wind_y': (0, 0),
'wind_z': (0, 0),
'delta_wind_x': (0, 0),
'delta_wind_y': (0, 0),
'delta_wind_z': (0, 0),
'alpha0': (0.0, 0.0)
}
boundsVec = [bounds[n] for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()] + \
[dotBounds[n] for n in dae.xNames()]
# gfcn.setInput(guessVec)
# gfcn.evaluate()
# ret = gfcn.output()
# for k,v in enumerate(ret):
# if math.isnan(v):
# print 'index ',k,' is nan: ',g._tags[k]
# import sys; sys.exit()
# context = zmq.Context(1)
# publisher = context.socket(zmq.PUB)
# publisher.bind("tcp://*:5563")
# class MyCallback:
# def __init__(self):
# import rawekite.kiteproto as kiteproto
# import rawekite.kite_pb2 as kite_pb2
# self.kiteproto = kiteproto
# self.kite_pb2 = kite_pb2
# self.iter = 0
# def __call__(self,f,*args):
# x = C.DMatrix(f.input('x'))
# sol = {}
# for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
# sol[name] = x[k].at(0)
# lookup = lambda name: sol[name]
# kp = self.kiteproto.toKiteProto(lookup,
# lineAlpha=0.2)
# mc = self.kite_pb2.MultiCarousel()
# mc.horizon.extend([kp])
# mc.messages.append("iter: "+str(self.iter))
# self.iter += 1
# publisher.send_multipart(["multi-carousel", mc.SerializeToString()])
nlp = C.SXFunction(C.nlpIn(x=dvs), C.nlpOut(f=ffcn, g=gfcn))
solver = C.NlpSolver("ipopt", nlp)
# def addCallback():
# nd = len(boundsVec)
# nc = g.getLb().size()
# c = C.PyFunction( MyCallback(), C.nlpsolverOut(x=C.sp_dense(nd,1), f=C.sp_dense(1,1), lam_x=C.sp_dense(nd,1), lam_p = C.sp_dense(0,1), lam_g = C.sp_dense(nc,1), g = C.sp_dense(nc,1) ), [C.sp_dense(1,1)] )
# c.init()
# solver.setOption("iteration_callback", c)
# addCallback()
# solver.setOption('max_iter', 10000)
# solver.setOption('tol',1e-14)
if verbose is False:
solver.setOption('suppress_all_output', 'yes')
solver.setOption('print_time', False)
solver.init()
solver.setInput(lbg, 'lbg')
solver.setInput(ubg, 'ubg')
solver.setInput(guessVec, 'x0')
lb, ub = zip(*boundsVec)
solver.setInput(C.DMatrix(lb), 'lbx')
solver.setInput(C.DMatrix(ub), 'ubx')
solver.evaluate()
ret = solver.getStat('return_status')
assert ret in ['Solve_Succeeded', 'Solved_To_Acceptable_Level',
1], 'Solver failed: ' + str(ret)
# publisher.close()
# context.destroy()
xOpt = solver.output('x')
k = 0
sol = {}
for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
sol[name] = xOpt[k].at(0)
k += 1
# print name+':\t',sol[name]
dotSol = {}
for name in dae.xNames():
dotSol[name] = xOpt[k].at(0)
k += 1
# print 'DDT('+name+'):\t',dotSol[name]
return sol, dotSol
obj = [x_end - ydata_noise[0,:].T]
for k in range(N):
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat([ca.vertcat(obj), EPS_U])
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Z = ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1])))
Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv)
nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), \
ca.nlpOut(f = 0.5 * ca.mul([r.T, Sigma, r])))
nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat([
pl.ones(3), \
pl.zeros(N), \
ydata[0,:].T
])
sol = nlpsolver(x0 = V0)
p_est_single_shooting = sol["x"][:3]
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat([ca.vertcat(obj), EPS_U])
wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape)
weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape)
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\
ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv)
nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r])))
nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat([
pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
])
sol = nlpsolver(x0 = V0)
p_est_single_shooting = sol["x"][:3]
def run_parameter_estimation(self, hessian="gauss-newton"):
r'''
:param hessian: Method of hessian calculation/approximation; possible
values are `gauss-newton` and `exact-hessian`
:type hessian: str
This functions will run a least squares parameter estimation for the
given problem and data set.
For this, an NLP of the following
structure is set up with a direct collocation approach and solved
using IPOPT:
.. math::
\begin{aligned}
& \text{arg}\,\underset{x, p, v, \epsilon_e, \epsilon_u}{\text{min}} & & \frac{1}{2} \| R(w, v, \epsilon_e, \epsilon_u) \|_2^2 \\
& \text{subject to:} & & R(w, v, \epsilon_e, \epsilon_u) = w^{^\mathbb{1}/_\mathbb{2}} \begin{pmatrix} {v} \\ {\epsilon_e} \\ {\epsilon_u} \end{pmatrix} \\
& & & w = \begin{pmatrix} {w_{v}}^T & {w_{\epsilon_{e}}}^T & {w_{\epsilon_{u}}}^T \end{pmatrix} \\
& & & v_{l} + y_{l} - \phi(t_{l}, u_{l}, x_{l}, p) = 0 \\
& & & (t_{k+1} - t_{k}) f(t_{k,j}, u_{k,j}, x_{k,j}, p, \epsilon_{e,k,j}, \epsilon_{u,k,j}) - \sum_{r=0}^{d} \dot{L}_r(\tau_j) x_{k,r} = 0 \\
& & & x_{k+1,0} - \sum_{r=0}^{d} L_r(1) x_{k,r} = 0 \\
& & & t_{k,j} = t_k + (t_{k+1} - t_{k}) \tau_j \\
& & & L_r(\tau) = \prod_{r=0,r\neq j}^{d} \frac{\tau - \tau_r}{\tau_j - \tau_r}\\
& \text{for:} & & k = 1, \dots, N, ~~~ l = 1, \dots, M, ~~~ j = 1, \dots, d, ~~~ r = 1, \dots, d \\
& & & \tau_j = \text{time points w. r. t. scheme and order}
\end{aligned}
The status of IPOPT provides information whether the computation could
be finished sucessfully. The optimal values for all optimization
variables :math:`\hat{x}` can be accessed
via the class variable ``LSq.Xhat``, while the estimated parameters
:math:`\hat{p}` can also be accessed separately via the class attribute
``LSq.phat``.
**Please be aware:** IPOPT finishing sucessfully does not necessarly
mean that the estimation results for the unknown parameters are useful
for your purposes, it just means that IPOPT was able to solve the given
optimization problem.
You have in any case to verify your results, e. g. by simulation using
the class function :func:`run_simulation`.
'''
intro.pecas_intro()
print('\n' + 18 * '-' + \
' PECas least squares parameter estimation ' + 18 * '-')
print('''
Starting least squares parameter estimation using IPOPT,
this might take some time ...
''')
self.tstart_estimation = time.time()
g = ca.vertcat([ca.vec(self.pesetup.phiN) - self.yN + \
ca.vec(self.pesetup.V)])
self.R = ca.sqrt(self.w) * \
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])
if self.pesetup.g.size():
g = ca.vertcat([g, self.pesetup.g])
self.g = g
self.Vars = ca.veccat([
self.pesetup.P, \
self.pesetup.X, \
self.pesetup.XF, \
self.pesetup.V, \
self.pesetup.EPS_E, \
self.pesetup.EPS_U, \
])
nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.Vars), \
ca.nlpOut(f=(0.5 * ca.mul([self.R.T, self.R])), g=self.g))
options = {}
options["tol"] = 1e-10
options["linear_solver"] = self.linear_solver
if hessian == "gauss-newton":
# ipdb.set_trace()
gradF = nlp.gradient()
jacG = nlp.jacobian("x", "g")
# Can't the following be implemented more efficiently?!
# gradF.derivative(0, 1)
J = ca.jacobian(self.R, self.Vars)
sigma = ca.MX.sym("sigma")
hessLag = ca.MXFunction("H", \
ca.hessLagIn(x = self.Vars, lam_f = sigma), \
ca.hessLagOut(hess = sigma * ca.mul(J.T, J)))
options["hess_lag"] = hessLag
options["grad_f"] = gradF
options["jac_g"] = jacG
elif hessian == "exact-hessian":
# let NlpSolver-class compute everything
pass
else:
raise NotImplementedError( \
"Requested method is not implemented. Availabe methods " + \
"are 'gauss-newton' (default) and 'exact-hessian'.")
# Initialize the solver, solve the optimization problem
solver = ca.NlpSolver("solver", "ipopt", nlp, options)
# Store the results of the computation
Varsinit = ca.veccat([
self.pesetup.Pinit, \
self.pesetup.Xinit, \
self.pesetup.XFinit, \
self.pesetup.Vinit, \
self.pesetup.EPS_Einit, \
self.pesetup.EPS_Uinit, \
])
sol = solver(x0=Varsinit, lbg=0, ubg=0)
self.Varshat = sol["x"]
R_squared_fcn = ca.MXFunction("R_squared_fcn", [self.Vars],
[ca.mul([ \
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]).T,
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])])])
[self.R_squared] = R_squared_fcn([self.Varshat])
self.tend_estimation = time.time()
self.duration_estimation = self.tend_estimation - \
self.tstart_estimation
print('''
Parameter estimation finished. Check IPOPT output for status information.''')
def findConstrainedSteadyState(self,
altUTags,
altUVals,
u0,
x0=None,
z0=None,
consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
u0 = u0 / self.controlScaling
ocp = self.ocp
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
altUScaling = C.vertcat(
[ocp.variable(altUTags[k]).nominal for k in range(len(altUTags))])
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
altU = C.vertcat(
[ocp.variable(altUTags[k]).beq for k in range(len(altUTags))])
altU = C.substitute(
altU, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / altUScaling
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
split = C.SXFunction([C.vertcat([ocp.x, ocp.z, ocp.u])],
[ocp.x, ocp.z, ocp.u])
split.init()
nlp = C.SXFunction(
C.nlpIn(x=C.vertcat([ocp.x, ocp.z, ocp.u])),
C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, altU, mSX])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('tol', 1e-10)
solver.setOption('print_user_options', 'yes')
solver.setOption("max_iter", 200) # IPOPT maximum iterations
solver.init()
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
]) / controlScaling
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
]) / controlScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMax]),
'ubg') # g_U
solver.setInput(C.vertcat([xMin, zMin, uMin]), 'lbx') # u_L
solver.setInput(C.vertcat([xMax, zMax, uMax]), 'ubx') # u_U
solver.setInput(C.vertcat([x0, z0, u0]), 'x0')
solver.evaluate()
xzu = solver.output('x')
split.setInput(xzu)
split.evaluate()
x0 = split.getOutput(0)
z0 = split.getOutput(1)
u0 = split.getOutput(2)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u0, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return x0 * stateScaling, u0 * controlScaling, z0 * algStateScaling, y0 * measurementScaling
def steadyState(self, u, T=0):
ocp = self.ocp
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
])
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
])
x0 = C.vertcat([
ocp.variable(ocp.x[i].getName()).initialGuess.getValue()
for i in range(ocp.x.size())
])
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
])
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
])
z0 = C.vertcat([
ocp.variable(ocp.z[i].getName()).initialGuess.getValue()
for i in range(ocp.z.size())
])
if T > 0:
tout, xh, zh, outs = self.simulatePlot(x0, [u for k in range(T)],
outputVars=None)
x0 = xh[-1]
z0 = zh[-1]
varList = [ocp.x, ocp.z]
varMax = [xMax, zMax]
varMin = [xMin, zMin]
var0 = [x0, z0]
cons = [ocp.ode(ocp.x), ocp.alg]
consMax = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())]
consMin = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())]
C.NlpSolver.loadPlugin('ipopt')
nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList), p=ocp.u),
C.nlpOut(f=C.SX(0), g=C.vertcat(cons)))
nlp.init()
solver = C.NlpSolver('ipopt', nlp)
solver.setOption("max_iter", 100) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat(varMin), 'lbx')
solver.setInput(C.vertcat(varMax), 'ubx')
solver.setInput(C.vertcat(consMin), 'lbg') # g_L
solver.setInput(C.vertcat(consMax), 'ubg') # g_U
solver.setInput(C.vertcat(var0), 'x0')
solver.setInput(u, 'p')
solver.evaluate()
outX = solver.getOutput()
splitF = C.SXFunction([C.vertcat(varList)], varList)
splitF.init()
splitF.setInput(outX)
splitF.evaluate()
xss = splitF.getOutput(0)
zss = splitF.getOutput(1)
return xss, zss
g = []
for k in range(n_sim):
g.append(V['X', k + 1] - F([V['X', k], V['U', k], dt])[0])
g = ca.vertcat(g)
# Objective
[final_cost] = lf([V['X', n_sim]])
J = final_cost
# Regularize controls
for k in range(n_sim):
[stage_cost] = l([V['X', k], V['U', k], dt])
J += stage_cost
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
# Create solver
opts = {'linear_solver': 'ma57'}
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# x(t=0) = x0
lbx['X', 0] = ubx['X', 0] = x0
# Solve nlp
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
sol = V(sol['x'])
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'])
def __init__(self, dae, conf, omega0, r0, ref_dict = {},
bounds = None, dotBounds = None, guess = None, dotGuess = None,
robustVersion = False, verbose = True):
self.dae, self.conf = dae, conf
self.omega0 = omega0
self.r0 = r0
self.ref_dict = ref_dict
self.robustVersion = robustVersion
if robustVersion == True:
self.dvs, self.ffcn, self.gfcn, self.lbg, self.ubg, zSol = self.getRobustSteadyStateNlpFunctions(dae, ref_dict)
self.dvsNames = dae.xNames() + dae.uNames() + dae.pNames()
zIn = C.veccat([dae[ name ] for name in self.dvsNames])
zOut = C.veccat([zSol[ el ] for el in dae.zNames()])
self.zFcn = C.SXFunction([ zIn ], [ zOut ])
self.zFcn.init()
else:
self.dvs, self.ffcn, self.gfcn, self.lbg, self.ubg = self.getSteadyStateNlpFunctions(dae, ref_dict)
self.dvsNames = dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
self.nlp = C.SXFunction(C.nlpIn(x = self.dvs), C.nlpOut(f = self.ffcn, g = self.gfcn))
self.solver = C.IpoptSolver( self.nlp )
self.solver.setOption('max_iter', 10000)
self.solver.setOption('tol', 1e-9)
if verbose is False:
self.solver.setOption('suppress_all_output', 'yes')
self.solver.setOption('print_time', False)
self.solver.init()
self.guess = guess
if self.guess is None:
self.guess = {'x':r0, 'y':0, 'z':0,
'r':r0, 'dr':0,
'e11':0, 'e12':-1, 'e13':0,
'e21':0, 'e22':0, 'e23':1,
'e31':-1, 'e32':0, 'e33':0,
'dx':0, 'dy':0, 'dz':0,
'w_bn_b_x':0, 'w_bn_b_y':omega0, 'w_bn_b_z':0,
'ddelta':omega0,
'cos_delta':1, 'sin_delta':0,
'aileron':0, 'elevator':0, 'rudder':0, 'flaps':0,
'daileron':0, 'delevator':0, 'drudder':0, 'dflaps':0,
'nu':100, 'motor_torque':0,
'dmotor_torque':0, 'ddr':0,
'dddr':0.0, 'w0':0.0,
'dt1_disturbance':0.0, 'dt2_disturbance':0.0, 'dt3_disturbance':0.0,
't1_disturbance':0.0, 't2_disturbance':0.0, 't3_disturbance':0.0,
'df1_disturbance':0.0, 'df2_disturbance':0.0, 'df3_disturbance':0.0,
'f1_disturbance':0.0, 'f2_disturbance':0.0, 'f3_disturbance':0.0,
'wind_x':0.0, 'wind_y':0.0, 'wind_z':0.0,
'delta_wind_x':0.0, 'delta_wind_y':0.0, 'delta_wind_z':0.0,
'alpha0': 0.0}
self.dotGuess = dotGuess
if self.dotGuess is None:
self.dotGuess = {'x':0, 'y':0, 'z':0, 'dx':0, 'dy':0, 'dz':0,
'r':0, 'dr':0,
'e11':0, 'e12':0, 'e13':0,
'e21':0, 'e22':0, 'e23':0,
'e31':0, 'e32':0, 'e33':0,
'w_bn_b_x':0, 'w_bn_b_y':0, 'w_bn_b_z':0,
'ddelta':0,
'cos_delta':0, 'sin_delta':omega0,
'aileron':0, 'elevator':0, 'rudder':0, 'flaps':0,
'motor_torque':0, 'ddr':0,
'dt1_disturbance':0.0, 'dt2_disturbance':0.0, 'dt3_disturbance':0.0,
't1_disturbance':0.0, 't2_disturbance':0.0, 't3_disturbance':0.0,
'df1_disturbance':0.0, 'df2_disturbance':0.0, 'df3_disturbance':0.0,
'f1_disturbance':0.0, 'f2_disturbance':0.0, 'f3_disturbance':0.0,
'wind_x':0.0, 'wind_y':0.0, 'wind_z':0.0,
'delta_wind_x':0.0, 'delta_wind_y':0.0, 'delta_wind_z':0.0,
'alpha0': 0.0}
self.bounds = bounds
if self.bounds is None:
self.bounds = {'x':(-0.1 * r0, r0 * 2), 'y':(-1.1 * r0, 1.1 * r0), 'z':(-1.1 * r0, 1.1 * r0),
'dx':(0, 0), 'dy':(0, 0), 'dz':(0, 0),
'r':(r0, r0), 'dr':(-100, 100),
'e11':(-2, 2), 'e12':(-2, 2), 'e13':(-2, 2),
'e21':(-2, 2), 'e22':(-2, 2), 'e23':(-2, 2),
'e31':(-2, 2), 'e32':(-2, 2), 'e33':(-2, 2),
'w_bn_b_x':(-50, 50), 'w_bn_b_y':(-50, 50), 'w_bn_b_z':(-50, 50),
'ddelta':(omega0, omega0),
'cos_delta':(1, 1), 'sin_delta':(0, 0),
'aileron':(-0.2, 0.2), 'elevator':(-0.2, 0.2), 'rudder':(-0.2, 0.2), 'flaps':(-0.2, 0.2),
'daileron':(0, 0), 'delevator':(0, 0), 'drudder':(0, 0), 'dflaps':(0, 0),
'nu':(0, 3000), 'motor_torque':(-1000, 1000),
'ddr':(0, 0),
'dmotor_torque':(0, 0), 'dddr':(0, 0), 'w0':(0, 0),
'dt1_disturbance':(0, 0), 'dt2_disturbance':(0, 0), 'dt3_disturbance':(0, 0),
't1_disturbance':(0, 0), 't2_disturbance':(0, 0), 't3_disturbance':(0, 0),
'df1_disturbance':(0, 0), 'df2_disturbance':(0, 0), 'df3_disturbance':(0, 0),
'f1_disturbance':(0, 0), 'f2_disturbance':(0, 0), 'f3_disturbance':(0, 0),
'wind_x':(0, 0), 'wind_y':(0, 0), 'wind_z':(0, 0),
'delta_wind_x':(0, 0), 'delta_wind_y':(0, 0), 'delta_wind_z':(0, 0),
'alpha0': (0.0, 0.0)}
if ref_dict is not None:
for name in ref_dict: self.bounds[name] = ref_dict[name]
self.dotBounds = dotBounds
if self.dotBounds is None:
self.dotBounds = {'x':(-1, 1), 'y':(-1, 1), 'z':(-1, 1),
'dx':(0, 0), 'dy':(0, 0), 'dz':(0, 0),
'r':(-100, 100), 'dr':(-1, 1),
'e11':(-50, 50), 'e12':(-50, 50), 'e13':(-50, 50),
'e21':(-50, 50), 'e22':(-50, 50), 'e23':(-50, 50),
'e31':(-50, 50), 'e32':(-50, 50), 'e33':(-50, 50),
'w_bn_b_x':(0, 0), 'w_bn_b_y':(0, 0), 'w_bn_b_z':(0, 0),
'ddelta':(0, 0),
'cos_delta':(-1, 1), 'sin_delta':(omega0 - 1, omega0 + 1),
'aileron':(-1, 1), 'elevator':(-1, 1), 'rudder':(-1, 1), 'flaps':(-1, 1),
'motor_torque':(-1000, 1000), 'ddr':(-100, 100),
'dt1_disturbance':(0, 0), 'dt2_disturbance':(0, 0), 'dt3_disturbance':(0, 0),
't1_disturbance':(0, 0), 't2_disturbance':(0, 0), 't3_disturbance':(0, 0),
'df1_disturbance':(0, 0), 'df2_disturbance':(0, 0), 'df3_disturbance':(0, 0),
'f1_disturbance':(0, 0), 'f2_disturbance':(0, 0), 'f3_disturbance':(0, 0),
'wind_x':(0, 0), 'wind_y':(0, 0), 'wind_z':(0, 0),
'delta_wind_x':(0, 0), 'delta_wind_y':(0, 0), 'delta_wind_z':(0, 0),
'alpha0': (0.0, 0.0)}
if self.robustVersion == True:
# Joris's recommendation
for name in self.dae.xNames():
if name in ["r", "ddelta"] or "disturbance" in name:
continue
# Override bounds
if self.bounds[name][0] == self.bounds[name][1]:
self.bounds[name] = (-1e12, 1e12)
# Override dotBounds
if self.dotBounds[name][0] == self.dotBounds[name][1]:
self.dotBounds[name] = (-1e12, 1e12)
if name not in ["aileron", "elevator", "motor_torque", "ddr", "sin_delta"]:
self.dotBounds[ name ] = (0, 0)
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"))
ubx = dvs(solver.input("ubx"))
x0 = dvs(solver.input("x0"))
lbg["makeMeZero"] = 0.0
ubg["makeMeZero"] = 0.0
def run_parameter_estimation(self, hessian = "gauss-newton"):
r'''
:param hessian: Method of hessian calculation/approximation; possible
values are `gauss-newton` and `exact-hessian`
:type hessian: str
This functions will run a least squares parameter estimation for the
given problem and data set.
For this, an NLP of the following
structure is set up with a direct collocation approach and solved
using IPOPT:
.. math::
\begin{aligned}
& \text{arg}\,\underset{x, p, v, \epsilon_e, \epsilon_u}{\text{min}} & & \frac{1}{2} \| R(w, v, \epsilon_e, \epsilon_u) \|_2^2 \\
& \text{subject to:} & & R(w, v, \epsilon_e, \epsilon_u) = w^{^\mathbb{1}/_\mathbb{2}} \begin{pmatrix} {v} \\ {\epsilon_e} \\ {\epsilon_u} \end{pmatrix} \\
& & & w = \begin{pmatrix} {w_{v}}^T & {w_{\epsilon_{e}}}^T & {w_{\epsilon_{u}}}^T \end{pmatrix} \\
& & & v_{l} + y_{l} - \phi(t_{l}, u_{l}, x_{l}, p) = 0 \\
& & & (t_{k+1} - t_{k}) f(t_{k,j}, u_{k,j}, x_{k,j}, p, \epsilon_{e,k,j}, \epsilon_{u,k,j}) - \sum_{r=0}^{d} \dot{L}_r(\tau_j) x_{k,r} = 0 \\
& & & x_{k+1,0} - \sum_{r=0}^{d} L_r(1) x_{k,r} = 0 \\
& & & t_{k,j} = t_k + (t_{k+1} - t_{k}) \tau_j \\
& & & L_r(\tau) = \prod_{r=0,r\neq j}^{d} \frac{\tau - \tau_r}{\tau_j - \tau_r}\\
& \text{for:} & & k = 1, \dots, N, ~~~ l = 1, \dots, M, ~~~ j = 1, \dots, d, ~~~ r = 1, \dots, d \\
& & & \tau_j = \text{time points w. r. t. scheme and order}
\end{aligned}
The status of IPOPT provides information whether the computation could
be finished sucessfully. The optimal values for all optimization
variables :math:`\hat{x}` can be accessed
via the class variable ``LSq.Xhat``, while the estimated parameters
:math:`\hat{p}` can also be accessed separately via the class attribute
``LSq.phat``.
**Please be aware:** IPOPT finishing sucessfully does not necessarly
mean that the estimation results for the unknown parameters are useful
for your purposes, it just means that IPOPT was able to solve the given
optimization problem.
You have in any case to verify your results, e. g. by simulation using
the class function :func:`run_simulation`.
'''
intro.pecas_intro()
print('\n' + 18 * '-' + \
' PECas least squares parameter estimation ' + 18 * '-')
print('''
Starting least squares parameter estimation using IPOPT,
this might take some time ...
''')
self.tstart_estimation = time.time()
g = ca.vertcat([ca.vec(self.pesetup.phiN) - self.yN + \
ca.vec(self.pesetup.V)])
self.R = ca.sqrt(self.w) * \
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])
if self.pesetup.g.size():
g = ca.vertcat([g, self.pesetup.g])
self.g = g
self.Vars = ca.veccat([
self.pesetup.P, \
self.pesetup.X, \
self.pesetup.XF, \
self.pesetup.V, \
self.pesetup.EPS_E, \
self.pesetup.EPS_U, \
])
nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.Vars), \
ca.nlpOut(f=(0.5 * ca.mul([self.R.T, self.R])), g=self.g))
options = {}
options["tol"] = 1e-10
options["linear_solver"] = self.linear_solver
if hessian == "gauss-newton":
# ipdb.set_trace()
gradF = nlp.gradient()
jacG = nlp.jacobian("x", "g")
# Can't the following be implemented more efficiently?!
# gradF.derivative(0, 1)
J = ca.jacobian(self.R, self.Vars)
sigma = ca.MX.sym("sigma")
hessLag = ca.MXFunction("H", \
ca.hessLagIn(x = self.Vars, lam_f = sigma), \
ca.hessLagOut(hess = sigma * ca.mul(J.T, J)))
options["hess_lag"] = hessLag
options["grad_f"] = gradF
options["jac_g"] = jacG
elif hessian == "exact-hessian":
# let NlpSolver-class compute everything
pass
else:
raise NotImplementedError( \
"Requested method is not implemented. Availabe methods " + \
"are 'gauss-newton' (default) and 'exact-hessian'.")
# Initialize the solver, solve the optimization problem
solver = ca.NlpSolver("solver", "ipopt", nlp, options)
# Store the results of the computation
Varsinit = ca.veccat([
self.pesetup.Pinit, \
self.pesetup.Xinit, \
self.pesetup.XFinit, \
self.pesetup.Vinit, \
self.pesetup.EPS_Einit, \
self.pesetup.EPS_Uinit, \
])
sol = solver(x0 = Varsinit, lbg = 0, ubg = 0)
self.Varshat = sol["x"]
R_squared_fcn = ca.MXFunction("R_squared_fcn", [self.Vars],
[ca.mul([ \
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]).T,
ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])])])
[self.R_squared] = R_squared_fcn([self.Varshat])
self.tend_estimation = time.time()
self.duration_estimation = self.tend_estimation - \
self.tstart_estimation
print('''
Parameter estimation finished. Check IPOPT output for status information.''')
def findSteadyState(self, u, x0, z0=None, simCount=0, consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
ocp = self.ocp
u = u / self.controlScaling
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
for k in range(simCount):
x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling,
z0 * algStateScaling)
x0 = x0 / stateScaling
z0 = z0 / algStateScaling
y = y / measurementScaling
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
split = C.SXFunction([C.vertcat([ocp.x, ocp.z])], [ocp.x, ocp.z])
split.init()
nlp = C.SXFunction(C.nlpIn(x=C.vertcat([ocp.x, ocp.z]), p=ocp.u),
C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, mSX])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('tol', 1e-10)
solver.setOption('print_level', 5)
solver.setOption("max_iter", 2000) # IPOPT maximum iterations
solver.init()
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax]),
'ubg') # g_U
solver.setInput(C.vertcat([xMin, zMin]), 'lbx') # u_L
solver.setInput(C.vertcat([xMax, zMax]), 'ubx') # u_U
solver.setInput(C.vertcat([x0, z0]), 'x0')
solver.setInput(u, 'p')
solver.evaluate()
xz = solver.output('x')
split.setInput(xz)
split.evaluate()
x0 = split.getOutput(0)
z0 = split.getOutput(1)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
def findOptimalPoint(self, u0, x0=None, z0=None, simCount=0, consList=[]):
if z0 is not None:
z0 = z0 / self.algStateScaling
else:
z0 = self.z0
if x0 is not None:
x0 = x0 / self.stateScaling
else:
x0 = self.x0
ocp = self.ocp
u0 = u0 / self.controlScaling
measurementScaling = self.measurementScaling
stateScaling = self.stateScaling
algStateScaling = self.algStateScaling
controlScaling = self.controlScaling
consScaling = C.vertcat([ocp.variable(k).nominal for k in consList])
for k in range(simCount):
x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling,
z0 * algStateScaling)
x0 = x0 / stateScaling
z0 = z0 / algStateScaling
y = y / measurementScaling
odeS = C.substitute(
ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / stateScaling
algS = C.substitute(
ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
ltermS = C.substitute(
ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
]))
mSX = C.vertcat(
[ocp.variable(consList[k]).beq for k in range(len(consList))])
mSX = C.substitute(
mSX, C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / consScaling
pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)]
pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames])
pathS = C.substitute(
ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]),
C.vertcat([
stateScaling * ocp.x, algStateScaling * ocp.z,
controlScaling * ocp.u
])) / pathScaling
split = C.SXFunction([C.vertcat([ocp.u, ocp.x, ocp.z])],
[ocp.u, ocp.x, ocp.z])
split.init()
nlp = C.SXFunction(
C.nlpIn(x=C.vertcat([ocp.u, ocp.x, ocp.z])),
C.nlpOut(f=ltermS, g=C.vertcat([odeS, algS, mSX, pathS])))
C.NlpSolver.loadPlugin('ipopt')
solver = C.NlpSolver('ipopt', nlp)
solver.setOption('print_user_options', 'no')
solver.setOption('print_level', 5)
solver.setOption("tol", 1e-14)
solver.setOption("max_iter", 300) # IPOPT maximum iterations
solver.init()
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
]) / controlScaling
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
]) / controlScaling
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
]) / stateScaling
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
]) / stateScaling
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
]) / algStateScaling
cMin = C.vertcat([
ocp.variable(consList[i]).min.getValue()
for i in range(len(consList))
]) / consScaling
cMax = C.vertcat([
ocp.variable(consList[i]).max.getValue()
for i in range(len(consList))
]) / consScaling
pathMax = C.vertcat([
ocp.variable(pathVarNames[i]).max.getValue()
for i in range(ocp.path.size())
]) / pathScaling
pathMin = C.vertcat([
ocp.variable(pathVarNames[i]).min.getValue()
for i in range(ocp.path.size())
]) / pathScaling
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin, pathMin]),
'lbg') # g_L
solver.setInput(
C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax, pathMax]),
'ubg') # g_U
solver.setInput(C.vertcat([uMin, xMin, zMin]), 'lbx') # u_L
solver.setInput(C.vertcat([uMax, xMax, zMax]), 'ubx') # u_U
solver.setInput(C.vertcat([u0, x0, z0]), 'x0')
solver.evaluate()
xz = solver.output('x')
split.setInput(xz)
split.evaluate()
u0 = split.getOutput(0)
x0 = split.getOutput(1)
z0 = split.getOutput(2)
self.mSXF.setInput(x0, 'x')
self.mSXF.setInput(z0, 'z')
self.mSXF.setInput(u0, 'p')
self.mSXF.evaluate()
y0 = self.mSXF.getOutput()
return u0 * controlScaling, x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
def nlpIn(x = None):
return ca.nlpIn(x = x)
def steadyStateOptimal(self, x0=None, u0=None, z0=None):
ocp = self.ocp
uMin = C.vertcat([
ocp.variable(ocp.u[i].getName()).min.getValue()
for i in range(ocp.u.size())
])
uMax = C.vertcat([
ocp.variable(ocp.u[i].getName()).max.getValue()
for i in range(ocp.u.size())
])
if u0 is None:
u0 = C.vertcat([
ocp.variable(ocp.u[i].getName()).initialGuess.getValue()
for i in range(ocp.u.size())
])
xMin = C.vertcat([
ocp.variable(ocp.x[i].getName()).min.getValue()
for i in range(ocp.x.size())
])
xMax = C.vertcat([
ocp.variable(ocp.x[i].getName()).max.getValue()
for i in range(ocp.x.size())
])
if x0 is None:
x0 = C.vertcat([
ocp.variable(ocp.x[i].getName()).initialGuess.getValue()
for i in range(ocp.x.size())
])
zMin = C.vertcat([
ocp.variable(ocp.z[i].getName()).min.getValue()
for i in range(ocp.z.size())
])
zMax = C.vertcat([
ocp.variable(ocp.z[i].getName()).max.getValue()
for i in range(ocp.z.size())
])
if z0 is None:
z0 = C.vertcat([
ocp.variable(ocp.z[i].getName()).initialGuess.getValue()
for i in range(ocp.z.size())
])
## PATH CONSTRAINTS
pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t)
pathConstraints = C.SXFunction(pathIn, [ocp.beq(ocp.path)])
pathMax = [
ocp.variable(ocp.path[i].getName()).max.getValue()
for i in range(ocp.path.size())
]
pathMin = [
ocp.variable(ocp.path[i].getName()).min.getValue()
for i in range(ocp.path.size())
]
pathConstraints.setOption("name", "PATH")
pathConstraints.init()
varList = [ocp.x, ocp.z, ocp.u]
varMax = [xMax, zMax, uMax]
varMin = [xMin, zMin, uMin]
var0 = [x0, z0, u0]
cons = [ocp.ode(ocp.x), ocp.alg, ocp.beq(ocp.path)]
consMax = [
NP.zeros(ocp.x.size()),
NP.zeros(ocp.z.size()),
C.vertcat(pathMax)
]
consMin = [
NP.zeros(ocp.x.size()),
NP.zeros(ocp.z.size()),
C.vertcat(pathMin)
]
C.NlpSolver.loadPlugin('ipopt')
nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList)),
C.nlpOut(f=ocp.lterm, g=C.vertcat(cons)))
nlp.init()
solver = C.NlpSolver('ipopt', nlp)
solver.setOption("max_iter", 100) # IPOPT maximum iterations
solver.init()
### Variable Bounds and initial guess
solver.setInput(C.vertcat(varMin), 'lbx')
solver.setInput(C.vertcat(varMax), 'ubx')
solver.setInput(C.vertcat(consMin), 'lbg') # g_L
solver.setInput(C.vertcat(consMax), 'ubg') # g_U
solver.setInput(C.vertcat(var0), 'x0') # g_U
solver.evaluate()
outX = solver.getOutput()
splitF = C.SXFunction([C.vertcat(varList)], varList)
splitF.init()
splitF.setInput(outX)
splitF.evaluate()
xOpt = splitF.getOutput(0)
zOpt = splitF.getOutput(1)
uOpt = splitF.getOutput(2)
return uOpt, xOpt, zOpt
