From 871ec408138991820c64df36f6dddf60869e8e78 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Wed, 16 Oct 2024 21:18:45 -0500
Subject: [PATCH] traj_opt testing problems straight from the database
---
guided_mrmp/conflict_resolvers/traj_opt.py | 449 +++++++++++++--------
1 file changed, 290 insertions(+), 159 deletions(-)
diff --git a/guided_mrmp/conflict_resolvers/traj_opt.py b/guided_mrmp/conflict_resolvers/traj_opt.py
index e4e05b5..1b9302e 100644
--- a/guided_mrmp/conflict_resolvers/traj_opt.py
+++ b/guided_mrmp/conflict_resolvers/traj_opt.py
@@ -2,7 +2,7 @@ import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, Rectangle
from casadi import *
-# from guided_mrmp.conflict_resolvers.curve_path import smooth_path
+from guided_mrmp.conflict_resolvers.curve_path import smooth_path
class TrajOptMultiRobot():
def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles,
@@ -45,9 +45,19 @@ class TrajOptMultiRobot():
def log_normal_barrier(self, sigma, d, c):
return c*fmax(0, 2-(d/sigma))**2.5
- def solve(self, num_control_intervals, initial_guess):
+ def problem_setup(self, N, x_range, y_range):
+ """
+ Problem setup for the multi-robot collision resolution traj opt problem
+
+ inputs:
+ - N (int): number of control intervals
+ - x_range (tuple): range of x values
+ - y_range (tuple): range of y values
- N = num_control_intervals
+ outputs:
+ - problem (dict): dictionary containing the optimization problem
+ and the decision variables
+ """
opti = Opti() # Optimization problem
# ---- decision variables --------- #
@@ -57,16 +67,15 @@ class TrajOptMultiRobot():
y = pos[1::2,:]
heading = X[self.num_robots*2:,:] # heading is the last value
-
- circle_obs = DM(self.circle_obs) # make the obstacles casadi objects
-
U = opti.variable(self.num_robots*2, N) # control trajectory (v, omega)
vel = U[0::2,:]
omega = U[1::2,:]
T = opti.variable() # final time
-
- # sum up the cost of distance to obstacles
+ # ---- obstacle setup ------------ #
+ circle_obs = DM(self.circle_obs) # make the obstacles casadi objects
+
+ # ------ Obstacle dist cost ------ #
# TODO:: Include rectangular obstacles
dist_to_other_obstacles = 0
for r in range(self.num_robots):
@@ -74,29 +83,22 @@ class TrajOptMultiRobot():
for c in range(circle_obs.shape[0]):
circle = circle_obs[c, :]
d = self.dist(pos[2*r : 2*(r+1), k], circle)
- dist_to_other_obstacles += self.apply_quadratic_barrier(self.robot_radius + circle[2] + 0.5, d, 1)
- # dist_to_other_obstacles += self.log_normal_barrier(5, d, 5)
+ dist_to_other_obstacles += self.apply_quadratic_barrier(2*(self.robot_radius + circle[2]), d, 5)
+ # ------ Robot dist cost ------ #
dist_to_other_robots = 0
for k in range(N):
for r1 in range(self.num_robots):
for r2 in range(self.num_robots):
if r1 != r2:
- # print(f"\n{r1} position1 = {pos[2*r1 : 2*(r1+1), k]}")
- # print(f"{r2} position2 = {pos[2*r2 : 2*(r2+1), k]}")
-
- # note: using norm 2 here gives an invalid num detected error.
- # Must be the sqrt causing an issue
- # d = norm_2(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k]) - 2*self.robot_radius
d = sumsqr(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k])
- dist_to_other_robots += self.apply_quadratic_barrier(2*self.robot_radius+.5, d, 1)
-
- dt = T/N # length of a control interval
+ dist_to_other_robots += self.apply_quadratic_barrier(2*self.robot_radius, d, 1)
+
- # print("Initial pos:", pos[:, 0])
- # print("Initial heading:", heading[:, 0])
+ # ---- dynamics constraints ---- #
+ dt = T/N # length of a control interval
- pi = [3.14159*2]*self.num_robots
+ pi = [3.14159]*self.num_robots
pi = np.array(pi)
pi = DM(pi)
@@ -106,62 +108,101 @@ class TrajOptMultiRobot():
dthetadt = omega[:,k]
opti.subject_to(x[:,k+1]==x[:,k] + dt*dxdt)
opti.subject_to(y[:,k+1]==y[:,k] + dt*dydt)
- opti.subject_to(heading[:,k+1]==fmod(heading[:,k] + dt*dthetadt, pi))
+ opti.subject_to(heading[:,k+1]==fmod(heading[:,k] + dt*dthetadt, 2*pi))
+
+ # ------ Control panalty ------ #
# Calculate the sum of squared differences between consecutive heading angles
heading_diff_penalty = 0
for k in range(N-1):
- heading_diff_penalty += sumsqr(heading[:,k+1] - heading[:,k])
+ heading_diff_penalty += sumsqr(fmod(heading[:,k+1] - heading[:,k] + pi, 2*pi) - pi)
+
+ # ------ cost function ------ #
opti.minimize(self.rob_dist_weight*dist_to_other_robots
+ self.obs_dist_weight*dist_to_other_obstacles
+ self.time_weight*T
- + 5*heading_diff_penalty)
+ + self.control_weight*heading_diff_penalty)
- # ---- path constraints -----------
+ # ------ control constraints ------ #
for k in range(N):
for r in range(self.num_robots):
opti.subject_to(sumsqr(vel[r,k]) <= 0.2**2)
opti.subject_to(sumsqr(omega[r,k]) <= 0.2**2)
- opti.subject_to(opti.bounded(0,x,10))
- opti.subject_to(opti.bounded(0,y,10))
- # opti.subject_to(opti.bounded(-0.05,vel,0.05))
- # opti.subject_to(opti.bounded(-.1,U,.1)) # control is limited
+ # ------ bound x, y, and time ------ #
+ opti.subject_to(opti.bounded(x_range[0],x,x_range[1]))
+ opti.subject_to(opti.bounded(y_range[0],y,y_range[1]))
+ opti.subject_to(opti.bounded(0,T,100))
- # ---- boundary conditions --------
+ # ------ initial conditions ------ #
for r in range(self.num_robots):
- # opti.subject_to(vel[r, 0]==0)
- opti.subject_to(pos[2*r : 2*(r+1), 0]==self.starts[r])
+
+ 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), -1]==self.goals[r])
- # --- misc. constraints --- #
- opti.subject_to(opti.bounded(0,T,100))
+ return {'opti':opti, 'X':X, 'T':T}
- # --- initial values for solver --- #
- opti.set_initial(T, 20)
+ def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
+ opti = problem['opti']
- opti.set_initial(X,initial_guess)
-
-
-
- # --- solve NLP --- #
- opti.solver("ipopt") # set numerical backend
- sol = opti.solve() # actual solve
+ if initial_guesses:
+ for param, value in initial_guesses.items():
+ print(f"param = {param}")
+ print(f"value = {value}")
+ opti.set_initial(problem[param], value)
+
+ # Set numerical backend, with options if provided
+ if solver_options:
+ opti.solver('ipopt', solver_options)
+ else:
+ opti.solver('ipopt')
+
+ try:
+ sol = opti.solve() # actual solve
+ status = 'succeeded'
+ except:
+ sol = None
+ status = 'failed'
+
+ results = {
+ 'status' : status,
+ 'solution' : sol,
+ }
+
+ if sol:
+ for var_name, var in problem.items():
+ if var_name != 'opti':
+ results[var_name] = sol.value(var)
+
+ return results
+
+ def solve(self, N, x_range, y_range, initial_guesses):
+ """
+ Setup and solve a multi-robot traj opt problem
- # print(f"pos = {opti.debug.value(pos[2:4,:])}")
+ input:
+ - N (int): the number of control intervals
+ - x_range (tuple):
+ - y_range (tuple):
+ """
+ problem = self.problem_setup(N, x_range, y_range)
+ results = self.solve_optimization_problem(problem, initial_guesses)
- # Extract x and y values
- x_vals = np.array(sol.value(x))
- y_vals = np.array(sol.value(y))
+ X = results['X']
+ sol = results['solution']
- # Extract theta values
- theta_vals = np.array(sol.value(heading))
+ # Extract the values that we want from the optimizer's solution
+ pos = X[:self.num_robots*2,:]
+ x_vals = pos[0::2,:]
+ y_vals = pos[1::2,:]
+ theta_vals = X[self.num_robots*2:,:]
return sol,pos, x_vals, y_vals, theta_vals
- def plot_paths(self, x_opt, initial_guess):
+ def plot_paths(self, x_opt, initial_guess, x_range, y_range):
fig, ax = plt.subplots()
# Plot obstacles
@@ -171,12 +212,7 @@ class TrajOptMultiRobot():
# elif len(obstacle) == 4: # Rectangle
# ax.add_patch(Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], color='red'))
- if self.num_robots > 20:
- colors = plt.cm.hsv(np.linspace(0.2, 1.0, self.num_robots))
- elif self.num_robots > 10:
- colors = plt.cm.tab20(np.linspace(0, 1, self.num_robots))
- else:
- colors = plt.cm.tab10(np.linspace(0, 1, self.num_robots))
+ colors = plt.cm.Set1(np.linspace(0, 1, self.num_robots))
# Plot robot paths
for r,color in zip(range(self.num_robots),colors):
@@ -184,31 +220,73 @@ class TrajOptMultiRobot():
ax.scatter(x_opt[r*2, :], x_opt[r*2+1, :], color=color, s=10 )
ax.scatter(self.starts[r][0], self.starts[r][1], s=85,color=color)
ax.scatter(self.goals[r][0], self.goals[r][1], s=85,facecolors='none', edgecolors=color)
- ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
- ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
+ if initial_guess is not None:
+ ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
+ ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
+
+ plot_roomba(self.starts[r][0], self.starts[r][1], 0, color)
+ # plot_roomba(self.goals[r][0], self.goals[r][1], 0, color)
+
+
+
+ plt.ylim(0, y_range[1])
+ plt.xlim(0,x_range[1])
+ plt.axis("equal")
+ plt.axis("off")
+
- ax.set_xlabel('X')
- ax.set_ylabel('Y')
- ax.legend()
- ax.set_aspect('equal', 'box')
+ plt.tight_layout()
- plt.ylim(0,10)
- plt.xlim(0,10)
- plt.title('Robot Paths')
plt.grid(False)
plt.show()
+ def plot_paths_db(self, x_opt, initial_guess,x_range, y_range):
+ fig, ax = plt.subplots()
+
+ # Plot obstacles
+ for obstacle in self.circle_obs:
+ # if len(obstacle) == 2: # Circle
+ ax.add_patch(Circle(obstacle, obstacle[2], color='red'))
+ # elif len(obstacle) == 4: # Rectangle
+ # ax.add_patch(Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], color='red'))
+
+ colors = plt.cm.Set1(np.linspace(0, 1, self.num_robots))
+
+ # Plot robot paths
+ for r,color in zip(range(self.num_robots),colors):
+ if x_opt is not None:
+ ax.plot(x_opt[r*2, :], x_opt[r*2+1, :], label=f'Robot {r+1}', color=color)
+ ax.scatter(x_opt[r*2, :], x_opt[r*2+1, :], color=color, s=10 )
+ ax.scatter(self.starts[r][0], self.starts[r][1], s=85,color=color)
+ ax.scatter(self.goals[r][0], self.goals[r][1], s=135,facecolors='none', edgecolors=color)
+ if initial_guess is not None:
+ ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
+ ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
-def plot_sim(x_histories, y_histories, h_histories):
+ if x_opt is not None: plot_roomba(self.starts[r][0], self.starts[r][1], 0, color)
+ # plot_roomba(self.goals[r][0], self.goals[r][1], 0, color)
- if len(x_histories) > 20:
- colors = plt.cm.hsv(np.linspace(0.2, 1.0, len(x_histories)))
- elif len(x_histories) > 10:
- colors = plt.cm.tab20(np.linspace(0, 1, len(x_histories)))
- else:
- colors = plt.cm.tab10(np.linspace(0, 1, len(x_histories)))
-
+
+ plt.ylim(0, y_range[1])
+ plt.xlim(0,x_range[1])
+ plt.axis("equal")
+ # plt.axis("off")
+
+
+ plt.tight_layout()
+
+ plt.grid(False)
+ plt.show()
+
+
+def plot_sim(x_histories, y_histories, h_histories, x_range, y_range):
+
+ x_histories = np.array(x_histories)
+ y_histories = np.array(y_histories)
+ h_histories = np.array(h_histories)
+
+ colors = plt.cm.Set1(np.linspace(0, 1, len(x_histories)))
longest_traj = max([len(x) for x in x_histories])
@@ -234,17 +312,23 @@ def plot_sim(x_histories, y_histories, h_histories):
plot_roomba(x_history[-1], y_history[-1], h_history[-1], color)
- ax = plt.gca()
- ax.set_xlim([0, 10])
- ax.set_ylim([0, 10])
+ plt.ylim(0, y_range[1])
+ plt.xlim(0,x_range[1])
+ plt.axis("equal")
+ # plt.axis("off")
+
+
plt.tight_layout()
+ plt.grid(False)
+
plt.draw()
+ plt.savefig(f"frames/sim_{i}.png")
plt.pause(0.2)
input()
-def plot_roomba(x, y, yaw, color):
+def plot_roomba(x, y, yaw, color, radius=.5):
"""
Args:
@@ -252,37 +336,114 @@ def plot_roomba(x, y, yaw, color):
y ():
yaw ():
"""
- LENGTH = 0.5 # [m]
- WIDTH = 0.25 # [m]
- OFFSET = LENGTH # [m]
-
fig = plt.gcf()
ax = fig.gca()
- circle = plt.Circle((x, y), .5, color=color, fill=False)
+ circle = plt.Circle((x, y), radius, color=color, fill=False)
ax.add_patch(circle)
# Plot direction marker
- dx = 1 * np.cos(yaw)
- dy = 1 * np.sin(yaw)
- ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
+ dx = radius * np.cos(yaw)
+ dy = radius * np.sin(yaw)
+ ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.05, fc='r', ec='r')
+
+def generate_prob_from_db(N, cp_dist=.5):
+ from guided_mrmp.utils import Library
+ import random
+ lib = Library("guided_mrmp/database/5x2_library")
+ lib.read_library_from_file()
+
+ d = lib.key_to_idx
+
+ # get a random key from the library
+ key, idx = random.choice(list(d.items()))
+
+ print(key)
+ print(len(key))
+
+ num_robots = len(key) // 4
+
+ start_nodes = []
+ goal_nodes = []
+
+ for i in range(0, len(key), 4):
+ start = [int(key[i]), int(key[i+1])]
+ goal = [int(key[i+2]), int(key[i+3])]
+ start_heading = np.arctan2(goal[1] - start[1], goal[0] - start[0])
+ start.append(start_heading)
+ start_nodes.append(start)
+ goal_nodes.append(goal)
+
+
+ sol = lib.get_matching_solution(start_nodes, goal_nodes)
+
+ print(f"sol = {sol}")
+ # turn this solution into an initial guess
+ initial_guess = np.zeros((num_robots*3, N+1))
+ for i in range(num_robots):
+ print(f"Robot {i+1} solution:")
+ rough_points = np.array(sol[i])
+ points = []
+ for point in rough_points:
+ if point[0] == -1: break
+ points.append(point)
+
+ points = np.array(points)
+ print(f"points = {points}")
+
+ smoothed_curve = smooth_path(points, cp_dist, N)
+ print(f"smoothed_curve = {smoothed_curve}")
+ initial_guess[i*3, :] = smoothed_curve[:, 0] # x
+ initial_guess[i*3 + 1, :] = smoothed_curve[:, 1] # y
+
+ for j in range(N):
+ dx = smoothed_curve[j+1, 0] - smoothed_curve[j, 0]
+ dy = smoothed_curve[j+1, 1] - smoothed_curve[j, 1]
+ initial_guess[i*3 + 2, j] = np.arctan2(dy, dx)
+
+ # initial_guess[i*3 + 2, :] = np.arctan2(np.diff(smoothed_curve[:, 1]),
+ # np.diff(smoothed_curve[:, 0]))
+
+
+
+
+ print(sol)
+
+ for i in range(num_robots):
+ print(f"Robot {i+1} initial guess:")
+ print(f"x: {initial_guess[i*3, :]}")
+ print(f"y: {initial_guess[i*3 + 1, :]}")
+ print(f"theta: {initial_guess[i*3 + 2, :]}")
+
+ return start_nodes, goal_nodes, initial_guess
if __name__ == "__main__":
+ import os
+ import numpy as np
+ import random
+
+ seed = 1123581
+ seed = 112
+ print(f"***Setting Python Seed {seed}***")
+ os.environ['PYTHONHASHSEED'] = str(seed)
+ np.random.seed(seed)
+ random.seed(seed)
+
# define obstacles
- circle_obs = np.array([[5,5,1],
- [7,7,1],
- [3,3,1]])
+ circle_obs = np.array([[5,3,1]])
- circle_obs = np.array([])
+ # circle_obs = np.array([])
rectangle_obs = np.array([])
# define all the robots' starts and goals
robot_starts = [[1,6],[9,1],[2,2],[1,3]]
robot_goals = [[9,1],[1,6],[8,8],[7,3]]
+
+
# robot_starts = [[9,5]]
# robot_goals = [[1,5]]
@@ -294,76 +455,38 @@ if __name__ == "__main__":
# other params
num_robots = 4
- rob_radius = 0.25
- N = 20
-
+ rob_radius = .75
+ N = 30
- # ---- straight line initial guess ---- #
- print(f"N = {N}")
- initial_guess = np.zeros((num_robots*3,N+1))
- print(initial_guess)
- # for i,(start,goal) in enumerate(zip(robot_starts, robot_goals)):
- for i in range(0,num_robots*3,3):
- # print(f"i = {i}")
- start=robot_starts[int(i/3)]
- goal=robot_goals[int(i/3)]
- # print(f"start = {start}")
- # print(f"goal = {goal}")
- initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
- initial_guess[i+1,:] = np.linspace(start[1], goal[1], N+1)
- dx = goal[0] - start[0]
- dy = goal[1] - start[1]
- initial_guess[i+2,:] = np.arctan2(dy,dx)*np.ones(N+1)
- # initial_guess[i+2,:] = np.linspace(.5, .5, N+1)
- # initial_guess[i+3,:] = np.linspace(.5, .5, N+1)
-
- print(initial_guess)
-
- # jagged initial guess
- # initial_guess = np.array([
- # [1, 1, 1, 1, 1, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9],
- # [6, 5, 4, 3, 2, 1, 1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1],
- # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ],
- # [9, 9, 9, 9, 9, 9, 8 ,7, 6, 5, 4, 3, 2, 1, 1, 1, 1, 1, 1, 1, 1],
- # [1, 2, 3, 4, 5, 6, 6, 6,6,6,6,6,6,6,6,6,6,6,6,6,6],
- # [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ]
- # ])
-
- # points1 = np.array([[1,6],
- # [1,1],
- # [9,1]])
-
- # points2 = np.array([[9,1],
- # [9,6],
- # [1,6]])
-
- # points3 = np.array([[2,2],
- # [4,4],
- # [8,8]])
-
- # points4 = np.array([[1,3],
- # [3,3],
- # [7,3]])
+ robot_starts, robot_goals, initial_guess = generate_prob_from_db(N)
- # smoothed_curve1 = smooth_path(points1, 3)
- # smoothed_curve2 = smooth_path(points2, 3)
- # smoothed_curve3 = smooth_path(points3, 3)
- # smoothed_curve4 = smooth_path(points4, 3)
+ num_robots = len(robot_starts)
+ h = 2
+ x_range = (0, 5*h)
+ y_range = (0, 2*h)
+ robot_starts = np.array(robot_starts)
+ robot_goals = np.array(robot_goals)
+ robot_starts = robot_starts*h + .5*h
+ robot_goals = robot_goals*h + .5*h
+ initial_guess = initial_guess*h + .5*h
- # print(f"smoothed_curve = {smoothed_curve}")
+ print(f"robot_starts = {robot_starts}")
+ print(f"robot_goals = {robot_goals}")
- # initial_guess = np.zeros((num_robots*3,N+1))
- # initial_guess[0,:] = smoothed_curve1[:,0]
- # initial_guess[1,:] = smoothed_curve1[:,1]
- # initial_guess[3,:] = smoothed_curve2[:,0]
- # initial_guess[4,:] = smoothed_curve2[:,1]
- # initial_guess[6,:] = smoothed_curve3[:,0]
- # initial_guess[7,:] = smoothed_curve3[:,1]
- # initial_guess[9,:] = smoothed_curve4[:,0]
- # initial_guess[10,:] = smoothed_curve4[:,1]
+ # ---- straight line initial guess ---- #
+ straight_line = False
+ if straight_line:
+ initial_guess = np.zeros((num_robots*3,N+1))
+ for i in range(0,num_robots*3,3):
+ start=robot_starts[int(i/3)]
+ goal=robot_goals[int(i/3)]
+ initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
+ initial_guess[i+1,:] = np.linspace(start[1], goal[1], N+1)
+ dx = goal[0] - start[0]
+ dy = goal[1] - start[1]
+ initial_guess[i+2,:] = np.arctan2(dy,dx)*np.ones(N+1)
-
solver = TrajOptMultiRobot(num_robots=num_robots,
robot_radius=rob_radius,
@@ -376,13 +499,21 @@ if __name__ == "__main__":
control_weight=control_costs_weight,
time_weight=time_weight
)
- sol,pos, xs, ys, thetas = solver.solve(N, initial_guess)
+
+ initial_guesses = {
+ 'X': initial_guess,
+ 'T': 20
+ }
+
+ solver.plot_paths_db(None, initial_guess, x_range, y_range)
+ sol,pos, xs, ys, thetas = solver.solve(N, x_range, y_range, initial_guesses)
pos_vals = np.array(sol.value(pos))
-
- solver.plot_paths(pos_vals, initial_guess)
+ solver.plot_paths_db(None, initial_guess, x_range, y_range)
+ solver.plot_paths_db(pos_vals, None, x_range, y_range)
+ plot_sim(xs, ys, thetas, x_range, y_range)
- print(pos_vals)
+ # print(pos_vals)
@@ -392,6 +523,6 @@ if __name__ == "__main__":
# ys.append(pos_vals[r*2+1, :])
# thetas.append(pos_vals[num_robots*2 + r, :])
- plot_sim(xs, ys, thetas)
+ # plot_sim(xs, ys, thetas)
--
GitLab