Skip to content
Snippets Groups Projects
Commit 2ee78847 authored by rachelmoan's avatar rachelmoan
Browse files

Updating visualization. Attempting warm start & seeding with an optimal solution

parent f7b66a2c
No related branches found
No related tags found
No related merge requests found
...@@ -4,12 +4,13 @@ from matplotlib.patches import Circle, Rectangle ...@@ -4,12 +4,13 @@ from matplotlib.patches import Circle, Rectangle
from guided_mrmp.optimizer import Optimizer from guided_mrmp.optimizer import Optimizer
from casadi import * from casadi import *
class TrajOptResolver(): class TrajOptResolver():
""" """
A class that resolves conflicts using trajectoy optimization. A class that resolves conflicts using trajectoy optimization.
""" """
def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles, def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles,
rob_dist_weight, obs_dist_weight, control_weight, time_weight, goal_weight): rob_dist_weight, obs_dist_weight, control_weight, time_weight, goal_weight, conflicts, all_robots):
self.num_robots = num_robots self.num_robots = num_robots
self.starts = starts self.starts = starts
self.goals = goals self.goals = goals
...@@ -21,6 +22,8 @@ class TrajOptResolver(): ...@@ -21,6 +22,8 @@ class TrajOptResolver():
self.time_weight = time_weight self.time_weight = time_weight
self.robot_radius = MX(robot_radius) self.robot_radius = MX(robot_radius)
self.goal_weight = goal_weight self.goal_weight = goal_weight
self.conflicts = conflicts
self.all_robots = all_robots
def dist(self, robot_position, circle): def dist(self, robot_position, circle):
""" """
...@@ -97,7 +100,7 @@ class TrajOptResolver(): ...@@ -97,7 +100,7 @@ class TrajOptResolver():
for r2 in range(self.num_robots): for r2 in range(self.num_robots):
if r1 != r2: if r1 != r2:
d = sumsqr(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k]) d = sumsqr(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k])
dist_to_other_robots += self.apply_quadratic_barrier(6*self.robot_radius, d, 6) dist_to_other_robots += self.apply_quadratic_barrier(3*self.robot_radius, d, 2)
# ---- dynamics constraints ---- # # ---- dynamics constraints ---- #
...@@ -136,18 +139,19 @@ class TrajOptResolver(): ...@@ -136,18 +139,19 @@ class TrajOptResolver():
+ self.obs_dist_weight*dist_to_other_obstacles + self.obs_dist_weight*dist_to_other_obstacles
+ self.time_weight*T + self.time_weight*T
+ self.control_weight*heading_diff_penalty + self.control_weight*heading_diff_penalty
+ 5*dist_to_goal) + 10*dist_to_goal
)
# ------ control constraints ------ # # ------ control constraints ------ #
for k in range(N): for k in range(N):
for r in range(self.num_robots): for r in range(self.num_robots):
opti.subject_to(sumsqr(vel[r,k]) <= 0.2**2) opti.subject_to(sumsqr(vel[r,k]) <= .5**2)
opti.subject_to(sumsqr(omega[r,k]) <= 0.2**2) opti.subject_to(sumsqr(omega[r,k]) <= .5**2)
# ------ bound x, y, and time ------ # # ------ bound x, y, and time ------ #
opti.subject_to(opti.bounded(x_range[0]+self.robot_radius,x,x_range[1]-self.robot_radius)) # opti.subject_to(opti.bounded(x_range[0]+self.robot_radius,x,x_range[1]-self.robot_radius))
opti.subject_to(opti.bounded(y_range[0]+self.robot_radius,y,y_range[1]-self.robot_radius)) # opti.subject_to(opti.bounded(y_range[0]+self.robot_radius,y,y_range[1]-self.robot_radius))
opti.subject_to(opti.bounded(0,T,100)) opti.subject_to(opti.bounded(0,T,100))
# ------ initial conditions ------ # # ------ initial conditions ------ #
...@@ -155,7 +159,7 @@ class TrajOptResolver(): ...@@ -155,7 +159,7 @@ class TrajOptResolver():
opti.subject_to(heading[r, 0]==self.starts[r][2]) opti.subject_to(heading[r, 0]==self.starts[r][2])
opti.subject_to(pos[2*r : 2*(r+1), 0]==self.starts[r][0:2]) opti.subject_to(pos[2*r : 2*(r+1), 0]==self.starts[r][0:2])
# opti.subject_to(sumsqr(pos[2*r : 2*(r+1), -1] - self.goals[r]) < 1**2) # opti.subject_to(sumsqr(pos[2*r : 2*(r+1), -1] - self.goals[r]) < 5**2)
return {'opti':opti, 'X':X, 'U':U, 'T':T} return {'opti':opti, 'X':X, 'U':U, 'T':T}
...@@ -174,14 +178,19 @@ class TrajOptResolver(): ...@@ -174,14 +178,19 @@ class TrajOptResolver():
- y_range (tuple): - y_range (tuple):
""" """
problem = self.problem_setup(N, x_range, y_range) problem = self.problem_setup(N, x_range, y_range)
results = self.solve_optimization_problem(problem, initial_guesses, solver_options) results = self.solve_optimization_problem(problem, initial_guesses, solver_options)
if results['status'] == 'failed': if results['status'] == 'failed':
return None, None, None, None, None, None, None return None, None, None, None, None, None, None, None
X = results['X'] X = results['X']
sol = results['solution'] sol = results['solution']
U = results['U'] U = results['U']
T = results['T']
# Extract the values that we want from the optimizer's solution # Extract the values that we want from the optimizer's solution
pos = X[:self.num_robots*2,:] pos = X[:self.num_robots*2,:]
...@@ -192,7 +201,7 @@ class TrajOptResolver(): ...@@ -192,7 +201,7 @@ class TrajOptResolver():
vels = U[0::2,:] vels = U[0::2,:]
omegas = U[1::2,:] omegas = U[1::2,:]
return sol,pos, vels, omegas, x_vals, y_vals, theta_vals return sol,pos, vels, omegas, x_vals, y_vals, theta_vals, T
def get_local_controls(self, controls): def get_local_controls(self, controls):
""" """
...@@ -209,13 +218,22 @@ class TrajOptResolver(): ...@@ -209,13 +218,22 @@ class TrajOptResolver():
# Solve the trajectory optimization problem # Solve the trajectory optimization problem
initial_guess = None initial_guess = None
sol, x_opt, vels, omegas, xs,ys = self.solve(20, initial_guess) solver_options = {'ipopt.print_level': 1, 'print_time': 1}
# y range is the smallest y of the starts/goals to the largest y of the starts/goals
y_range = (min([r[1]-self.robot_radius for r in self.starts + self.goals]), max([r[1]+self.robot_radius for r in self.starts + self.goals]))
x_range = (min([r[0]-self.robot_radius for r in self.starts + self.goals]), max([r[0]+self.robot_radius for r in self.starts + self.goals]))
sol, x_opt, vels, omegas, xs,ys, thetas, T = self.solve(20, x_range, y_range,initial_guess, solver_options)
if sol is None:
print("Failed to solve the optimization problem")
pos_vals = np.array(sol.value(x_opt)) pos_vals = np.array(sol.value(x_opt))
# Update the controls for the robots # Update the controls for the robots
for r, vel, omega, x,y in zip(robots, vels, omegas, xs,ys): for r, vel, omega, x,y in zip(robots, vels, omegas, xs,ys):
controls[r.label] = [vel, omega] controls[r.label] = [vel[0], omega[0]]
final_trajs[r.label] = [x,y] final_trajs[r.label] = [x,y]
return controls, final_trajs return controls, final_trajs
...@@ -255,3 +273,113 @@ class TrajOptResolver(): ...@@ -255,3 +273,113 @@ class TrajOptResolver():
plt.grid(False) plt.grid(False)
plt.show() plt.show()
def check_goal_overlap(goal1, goal2, rad):
"""
Check if the goals overlap
"""
# get the vector between the two goals
v = np.array(goal2) - np.array(goal1)
# check if the distance between the two goals is less than 2*rad
if np.linalg.norm(v) < 2*rad:
return True
return False
def fix_goal_overlap(start1, goal1, start2, goal2):
"""
Fix the goal overlap
"""
# get the vectors between the starts and goals
v1 = np.array(goal1) - np.array(start1)
v2 = np.array(goal2) - np.array(start2)
# get the vectors orthogonal to the vectors between the starts and goals
v1_orth = np.array([-v1[1], v1[0]])
v2_orth = np.array([-v2[1], v2[0]])
# move the goals in the direction of the orthogonal vectors
goal1 = goal1 + .5*v1_orth
goal2 = goal2 + .5*v2_orth
return goal1, goal2
if __name__ == "__main__":
# load all the data from test/db_opt_data1.yaml
import yaml
with open('guided_mrmp/tests/db_opt_data2.yaml') as file:
data = yaml.load(file, Loader=yaml.Loader)
from guided_mrmp.utils import Env
starts = data['starts']
goals = data['goals']
circle_obstacles = data['env'].circle_obs
rectangle_obstacles = data['env'].rect_obs
rob_dist_weight = data['rob_dist_weight']
obs_dist_weight = data['obstacle_weight']
control_weight = data['control_weight']
time_weight = data['time_weight']
goal_weight = data['goal_weight']
robot_radius = data['rad']
conflicts = data['conflicts']
all_robots = data['robots']
next_desired_controls = data['next_desired_controls']
current_trajs = data['trajectories']
old_goals = goals.copy()
start1 = starts[0][:2]
start2 = starts[1][:2]
goals[0], goals[1] = fix_goal_overlap(start1, goals[0], start2, goals[1])
# create the TrajOptResolver object
resolver = TrajOptResolver(len(starts),
robot_radius,
starts,
goals,
circle_obstacles,
rectangle_obstacles,
rob_dist_weight,
obs_dist_weight,
2,
time_weight,
goal_weight,
conflicts,
all_robots)
next_desired_controls, new_trajs = resolver.get_local_controls(next_desired_controls)
# use matplotlib to plot the current trajectories of the robots, and
# the new trajectories of the robots
import matplotlib.pyplot as plt
fig, ax = plt.subplots()
for r in range(len(current_trajs)):
# plot the starts and goals of the robots as circles with radius rad
ax.add_patch(plt.Circle(starts[r], robot_radius, color='green', fill=True))
ax.add_patch(plt.Circle(goals[r], robot_radius, color='green', fill=False))
# plot the old goals of the robots as circles with radius rad
ax.add_patch(plt.Circle(old_goals[r], robot_radius, color='red', fill=False))
ax.plot(current_trajs[r][0], current_trajs[r][1], label=f'Robot {r+1} current', color='red')
ax.plot(new_trajs[r][0], new_trajs[r][1], label=f'Robot {r+1} new', color='blue')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.legend()
plt.show()
...@@ -4,6 +4,10 @@ class Optimizer: ...@@ -4,6 +4,10 @@ class Optimizer:
def solve_optimization_problem(self, initial_guesses=None, solver_options=None): def solve_optimization_problem(self, initial_guesses=None, solver_options=None):
opti = self.problem['opti'] opti = self.problem['opti']
X = self.problem['X']
U = self.problem['U']
if initial_guesses: if initial_guesses:
for param, value in initial_guesses.items(): for param, value in initial_guesses.items():
...@@ -15,6 +19,38 @@ class Optimizer: ...@@ -15,6 +19,38 @@ class Optimizer:
else: else:
opti.solver('ipopt') opti.solver('ipopt')
def print_intermediates_callback(i):
# print("Iteration:", i, "Current solution:", opti.debug.value(X), opti.debug.value(U))
X_debug = opti.debug.value(X)
U_debug = opti.debug.value(U)
# plot the state and the control
# split a figure in half. The left side will show the positions, the right side will plot the controls
# X[i*3, :] is the ith robot's x position, X[i*3+1, :] is the y position, X[i*3+2, :] is the heading
# U[i*2, :] is the ith robot's linear velocity, U[i*2+1, :] is the ith robot's angular velocity
import matplotlib.pyplot as plt
fig, axs = plt.subplots(1, 2, figsize=(12, 6))
for j in range(X_debug.shape[0]//3):
axs[0].plot(X_debug[j*3, :], X_debug[j*3+1, :], label=f"Robot {j}")
axs[0].scatter(X_debug[j*3, 0], X_debug[j*3+1, 0], color='green')
axs[0].scatter(X_debug[j*3, -1], X_debug[j*3+1, -1], color='red')
axs[0].set_title("Robot Positions")
axs[0].set_xlabel("X")
axs[0].set_ylabel("Y")
axs[0].legend()
axs[1].plot(U_debug[j*2, :], label=f"Robot {j}")
axs[1].plot(U_debug[j*2+1, :], label=f"Robot {j}")
axs[1].set_title("Robot Controls")
axs[1].set_xlabel("Time")
axs[1].set_ylabel("Control")
axs[1].legend()
plt.show()
# opti.callback(print_intermediates_callback)
try: try:
sol = opti.solve() # actual solve sol = opti.solve() # actual solve
status = 'succeeded' status = 'succeeded'
......
library: library:
name: "5x2" # change to "2x3" for 2x3 name: "2x3" # change to "2x3" for 2x3
x_max: 5 # change to 2 for 2x3 x_max: 2 # change to 2 for 2x3
y_max: 2 # change to 3 for 2x3 y_max: 3 # change to 3 for 2x3
initial_guess: initial_guess_type: "db" # options are db, line, None
X: "None" # options are db, line, none
T: 20
robot_radius: .5 robot_radius: .5
environment: environment:
circle_obs: [[5,3,1]] # [x, y, radius] # change to [] for 2x3 circle_obs: [] # [[5,3,1]] # [x, y, radius] # change to [] for 2x3
rectangle_obs: [] rectangle_obs: []
cost_weights: cost_weights:
...@@ -18,13 +16,13 @@ cost_weights: ...@@ -18,13 +16,13 @@ cost_weights:
dist_obstacles_weight: 10 dist_obstacles_weight: 10
control_costs_weight: 1.0 control_costs_weight: 1.0
time_weight: 5.0 time_weight: 5.0
goal_weight: 5.0 goal_weight: 50.0
N: 30 N: 40
num_trials: 1 num_trials: 1
control_point_distance: -.5 control_point_distance: -.8
grid_resolution: 2 grid_resolution: 2
......
This diff is collapsed.
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