def MILP_compute_traj(self,
obst_idx,
x_out,
y_out,
dx,
dy,
pose_initial=[0., 0.]):
'''
Find trajectory with MILP
Outputs trajectory (waypoints) and new K for control
'''
mp = MathematicalProgram()
N = 8
k = 0
# define state traj
st = mp.NewContinuousVariables(2, "state_%d" % k)
# # binary variables for obstalces constraint
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = c
states = st
for k in range(1, N):
st = mp.NewContinuousVariables(2, "state_%d" % k)
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
st = mp.NewContinuousVariables(2, "state_%d" % (N + 1))
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
# variables encoding max x y dist from obstacle
x_margin = mp.NewContinuousVariables(1, "x_margin")
y_margin = mp.NewContinuousVariables(1, "y_margin")
### define cost
for i in range(N):
mp.AddLinearCost(-states[i, 0]) # go as far forward as possible
mp.AddLinearCost(-states[-1, 0])
mp.AddLinearCost(-x_margin[0])
mp.AddLinearCost(-y_margin[0])
# bound x y margin so it doesn't explode
mp.AddLinearConstraint(x_margin[0] <= 3.)
mp.AddLinearConstraint(y_margin[0] <= 3.)
# x y is non ngative adn at least above robot radius
mp.AddLinearConstraint(x_margin[0] >= 0.05)
mp.AddLinearConstraint(y_margin[0] >= 0.05)
M = 1000 # slack var for integer things
# state constraint
for i in range(2): # initial state constraint x y
mp.AddLinearConstraint(states[0, i] <= pose_initial[i])
mp.AddLinearConstraint(states[0, i] >= pose_initial[i])
for i in range(N):
mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2)
mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx)
mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx)
mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy)
mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy)
# obstacle constraint
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[i, 0] <= rng_min - x_margin[0] +
M * obs[i, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[i, 0] >= rng_max + x_margin[0] -
M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[i, 1] <=
states[i, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[i, 1] >=
states[i, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
# obstacle constraint for last state
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[N, 0] <= rng_min - x_margin[0] +
M * obs[N, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[N, 0] >= rng_max + x_margin[0] -
M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[N,
1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[N,
1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
mp.Solve()
trajectory = mp.GetSolution(states)
xm = mp.GetSolution(x_margin)
ym = mp.GetSolution(y_margin)
x_out[:] = trajectory[:, 0]
y_out[:] = trajectory[:, 1]
return trajectory, xm[0], ym[0]
def intercepting_with_obs_avoidance_bb(self,
p0,
v0,
pf,
vf,
T,
obstacles=None,
p_puck=None):
"""kick while avoiding obstacles using big M formulation and branch and bound"""
x0 = np.array(np.concatenate((p0, v0), axis=0))
xf = np.concatenate((pf, vf), axis=0)
prog = MathematicalProgram()
# state and control inputs
N = int(T / self.params.dt) # number of command steps
state = prog.NewContinuousVariables(N + 1, 4, 'state')
cmd = prog.NewContinuousVariables(N, 2, 'input')
# Initial and final state
prog.AddLinearConstraint(eq(state[0], x0))
prog.AddLinearConstraint(eq(state[-1], xf))
self.add_dynamics(prog, N, state, cmd)
## Input saturation
self.add_input_limits(prog, cmd, N)
# Arena constraints
self.add_arena_limits(prog, state, N)
# Add Mixed-integer constraints - will solve with BB
# avoid hitting the puck while generating a kicking trajectory
# MILP formulation with B&B solver
x_obs_puck = prog.NewBinaryVariables(rows=N + 1,
cols=2) # obs x_min, obs x_max
y_obs_puck = prog.NewBinaryVariables(rows=N + 1,
cols=2) # obs y_min, obs y_max
self.avoid_puck_bigm(prog, x_obs_puck, y_obs_puck, state, N, p_puck)
# Avoid other players
if obstacles != None:
x_obs_player = list()
y_obs_player = list()
for i, obs in enumerate(obstacles):
x_obs_player.append(prog.NewBinaryVariables(
rows=N + 1, cols=2)) # obs x_min, obs x_max
y_obs_player.append(prog.NewBinaryVariables(
rows=N + 1, cols=2)) # obs y_min, obs y_max
self.avoid_other_player_bigm(prog, x_obs_player[i],
y_obs_player[i], state, obs, N)
# Solve with simple b&b solver
for k in range(N):
prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k]))
bb = branch_and_bound.MixedIntegerBranchAndBound(
prog,
OsqpSolver().solver_id())
result = bb.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem.')
u_values = np.array([bb.GetSolution(u) for u in cmd]).T
solution_found = result.kSolutionFound
return solution_found, u_values
def compute_trajectory(self,
obst_idx,
x_out,
y_out,
ux_out,
uy_out,
pose_initial=[0., 0., 0., 0.],
dt=0.05):
'''
Find trajectory with MILP
input u are tyhe velocities (xd, yd)
dt 0.05 according to a rate of 20 Hz
'''
mp = MathematicalProgram()
N = 30
k = 0
# define input trajectory and state traj
u = mp.NewContinuousVariables(2, "u_%d" % k) # xd yd
input_trajectory = u
st = mp.NewContinuousVariables(4, "state_%d" % k)
# # binary variables for obstalces constraint
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = c
states = st
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
input_trajectory = np.vstack((input_trajectory, u))
st = mp.NewContinuousVariables(4, "state_%d" % k)
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
st = mp.NewContinuousVariables(4, "state_%d" % (N + 1))
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
### define cost
mp.AddLinearCost(100 *
(-states[-1, 0])) # go as far forward as possible
# mp.AddQuadraticCost(states[-1,1]*states[-1,1])
# time constraint
M = 1000 # slack var for obst costraint
# state constraint
for i in range(2): # initial state constraint x y yaw
mp.AddLinearConstraint(states[0, i] <= pose_initial[i])
mp.AddLinearConstraint(states[0, i] >= pose_initial[i])
for i in range(2): # initial state constraint xd yd yawd
mp.AddLinearConstraint(states[0, i] <= pose_initial[2 + i] + 1)
mp.AddLinearConstraint(states[0, i] >= pose_initial[2 + i] - 1)
for i in range(N):
# state update according to dynamics
state_next = self.quad_dynamics(states[i, :],
input_trajectory[i, :], dt)
for j in range(4):
mp.AddLinearConstraint(states[i + 1, j] <= state_next[j])
mp.AddLinearConstraint(states[i + 1, j] >= state_next[j])
# obstacle constraint
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[i, 0] <= rng_min - 0.05 +
M * obs[i, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[i, 0] >= rng_max + 0.005 -
M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[i, 1] <= states[i, 0] * np.tan(ang_min) - 0.05 +
M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[i, 1] >= states[i, 0] * np.tan(ang_max) + 0.05 -
M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
# environmnt constraint, dont leave fov
mp.AddLinearConstraint(
states[i, 1] >= states[i, 0] * np.tan(-self.theta))
mp.AddLinearConstraint(
states[i, 1] <= states[i, 0] * np.tan(self.theta))
# bound the inputs
# mp.AddConstraint(input_trajectory[i,:].dot(input_trajectory[i,:]) <= 2.5*2.5) # dosnt work with multi int
mp.AddLinearConstraint(input_trajectory[i, 0] <= 2.5)
mp.AddLinearConstraint(input_trajectory[i, 0] >= -2.5)
mp.AddLinearConstraint(input_trajectory[i, 1] <= 0.5)
mp.AddLinearConstraint(input_trajectory[i, 1] >= -0.5)
mp.Solve()
input_trajectory = mp.GetSolution(input_trajectory)
trajectory = mp.GetSolution(states)
x_out[:] = trajectory[:, 0]
y_out[:] = trajectory[:, 1]
ux_out[:] = input_trajectory[:, 0]
uy_out[:] = input_trajectory[:, 1]
return trajectory, input_trajectory
def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs):
"""
Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
The first nc variables in x are continuous, the remaining are binaries.
Arguments
----------
nc : int
Number of continuous variables in the problem.
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
Returns
----------
sol : dict
Dictionary with the solution of the MIQP.
Fields
----------
min : float
Minimum of the MIQP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the MIQP (None if the problem is unfeasible).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = np.hstack((
prog.NewContinuousVariables(nc),
prog.NewBinaryVariables(n_x - nc)
))
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin'])
return sol
def __init__(self, start, goal, degree, n_segments, duration, regions,
H): #sample_times, num_vars, continuity_degree):
R = len(regions)
N = n_segments
M = 100
prog = MathematicalProgram()
# Set H
if H is None:
H = prog.NewBinaryVariables(R, N)
for n_idx in range(N):
prog.AddLinearConstraint(np.sum(H[:, n_idx]) == 1)
poly_xs, poly_ys, poly_zs = [], [], []
vars_ = np.zeros((N, )).astype(object)
for n_idx in range(N):
# for each segment
t = prog.NewIndeterminates(1, "t" + str(n_idx))
vars_[n_idx] = t[0]
poly_x = prog.NewFreePolynomial(Variables(t), degree,
"c_x_" + str(n_idx))
poly_y = prog.NewFreePolynomial(Variables(t), degree,
"c_y_" + str(n_idx))
poly_z = prog.NewFreePolynomial(Variables(t), degree,
"c_z_" + str(n_idx))
for var_ in poly_x.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
for var_ in poly_y.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
for var_ in poly_z.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
poly_xs.append(poly_x)
poly_ys.append(poly_y)
poly_zs.append(poly_z)
phi = np.array([poly_x, poly_y, poly_z])
for r_idx, region in enumerate(regions):
# if r_idx == 0:
# break
A = region[0]
b = region[1]
b = b + (1 - H[r_idx, n_idx]) * M
b = [Polynomial(this_b) for this_b in b]
q = b - A.dot(phi)
sigma = []
for q_idx in range(len(q)):
sigma_1 = prog.NewFreePolynomial(Variables(t), degree - 1)
prog.AddSosConstraint(sigma_1)
sigma_2 = prog.NewFreePolynomial(Variables(t), degree - 1)
prog.AddSosConstraint(sigma_2)
sigma.append(
Polynomial(t[0]) * sigma_1 +
(1 - Polynomial(t[0])) * sigma_2)
# for var_ in sigma[q_idx].decision_variables():
# prog.AddLinearConstraint(var_<=M)
# prog.AddLinearConstraint(var_>=-M)
q_coeffs = q[q_idx].monomial_to_coefficient_map()
sigma_coeffs = sigma[q_idx].monomial_to_coefficient_map()
both_coeffs = set(q_coeffs.keys()) & set(
sigma_coeffs.keys())
for coeff in both_coeffs:
# import pdb; pdb.set_trace()
prog.AddConstraint(
q_coeffs[coeff] == sigma_coeffs[coeff])
# res = Solve(prog)
# print("x: " + str(res.GetSolution(poly_xs[0].ToExpression())))
# print("y: " + str(res.GetSolution(poly_ys[0].ToExpression())))
# print("z: " + str(res.GetSolution(poly_zs[0].ToExpression())))
# import pdb; pdb.set_trace()
# for this_q in q:
# prog.AddSosConstraint(this_q)
# import pdb; pdb.set_trace()
# cost = 0
print("Constraint: x0(0)=x0")
prog.AddConstraint(
poly_xs[0].ToExpression().Substitute(vars_[0], 0.0) == start[0])
prog.AddConstraint(
poly_ys[0].ToExpression().Substitute(vars_[0], 0.0) == start[1])
prog.AddConstraint(
poly_zs[0].ToExpression().Substitute(vars_[0], 0.0) == start[2])
for idx, poly_x, poly_y, poly_z in zip(range(N), poly_xs, poly_ys,
poly_zs):
if idx < N - 1:
print("Constraint: x" + str(idx) + "(1)=x" + str(idx + 1) +
"(0)")
next_poly_x, next_poly_y, next_poly_z = poly_xs[
idx + 1], poly_ys[idx + 1], poly_zs[idx + 1]
prog.AddConstraint(
poly_x.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_x.ToExpression().Substitute(vars_[idx + 1], 0.0))
prog.AddConstraint(
poly_y.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_y.ToExpression().Substitute(vars_[idx + 1], 0.0))
prog.AddConstraint(
poly_z.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_z.ToExpression().Substitute(vars_[idx + 1], 0.0))
else:
print("Constraint: x" + str(idx) + "(1)=xf")
prog.AddConstraint(poly_x.ToExpression().Substitute(
vars_[idx], 1.0) == goal[0])
prog.AddConstraint(poly_y.ToExpression().Substitute(
vars_[idx], 1.0) == goal[1])
prog.AddConstraint(poly_z.ToExpression().Substitute(
vars_[idx], 1.0) == goal[2])
# for
cost = Expression()
for var_, polys in zip(vars_, [poly_xs, poly_ys, poly_zs]):
for poly in polys:
for _ in range(2): #range(2):
poly = poly.Differentiate(var_)
poly = poly.ToExpression().Substitute({var_: 1.0})
cost += poly**2
# for dv in poly.decision_variables():
# cost += (Expression(dv))**2
prog.AddCost(cost)
res = Solve(prog)
print("x: " + str(res.GetSolution(poly_xs[0].ToExpression())))
print("y: " + str(res.GetSolution(poly_ys[0].ToExpression())))
print("z: " + str(res.GetSolution(poly_zs[0].ToExpression())))
self.poly_xs = [
res.GetSolution(poly_x.ToExpression()) for poly_x in poly_xs
]
self.poly_ys = [
res.GetSolution(poly_y.ToExpression()) for poly_y in poly_ys
]
self.poly_zs = [
res.GetSolution(poly_z.ToExpression()) for poly_z in poly_zs
]
self.vars_ = vars_
self.degree = degree
class HybridModelPredictiveController(object):
def __init__(self, S, N, Q, R, P, X_N):
# store inputs
self.S = S
self.N = N
self.Q = Q
self.R = R
self.P = P
self.X_N = X_N
# mpMIQP
self.build_mpmiqp()
def build_mpmiqp(self):
# express the constrained dynamics as a list of polytopes in the (x,u,x+)-space
P = graph_representation(self.S)
m = big_m(P)
# initialize program
self.prog = MathematicalProgram()
self.x = []
self.u = []
self.d = []
obj = 0.
self.binaries_lower_bound = []
# initial conditions (set arbitrarily to zero in the building phase)
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
self.initial_condition = []
for k in range(self.S.nx):
self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator())
# loop over time
for t in range(self.N):
# create input, mode and next state variables
self.u.append(self.prog.NewContinuousVariables(self.S.nu))
self.d.append(self.prog.NewBinaryVariables(self.S.nm))
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
# enforce constrained dynamics (big-m methods)
xux = np.concatenate((self.x[t], self.u[t], self.x[t+1]))
for i in range(self.S.nm):
mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0)
for k in range(P[i].A.shape[0]):
self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k])
# SOS1 on the binaries
self.prog.AddLinearConstraint(sum(self.d[t]) == 1.)
# stage cost to the objective
obj += .5 * self.u[t].dot(self.R).dot(self.u[t])
obj += .5 * self.x[t].dot(self.Q).dot(self.x[t])
# terminal constraint
for k in range(self.X_N.A.shape[0]):
self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k])
# terminal cost
obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N])
self.objective = self.prog.AddQuadraticCost(obj)
# set solver
self.solver = GurobiSolver()
self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1)
def set_initial_condition(self, x0):
for k, c in enumerate(self.initial_condition):
c.UpdateLowerBound(x0[k:k+1])
c.UpdateUpperBound(x0[k:k+1])
def feedforward(self, x0):
# overwrite initial condition
self.set_initial_condition(x0)
# solve MIQP
result = self.solver.Solve(self.prog)
# check feasibility
if result != SolutionResult.kSolutionFound:
return None, None, None, None
# get cost
obj = self.prog.EvalBindingAtSolution(self.objective)[0]
# store argmin in list of vectors
u = [self.prog.GetSolution(ut) for ut in self.u]
x = [self.prog.GetSolution(xt) for xt in self.x]
d = [self.prog.GetSolution(dt) for dt in self.d]
# retrieve mode sequence and check integer feasibility
ms = [np.argmax(dt) for dt in d]
return u, x, ms, obj
def feedback(self, x0):
# get feedforward and extract first input
u_feedforward = self.feedforward(x0)[0]
if u_feedforward is None:
return None
return u_feedforward[0]
