def linear_optimization(env, x_ref, x_bar, u_bar, x0, N, dt):
''' Builds and solves single step linear optimization problem
with convex polygon obstacles and linearized dynamics
'''
prog, dvs = initialize_problem(n_x, n_u, N, x0)
prog = linear_dynamics_constraints(prog, dvs, x_bar, u_bar, N, dt)
prog = add_input_state_constraints(prog, dvs, bounds)
prog, dvs = add_polyhedron_obstacle_constraints(
prog, dvs, env.obstacles, buffer, N)
prog = add_costs(prog, dvs, env, x_ref, Q, R, N)
solver = gurobi.GurobiSolver()
# solver.AcquireLicense()
result = solver.Solve(prog)
# assert(result.is_success), "Optimization Failed"
status = result.get_solver_details().optimization_status
assert(status == 2), "Optimization failed with code: " + str(status)
x, u, z, v = dvs
x_sol = result.GetSolution(x)
u_sol = result.GetSolution(u)
z_sol = [result.GetSolution(zj) for zj in z]
v_sol = [result.GetSolution(vj) for vj in v]
return result, (x_sol, u_sol, z_sol, v_sol)
@author: sadra
"""
# Numpy ans scipy
import numpy as np
import scipy.linalg as spa
# pydrake
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
import pydrake.solvers.osqp as OSQP_drake
# Pypolycontain
from pypolycontain.lib.objects import zonotope
from pypolycontain.lib.zonotope_order_reduction.methods import G_cut, Girard
# use Gurobi solver
global gurobi_solver, OSQP_solver
gurobi_solver = Gurobi_drake.GurobiSolver()
OSQP_solver = OSQP_drake.OsqpSolver()
def reduced_order_new(sys, T):
M, N, Z = {}, {}, {}
for t in range(T):
# 1: intiial state
Omega_1 = np.dot(sys.Q["x", t], sys.X0.G)
e_1 = np.dot(sys.C[t + 1], sys.P["x", t + 1])
Gamma_1 = np.dot(e_1, sys.X0.G)
gamma_1 = np.dot(e_1, sys.X0.x)
# 2: process noice
K_2 = np.dot(sys.Q["w", t],
spa.block_diag(*[sys.W[t].G for tau in range(0, t + 1)]))
e_2 = np.dot(sys.C[t + 1], sys.E["w", t])