def generate_bartelsconn(n=2, func_opts={}, data_type=cs.SX):
if n != 2:
raise ValueError("bartelsconn is only defined for n=2")
x = data_type.sym("x", n)
f = cs.norm_1(cs.sumsqr(x) + x[0] * x[1])
f += cs.norm_1(cs.sin(x[0]))
f += cs.norm_1(cs.cos(x[1]))
func = cs.Function("bartelsconn", [x], [f], ["x"], ["f"], func_opts)
return func, [[-500., 500.]] * n, [[0.] * n]
def compute_matrices(self, variables_dot, variables0):
for priority in self.constraints:
constraints = self.constraints[priority]
if 'lub' in constraints:
A_vals = constraints['lub']['A_fun'](variables_dot, variables0)
lb_vals = constraints['lub']['lb_fun'](variables_dot,
variables0)
ub_vals = constraints['lub']['ub_fun'](variables_dot,
variables0)
for i in range(A_vals.shape[0]):
row_vec_norm = cs.norm_1(A_vals[i, :])
lb_vals[i] /= row_vec_norm
ub_vals[i] /= row_vec_norm
for j in range(A_vals.shape[1]):
A_vals[i, j] /= row_vec_norm
constraints['lub']['A_vals'] = A_vals.full()
constraints['lub']['lb_vals'] = lb_vals.full().ravel()
constraints['lub']['ub_vals'] = ub_vals.full().ravel()
if 'ub' in constraints:
A_vals = constraints['ub']['A_fun'](variables_dot, variables0)
ub_vals = constraints['ub']['ub_fun'](variables_dot,
variables0)
for i in range(A_vals.shape[0]):
row_vec_norm = cs.norm_1(A_vals[i, :])
ub_vals[i] /= row_vec_norm
for j in range(A_vals.shape[1]):
A_vals[i, j] /= row_vec_norm
constraints['ub']['A_vals'] = A_vals.full()
constraints['ub']['ub_vals'] = ub_vals.full().ravel()
if 'eq' in constraints:
A_vals = constraints['eq']['A_fun'](variables_dot, variables0)
b_vals = constraints['eq']['b_fun'](variables_dot, variables0)
for i in range(A_vals.shape[0]):
row_vec_norm = cs.norm_1(A_vals[i, :])
b_vals[i] /= row_vec_norm
for j in range(A_vals.shape[1]):
A_vals[i, j] /= row_vec_norm
constraints['eq']['A_vals'] = A_vals.full()
constraints['eq']['b_vals'] = b_vals.full().ravel()
def information_gain():
opti = casadi.Opti()
B = 0.5
ui = 0
u = [] #This is the initial training point
N = len(
u
) + 1 #The amount of previous training data (so we see if we are actually learning anything new). This is not usually a constant
I = 0
for i in range(0, N):
u.append(opti.variable()) #uk
if i != 0:
I += casadi.norm_1(u[-1] - u[-2])
else:
I += casadi.norm_1(u[-1] - ui)
def generate_alpinen1(n=2, a=0.1, func_opts={}, data_type=cs.SX):
x = data_type.sym("x", n)
f = 0.
for i in range(n):
f += cs.norm_1(x[i] * cs.sin(x[i]) + a * x[i])
func = cs.Function("alpinen1", [x], [f], ["x"], ["f"], func_opts)
return func, [[0., 10.]] * n, [[0.] * n]
# hqp.time_taken = 0
print("iter :" + str(i))
print(hlp.Mw_dict[4]['eq_b'].getValue())
if not sequential_method:
hlp.solve_weighted_method([150,150,150,150])
var_dot_sol = hlp.Mw_dict['x'].level()
# sol = hqp.solve_HQPl1(q_opt, q_dot_opt, gamma_init = 10.0)
q_dot1_sol = var_dot_sol[0:7]
q_dot2_sol = var_dot_sol[7:14]
s_dot1_sol = var_dot_sol[14]
s_dot2_sol = var_dot_sol[15]
q_torso_dot_sol = var_dot_sol[16]
con_viol1 = cs.norm_1(hlp.Mw_dict[1]['eq_slack'].level()) + cs.norm_1(hlp.Mw_dict[1]['ub_slack'].level())
con_viol2 = cs.norm_1(hlp.Mw_dict[2]['eq_slack'].level())
con_viol3 = cs.norm_1(hlp.Mw_dict[3]['eq_slack'].level())
con_viol4 = cs.norm_1(hlp.Mw_dict[4]['eq_slack'].level())
# con_viols = sol.value(cs.vertcat(hqp.slacks[1], hqp.slacks[2], hqp.slacks[3], hqp.slacks[4]))
constraint_violations = cs.horzcat(constraint_violations, cs.vertcat(con_viol1, con_viol2, con_viol3, con_viol4))
# enablePrint()
# sol_time = hlp.Mw.getSolverDoubleInfo("optimizerTime")
sol_time = hlp.Mw.getSolverDoubleInfo("simPrimalTime") + hlp.Mw.getSolverDoubleInfo("simDualTime")
simplex_iters.append(hlp.Mw.getSolverIntInfo("simPrimalIter") + hlp.Mw.getSolverIntInfo("simDualIter"))
# print("Solver time is = " + str(sol_time))
# print("Solver time is = " + str(hlp.Mw.getSolverDoubleInfo("simTime")))
# print("q_torso_dot = "+str(q_torso_dot_sol))
comp_time.append(sol_time)
# blockPrint()
obj.setController(kukaID, "velocity", joint_indices, targetVelocities = q_vel_current1)
q_vel_current2 = 0.5*(q_dot_sol2[i] + q_dot_sol2[i+1])
obj.setController(kukaID2, "velocity", joint_indices, targetVelocities = q_vel_current2)
obj.run_simulation(no_samples)
obj.setController(kukaID, "velocity", joint_indices, targetVelocities = q_dot_sol1[-1])
obj.setController(kukaID2, "velocity", joint_indices, targetVelocities = q_dot_sol2[-1])
obj.run_simulation(100)
obj.end_simulation()
t_s, obs_avoidance = sol.sample(cs.fmax(dist_ees, 0), grid="control")
t_s, stay_on_path = sol.sample((cs.vertcat(stay_path1, stay_path2)), grid="control")
stay_on_path_norm = []
for i in range(len(t_s)):
stay_on_path_norm.append(cs.norm_1(stay_on_path[i, :]))
t_s, s_1_sol = sol.sample(cs.fabs(s_1-1), grid = 'control')
t_s, s_2_sol = sol.sample(cs.fabs(s_2-1), grid = 'control')
figure()
plot(t_s, obs_avoidance, label = 'obstacle avoidance (2)')
plot(t_s, stay_on_path_norm, label = 'stay on path constraint (3)')
plot(t_s, s_2_sol, label = 'distance from goal robot 1 (4)')
plot(t_s, s_1_sol, label = 'distance from goal robot 2 (5)')
# plot(list(range(counter)), constraint_violations[3,:].full().T, label = '5th priority')
title("Constraint violations")
xlabel("Time step (Control sampling time = 0.005s)")
legend()
show(block=True)
def hqpvschqp(params):
result = {}
n = params['n']
total_eq_constraints = params['total_eq_constraints']
total_ineq_constraints = params['total_ineq_constraints']
pl = params['pl'] #priority levels
gamma = params['gamma']
rand_seed = params['rand_seed']
adaptive_method = params['adaptive_method'] #True
n_eq_per_level = int(total_eq_constraints / pl)
eq_last_level = n_eq_per_level + (total_eq_constraints -
n_eq_per_level * pl)
n_ineq_per_level = int(total_ineq_constraints / pl)
ineq_last_level = n_ineq_per_level + (total_ineq_constraints -
n_ineq_per_level * pl)
# print(n_eq_per_level)
# print(n_ineq_per_level)
# print(eq_first_level)
# print(ineq_first_level)
count_hierarchy_failue = 0
count_same_constraints = 0
count_identical_solution = 0
count_geq_constraints = 0
hlp = lexopt_mosek()
print("Using random seed " + str(rand_seed))
# n_eq_per_level = 6
# n_ineq_per_level = 6
np.random.seed(
rand_seed) #for 45, 1, 8 HQP-l1 does not agree with the cHQP
#15, 18, 11 for 8 priority levels
x, x_dot = hlp.create_variable(n, [-1e6] * n, [1e6] *
n) #high enough bounds to not matter
A_eq_all = cs.DM(np.random.randn(params['eq_con_rank'], n))
A_extra = (A_eq_all.T @ cs.DM(
np.random.randn(params['eq_con_rank'],
total_eq_constraints - params['eq_con_rank']))).T
A_eq_all = cs.vertcat(A_eq_all, A_extra)
A_eq_all = A_eq_all.full()
np.random.shuffle(A_eq_all)
A_eq_all += np.random.randn(total_eq_constraints, n) * 1e-5
b_eq_all = np.random.randn(total_eq_constraints)
#normalizing the each row vector
row_vec_norm = []
for i in range(A_eq_all.shape[0]):
row_vec_norm.append(cs.norm_1(A_eq_all[i, :]))
# print(row_vec_norm)
b_eq_all[i] /= row_vec_norm[i]
for j in range(A_eq_all.shape[1]):
A_eq_all[i, j] = A_eq_all[i, j].T / row_vec_norm[i]
A_ineq_all = cs.DM(np.random.randn(params['ineq_con_rank'], n))
A_ineq_extra = (A_ineq_all.T @ cs.DM(
np.random.randn(params['ineq_con_rank'],
total_ineq_constraints - params['ineq_con_rank']))).T
A_ineq_all = cs.vertcat(A_ineq_all, A_ineq_extra)
A_ineq_all = A_ineq_all.full()
np.random.shuffle(A_ineq_all)
b_ineq_all = np.random.randn(total_ineq_constraints)
b_ineq_all_lower = b_ineq_all - 1
# print(b_ineq_all)
# print(b_ineq_all_lower)
#normalizing the each row vector
row_ineqvec_norm = []
for i in range(A_ineq_all.shape[0]):
row_ineqvec_norm.append(cs.norm_1(A_ineq_all[i, :]))
b_ineq_all[i] /= row_ineqvec_norm[i]
b_ineq_all_lower[i] /= row_ineqvec_norm[i]
for j in range(A_ineq_all.shape[1]):
A_ineq_all[i, j] = A_ineq_all[i, j] / row_ineqvec_norm[i]
A_eq = {}
b_eq = {}
A_eq_opti = {}
b_eq_opti = {}
A_ineq_opti = {}
b_upper_opti = {}
A_ineq = {}
b_upper = {}
b_lower = {}
params = x
params_init = [0] * n
#create these tasks
counter_eq = 0
counter_ineq = 0
# print(row_ineqvec_norm)
for i in range(pl):
if i != pl - 1:
A_eq[i] = A_eq_all[counter_eq:counter_eq + n_eq_per_level, :]
b_eq[i] = b_eq_all[counter_eq:counter_eq + n_eq_per_level]
counter_eq += n_eq_per_level
hlp.create_constraint(
cs.mtimes(A_eq[i], x_dot) - b_eq[i], 'eq', i, {'b': 0})
A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq +
n_ineq_per_level, :]
b_upper[i] = b_ineq_all[counter_ineq:counter_ineq +
n_ineq_per_level]
b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq +
n_ineq_per_level]
counter_ineq += n_ineq_per_level
hlp.create_constraint(A_ineq[i] @ x_dot, 'lub', i, {
'lb': b_lower[i],
'ub': b_upper[i]
})
else:
A_eq[i] = A_eq_all[counter_ineq:counter_ineq + eq_last_level, :]
b_eq[i] = b_eq_all[counter_eq:counter_eq + eq_last_level]
counter_eq += eq_last_level
hlp.create_constraint(
cs.mtimes(A_eq[i], x_dot) - b_eq[i], 'eq', i, {'b': 0})
A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq +
ineq_last_level, :]
b_upper[i] = b_ineq_all[counter_ineq:counter_ineq +
ineq_last_level]
b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq +
ineq_last_level]
counter_ineq += ineq_last_level
hlp.create_constraint(A_ineq[i] @ x_dot, 'lub', i, {
'lb': b_lower[i],
'ub': b_upper[i]
})
hlp.configure_constraints()
hlp.compute_matrices([0] * n, [0] * n)
hlp.configure_weighted_problem()
hlp.configure_sequential_problem()
try:
hlp.solve_sequential_problem()
result['slp_time'] = hlp.time_taken
result['slp_status'] = True
except:
result['slp_status'] = False
if not adaptive_method:
hlp.time_taken = 0
sol = hlp.solve_weighted_method([gamma] * (pl - 2))
else:
# tic = time.time()
sol, hierarchical_failure = hqp.solve_adaptive_hqp3(params_init,
[0] * n,
gamma_init=gamma)
# toc = time.time() - tic
tp = 0 #true positive
fp = 0
tn = 0
fn = 0
hlp_status = True
result['hlp_status'] = hlp_status
if not hlp_status:
print("hlp-l1 failed")
else:
result['hlp_time'] = hlp.time_taken
if adaptive_method:
result['heuristic_prediction'] = len(hierarchical_failure) == 0
# if not adaptive_method:
# return False, False, False, True, False, False
# else:
# return False, False, False, True, False, False, False
#further analysis and comparison only if both hqp and chqp returned without failing
lex_opt = True
if hlp_status and result['slp_status']:
for i in range(1, pl - 1):
weighted_obj = sum(hlp.Mw_dict[i]['eq_slack'].level()) + sum(
hlp.Mw_dict[i]['lub_slack'].level())
sequential_obj = hlp.optimal_slacks[i]
if weighted_obj > sequential_obj + 1e-5:
lex_opt = False
if verbose:
print("Lex norm unsatisfied!!!!!!!!!!!!!!!!! by " +
str(weighted_obj - sequential_obj))
result['lex_opt'] = lex_opt
return result
if not sequential_method:
sol, hierarchy_failure = hqp.solve_adaptive_hqp3(q_opt,
q_dot_opt,
gamma_init=0.5,
iter_lim=5)
q_dot1_sol = sol.value(q_dot1)
s_dot1_sol = sol.value(s_dot1)
# sol = hqp.solve_HQPl1(q_opt, q_dot_opt, gamma_init = 10.0)
# q_dot1_sol = var_dot_sol[0:14]
# s_dot1_sol = var_dot_sol[14]
con_viols = sol.value(
cs.vertcat(hqp.slacks[1], hqp.slacks[2], hqp.slacks[3]))
constraint_violations = cs.horzcat(
constraint_violations,
cs.vertcat(cs.norm_1(sol.value(hqp.slacks[1])),
cs.norm_1(sol.value(hqp.slacks[2])),
cs.norm_1(sol.value(hqp.slacks[3]))))
print(hqp.time_taken)
comp_time.append(hqp.time_taken)
#sol = hqp.solve_adaptive_hqp2(q_opt, q_dot_opt, gamma_init = 0.2)
if sequential_method:
hqp.solve_sequential_problem()
var_dot_sol = hqp.Mdict_seq[5]['x'].level()
q_dot1_sol = var_dot_sol[0:14]
s_dot1_sol = var_dot_sol[14]
print("Solver time is = " + str(hqp.time_taken))
comp_time.append(hqp.time_taken)
con_viol1 = hqp.optimal_slacks[1]
def hqpvschqp(params):
result = {}
n = params['n']
total_eq_constraints = params['total_eq_constraints']
total_ineq_constraints = params['total_ineq_constraints']
pl = params['pl'] #priority levels
gamma = params['gamma']
rand_seed = params['rand_seed']
adaptive_method = params['adaptive_method'] #True
n_eq_per_level = int(total_eq_constraints / pl)
eq_last_level = n_eq_per_level + (total_eq_constraints -
n_eq_per_level * pl)
n_ineq_per_level = int(total_ineq_constraints / pl)
ineq_last_level = n_ineq_per_level + (total_ineq_constraints -
n_ineq_per_level * pl)
# print(n_eq_per_level)
# print(n_ineq_per_level)
# print(eq_first_level)
# print(ineq_first_level)
count_hierarchy_failue = 0
count_same_constraints = 0
count_identical_solution = 0
count_geq_constraints = 0
hqp = hqp_p.hqp()
print("Using random seed " + str(rand_seed))
# n_eq_per_level = 6
# n_ineq_per_level = 6
np.random.seed(
rand_seed) #for 45, 1, 8 HQP-l1 does not agree with the cHQP
#15, 18, 11 for 8 priority levels
x, x_dot = hqp.create_variable(n, 1e-12)
A_eq_all = cs.DM(np.random.randn(params['eq_con_rank'], n))
A_extra = (A_eq_all.T @ cs.DM(
np.random.randn(params['eq_con_rank'],
total_eq_constraints - params['eq_con_rank']))).T
A_eq_all = cs.vertcat(A_eq_all, A_extra)
A_eq_all = A_eq_all.full()
np.random.shuffle(A_eq_all)
A_eq_all += np.random.randn(total_eq_constraints, n) * 1e-5
b_eq_all = np.random.randn(total_eq_constraints)
#normalizing the each row vector
row_vec_norm = []
for i in range(A_eq_all.shape[0]):
row_vec_norm.append(cs.norm_1(A_eq_all[i, :]))
# print(row_vec_norm)
b_eq_all[i] /= row_vec_norm[i]
for j in range(A_eq_all.shape[1]):
A_eq_all[i, j] = A_eq_all[i, j].T / row_vec_norm[i]
A_ineq_all = cs.DM(np.random.randn(params['ineq_con_rank'], n))
A_ineq_extra = (A_ineq_all.T @ cs.DM(
np.random.randn(params['ineq_con_rank'],
total_ineq_constraints - params['ineq_con_rank']))).T
A_ineq_all = cs.vertcat(A_ineq_all, A_ineq_extra)
A_ineq_all = A_ineq_all.full()
np.random.shuffle(A_ineq_all)
b_ineq_all = np.random.randn(total_ineq_constraints)
b_ineq_all_lower = b_ineq_all - 1
# print(b_ineq_all)
# print(b_ineq_all_lower)
#normalizing the each row vector
row_ineqvec_norm = []
for i in range(A_ineq_all.shape[0]):
row_ineqvec_norm.append(cs.norm_1(A_ineq_all[i, :]))
b_ineq_all[i] /= row_ineqvec_norm[i]
b_ineq_all_lower[i] /= row_ineqvec_norm[i]
for j in range(A_ineq_all.shape[1]):
A_ineq_all[i, j] = A_ineq_all[i, j] / row_ineqvec_norm[i]
A_eq = {}
b_eq = {}
A_eq_opti = {}
b_eq_opti = {}
A_ineq_opti = {}
b_upper_opti = {}
A_ineq = {}
b_upper = {}
b_lower = {}
params = x
params_init = [0] * n
#create these tasks
counter_eq = 0
counter_ineq = 0
# print(row_ineqvec_norm)
for i in range(pl):
if i != pl - 1:
A_eq[i] = A_eq_all[counter_eq:counter_eq + n_eq_per_level, :]
b_eq[i] = b_eq_all[counter_eq:counter_eq + n_eq_per_level]
counter_eq += n_eq_per_level
hqp.create_constraint(cs.mtimes(A_eq[i], x_dot) - b_eq[i],
'equality',
priority=i)
A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq +
n_ineq_per_level, :]
b_upper[i] = b_ineq_all[counter_ineq:counter_ineq +
n_ineq_per_level]
b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq +
n_ineq_per_level]
counter_ineq += n_ineq_per_level
hqp.create_constraint(A_ineq[i] @ x_dot,
'lub',
priority=i,
options={
'lb': b_lower[i],
'ub': b_upper[i]
})
else:
A_eq[i] = A_eq_all[counter_ineq:counter_ineq + eq_last_level, :]
b_eq[i] = b_eq_all[counter_eq:counter_eq + eq_last_level]
counter_eq += eq_last_level
hqp.create_constraint(cs.mtimes(A_eq[i], x_dot) - b_eq[i],
'equality',
priority=i)
A_ineq[i] = A_ineq_all[counter_ineq:counter_ineq +
ineq_last_level, :]
b_upper[i] = b_ineq_all[counter_ineq:counter_ineq +
ineq_last_level]
b_lower[i] = b_ineq_all_lower[counter_ineq:counter_ineq +
ineq_last_level]
counter_ineq += ineq_last_level
hqp.create_constraint(A_ineq[i] @ x_dot,
'lub',
priority=i,
options={
'lb': b_lower[i],
'ub': b_upper[i]
})
# print(A_eq)
# p_opts = {"expand":True}
# s_opts = {"max_iter": 100, 'tol':1e-8}#, 'hessian_approximation':'limited-memory', 'limited_memory_max_history' : 5, 'tol':1e-6}
# hqp.opti.solver('ipopt', p_opts, s_opts)
qpsol_options = {'error_on_fail': False}
hqp.opti.solver(
"sqpmethod", {
"expand": True,
"qpsol": 'qpoases',
'qpsol_options': qpsol_options,
'print_iteration': False,
'print_header': True,
'print_status': True,
"print_time": True,
'max_iter': 1000
})
blockPrint()
hqp.variables0 = params
hqp.configure()
# hqp.setup_cascadedQP()
sol_chqp = hqp.solve_cascadedQP5(params_init, [0] * n)
enablePrint()
chqp_status = sol_chqp != False
result['chqp_status'] = chqp_status
if not chqp_status:
print("cHQP failed")
else:
result['chqp_time'] = hqp.time_taken
print("Time taken by chqp = " + str(hqp.time_taken))
# if not adaptive_method:
# return False, False, False, False, False, False
# else:
# return False, False, False, False, False, False, False
blockPrint()
if not adaptive_method:
hqp.time_taken = 0
sol = hqp.solve_HQPl1(params_init, [0] * n, gamma_init=gamma)
enablePrint()
print("Time taken by the non-adaptive method = " + str(hqp.time_taken))
else:
# tic = time.time()
sol, hierarchical_failure = hqp.solve_adaptive_hqp3(params_init,
[0] * n,
gamma_init=gamma)
# toc = time.time() - tic
tp = 0 #true positive
fp = 0
tn = 0
fn = 0
hqp_status = sol != False
enablePrint()
# print("Total time taken adaptive HQP = " + str(toc))
result['hqp_status'] = hqp_status
if not hqp_status:
print("hqp-l1 failed")
else:
result['hqp_time'] = hqp.time_taken
if adaptive_method:
result['heuristic_prediction'] = len(hierarchical_failure) == 0
# if not adaptive_method:
# return False, False, False, True, False, False
# else:
# return False, False, False, True, False, False, False
#further analysis and comparison only if both hqp and chqp returned without failing
if hqp_status and chqp_status:
x_dot_sol = sol.value(x_dot)
# print(x_dot_sol)
con_viol2 = []
# x_dot_sol2 = sol_chqp[pl - 1].value(x_dot)
x_dot_sol2 = sol_chqp[pl - 1].value(hqp.cHQP_xdot[pl - 1])
# print(x_dot_sol2)
con_viol = []
geq_constraints_satisfied = True
same_constraints_satisfied = True
lex_con_norm = True
verbose = False
running_counter_satisfied_con_hqp = 0
running_counter_satisfied_con_chqp = 0
for i in range(1, pl):
sol_hqp = sol.value(hqp.slacks[i])
obj_sol_hqp = sum(list(sol_hqp)) + 0.1 * cs.sumsqr(sol_hqp)
con_viol.append(cs.norm_1(sol_hqp))
# sol_cHQP = sol_chqp[pl - 1].value(hqp.slacks[i])
sol_cHQP = sol_chqp[i].value(hqp.cHQP_slacks[i])
obj_sol_chqp = sum(list(sol_cHQP)) + 0.1 * cs.sumsqr(sol_cHQP)
con_viol2.append(cs.norm_1(sol_cHQP))
#Computing which constraints are satisfied
satisfied_con_hqp = sol_hqp <= 1e-8
satisfied_con_chqp = sol_cHQP <= 1e-8
#computing whether the same constriants are satisfied
if (satisfied_con_hqp != satisfied_con_chqp).any():
same_constraints_satisfied = False
#compute if a geq number of constraints are satisfied by the hqp
running_counter_satisfied_con_chqp += sum(satisfied_con_chqp)
running_counter_satisfied_con_hqp += sum(satisfied_con_hqp)
if running_counter_satisfied_con_hqp < running_counter_satisfied_con_chqp:
geq_constraints_satisfied = False
# if cs.norm_1(sol_hqp) > cs.norm_1(sol_cHQP) + 1e-4:
if obj_sol_hqp > obj_sol_chqp + 1e-4:
lex_con_norm = False
if verbose:
print("Lex norm unsatisfied!!!!!!!!!!!!!!!!!")
if verbose: #make true if print
print("Level hqp-l1" + str(i))
print(sol_hqp)
print("Level chqp" + str(i))
print(sol_cHQP)
print(satisfied_con_hqp)
print(satisfied_con_chqp)
if verbose:
print(x_dot_sol)
print(x_dot_sol2)
print(hqp.slack_weights)
if verbose:
print(con_viol)
print(con_viol2)
# print("diff between solutions = " + str(max(cs.fabs(x_dot_sol - x_dot_sol2).full())))
identical_solution = max(
cs.fabs(x_dot_sol - x_dot_sol2).full()) <= 1e-4
if identical_solution:
# print("Identical solution by both methods!!")
count_identical_solution += 1
count_geq_constraints += 1
count_same_constraints += 1
if adaptive_method:
if len(hierarchical_failure) > 0:
fp += 1
else:
tn += 1
elif same_constraints_satisfied:
# print("same constraints are satisfied")
count_same_constraints += 1
count_geq_constraints += 1
if adaptive_method:
if len(hierarchical_failure) > 0:
fp += 1
else:
tn += 1
elif geq_constraints_satisfied or lex_con_norm:
# print("The same of greater number of constriants satisfied at each level")
count_geq_constraints += 1
if adaptive_method:
if len(hierarchical_failure) > 0:
fp += 1
else:
tn += 1
else:
# print("hierarchy failed!!!!!!!!!!!!!!!!")
count_hierarchy_failue += 1
if adaptive_method:
if len(hierarchical_failure) > 0:
tp += 1
else:
fn += 1
if verbose:
print("hierarchy failed " + str(count_hierarchy_failue))
print("Identical solution " + str(count_identical_solution))
print("Same constraints satisfied " + str(count_same_constraints))
print("Geq constraints satisfied " + str(count_geq_constraints))
result['identical_solution'] = identical_solution[0]
result['same_constraints_satisfied'] = same_constraints_satisfied
result['geq_constraints_satisfied'] = geq_constraints_satisfied
result['lex_con_norm'] = lex_con_norm
if adaptive_method:
result['heuristic_predictor'] = {
'tn': tn,
'tp': tp,
'fp': fp,
'fn': fn
}
return result
