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
