Skip to content
Snippets Groups Projects
Commit 202c2569 authored by rachelmoan's avatar rachelmoan
Browse files

try setting dual variables

parent aed63442
No related branches found
No related tags found
No related merge requests found
......@@ -169,12 +169,12 @@ class TrajOptResolver():
return {'opti':opti, 'X':X, 'U':U, 'T':T, 'cost':cost, 'robot_cost':robot_cost, 'obs_cost':obs_cost, 'time_cost':time_cost, 'control_cost':control_cost, 'goal_cost':goal_cost}
def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None, prior_solution=None):
opt = Optimizer(problem)
results = opt.solve_optimization_problem(initial_guesses, solver_options)
return results
results,sol = opt.solve_optimization_problem(initial_guesses, solver_options, prior_solution)
return results,sol
def solve(self, N, x_range, y_range, initial_guesses=None, solver_options=None):
def solve(self, N, x_range, y_range, initial_guesses=None, solver_options=None, prior_solution=None):
"""
Setup and solve a multi-robot traj opt problem
......@@ -186,9 +186,7 @@ class TrajOptResolver():
problem = self.problem_setup(N, x_range, y_range)
results = self.solve_optimization_problem(problem, initial_guesses, solver_options)
results,old_sol = self.solve_optimization_problem(problem, initial_guesses, solver_options, prior_solution)
if results['status'] == 'failed':
return None, None, None, None, None, None, None, None
......@@ -197,6 +195,7 @@ class TrajOptResolver():
sol = results['solution']
U = results['U']
T = results['T']
lam_g = results['lam_g']
# Extract the values that we want from the optimizer's solution
pos = X[:self.num_robots*2,:]
......@@ -207,7 +206,7 @@ class TrajOptResolver():
vels = U[0::2,:]
omegas = U[1::2,:]
return sol,pos, vels, omegas, x_vals, y_vals, theta_vals, T
return lam_g,sol,pos, vels, omegas, x_vals, y_vals, theta_vals, T
def get_local_controls(self, controls):
"""
......
......@@ -2,7 +2,7 @@ class Optimizer:
def __init__(self, problem):
self.problem = problem
def solve_optimization_problem(self, initial_guesses=None, solver_options=None):
def solve_optimization_problem(self, initial_guesses=None, solver_options=None, lam_g=None):
opti = self.problem['opti']
X = self.problem['X']
......@@ -13,6 +13,9 @@ class Optimizer:
for param, value in initial_guesses.items():
opti.set_initial(self.problem[param], value)
if lam_g is not None:
opti.set_initial(opti.lam_g, lam_g)
# Set numerical backend, with options if provided
if solver_options:
opti.solver('ipopt', solver_options)
......@@ -48,6 +51,7 @@ class Optimizer:
plt.show()
# opti.callback(print_intermediates_callback)
......@@ -62,6 +66,7 @@ class Optimizer:
'status' : status,
'solution' : sol,
}
# print(f"Final total = {sol.value(self.problem['cost'])}")
# print(f"robot costs = {sol.value(self.problem['robot_cost'])}")
......@@ -75,4 +80,8 @@ class Optimizer:
if var_name != 'opti':
results[var_name] = sol.value(var)
return results
opti = self.problem['opti']
lam_g = sol.value(opti.lam_g)
results['lam_g'] = lam_g
return results,sol
......@@ -352,11 +352,12 @@ if __name__ == "__main__":
import time
start = time.time()
sol,pos, vels, omegas, xs, ys, thetas, T = solver.solve(N, x_range, y_range, initial_guesses, solver_options)
old_sol, sol,pos, vels, omegas, xs, ys, thetas, T = solver.solve(N, x_range, y_range, initial_guesses, solver_options)
end = time.time()
# plot_paths_db(circle_obs, num_robots, robot_starts, robot_goals, pos, None, x_range, y_range, rob_radius, "Optimizer solution")
# plot_sim(xs, ys, thetas, x_range, y_range, rob_radius, "Optimizer solution")
......@@ -411,7 +412,7 @@ if __name__ == "__main__":
import time
start = time.time()
sol,pos, vels, omegas, xs, ys, thetas, T = solver.solve(N, x_range, y_range, initial_guesses, solver_options)
lam_g,sol,pos, vels, omegas, xs, ys, thetas, T = solver.solve(N, x_range, y_range, initial_guesses, solver_options, old_sol)
end = time.time()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment