def t_dccp():
x = cp.Variable(2)
y = cp.Variable(2)
myprob = cp.Problem(cp.Maximize(cp.norm(x - y, 2)),
[0 <= x, x <= 1, 0 <= y, y <= 1])
print("problem is DCP:", myprob.is_dcp()) # false
print("problem is DCCP:", dccp.is_dccp(myprob)) # true
result = myprob.solve(method='dccp')
print("x =", x.value)
print("y =", y.value)
print("cost value =", result[0])
def distance_from_separating_hyperplane(self,tple): no_of_weights=self.dimensions no_of_tuple=self.dimensions weights=cvxpy.Variable(no_of_weights,1) tuple=numpy.array(tple) print "weights:",weights print "tuple:",tuple bias=self.bias svm_function = 0.0 for i in xrange(no_of_weights): svm_function += cvxpy.abs(weights[i,0]) objective=cvxpy.Minimize(cvxpy.abs(svm_function)*0.5) print "============================================" print "Objective Function" print "============================================" print objective constraint=0.0 constraints=[] for i,k in zip(xrange(no_of_weights),xrange(no_of_tuple)): constraint += weights[i,0]*tuple[k] constraint += bias print "constraint:",constraint constraints.append(cvxpy.abs(constraint) >= 1) print "============================================" print "Constraints" print "============================================" print constraints problem=cvxpy.Problem(objective,constraints) print "=====================================" print "Installed Solvers:" print "=====================================" print cvxpy.installed_solvers() print "Is Problem DCCP:",dccp.is_dccp(problem) print "=====================================" print "CVXPY args:" print "=====================================" result=problem.solve(solver=cvxpy.SCS,verbose=True,method='dccp') print "=====================================" print "Problem value:" print "=====================================" print problem.value print "=====================================" print "Result:" print "=====================================" print result return (result,tuple)
def test_readme_example(self):
"""Test the example in the readme.
"""
x = Variable(2)
y = Variable(2)
myprob = Problem(Maximize(norm(x - y, 2)),
[0 <= x, x <= 1, 0 <= y, y <= 1])
assert not myprob.is_dcp() # false
assert dccp.is_dccp(myprob) # true
result = myprob.solve(method='dccp')
self.assertItemsAlmostEqual(x.value, [0, 0])
self.assertItemsAlmostEqual(y.value, [1, 1])
self.assertAlmostEqual(result[0], np.sqrt(2))
def test_readme_example(self):
"""
Test the example in the readme.
self.sol - All known possible solutions to the problem in the readme.
"""
self.sol = [[0,0], [0,1], [1,0], [1,1]]
myprob = cvx.Problem(cvx.Maximize(cvx.norm(self.y-self.z,2)), [0<=self.y, self.y<=1, 0<=self.z, self.z<=1])
assert not myprob.is_dcp() # false
assert dccp.is_dccp(myprob) # true
result = myprob.solve(method = 'dccp')
#print(self.y.value, self.z.value)
self.assertIsAlmostIn(self.y.value, self.sol)
self.assertIsAlmostIn(self.z.value, self.sol)
self.assertAlmostEqual(result[0], np.sqrt(2))
def test_readme_example(self):
"""
Test the example in the readme.
self.sol - All known possible solutions to the problem in the readme.
"""
self.sol = [[0, 0], [0, 1], [1, 0], [1, 1]]
myprob = cvx.Problem(cvx.Maximize(
cvx.norm(self.y - self.z,
2)), [0 <= self.y, self.y <= 1, 0 <= self.z, self.z <= 1])
assert not myprob.is_dcp() # false
assert dccp.is_dccp(myprob) # true
result = myprob.solve(method='dccp')
#print(self.y.value, self.z.value)
self.assertIsAlmostIn(self.y.value, self.sol)
self.assertIsAlmostIn(self.z.value, self.sol)
self.assertAlmostEqual(result[0], np.sqrt(2))
def describe_problem(problem: Problem):
"""
DMCP https://github.com/cvxgrp/dmcp
dccp
"""
st = 'Curvature {}'.format(problem.objective.expr.curvature)
st += '\nis disciplined quasiconvex {}'.format(problem.is_dqcp())
st += '\nis disciplined geometric {}'.format(problem.is_dgp())
st += '\nis disciplined quadratic {}'.format(problem.is_qp())
st += '\nis disciplined convex {}'.format(problem.is_dcp())
st += '\nis disciplined concave-convex {}'.format(dccp.is_dccp(problem))
st += '\nis disciplined multi-convex {}'.format(dmcp.is_dmcp(problem))
# todo
# SQP sequential quadratic program
# SCP seperable convex program
#
return st
# prob = Problem(
# Minimize(
# cvx.max(
# cvx.max(cvx.abs(c), axis=1) + r # max dim = c_max + r
# )
# ),
# constr
# )
prob = Problem(
Minimize(
cvx.max(cvx.abs(c[:, 0]) + r) + cvx.max(cvx.abs(c[:, 1]) + r)),
constr)
print(prob.is_dcp(), prob.is_dcp())
print(dccp.is_dccp(prob))
prob.solve(method='dccp',
solver='ECOS',
ep=1e-2,
max_slack=1e-2,
verbose=True)
l = cvx.max(cvx.max(cvx.abs(c), axis=1) + r).value * 2
pi = np.pi
ratio = pi * cvx.sum(cvx.square(r)).value / cvx.square(l).value
print("ratio =", ratio)
# plot
plt.figure(figsize=(5, 5))
circ = np.linspace(0, 2 * pi)
x_border = [-l / 2, l / 2, l / 2, -l / 2, -l / 2]
def eisenberg_gale_convex_program(self, no_of_goods, no_of_buyers):
moneys = 21234 * numpy.random.rand(1, no_of_buyers)
utilities = Variable(no_of_buyers, 1)
goods_utilities = 10 * numpy.random.rand(no_of_goods, no_of_buyers)
goods_buyers = Variable(no_of_goods, no_of_buyers)
###############################################
#for i in xrange(3):
# money=Variable(numpy.random.randn())
# print money
# moneys[i]=money
#for i in xrange(3):
# utility=Variable(numpy.random.randn())
# print utility
# utilities[i]=utility
###############################################
eisenberg_gale_convex_function = 0.0
for i in xrange(no_of_buyers):
eisenberg_gale_convex_function += log(utilities[i, 0]) * moneys[0,
i]
print eisenberg_gale_convex_function
print "Curvature of Function:", eisenberg_gale_convex_function.curvature
print "Sign of Function:", eisenberg_gale_convex_function.sign
objective = Maximize(eisenberg_gale_convex_function)
print objective
constraints = []
print "Number of Goods:", no_of_goods
print "Number of Buyers:", no_of_buyers
print "Per unit Goods utilities matrix:"
print goods_utilities
print "Goods and buyers matrix:"
print goods_buyers
print "constraints:"
for i in xrange(no_of_buyers):
constraint = 0.0
print constraint
for j in xrange(no_of_goods):
constraint += goods_utilities[i, j] * abs(goods_buyers[i, j])
print constraint
print constraint.curvature
print constraint.sign
constraints.append(utilities[i] == constraint)
for j in xrange(no_of_buyers):
constraint = 0.0
for i in xrange(no_of_buyers):
constraint += abs(goods_buyers[i, j])
print constraint
print constraint.curvature
print constraint.sign
constraints.append(constraint <= 1)
for i in xrange(no_of_buyers):
for j in xrange(no_of_goods):
constraint = abs(goods_buyers[i, j])
print constraint
print constraint.curvature
print constraint.sign
constraints.append(constraint >= 0)
problem = Problem(objective, constraints)
print "====================================="
print "Installed Solvers:"
print "====================================="
print installed_solvers()
print "Is Problem DCCP:", dccp.is_dccp(problem)
print "Solver used is SCS"
args = problem.get_problem_data(SCS)
print "====================================="
print "CVXPY args:"
print "====================================="
print args
result = problem.solve(solver=SCS, verbose=True, method='dccp')
print "====================================="
print "Problem value:"
print "====================================="
print problem.value
print "====================================="
print "Result:"
print "====================================="
print result
print "====================================="
print "Utilities:"
print "====================================="
for i in xrange(no_of_buyers):
print utilities[i, 0].value
print "====================================="
print "Moneys:"
print "====================================="
for i in xrange(no_of_buyers):
print moneys[0, i]
print "====================================="
print "Per Buyer Good Allocation:"
print "====================================="
for i in xrange(no_of_buyers):
print "=================================="
print "Buyer:", i
print "=================================="
for j in xrange(no_of_goods):
print "Good:", j
print goods_buyers[i, j].value
print "====================================="
print "Per Good Utility for Each Buyer:"
print "====================================="
for i in xrange(no_of_buyers):
print "=================================="
print "Buyer:", i
print "=================================="
for j in xrange(no_of_goods):
print "Good:", j
print goods_utilities[i, j]
print "======================================"
print "Per Good Market Clearing Price (from Karush-Kuhn-Tucker conditions):"
print "======================================"
prices = []
for b in xrange(no_of_buyers):
for g in xrange(no_of_goods):
b1 = b
for g1 in xrange(no_of_goods):
per_buyer_objective = goods_utilities[
b1, g1] * goods_buyers[b1, g1]
#print "Per buyer objective:",per_buyer_objective.value
priceg = goods_utilities[g, b] * moneys[0, b] / (
per_buyer_objective.value)
prices.append(priceg)
for g in xrange(no_of_goods):
print "Good ", g, " Market Clearing Price From Eisenberg-Gale Convex Program:", prices[
g]
def limited_information_privacy_utility(rho: float,
lmbd: float,
P0: np.ndarray,
P1: np.ndarray,
R0: np.ndarray,
R1: np.ndarray,
initial_points: int = 1,
max_iterations: int = 30,
solver=cp.ECOS,
debug: bool = False,
pi0: np.ndarray = None):
""" Optimize the privacy-utility value function over the two policies
in the limited information setting
Parameters
----------
rho : float
Weight given to policy pi_1 (1-rho for policy pi_0)
lmbd : float
Weight given to the privacy term
P0, P1 : np.ndarray
Numpy matrices containing the transition probabilities for model M0 and M1
Each matrix should have dimensions |actions|x|states|x|states|
R0, R1 : np.ndarray
Numpy matrices containing the rewards for model M0 and M1
Each matrix should have dimensions |states|x|actions|
initial_points : int, optional
Number of initial random points to use to solve the concave problem.
Default value is 1.
max_iterations : int, optional
Maximum number of iterations. Should be larger than initial_points.
Default value is 30.
solver : cvxpy.Solver, optional
Solver used to solve the problem. Default solver is ECOS
debug : bool, optional
If true, prints the solver output.
pi0 : np.ndarray, optional
If a policy pi0 is provided, then we optimize over pi1
the problem max_{pi1} V(pi1) - lambda I_F(pi0,pi1).
In this case rho is set to 1 for simplicity.
Returns
-------
I_L : float
Inverse of the privacy level
xi1, xi0 : np.ndarray
Stationary distributions over states and actions achieving the best
level of utility-privacy
"""
# Sanity checks
P0, P1 = sanity_check_probabilities(P0, P1)
R0, R1 = sanity_check_rewards(R0, R1)
initial_points = int(initial_points) if initial_points >= 1 else 1
max_iterations = initial_points if initial_points > max_iterations else int(
max_iterations)
if rho < 0 or rho > 1:
raise ValueError('Rho should be in [0,1]')
if lmbd < 0:
raise ValueError('Lambda should be non-negative')
na = P0.shape[0]
ns = P1.shape[1]
if pi0 is not None:
_xi0, _ = compute_stationary_distribution(P0, pi0)
rho = 1
best_res, best_xi1, best_xi0 = np.inf, None, None
# Loop through initial points and return best result
i = 0
n = 0
while i == 0 or (i < initial_points and n < max_iterations):
n += 1
# Construct the problem to find minimum privacy
gamma = cp.Variable(1, nonneg=True)
xi0 = cp.Variable((ns, na), nonneg=True) if pi0 is None else _xi0
xi1 = cp.Variable((ns, na), nonneg=True)
kl_div_stationary_dis = 0
for s in range(ns):
kl_div_stationary_dis += cp.kl_div(cp.sum(
xi1[s, :]), cp.sum(xi0[s, :])) + cp.sum(xi1[s, :]) - cp.sum(
xi0[s, :])
objective = gamma - lmbd * kl_div_stationary_dis
# stationarity constraints
stationarity_constraint0 = 0
stationarity_constraint1 = 0
for a in range(na):
stationarity_constraint0 += xi0[:,
a].T @ (P0[a, :, :] - np.eye(ns))
stationarity_constraint1 += xi1[:,
a].T @ (P1[a, :, :] - np.eye(ns))
constraints = [stationarity_constraint1 == 0, cp.sum(xi1) == 1]
if pi0 is None:
constraints += [cp.sum(xi0) == 1, stationarity_constraint0 == 0]
# Privacy-utility constraints
privacy_utility_constraint = 0
for s in range(ns):
for y in range(ns):
privacy_utility_constraint += lmbd * (
cp.kl_div(xi1[s, :] @ P1[:, s, y], xi0[s, :] @ P0[:, s, y])
+ (xi1[s, :] @ P1[:, s, y]) - (xi0[s, :] @ P0[:, s, y]))
for a in range(na):
privacy_utility_constraint -= (
rho * xi1[s, a] * R1[s, a] +
(1 - rho) * xi0[s, a] * R0[s, a])
constraints += [privacy_utility_constraint <= gamma]
# Solve problem
problem = cp.Problem(cp.Minimize(objective), constraints)
if not dccp.is_dccp(problem):
raise Exception('Problem is not Concave with convex constraints!')
try:
result = problem.solve(method='dccp',
ccp_times=1,
verbose=debug,
solver=solver)
except Exception as err:
continue
# Check if results are better than previous ones
if result[0] is not None:
i += 1
if result[0] < best_res:
best_res, best_xi1, best_xi0 = result[0], xi1.value, \
xi0.value if pi0 is None else xi0
# Make sure to normalize the results
best_xi0 += eps
best_xi1 += eps
best_xi0 /= np.sum(best_xi0) if not np.isclose(np.sum(best_xi0), 0) else 1.
best_xi1 /= np.sum(best_xi1) if not np.isclose(np.sum(best_xi1), 0) else 1.
return best_res, best_xi1, best_xi0
def limited_information_privacy_lb(P0: np.ndarray,
P1: np.ndarray,
initial_points: int = 1,
max_iterations: int = 30,
solver=cp.ECOS,
debug=False):
""" Computes the policies that achieves the best level of privacy in the
limited information setting
Parameters
----------
P0, P1 : np.ndarray
Numpy matrices containing the transition probabilities for models M0 and M1
Each matrix should have dimensions |actions|x|states|x|states|
initial_points : int, optional
Number of initial random points to use to solve the concave problem.
Default value is 1.
max_iterations : int, optional
Maximum number of iterations. Should be larger than initial_points.
Default value is 30.
solver : cvxpy.Solver, optional
Solver used to solve the problem. Default solver is ECOS
debug : bool, optional
If true, prints the solver output.
Returns
-------
I_L : float
Inverse of the privacy level
xi1, xi0 : np.ndarray
Stationary distributions over states and actions achieving the best
level of privacy
"""
P0, P1 = sanity_check_probabilities(P0, P1)
initial_points = int(initial_points) if initial_points >= 1 else 1
max_iterations = initial_points if initial_points > max_iterations else int(
max_iterations)
na, ns = P0.shape[0], P0.shape[1]
best_res, best_xi1, best_xi0 = np.inf, None, None
# Compute KL divergences
I = compute_KL_divergence_models(P0, P1)
# Loop through initial points and return best result
i = 0
n = 0
while i == 0 or (i < initial_points and n < max_iterations):
n += 1
gamma = cp.Variable(1)
xi0 = cp.Variable((ns, na), nonneg=True)
xi1 = cp.Variable((ns, na), nonneg=True)
kl_div_statinary_dis = 0
for s in range(ns):
kl_div_statinary_dis += cp.entr(cp.sum(xi1[s, :]))
# stationarity constraints
stationarity_constraint = 0
for a in range(na):
stationarity_constraint += xi1[:, a].T @ (P1[a, :, :] - np.eye(ns))
constraints = [stationarity_constraint == 0, cp.sum(xi1) == 1]
# Privacy constraints
privacy_constraint = 0
for s in range(ns):
constraints += [cp.sum(xi0[s, :]) == 1]
for y in range(ns):
privacy_constraint += cp.kl_div(
xi1[s, :] @ P1[:, s, y], xi0[s, :] @ P0[:, s, y]) + (
xi1[s, :] @ P1[:, s, y]) - (xi0[s, :] @ P0[:, s, y])
constraints += [privacy_constraint <= gamma]
objective = gamma + kl_div_statinary_dis
# Solve problem
problem = cp.Problem(cp.Minimize(objective), constraints)
if not dccp.is_dccp(problem):
raise Exception('Problem is not Concave with convex constraints!')
try:
result = problem.solve(method='dccp',
ccp_times=1,
verbose=debug,
solver=solver)
except Exception as err:
continue
# Check if results are better than previous ones
if result[0] is not None:
i += 1
if result[0] < best_res:
best_res, best_xi1, best_xi0 = result[0], xi1.value, xi0.value
# Make sure to normalize the results
best_xi0 += eps
best_xi1 += eps
best_xi0 /= np.sum(best_xi0) if not np.isclose(np.sum(best_xi0), 0) else 1.
best_xi1 /= np.sum(best_xi1) if not np.isclose(np.sum(best_xi1), 0) else 1.
return best_res, best_xi1, best_xi0
def eisenberg_gale_convex_program(self,no_of_goods,no_of_buyers): moneys=21234*numpy.random.rand(1,no_of_buyers) utilities=Variable(no_of_buyers,1) goods_utilities=10*numpy.random.rand(no_of_goods,no_of_buyers) goods_buyers=Variable(no_of_goods,no_of_buyers) ############################################### #for i in xrange(3): # money=Variable(numpy.random.randn()) # print money # moneys[i]=money #for i in xrange(3): # utility=Variable(numpy.random.randn()) # print utility # utilities[i]=utility ############################################### eisenberg_gale_convex_function=0.0 for i in xrange(no_of_buyers): eisenberg_gale_convex_function += log(utilities[i,0])*moneys[0,i] print eisenberg_gale_convex_function print "Curvature of Function:",eisenberg_gale_convex_function.curvature print "Sign of Function:",eisenberg_gale_convex_function.sign objective=Maximize(eisenberg_gale_convex_function) print objective constraints=[] print "Number of Goods:",no_of_goods print "Number of Buyers:",no_of_buyers print "Per unit Goods utilities matrix:" print goods_utilities print "Goods and buyers matrix:" print goods_buyers print "constraints:" for i in xrange(no_of_buyers): constraint=0.0 print constraint for j in xrange(no_of_goods): constraint += goods_utilities[i,j]*abs(goods_buyers[i,j]) print constraint print constraint.curvature print constraint.sign constraints.append(utilities[i] == constraint) for j in xrange(no_of_buyers): constraint=0.0 for i in xrange(no_of_buyers): constraint += abs(goods_buyers[i,j]) print constraint print constraint.curvature print constraint.sign constraints.append(constraint <= 1) for i in xrange(no_of_buyers): for j in xrange(no_of_goods): constraint = abs(goods_buyers[i,j]) print constraint print constraint.curvature print constraint.sign constraints.append(constraint >= 0) problem=Problem(objective,constraints) print "=====================================" print "Installed Solvers:" print "=====================================" print installed_solvers() print "Is Problem DCCP:",dccp.is_dccp(problem) print "Solver used is SCS" args=problem.get_problem_data(SCS) print "=====================================" print "CVXPY args:" print "=====================================" print args result=problem.solve(solver=SCS,verbose=True,method='dccp') print "=====================================" print "Problem value:" print "=====================================" print problem.value print "=====================================" print "Result:" print "=====================================" print result print "=====================================" print "Utilities:" print "=====================================" for i in xrange(no_of_buyers): print utilities[i,0].value print "=====================================" print "Moneys:" print "=====================================" for i in xrange(no_of_buyers): print moneys[0,i] print "=====================================" print "Per Buyer Good Allocation:" print "=====================================" for i in xrange(no_of_buyers): print "==================================" print "Buyer:",i print "==================================" for j in xrange(no_of_goods): print "Good:",j print goods_buyers[i,j].value print "=====================================" print "Per Good Utility for Each Buyer:" print "=====================================" for i in xrange(no_of_buyers): print "==================================" print "Buyer:",i print "==================================" for j in xrange(no_of_goods): print "Good:",j print goods_utilities[i,j] print "======================================" print "Per Good Market Clearing Price (from Karush-Kuhn-Tucker conditions):" print "======================================" prices=[] for b in xrange(no_of_buyers): for g in xrange(no_of_goods): b1=b for g1 in xrange(no_of_goods): per_buyer_objective = goods_utilities[b1,g1] * goods_buyers[b1,g1] #print "Per buyer objective:",per_buyer_objective.value priceg = goods_utilities[g,b] * moneys[0,b] / (per_buyer_objective.value) prices.append(priceg) for g in xrange(no_of_goods): print "Good ",g," Market Clearing Price From Eisenberg-Gale Convex Program:",prices[g]
import ScalingAttack as sa
from PIL import Image
import cvxpy as cvx
import dccp
x = cvx.Variable(2)
y = cvx.Variable(2)
myprob = cvx.Problem(cvx.Maximize(cvx.norm(x - y, 2)),
[0 <= x, x <= 1, 0 <= y, y <= 1])
print("problem is DCP:", myprob.is_dcp()) # false
print("problem is DCCP:", dccp.is_dccp(myprob)) # true
result = myprob.solve(method='dccp')
print("x =", x.value)
print("y =", y.value)
print("cost value =", result[0])
'''
try:
src_img = Image.open("images/sheep.jpg")
tgt_img = Image.open("images/wolf_icon.jpg")
except IOError:
print("One of the files was not found.")
CR_red = sa.get_coefficients(src_img, tgt_img, "R", "R")
CR_green = sa.get_coefficients(src_img, tgt_img, "G", "R")
CR_blue = sa.get_coefficients(src_img, tgt_img, "B", "R")
CL_red = sa.get_coefficients(src_img, tgt_img, "R", "L")
CL_green = sa.get_coefficients(src_img, tgt_img, "G", "L")
CL_blue = sa.get_coefficients(src_img, tgt_img, "B", "L")
'''
def optimized_scenario(initial_vehicles: List[VehicleInfo],
epsilon: float,
it_max: int,
my: int,
a_ref: ndarray,
scenario: Scenario,
planning_problem: PlanningProblem,
W: np.matrix = None):
# Require
# x_0 initial state vector
# epsilon threshold
# it_max iteration limit
# my binary search iteration limit
# W weighted matrix
# a_ref reference area profile
# Ensure
# critical scenario S
# kappa_new <- 0
# kappa_old <- -inf
# it <- 0
# x_{0,curr} <- x_o
# while abs(kappa_new - kappa_old) >= epsilon and it < it_max do
# success <- true
# while abs(kappa_new - kappa_old) >= epsilon and success = true do
# kappa_old <- kappa_new
# x_{0,old} <- x_{0,curr}
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
# kappa_new <- kappa(S, a_ref, W)
# end while
# x_{0,s)^v <- binarySearch(x_{0,old}, x_{0,curr}, my)
# S <- updateScenario(S, x_{0,s}^v)
# kappa_new <- kappa(S, a_ref, W)
# it <- it + 1
# end while
p: int = len(initial_vehicles)
r: int = len(MyState.variables)
total_steps: int = len(a_ref)
if not W:
W = eye(p)
kappa_new: float = 0
kappa_old: float = -np.inf
it: int = 0
current_initial_vehicles = initial_vehicles
while abs(kappa_new - kappa_old) >= epsilon and it < it_max:
success: bool = True
while abs(kappa_new - kappa_old) >= epsilon and success:
kappa_old = kappa_new
old_initial_vehicles: List[VehicleInfo] = current_initial_vehicles
current_generated_vehicles, _ = GenerationHelp.generate_states(
scenario, current_initial_vehicles[0].state,
total_steps) # FIXME Only ego vehicle considered
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
B: matrix = calculate_B_as_matrix(old_initial_vehicles,
current_initial_vehicles,
scenario, total_steps)
W_tilde: matrix = np.asmatrix(B.transpose() * W * B)
delta_a0: matrix = np.asmatrix(
calculate_area_profile(
flatten_dict_values(current_generated_vehicles)) - a_ref)
c: ndarray = delta_a0.transpose() * (W * B + W.transpose() * B)
delta_x: Variable = Variable((1, r))
objective: Minimize = Minimize(
cvxpy.norm(quad_form(delta_x, W_tilde) + c * delta_x))
# Instead of the "real" goal region constraint use a minimum velocity needed for getting to the goal region
# within the given amount of steps
# FIXME Only ego vehicle considered
current_initial_position: Point = Point(
current_initial_vehicles[0].state.state.position)
# FIXME Only first goal region entry recognized
goal_region: Polygon = planning_problem.goal.state_list[
0].position.shapely_object
distance_to_goal_region: float = current_initial_position.distance(
goal_region)
min_velocity: float = distance_to_goal_region / (scenario.dt *
total_steps)
print("min_velocity: " + str(min_velocity))
# FIXME Really use delta_a0?
# FIXME If min_velocity is greater than zero the optimization fails
constraints = [
delta_x >= 0, delta_a0 + B * delta_x >= 0,
delta_x[0] >= min_velocity
]
problem = Problem(objective, constraints)
assert is_dccp(problem)
print("Problem solve result: " +
str(problem.solve(method='dccp', solver='ECOS')))
print("Current optimization result: " + str(delta_x.value))
# FIXME Change current initial vehicles like this?
for i in range(len(current_initial_vehicles)):
for j in range(len(MyState.variables)):
# FIXME Only ego vehicle considered
current_initial_vehicles[i].state.set_variable(
j, delta_x.value[i][j])
kappa_new = kappa(delta_a0, a_ref, W) # FIXME Really use delta_a0?
print("Difference between new and old costs: " +
str(abs(kappa_new - kappa_old)))
binary_search(my, old_initial_vehicles, current_initial_vehicles,
scenario,
total_steps) # FIXME What to do with this value?
# initial_vehicles = ? # FIXME What to do here?
update_scenario_vehicles(scenario, planning_problem, initial_vehicles)
kappa_new = kappa(calculate_area_profile(initial_vehicles), a_ref, W)
it += 1