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)