def __init__(self, ocpVars, lbw, ubw, ocpParams, ocpCosts, ocpConstr, NX, NU, M, w0, Q, R, solvOpts={}): self.var = ocpVars self.lbw = lbw self.ubw = ubw self.params = ocpParams self.costs = sum(ocpCosts) self.g = ca.vertcat(*[c[0] for c in ocpConstr]) self.lbg = ca.vertcat(*[c[1] for c in ocpConstr]) self.ubg = ca.vertcat(*[c[2] for c in ocpConstr]) self.w0 = w0 self.lagrMulConstr = np.zeros((self.g.numel(),)) self.lagrMulOptVars = np.zeros((self.var.cat.numel(),)) self.NX = NX self.NU = NU self.M = M # symbolic variables w = ca.vertcat(self.var['x'][:], self.var['u'] [:]) # primal decision variables # constraint lagrange multipliers (lambda + mu, i.e. dual decision variables) lagrMult = ca.SX.sym('lagrMult', self.g.size()) wGuess = ca.SX.sym('wGuess', w.shape) # guess / linearization point # single weight matrix for all decision variables W = np.diag(np.concatenate([np.tile(Q, M+1), np.tile(R, M)])) L = self.costs + lagrMult.T @ self.g B = ca.Function('B', [w], [ca.hessian(L,w)[0]])(wGuess) wErr = w - wGuess # reference as QP parameters wRef = ca.vertcat(self.params['x_ref'][:], self.params['u_ref'][:]) #B = W # gauss-newton hessian approximation J = W @ (wGuess - wRef) fqp = 1./2. * wErr.T @ B @ wErr + J.T @ wErr gqp = linearize(self.g, w, wGuess) pqp = ca.vertcat(self.params.cat, wGuess, lagrMult) # assemble QP if not solvOpts: solvOpts = {'jit': False, 'print_time': 0, 'printLevel': 'low', 'sparse': True, 'enableEqualities': True} #solvOpts = {'jit' : True, 'print_time' : 0, 'printLevel' : 'high', 'sparse' : True} qp = {'x': self.var, 'f': fqp, 'g': gqp, 'p': pqp} self.solver = ca.qpsol('S', 'qpoases', qp, solvOpts) self.solverResults = [] self.durations = []
# #"hessian_constant" : "yes" # }, # "print_time" : False, # "ipopt.print_level" : 0, # "ipopt.max_cpu_time" : 60, # #"ipopt.jac_c_constant" : "yes", # #"ipopt.jac_d_constant" : "yes", # #"ipopt.hessian_constant" : "yes", # "ipopt.linear_solver" : "ma27", #} #solver = casadi.nlpsol("solver", # "ipopt", nlp, nlpoptions) qpoptions = {"printLevel": 'none', "print_time": False, 'sparse': True} qp = {'x': var, 'f': obj, 'g': con, 'p': par} solver = casadi.qpsol('solver', 'qpoases', qp, qpoptions) #<<ENDCHUNK>> varguess["x", 0, :] = x0 for i in range(1, Nt + 1): vdpargs = dict(x0=np.array(varguess["x", i - 1, :]).flatten(), p=u0) out = vdp(**vdpargs) varguess["x", i, :] = np.array(out["xf"]).flatten() # Now simulate. Nsim = 100 times = Delta * Nsim * np.linspace(0, 1, Nsim + 1) x = np.zeros((Nsim + 1, Nx)) x[0, :] = x0 u = np.zeros((Nsim, Nu))
def calculate_network( nodes: pd.DataFrame, pipes: pd.DataFrame, penalty_order: float = 1.0, head_loss_option: HeadLossOption = HeadLossOption.Q_ONLY): if head_loss_option not in HeadLossOption.__members__.values(): raise Exception( f"Head loss option '{head_loss_option}' does not exist") pipes.set_index('pipe_id', inplace=True) nodes.set_index('node_id', inplace=True) nodes_df = nodes pipes_df = pipes # Convert to dict of Node/Pipe instances nodes = {} for k, v in nodes_df.to_dict('index').items(): nodes[k] = Node(k, **v) pipes = {} for k, v in pipes_df.to_dict('index').items(): pipes[k] = Pipe(k, **v) # Convention is that positive is into a branch and negative is out of a # branch. For nodes, that means that their demand has to be signed # accordingly (as if a branch was attached) equations = [] node_balance = {n: 0 for n in nodes} for p in pipes.values(): equations.append((p.q_start + p.q_end, 0.0, 0.0)) # Continuity equations nodes[p.start].q_balance.append(p.q_start) nodes[p.end].q_balance.append(p.q_end) if head_loss_option >= HeadLossOption.LINEAR: # Head at node equal to head at pipe equations.append((nodes[p.start].head - p.h_start, 0.0, 0.0)) equations.append((nodes[p.end].head - p.h_end, 0.0, 0.0)) # Head loss definition is h_start - h_end. In other words, a positive # discharge means a positive head _loss_ # If a pipe has a booster, the booster is located at the start. The head # loss is then the difference between the downstream head of the booster, # and the end of the pipe. # TODO: We do not actually need the head loss symbol. equations.append( (p.head_loss - (p.h_start + p.booster_head - p.h_end), 0.0, 0.0)) if p.has_booster: equations.extend(p.pump_qh_constraints) if head_loss_option == HeadLossOption.LINEAR: a = _get_head_loss_coeff_linear(p.length, p.diameter / 1000) equations.append((p.head_loss - a * p.q_start, 0.0, 0.0)) elif head_loss_option == HeadLossOption.NONLINEAR: # Flow direction is positive --> flow_dir_sym is 1 # Flow direction is negative --> flow_dir_sym is 0 max_discharge = p.max_velocity * p.area head_loss_coeffs = list( zip(*_get_head_loss_coeffs(p.length, p.diameter / 1000))) max_head_loss = head_loss_coeffs[-1][ 0] * p.max_velocity + head_loss_coeffs[-1][1] equations.append( (p.q_start - p.flow_dir_sym * max_discharge, -np.inf, 0.0)) equations.append( (p.q_start + (1 - p.flow_dir_sym) * max_discharge, 0.0, np.inf)) for a, b in head_loss_coeffs: equations.append( (p.head_loss - (a * p.q_start + b) + (1 - p.flow_dir_sym) * max_head_loss, 0.0, np.inf)) equations.append( (-p.head_loss - (a * -p.q_start + b) + p.flow_dir_sym * max_head_loss, 0.0, np.inf)) # Whether to use a quadratic drop-off, or a linear apprixation thereof for the Node discharge node_quadratic = None if head_loss_option <= HeadLossOption.LINEAR: if penalty_order == 1.0: node_quadratic = False else: node_quadratic = True else: node_quadratic = False for n in nodes.values(): equations.append((sum(n.q_balance), 0.0, 0.0)) if head_loss_option > HeadLossOption.Q_ONLY: if n.type == 'demand': if node_quadratic: equations.extend(n.qh_constraints_quadratic) else: equations.extend(n.qh_constraints_linear) elif n.type == 'supply' and n.use_pump_curve: equations.extend(n.pump_qh_constraints) # Build the state vector with the appropriate bounds x = [] lbx = [] ubx = [] discrete = [] for n in nodes.values(): if n.q_control is not None: x.append(n.q_control) lb, ub = n.q_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) if head_loss_option >= HeadLossOption.LINEAR: for n in nodes.values(): if n.head is not None: x.append(n.head) lb, ub = n.h_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) for p in pipes.values(): x.append(p.q_start) lb, ub = p.q_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) x.append(p.q_end) lbx.append(lb) ubx.append(ub) discrete.append(False) if head_loss_option >= HeadLossOption.LINEAR: x.append(p.h_start) lbx.append(0.0) ubx.append(np.inf) discrete.append(False) x.append(p.h_end) lbx.append(0.0) ubx.append(np.inf) discrete.append(False) x.append(p.head_loss) lbx.append(-np.inf) ubx.append(np.inf) discrete.append(False) if p.has_booster: x.append(p.booster_head) lbx.append(-np.inf) ubx.append(np.inf) discrete.append(False) if head_loss_option == HeadLossOption.NONLINEAR: x.append(p.flow_dir_sym) lbx.append(0.0) ubx.append(1.0) discrete.append(True) # Build the indices for easy lookup index_to_name = {i: v.name() for i, v in enumerate(x)} # Build the constraints g, lbg, ubg = zip(*equations) # Build the objective f = 0.0 for n in nodes.values(): f += n.objective**penalty_order # Construct the qp, and solve qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)} cbc_options = {'cbc': {'slog': 0, 'log': 0}} ipopt_options = { 'print_time': 0, 'ipopt': { 'print_level': 0, 'sb': 'yes', } } if head_loss_option <= HeadLossOption.LINEAR: if penalty_order == 1.0: # We use cbc instead of clp, because we can make it shut up (and # Clp cannot really be) solver = ca.qpsol('qp', 'cbc', qp, { "discrete": discrete, **cbc_options }) else: # 'sb' = 'yes' disable the license header. Very much an obscure # 'and undocumented option. solver = ca.nlpsol('qp', 'ipopt', qp, { "discrete": discrete, **ipopt_options }) else: if penalty_order != 1.0: raise Exception( "Mixed-integer solving requires penalty order of 1.0 (for now)" ) solver = ca.qpsol('qp', 'cbc', qp, { "discrete": discrete, **cbc_options }) ret = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) x_solved = np.array(ret['x']).ravel() results = {index_to_name[i]: x_solved[i] for i in range(len(x_solved))} discharge_dict = {} flow_dict = {} total_shortage = 0.0 max_shortage = 0.0 for n in nodes.values(): if n.q_control is not None: q = results[n.q_control.name()] if n.type == 'supply': q = -q else: total_shortage += n.discharge - results[n.q_control.name()] discharge_dict[n.id] = q else: discharge_dict[n.id] = 0.0 for p in pipes.values(): flow_dict[p.id] = results[p.q_start.name()] return total_shortage, discharge_dict, flow_dict
# g_l = ...; # YOUR CODE HERE: define the linearized inequalities # Jtemp = Jh({xk}); # g_temp = H({xk}); # h_l = ...; # YOUR CODE HERE: Gauss-Newton Hessian approximation (Task 5.3) # j_out = Jr({xk}); # jf_out = Jf({xk}); # r_out = R({xk}); # f_gn = ...; # Allocate QP solver qp = {'x': x, 'f': f_gn, 'p': xk} solver = C.qpsol('solver', 'qpoases', qp) #qp = {'x':x, 'f':f_gn,'g':g_l,'p':xk} #solver = C.qpsol('solver', 'qpoases', qp) #qp = {'x':x, 'f':f_gn,'g':C.vertcat([g_l,h_l]),'p':xk} #solver = C.qpsol('solver', 'qpoases', qp) # SQP solver max_it = 100 xk = np.array([1, 1]) # Initial guess iters = [xk] for i in range(1, max_it): # YOUR CODE HERE: formulate the QP (Tasks 5.3, 5.4, 5.5)
def mpcontrol(self, b_orient, rz_phi, x_in, x_ref, c_l, c_r, pf_l, pf_r): # vector from CoM to hip in global frame (should just use body frame?) rh_l_g = np.dot(b_orient, self.rh_l) # TODO: should this still be rz_phi? rh_r_g = np.dot(b_orient, self.rh_r) # rh_l_g = np.dot(rz_phi, self.rh_l) # rh_r_g = np.dot(rz_phi, self.rh_r) # actual initial footstep position vector from CoM to end effector r1 = pf_l + rh_l_g r2 = pf_r + rh_r_g # inertia matrix inverse # i_global = np.dot(np.dot(rz_phi, self.inertia), rz_phi.T) i_global = np.dot(np.dot(b_orient, self.inertia), b_orient.T) # TODO: should this still be rz_phi? i_inv = np.linalg.inv(i_global) i11 = i_inv[0, 0] i12 = i_inv[0, 1] i13 = i_inv[0, 2] i21 = i_inv[1, 0] i22 = i_inv[1, 1] i23 = i_inv[1, 2] i31 = i_inv[2, 0] i32 = i_inv[2, 1] i33 = i_inv[2, 2] rz11 = rz_phi[0, 0] rz12 = rz_phi[0, 1] rz13 = rz_phi[0, 2] rz21 = rz_phi[1, 0] rz22 = rz_phi[1, 1] rz23 = rz_phi[1, 2] rz31 = rz_phi[2, 0] rz32 = rz_phi[2, 1] rz33 = rz_phi[2, 2] # r = foot position r1x = r1[0] r1y = r1[1] r1z = r1[2] r2x = r2[0] r2y = r2[1] r2z = r2[2] theta_x = cs.SX.sym('theta_x') theta_y = cs.SX.sym('theta_y') theta_z = cs.SX.sym('theta_z') p_x = cs.SX.sym('p_x') p_y = cs.SX.sym('p_y') p_z = cs.SX.sym('p_z') omega_x = cs.SX.sym('omega_x') omega_y = cs.SX.sym('omega_y') omega_z = cs.SX.sym('omega_z') pdot_x = cs.SX.sym('pdot_x') pdot_y = cs.SX.sym('pdot_y') pdot_z = cs.SX.sym('pdot_z') states = [ theta_x, theta_y, theta_z, p_x, p_y, p_z, omega_x, omega_y, omega_z, pdot_x, pdot_y, pdot_z ] # state vector x n_states = len(states) # number of states f1_x = cs.SX.sym('f1_x') # controls f1_y = cs.SX.sym('f1_y') # controls f1_z = cs.SX.sym('f1_z') # controls f2_x = cs.SX.sym('f2_x') # controls f2_y = cs.SX.sym('f2_y') # controls f2_z = cs.SX.sym('f2_z') # controls controls = [f1_x, f1_y, f1_z, f2_x, f2_y, f2_z] n_controls = len(controls) # number of controls gravity = -9.807 dt = self.dt mass = self.mass # gravity = cs.SX.sym("gravity") # dt = cs.SX.sym("dt") # mass = cs.SX.sym("mass") # x_next = np.dot(A, states) + np.dot(B, controls) + g # the discrete dynamics of the system x_next = [ dt * omega_x * rz11 + dt * omega_y * rz12 + dt * omega_z * rz13 + theta_x, dt * omega_x * rz21 + dt * omega_y * rz22 + dt * omega_z * rz23 + theta_y, dt * omega_x * rz31 + dt * omega_y * rz32 + dt * omega_z * rz33 + theta_z, dt * pdot_x + p_x, dt * pdot_y + p_y, dt * pdot_z + p_z, dt * f1_x * (i12 * r1z - i13 * r1y) + dt * f1_y * (-i11 * r1z + i13 * r1x) + dt * f1_z * (i11 * r1y - i12 * r1x) + dt * f2_x * (i12 * r2z - i13 * r2y) + dt * f2_y * (-i11 * r2z + i13 * r2x) + dt * f2_z * (i11 * r2y - i12 * r2x) + omega_x, dt * f1_x * (i22 * r1z - i23 * r1y) + dt * f1_y * (-i21 * r1z + i23 * r1x) + dt * f1_z * (i21 * r1y - i22 * r1x) + dt * f2_x * (i22 * r2z - i23 * r2y) + dt * f2_y * (-i21 * r2z + i23 * r2x) + dt * f2_z * (i21 * r2y - i22 * r2x) + omega_y, dt * f1_x * (i32 * r1z - i33 * r1y) + dt * f1_y * (-i31 * r1z + i33 * r1x) + dt * f1_z * (i31 * r1y - i32 * r1x) + dt * f2_x * (i32 * r2z - i33 * r2y) + dt * f2_y * (-i31 * r2z + i33 * r2x) + dt * f2_z * (i31 * r2y - i32 * r2x) + omega_z, dt * f1_x / mass + dt * f2_x / mass + pdot_x, dt * f1_y / mass + dt * f2_y / mass + pdot_y, dt * f1_z / mass + dt * f2_z / mass + gravity + pdot_z ] self.fn = cs.Function('fn', [ theta_x, theta_y, theta_z, p_x, p_y, p_z, omega_x, omega_y, omega_z, pdot_x, pdot_y, pdot_z, f1_x, f1_y, f1_z, f2_x, f2_y, f2_z ], x_next) # nonlinear mapping of function f(x,u) u = cs.SX.sym('u', n_controls, self.N) # decision variables, control action matrix st_ref = cs.SX.sym('st_ref', n_states + n_states) # initial and reference states x = cs.SX.sym( 'x', n_states, (self.N + 1)) # represents the states over the opt problem. obj = 0 # objective function constr = [] # constraints vector k = 10 Q = np.zeros((12, 12)) # state weighing matrix Q[0, 0] = k Q[1, 1] = k Q[2, 2] = k Q[3, 3] = k Q[4, 4] = k Q[5, 5] = k Q[6, 6] = k Q[7, 7] = k Q[8, 8] = k Q[9, 9] = k Q[10, 10] = k Q[11, 11] = k R = np.zeros((6, 6)) # control weighing matrix R[0, 0] = k / 2 R[1, 1] = k / 2 R[2, 2] = k / 2 R[3, 3] = k / 2 R[4, 4] = k / 2 R[5, 5] = k / 2 constr = cs.vertcat( constr, x[:, 0] - st_ref[0:n_states]) # initial condition constraints # compute objective and constraints for k in range(0, self.N): st = x[:, k] # state con = u[:, k] # control action # calculate objective # why not just plug x_in and x_ref directly into st_ref?? obj = obj + cs.mtimes(cs.mtimes((st - st_ref[n_states:(n_states * 2)]).T, Q), st - st_ref[n_states:(n_states * 2)]) \ + cs.mtimes(cs.mtimes(con.T, R), con) st_next = x[:, k + 1] f_value = self.fn(st[0], st[1], st[2], st[3], st[4], st[5], st[6], st[7], st[8], st[9], st[10], st[11], con[0], con[1], con[2], con[3], con[4], con[5]) st_n_e = np.array(f_value) constr = cs.vertcat(constr, st_next - st_n_e) # compute constraints # add additional constraints for k in range(0, self.N): constr = cs.vertcat(constr, u[0, k] - self.mu * u[2, k]) # f1x - mu*f1z constr = cs.vertcat(constr, -u[0, k] - self.mu * u[2, k]) # -f1x - mu*f1z constr = cs.vertcat(constr, u[1, k] - self.mu * u[2, k]) # f1y - mu*f1z constr = cs.vertcat(constr, -u[1, k] - self.mu * u[2, k]) # -f1y - mu*f1z constr = cs.vertcat(constr, u[3, k] - self.mu * u[5, k]) # f2x - mu*f2z constr = cs.vertcat(constr, -u[3, k] - self.mu * u[5, k]) # -f2x - mu*f2z constr = cs.vertcat(constr, u[4, k] - self.mu * u[5, k]) # f2y - mu*f2z constr = cs.vertcat(constr, -u[4, k] - self.mu * u[5, k]) # -f2y - mu*f2z opt_variables = cs.vertcat(cs.reshape(x, n_states * (self.N + 1), 1), cs.reshape(u, n_controls * self.N, 1)) qp = {'x': opt_variables, 'f': obj, 'g': constr, 'p': st_ref} opts = { 'print_time': 0, 'error_on_fail': 0, 'printLevel': "none", 'boundTolerance': 1e-6, 'terminationTolerance': 1e-6 } solver = cs.qpsol('S', 'qpoases', qp, opts) c_length = np.shape(constr)[0] o_length = np.shape(opt_variables)[0] lbg = list(itertools.repeat(-1e10, c_length) ) # inequality constraints: big enough to act like infinity lbg[0:(self.N + 1)] = itertools.repeat( 0, self.N + 1) # IC + dynamics equality constraint ubg = list(itertools.repeat(0, c_length)) # inequality constraints # constraints for optimization variables lbx = list(itertools.repeat(-1e10, o_length)) # input inequality constraints ubx = list(itertools.repeat(1e10, o_length)) # input inequality constraints st_len = n_states * (self.N + 1) lbx[(st_len + 2)::3] = [0 for i in range(20) ] # lower bound on all f1z and f2z if c_l == 0: # if left leg is not in contact... don't calculate output forces for that leg. ubx[(n_states * (self.N + 1))::6] = [0 for i in range(10) ] # upper bound on all f1x ubx[(n_states * (self.N + 1) + 1)::6] = [0 for i in range(10)] # upper bound on all f1y lbx[(n_states * (self.N + 1))::6] = [0 for i in range(10) ] # lower bound on all f1x lbx[(n_states * (self.N + 1) + 1)::6] = [0 for i in range(10)] # lower bound on all f1y ubx[(n_states * (self.N + 1) + 2)::6] = [0 for i in range(10)] # upper bound on all f1z else: ubx[(n_states * (self.N + 1))::6] = [1 for i in range(10) ] # upper bound on all f1x ubx[(n_states * (self.N + 1) + 1)::6] = [1 for i in range(10)] # upper bound on all f1y lbx[(n_states * (self.N + 1))::6] = [-1 for i in range(10) ] # lower bound on all f1x lbx[(n_states * (self.N + 1) + 1)::6] = [-1 for i in range(10)] # lower bound on all f1y ubx[(n_states * (self.N + 1) + 2)::6] = [2.5 for i in range(10)] # upper bound on all f1z if c_r == 0: # if right leg is not in contact... don't calculate output forces for that leg. ubx[(n_states * (self.N + 1) + 3)::6] = [0 for i in range(10)] # upper bound on all f2x ubx[(n_states * (self.N + 1) + 4)::6] = [0 for i in range(10)] # upper bound on all f2y lbx[(n_states * (self.N + 1) + 3)::6] = [0 for i in range(10)] # lower bound on all f2x lbx[(n_states * (self.N + 1) + 4)::6] = [0 for i in range(10)] # lower bound on all f2y ubx[(n_states * (self.N + 1) + 5)::6] = [0 for i in range(10)] # upper bound on all f2z else: ubx[(n_states * (self.N + 1) + 3)::6] = [1 for i in range(10)] # upper bound on all f2x ubx[(n_states * (self.N + 1) + 4)::6] = [1 for i in range(10)] # upper bound on all f2y lbx[(n_states * (self.N + 1) + 3)::6] = [-1 for i in range(10)] # lower bound on all f2x lbx[(n_states * (self.N + 1) + 4)::6] = [-1 for i in range(10)] # lower bound on all f2y ubx[(n_states * (self.N + 1) + 5)::6] = [2.5 for i in range(10)] # upper bound on all f2z # setup is finished, now solve-------------------------------------------------------------------------------- # u0 = np.zeros((self.N, n_controls)) # six control inputs X0 = np.matlib.repmat( x_in, 1, self.N + 1).T # initialization of the state's decision variables # parameters and xin must be changed every timestep parameters = cs.vertcat(x_in, x_ref) # set values of parameters vector # init value of optimization variables x0 = cs.vertcat(np.reshape(X0.T, (n_states * (self.N + 1), 1)), np.reshape(u0.T, (n_controls * self.N, 1))) sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg, p=parameters) solu = np.array(sol['x'][n_states * (self.N + 1):]) # u = np.reshape(solu.T, (n_controls, self.N)).T # get controls from the solution u = np.reshape( solu.T, (self.N, n_controls)).T # get controls from the solution # u_cl = u[0, :] # ignore rows other than new first row u_cl = u[:, 0] # ignore rows other than new first row # ss_error = np.linalg.norm(x0 - x_ref) # defaults to Euclidean norm # print("ss_error = ", ss_error) # print(u_cl) # print("Time elapsed for MPC: ", t1 - t0) return u_cl
# Build the constraints g = equations lbg = [0.0] * len(g) ubg = lbg.copy() # Build the objective f = 0.0 for n in nodes.values(): f += n.objective**2 # In[104]: # Construct the qp, and solver qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)} solver = ca.qpsol('qp', 'cplex', qp, {}) # In[105]: results = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) # In[112]: total_shortage = 0 max_shortage = 0 i = 0 for n in nodes.values(): if n.q_control is not None: print("Node", n.id, n.type, n.demand, results['x'][i])
def calculate_network( nodes: Union[str, pd.DataFrame, Dict[str, List]] = 'nodes.csv', pipes: Union[str, pd.DataFrame, Dict[str, List]] = 'pipes.csv', penalty_order: float = 1.0, head_loss_option: HeadLossOption = HeadLossOption.Q_ONLY): nodes_df = nodes pipes_df = pipes #Convert to dict of Node/Pipe instances nodes = {} for k, v in nodes_df.to_dict('index').items(): nodes[k] = Node(k, **v) pipes = {} for k, v in pipes_df.to_dict('index').items(): pipes[k] = Pipe(k, **v) # Convention is that positive is into a branch and negative is out of a # branch. For nodes, that means that their demand has to be signed # accordingly (as if a branch was attached) equations = [] node_balance = {n: 0 for n in nodes} for p in pipes.values(): equations.append((p.q_start + p.q_end, 0.0, 0.0)) # Continuity equations nodes[p.start].q_balance.append(p.q_start) nodes[p.end].q_balance.append(p.q_end) if head_loss_option >= HeadLossOption.LINEAR: # Head at node equal to head at pipe equations.append((nodes[p.start].head - p.h_start, 0.0, 0.0)) equations.append((nodes[p.end].head - p.h_end, 0.0, 0.0)) # Head loss definition is h_start - h_end. In other words, a positive # discharge means a positive head _loss_ # TODO: We do not actually need the head loss symbol. equations.append((p.head_loss - (p.h_start - p.h_end), 0.0, 0.0)) if head_loss_option == HeadLossOption.LINEAR: a = _get_head_loss_coeff_linear(p.length, p.diameter / 1000) equations.append((p.head_loss - a * p.q_start, 0.0, 0.0)) elif head_loss_option == HeadLossOption.NONLINEAR: # Flow direction is positive --> flow_dir is 1 # Flow direction is negative --> flow dir is 0 max_discharge = p.max_velocity * p.area equations.append( (p.q_start - p.flow_dir * max_discharge, -np.inf, 0.0)) equations.append((p.q_start + (1 - p.flow_dir) * max_discharge, 0.0, np.inf)) for a, b in zip(*_get_head_loss_coeffs(p.length, p.diameter / 1000)): equations.append( (p.head_loss - (a * p.q_start + b - (1 - p.flow_dir) * max_discharge), 0.0, np.inf)) equations.append( (-p.head_loss - (a * -p.q_start + b - p.flow_dir * max_discharge), 0.0, np.inf)) for n in nodes.values(): equations.append((sum(n.q_balance), 0.0, 0.0)) # Build the state vector with the appropriate bounds x = [] lbx = [] ubx = [] discrete = [] for n in nodes.values(): if n.q_control is not None: x.append(n.q_control) lb, ub = n.q_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) if head_loss_option >= HeadLossOption.LINEAR: for n in nodes.values(): if n.head is not None: x.append(n.head) lb, ub = n.h_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) for p in pipes.values(): x.append(p.q_start) lb, ub = p.q_bounds lbx.append(lb) ubx.append(ub) discrete.append(False) x.append(p.q_end) lbx.append(lb) ubx.append(ub) discrete.append(False) if head_loss_option >= HeadLossOption.LINEAR: x.append(p.h_start) lbx.append(0.0) ubx.append(np.inf) discrete.append(False) x.append(p.h_end) lbx.append(0.0) ubx.append(np.inf) discrete.append(False) x.append(p.head_loss) lbx.append(-np.inf) ubx.append(np.inf) discrete.append(False) if head_loss_option == HeadLossOption.NONLINEAR: x.append(p.flow_dir) lbx.append(0.0) ubx.append(1.0) discrete.append(True) # Build the indices for easy lookup index_to_name = {i: v.name() for i, v in enumerate(x)} # Build the constraints g, lbg, ubg = zip(*equations) # Build the objective f = 0.0 for n in nodes.values(): f += n.objective**penalty_order # Construct the qp, and solve qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)} if head_loss_option <= HeadLossOption.LINEAR: print(head_loss_option) print(int(head_loss_option)) if penalty_order == 1.0: solver = ca.qpsol('qp', 'clp', qp) else: solver = ca.nlpsol('qp', 'ipopt', qp, {"discrete": discrete}) else: if penalty_order != 1.0: raise Exception( "Mixed-integer solving requires penalty order of 1.0 (for now)" ) solver = ca.qpsol('qp', 'cbc', qp, {"discrete": discrete}) ret = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) x_solved = np.array(ret['x']).ravel() results = {index_to_name[i]: x_solved[i] for i in range(len(x_solved))} shortage_dict = {} flow_dict = {} total_shortage = 0.0 max_shortage = 0.0 for n in nodes.values(): if n.type == 'demand': if n.q_control is not None: q = results[n.q_control.name()] short = n.demand - q shortage_dict[n.id] = short total_shortage += short max_shortage = max(max_shortage, short) else: shortage_dict[n.id] = 0.0 for p in pipes.values(): flow_dict[p.id] = results[p.q_start.name()] return total_shortage, shortage_dict, flow_dict
def gen_mpc_solver(A, B, Hu, Hp, Q, R, B_d=None, S=None, operating_point=None): """ Consult at p55 in Jan M. book for better understanding as well as casadi documentation :param A:(mxm) Model dynamics matrix of type casadi.DM :param B:(mxn) Input dynamics matrix of type casadi.DM :param Hu:(int) Control horizon of type Integer :param Hp: (int) Prediction horizon of type Integer :param Q:(mxm) State cost matrix of type casadi.DM :param R:(mxm) Input change cost matrix of type casadi.DM :param B_d:(mxn) """ if B_d is None: B_d = B # TODO: Fix cost on Inputs u - Weight matrix S if S is None: S = ca.DM.zeros(R.size1(), R.size2()) # Useful dimensions number_of_states = A.size1() number_of_inputs = B.size2() number_of_disturbances = B_d.size2() if operating_point is None: operating_point = ca.DM.zeros(number_of_states) # Index for input variable initial_state_index_end = number_of_states prev_control_input_index_end = initial_state_index_end + number_of_inputs reference_index_end = prev_control_input_index_end + Hp * number_of_states prev_disturbance_index_end = reference_index_end + number_of_disturbances delta_disturbances_input_index_end = prev_disturbance_index_end + Hp * number_of_disturbances # Declaring and parting out input variables # Input = [x0, u_prev, ref, ud_prev, ud] input_variables = ca.SX.sym('i', delta_disturbances_input_index_end, 1) x0 = input_variables[0:initial_state_index_end, :] u_prev = input_variables[ initial_state_index_end:prev_control_input_index_end, :] ref = input_variables[prev_control_input_index_end:reference_index_end, :] ud_prev = input_variables[ reference_index_end:prev_disturbance_index_end, :] dud = input_variables[ prev_disturbance_index_end:delta_disturbances_input_index_end, :] # Declaring solver outputs x = ca.SX.sym('x', number_of_inputs * Hu, 1) du = x[:number_of_inputs * Hu] # The wrong way of declaring inputs # x0 = ca.SX.sym('x0', A.shape[0], 1) # u_prev = ca.SX.sym('u_prev', B.shape[1], 1) # ref = ca.SX.sym('ref', Hp * A.shape[0], 1) # input_variables = ca.vertcat(ca.vertcat(x0, u_prev), ref) # To formulate a MPC optimization problem we need to describe: # predicted_states = Z = psi x(k) + upsilon u(k-1) + Theta dU(x) + upsilon ud(k-1) + Theta dUd(x) psi = gen_psi(A, Hp) upsilon = gen_upsilon(A, B, Hp) theta = gen_theta(upsilon, B, Hu) upsilon_d = gen_upsilon(A, B_d, Hp) theta_d = gen_theta(upsilon_d, B_d, Hp) predicted_states = gen_predicted_states(psi, x0, upsilon, u_prev, theta, du, upsilon_d, ud_prev, theta_d, dud, op=operating_point) # Setup constraints # construct U fom dU U = ca.SX.ones(du.size1()) for i in range(0, number_of_inputs): U[i::number_of_inputs] = ca.cumsum(du[i::number_of_inputs]) U = U + ca.repmat(u_prev, Hu, 1) constraints = ca.vertcat(predicted_states, U) # construct IU fom U IU = ca.SX.ones(du.size1()) for i in range(0, number_of_inputs): IU[i::number_of_inputs] = ca.cumsum(U[i::number_of_inputs]) constraints = ca.vertcat(predicted_states, U) # Cost function: # Cost = (Z - T)' * Q * (Z - T) + dU' * R * dU error = predicted_states - ref # e = (Z - T) quadratic_cost = error.T @ Q @ error \ + du.T @ R @ du \ + IU.T @ S @ ca.DM.ones(U.size1(), U.size2()) #+ U.T @ S @ U # + ca.norm_1(U.T @ S) # Setup Solver # set print level: search for 'printLevel' in link # http://casadi.sourceforge.net/v3.1.0/api/internal/de/d94/qpoases__interface_8cpp_source.html # "tabular"; "none"; "low"; "medium"; "high"; "debug_iter"; opts = dict(printLevel='low') quadratic_problem = { 'x': du, 'p': input_variables, 'f': quadratic_cost, 'g': constraints } mpc_solver = ca.qpsol('mpc_solver', 'qpoases', quadratic_problem, opts) # print(quadratic_cost) print(mpc_solver) return mpc_solver
#!/usr/bin/env python # -*- coding: utf-8 -*- import casadi as ca import casadi.tools as ca_tools import numpy as np import time import math if __name__ == '__main__': print("begin the test program.") x = ca.SX.sym('x') # 定义一 维变量x y = ca.SX.sym('y') # 定义一 维变量y f = x ** 2 + y ** 2 # 定义目标函数 qp = {'x': ca.vertcat(x, y), 'f': f, 'g': x + y - 10} S = ca.qpsol('S', 'qpoases', qp) # 加载求解器 print(S) r = S(lbg=0) # 加载约束条件并进行求解 x_opt = r['x'] print('x_opt:', x_opt) # 显示求解结果 print(r['f'])
def solve_chain_mass_nmpc_qp(num_masses, num_intervals): time_step = 0.1 u, y, dot_y = get_chain_mass_ode(num_masses) init_state = get_equilibrium_states(num_masses, y, dot_y, [0, 1, 0]) ref_state = get_equilibrium_states(num_masses, y, dot_y, [1, 0, 0]) # NMPC formulation: num_states = y.shape[0] num_controls = 3 weights_states = numpy.ones(y.shape) weights_states[0:3 * num_masses] = 1e2 weights_states[3 * num_masses:6 * num_masses] = 1e0 weights_states[-3:] = 1e2 weights_controls = numpy.ones((num_controls, 1)) * 1e-2 # states 1..N states = casadi.SX.sym('states', num_states, num_intervals) # controls 0..N-1 controls = casadi.SX.sym('controls', num_controls, num_intervals) weights_states_sqrt = numpy.sqrt(weights_states) weights_controls_sqrt = numpy.sqrt(weights_controls) objective_residuals = [] ref_control = numpy.zeros(u.shape) for node in range(num_intervals): objective_residuals.append( (states[:, node] - ref_state) * weights_states_sqrt) objective_residuals.append( (controls[:, node] - ref_control) * weights_controls_sqrt) objective_residuals = casadi.veccat(*objective_residuals) ode = casadi.Function('ode', [u, y], [dot_y]) rk4_k1 = ode(u, y) rk4_k2 = ode(u, y + time_step / 2.0 * rk4_k1) rk4_k3 = ode(u, y + time_step / 2.0 * rk4_k2) rk4_k4 = ode(u, y + time_step * rk4_k3) final_y = y + time_step / 6.0 * (rk4_k1 + 2 * rk4_k2 + 2 * rk4_k3 + rk4_k4) integrate = casadi.Function('integrate', [u, y], [final_y]) states_for_integration = casadi.horzcat(init_state, states[:, :-1]) integrated_states = integrate.map(num_intervals)(controls, states_for_integration) equality_constraints = states - integrated_states equality_constraints = casadi.veccat(equality_constraints) # Prepare and condense the underlying QP. states_vec = casadi.veccat(states) controls_vec = casadi.veccat(controls) jac_obj_residuals_wrt_states = casadi.jacobian(objective_residuals, states_vec) jac_obj_residuals_wrt_controls = casadi.jacobian(objective_residuals, controls_vec) jac_eq_constraints_wrt_states = casadi.jacobian(equality_constraints, states_vec) jac_eq_constraints_wrt_controls = casadi.jacobian(equality_constraints, controls_vec) qp_h = jac_obj_residuals_wrt_controls.T @ jac_obj_residuals_wrt_controls delta_x_contrib = casadi.solve(jac_eq_constraints_wrt_states, jac_eq_constraints_wrt_controls) delta_x_qp_contrib = -jac_obj_residuals_wrt_states @ delta_x_contrib qp_h += delta_x_qp_contrib.T @ delta_x_qp_contrib qp_g = (jac_obj_residuals_wrt_controls + delta_x_qp_contrib).T @ objective_residuals states_lbx = numpy.zeros(y.shape) states_ubx = numpy.zeros(y.shape) states_lbx.fill(-numpy.inf) for m in range(num_masses): states_lbx[3 * m + 1] = -0.01 states_ubx.fill(numpy.inf) qp_lb_a = [] qp_ub_a = [] for _ in range(num_intervals): qp_lb_a.append(states_lbx) qp_ub_a.append(states_ubx) qp_lb_a = numpy.concatenate(qp_lb_a, axis=0) qp_ub_a = numpy.concatenate(qp_ub_a, axis=0) init_states = numpy.concatenate([init_state for _ in range(num_intervals)], axis=0) delta_x_bound_contrib = casadi.solve(jac_eq_constraints_wrt_states, equality_constraints) + init_states qp_lb_a -= delta_x_bound_contrib qp_ub_a -= delta_x_bound_contrib qp_a = -delta_x_contrib qp_fcn = casadi.Function('qp_h_fcn', [controls_vec, states_vec], [qp_h, qp_g, qp_a, qp_lb_a, qp_ub_a]) init_controls = numpy.zeros(controls_vec.shape) qp_h_eval, qp_g_eval, qp_a_eval, qp_lb_a_eval, qp_ub_a_eval = qp_fcn( init_controls, init_states) qp_lbx = -numpy.ones(qp_g.shape) qp_ubx = numpy.ones(qp_g.shape) # Reduce the number of rows of the A-matrix to the minimum. qp_a_indices = [] for el in range(qp_lb_a.shape[0]): if qp_lb_a_eval[el] <= -numpy.inf and qp_ub_a_eval[el] >= numpy.inf: continue qp_a_indices.append(el) qp_lb_a_eval = qp_lb_a_eval[qp_a_indices] qp_ub_a_eval = qp_ub_a_eval[qp_a_indices] qp_a_eval = qp_a_eval[qp_a_indices, :] qp_x = casadi.SX.sym('qp_x', *qp_g.shape) qp = { 'x': qp_x, 'f': 0.5 * qp_x.T @ qp_h_eval @ qp_x + qp_x.T @ qp_g_eval, 'g': casadi.densify(qp_a_eval @ qp_x) } qp_solver = casadi.qpsol('qp_solver', 'qpoases', qp) sol = qp_solver(lbx=qp_lbx, ubx=qp_ubx, lbg=qp_lb_a_eval, ubg=qp_ub_a_eval) x_opt = numpy.asarray(sol['x']).flatten() y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g'])) f_opt = sol['f'] num_variables = qp_x.shape[0] num_constraints = qp_a_eval.shape[0] qp_h_flat = numpy.asarray(qp_h_eval).flatten() qp_g_flat = numpy.asarray(qp_g_eval).flatten() qp_a_flat = numpy.asarray(qp_a_eval).flatten() qp_lb_a_flat = numpy.asarray(qp_lb_a_eval).flatten() qp_ub_a_flat = numpy.asarray(qp_ub_a_eval).flatten() return (num_variables, num_constraints, qp_h_flat, qp_g_flat, qp_a_flat, qp_lbx, qp_ubx, qp_lb_a_flat, qp_ub_a_flat, x_opt, y_opt, f_opt)
def solve_hanging_chain_qp(num_masses, use_contraints): m_i = 40.0 / num_masses D_i = 70.0 * num_masses g0 = 9.81 zmin = 0.5 # ground x = [] f = 0 g = [] lbx = [] ubx = [] lbg = [] ubg = [] y_start, z_start = -2, 1 y_end, z_end = 2, 1 # Loop over all chain elements y_prev = z_prev = None for i in range(0, num_masses): # Create variables for the (y_i, z_i) coordinates y_i = casadi.SX.sym('y_' + str(i)) z_i = casadi.SX.sym('z_' + str(i)) # Add to the list of variables x += [y_i, z_i] lbx += [-numpy.inf, zmin] ubx += [numpy.inf, numpy.inf] # Spring potential if i == 0: f += D_i / 2 * ((y_start - y_i)**2 + (z_start - z_i)**2) else: f += D_i / 2 * ((y_prev - y_i)**2 + (z_prev - z_i)**2) # Graviational potential f += g0 * m_i * z_i # Slanted ground constraints if use_contraints: g.append(z_i - 0.1 * y_i) lbg.append(0.5) ubg.append(numpy.inf) # Prepare for the next iteration y_prev = y_i z_prev = z_i f += D_i / 2 * ((y_i - y_end)**2 + (z_i - z_end)**2) num_variables = len(x) num_constraints = len(g) x = casadi.vertcat(*x) g = casadi.vertcat(*g) qp = {'x': x, 'f': f, 'g': g} solver = casadi.qpsol('solver', 'qpoases', qp) gradient_f = casadi.gradient(f, x) h = casadi.jacobian(gradient_f, x, {'symmetric': True}) c = casadi.substitute(gradient_f, x, casadi.SX.zeros(x.sparsity())) a = casadi.jacobian(g, x) prob = casadi.Function("qp_eval", [x], [h, c, a]) qp_items = prob(casadi.SX.sym('eval_args', x.shape)) h_flat = numpy.asarray(casadi.DM(qp_items[0])) c_flat = numpy.asarray(casadi.DM(qp_items[1])) a_flat = numpy.asarray(casadi.DM(qp_items[2])) sol = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) x_opt = numpy.asarray(sol['x']).flatten() y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g'])) f_opt = sol['f'] return (num_variables, num_constraints, h_flat, c_flat, a_flat, numpy.asarray(lbx), numpy.asarray(ubx), numpy.asarray(lbg), numpy.asarray(ubg), x_opt, y_opt, f_opt)
# Build the constraints g = equations lbg = [0.0] * len(g) ubg = lbg.copy() # Build the objective f = 0.0 for n in nodes.values(): f += n.objective # In[104]: # Construct the qp, and solver qp = {'f': f, 'g': ca.vertcat(*g), 'x': ca.vertcat(*x)} solver = ca.qpsol('qp', 'clp', qp, {}) # In[105]: results = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) # In[112]: total_shortage = 0 max_shortage = 0 i = 0 for n in nodes.values(): if n.q_control is not None: print("Node", n.id, n.type, n.demand, results['x'][i])
def _create_solver(self): problem_dict = self.get_problem_dict() return qpsol(self.name + '_solver', 'qpoases', problem_dict, self.solver_options)
import casadi as ca # opts = dict(printLevel='1') # {'qpoases':{'printLevel':0}} opts = {"printLevel": 0} mpc_solver = ca.qpsol('mpc_solver', 'qpoases', {}, opts)