def __init__(self, log=None, steepest_descent=None):
Solver.__init__(self, log)
self.current_solution = None
self.current_neighbors = None
self.neighbors_checked = None
self.best_neighbor = None
self.steepest_descent = (steepest_descent if steepest_descent is not None
else self.default_steepest_descent)
self.cpu_limit = None
self.init_params = None
def __init__(self, hl_params = None, hl_actions=None):
if hl_params is None:
self.hl_params = []
else:
self.hl_params = hl_params
if hl_actions is None:
self.hl_actions = []
else:
self.hl_actions = hl_actions
self.solver = Solver()
def __init__(self, **params):
Solver.__init__(self, **params)
self.status.fields.append(Field.TreeDims)
self.root = None
def solve_at_priority_regular(self, priority, fix_sampled_params=False, recently_sampled=None):
# initialize gurobi Model object
prob = OptProb()
self.recently_converged_vio_fluent = None
if recently_sampled is None:
recently_sampled = []
params = set()
for hla in self.hlas:
params.update(hla.get_params())
prob.add_hla(hla)
# check whether there are duplicate parameters (prob shouldn't add variables into the problem multiple times)
param_to_var = {}
for param in params:
if param in param_to_var:
continue
if param.is_resampled and fix_sampled_params:
print param.name, "is fixed."
const = Constant(param)
param_to_var[param] = const
elif not param.is_var:
print param.name, "is not a var."
const = Constant(param)
param_to_var[param] = const
else: # param.is_var
var = Variable(param, recently_sampled=(param in recently_sampled))
param_to_var[param] = var
prob.add_var(var)
prob.update()
# max(priority, 0) because priority = -1 used for straight-line init
self.add_cnts_to_opt_prob(prob, param_to_var=param_to_var, priority=max(priority, 0))
# Re-sampled params need to update their corresponding variables
for param, var in param_to_var.items():
var.set_val(param.get_value())
for hla in self.hlas:
if hla.cost != 0.0:
hla.cost.to_gurobi_fn(prob, param_to_var)
prob.inc_obj(hla.cost)
solver = Solver()
if priority == -1 or priority == 0:
# for initialization only because problem should be QP
solver.initial_trust_box_size = 1e5
solver.max_merit_coeff_increases = 1
if priority == -1:
# initialize straight-line trajectories
success = prob.initialize_traj(mode="straight")
elif priority == 0:
# initialize from adapted previous trajectories
success = prob.initialize_traj(mode=settings.INIT_MODE)
else:
if settings.DO_SQP or settings.BACKTRACKING_REFINEMENT or settings.BTSMOOTH_REFINEMENT:
do_early_converge = False
elif settings.DO_EARLY_CONVERGE:
do_early_converge = True
else:
raise NotImplementedError
success, converged_vio_fluent = solver.penalty_sqp(prob, do_early_converge=do_early_converge)
if not do_early_converge:
assert converged_vio_fluent is None
self.recently_converged_vio_fluent = converged_vio_fluent
for param, var in param_to_var.items():
var.update_hl_param()
self.traj_cost = sum(prob.val(0)[0])
return success
def solve_at_priority_admm(self, priority, fix_sampled_params=False, recently_sampled=None):
# initialize gurobi Model object
if recently_sampled is None:
recently_sampled = []
hla_to_prob = {}
param_to_var = {}
# construct opt probs for each action and create variables for each parameter
for hla in self.hlas:
prob = OptProb()
prob.add_hla(hla)
hla_to_prob[hla] = prob
params = hla.get_params()
for param in params:
if param.is_resampled and fix_sampled_params:
if param not in param_to_var:
# print param.name, "is fixed."
const = Constant(param)
param_to_var[param] = const
elif not param.is_var:
if param not in param_to_var:
# print param.name, "is not a var."
const = Constant(param)
param_to_var[param] = const
else: # param.is_var
if param in param_to_var:
var = param_to_var[param]
else:
if param.is_resampled:
# print param.name, "is a global var."
var = GlobalVariable(param, recently_sampled=(param in recently_sampled))
param_to_var[param] = var
else:
# print param.name, "is a var."
var = Variable(param, recently_sampled=(param in recently_sampled))
param_to_var[param] = var
prob.add_var(var)
prob.update()
self.add_cnts_to_opt_prob(prob, param_to_var=param_to_var, priority=max(priority, 0))
for hla, prob in hla_to_prob.items():
if hla.cost != 0.0:
hla.cost.to_gurobi_fn(prob, param_to_var)
prob.inc_obj(hla.cost)
solver = Solver()
if priority == -1 or priority == 0:
# for initialization only because problem should be QP
solver.initial_trust_box_size = 1e5
solver.max_merit_coeff_increases = 1
if priority == -1:
# initialize straight-line trajectories
for prob in hla_to_prob.values():
success = prob.initialize_traj(mode="straight")
elif priority == 0:
# initialize from adapted previous trajectories
for prob in hla_to_prob.values():
success = prob.initialize_traj(mode="adapt")
else:
success = solver.sqp_qp_admm(hla_to_prob.values(), param_to_var.values())
for param, var in param_to_var.items():
var.update_hl_param()
self.traj_cost = 0
for prob in hla_to_prob.values():
self.traj_cost += sum(prob.val(0)[0])
return success
class LLPlan(object):
def __init__(self, hl_params = None, hl_actions=None):
if hl_params is None:
self.hl_params = []
else:
self.hl_params = hl_params
if hl_actions is None:
self.hl_actions = []
else:
self.hl_actions = hl_actions
self.solver = Solver()
def add_hl_param(self, hl_param):
self.hl_params.append(hl_param)
def solve(self):
opt_probs = []
for hl_action in self.hl_actions:
opt_probs.append(hl_action.opt_prob)
self.sqp_convexify_admm(opt_probs)
# self.sqp_admm(opt_probs)
# self.admm_sqp(opt_probs)
# self.big_sqp(opt_probs)
def sqp_convexify_admm(self, opt_probs):
for opt_prob in opt_probs:
opt_prob.augment_lagrangian()
solver = self.solver
solver.improve_ratio_threshold = .25
# solver.min_trust_box_size = 1e-5
solver.min_trust_box_size = 1e-4
# solver.min_trust_box_size = 1e-3
# solver.min_trust_box_size = 1e-2
# solver.min_approx_improve = 1e-5
solver.min_approx_improve = 1e-4
# solver.min_approx_improve = 1e-3
# solver.min_approx_improve = 1e-2
# solver.min_approx_improve = 3e-2
# solver.min_approx_improve = 1e-1
# solver.min_approx_improve = 3e-1
solver.max_iter = 50
solver.trust_shrink_ratio = .1
solver.trust_expand_ratio = 1.5
# solver.cnt_tolerance = 1e-2
solver.cnt_tolerance = 1e-3
# solver.cnt_tolerance = 1e-4
# solver.max_merit_coeff_increases = 5
# solver.max_merit_coeff_increases = 4
solver.max_merit_coeff_increases = 3
# solver.max_merit_coeff_increases = 2
solver.merit_coeff_increase_ratio = 10
solver.initial_trust_box_size = 0.1
# solver.initial_trust_box_size = 0.3
# solver.initial_trust_box_size = 1
# solver.initial_trust_box_size = 2
# solver.initial_trust_box_size = 3
# solver.initial_trust_box_size = 10
# solver.initial_penalty_coeff = 1.
solver.initial_penalty_coeff = 10
# solver.initial_penalty_coeff = 0.3
# solver.initial_penalty_coeff = 0.2
# solver.initial_penalty_coeff = 0.1
# solver.initial_penalty_coeff = 0.03
# solver.initial_penalty_coeff = 0.01
solver.callback = []
# solver.epsilon = 5e-3
solver.epsilon = 1e-2
# solver.epsilon = 2e-2
# solver.epsilon = 2.5e-2
# solver.epsilon = 3e-2
# solver.epsilon = 1e-1
solver.sqp_iters = 0
self.solver.sqp_convexify_admm(opt_probs, self.hl_params)
def sqp_admm(self, opt_probs):
for opt_prob in opt_probs:
opt_prob.augment_lagrangian()
solver = self.solver
solver.improve_ratio_threshold = .25
# solver.min_trust_box_size = 1e-4
solver.min_trust_box_size = 1e-3
# solver.min_trust_box_size = 1e-2
# solver.min_approx_improve = 1e-5
solver.min_approx_improve = 1e-4
# solver.min_approx_improve = 1e-3
# solver.min_approx_improve = 1e-2
# solver.min_approx_improve = 3e-2
# solver.min_approx_improve = 1e-1
# solver.min_approx_improve = 3e-1
solver.max_iter = 50
solver.trust_shrink_ratio = .1
solver.trust_expand_ratio = 1.5
solver.cnt_tolerance = 1e-2
# solver.cnt_tolerance = 1e-4
solver.max_merit_coeff_increases = 5
# solver.max_merit_coeff_increases = 2
solver.merit_coeff_increase_ratio = 10
# solver.initial_trust_box_size = 1
# solver.initial_trust_box_size = 2
solver.initial_trust_box_size = 3
# solver.initial_trust_box_size = 10
# solver.initial_penalty_coeff = 1.
# solver.initial_penalty_coeff = 10
# solver.initial_penalty_coeff = 0.3
# solver.initial_penalty_coeff = 0.2
solver.initial_penalty_coeff = 0.1
# solver.initial_penalty_coeff = 0.03
# solver.initial_penalty_coeff = 0.01
solver.callback = []
# self.epsilon = 5e-3
# solver.epsilon = 1e-2
solver.epsilon = 2e-2
# solver.epsilon = 3e-2
# solver.epsilon = 1e-1
solver.sqp_iters = 0
self.solver.sqp_admm(opt_probs, self.hl_params)
def admm_sqp(self, opt_probs):
for opt_prob in opt_probs:
opt_prob.augment_lagrangian()
solver = self.solver
solver.improve_ratio_threshold = .25
# solver.min_trust_box_size = 1e-4
solver.min_trust_box_size = 1e-2
# solver.min_approx_improve = 1e-4
# solver.min_approx_improve = 1e-2
solver.min_approx_improve = 1e-1
# solver.min_approx_improve = 3e-1
solver.max_iter = 50
solver.trust_shrink_ratio = .1
solver.trust_expand_ratio = 1.5
solver.cnt_tolerance = 1e-4
solver.max_merit_coeff_increases = 5
solver.merit_coeff_increase_ratio = 10
# solver.initial_trust_box_size = 1
# solver.initial_trust_box_size = 2
solver.initial_trust_box_size = 3
# solver.initial_trust_box_size = 10
# solver.initial_penalty_coeff = 1.
# solver.initial_penalty_coeff = 10
# solver.initial_penalty_coeff = 0.3
solver.initial_penalty_coeff = 0.1
# solver.initial_penalty_coeff = 0.01
# solver.max_penalty_iter = 4
solver.max_penalty_iter = 8
solver.callback = []
# solver.epsilon = 5e-3
solver.epsilon = 1e-2
solver.sqp_iters = 0
solver.solver.admm_sqp(opt_probs, self.hl_params)
def big_sqp(self, opt_probs):
# for opt_prob in opt_probs:
# opt_prob.primal()
solver = self.solver
solver.improve_ratio_threshold = .25
# solver.min_trust_box_size = 1e-4
solver.min_trust_box_size = 1e-2
# solver.min_approx_improve = 1e-4
# solver.min_approx_improve = 1e-2
solver.min_approx_improve = 1e-1
solver.max_iter = 50
solver.trust_shrink_ratio = .1
solver.trust_expand_ratio = 1.5
solver.cnt_tolerance = 1e-4
solver.max_merit_coeff_increases = 5
solver.merit_coeff_increase_ratio = 10
# solver.initial_trust_box_size = 1
# solver.initial_trust_box_size = 2
solver.initial_trust_box_size = 8
# solver.initial_penalty_coeff = 1.
# solver.initial_penalty_coeff = 0.1
solver.initial_penalty_coeff = 0.01
solver.max_penalty_iter = 20
self.solver.big_sqp(opt_probs, self.hl_params)
def __init__(self):
Solver.__init__(self)
self.sense = min
self.objective = identity_fnc
