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]