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),