Skip to content
Snippets Groups Projects
Commit 0b22b300 authored by rachelmoan's avatar rachelmoan
Browse files

delete the old traj opt class in test file, use the one in the main part of code

parent 57a1c38e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
guided_mrmp/tests/initial_guesses.yaml
+ 1
− 1
View file @ 0b22b300
......@@ -22,7 +22,7 @@ cost_weights:
N: 30
num_trials: 10
num_trials: 1
control_point_distance: -.5
......
guided_mrmp/tests/test_traj_opt.py
+ 1
− 218
View file @ 0b22b300
......@@ -6,223 +6,6 @@ from guided_mrmp.conflict_resolvers.curve_path import smooth_path, calculate_hea
from guided_mrmp.conflict_resolvers.traj_opt_resolver import TrajOptResolver
class TrajOptMultiRobot():
def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles,
rob_dist_weight, obs_dist_weight, control_weight, time_weight):
self.num_robots = num_robots
self.starts = starts
self.goals = goals
self.circle_obs = circle_obstacles
self.rect_obs = rectangle_obstacles
self.rob_dist_weight = rob_dist_weight
self.obs_dist_weight = obs_dist_weight
self.control_weight =control_weight
self.time_weight = time_weight
self.robot_radius = MX(robot_radius)
def dist(self, robot_position, circle):
"""
Returns the distance between a robot and a circle
params:
robot_position [x,y]
circle [x,y,radius]
"""
return sumsqr(robot_position - transpose(circle[:2]))
def apply_quadratic_barrier(self, d_max, d, c):
"""
Applies a quadratic barrier to some given distance. The quadratic barrier
is a soft barrier function. We are using it for now to avoid any issues with
invalid initial solutions, which hard barrier functions cannot handle.
params:
d (float): distance to the obstacle
c (float): controls the steepness of curve.
higher c --> gets more expensive faster as you move toward obs
d_max (float): The threshold distance at which the barrier starts to apply
"""
return c*fmax(0, d_max-d)**2
def log_normal_barrier(self, sigma, d, c):
return c*fmax(0, 2-(d/sigma))**2.5
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
outputs:
- problem (dict): dictionary containing the optimization problem
and the decision variables
"""
opti = Opti() # Optimization problem
# ---- decision variables --------- #
X = opti.variable(self.num_robots*3, N+1) # state trajectory (x,y,heading)
pos = X[:self.num_robots*2,:] # position is the first two values
x = pos[0::2,:]
y = pos[1::2,:]
heading = X[self.num_robots*2:,:] # heading is the last value
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
# ---- 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):
for k in range(N):
for c in range(circle_obs.shape[0]):
circle = circle_obs[c, :]
# d = self.dist(pos[2*r : 2*(r+1), k], circle) - self.robot_radius - circle[2]
d = sumsqr(pos[2*r : 2*(r+1), k] - transpose(circle[:2])) - 2*self.robot_radius - circle[2]
dist_to_other_obstacles += self.apply_quadratic_barrier(3*(self.robot_radius + circle[2]), d, 10)
# ------ 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:
d = sumsqr(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k]) - 2*self.robot_radius
dist_to_other_robots += self.apply_quadratic_barrier(4*self.robot_radius, d, 12)
# ---- dynamics constraints ---- #
dt = T/N # length of a control interval
pi = [3.14159]*self.num_robots
pi = np.array(pi)
pi = DM(pi)
for k in range(N): # loop over control intervals
dxdt = vel[:,k] * cos(heading[:,k])
dydt = vel[:,k] * sin(heading[:,k])
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, 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(fmod(heading[:,k+1] - heading[:,k] + pi, 2*pi) - pi)
# ------ Distance to goal penalty ------ #
dist_to_goal = 0
for r in range(self.num_robots):
# calculate the distance to the goal in the final control interval
dist_to_goal += sumsqr(pos[2*r : 2*(r+1), -1] - self.goals[r])
# ------ cost function ------ #
opti.minimize(self.rob_dist_weight*dist_to_other_robots
+ self.obs_dist_weight*dist_to_other_obstacles
+ self.time_weight*T
+ self.control_weight*heading_diff_penalty
+ 20*dist_to_goal
+ 1*sumsqr(U))
# ------ 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)
# ------ 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(y_range[0]+self.robot_radius,y,y_range[1]-self.robot_radius))
opti.subject_to(opti.bounded(0,T,100))
# ------ initial conditions ------ #
for r in range(self.num_robots):
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] <= 1**2)
return {'opti':opti, 'X':X, 'U':U,'T':T}
def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
opti = problem['opti']
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
input:
- N (int): the number of control intervals
- x_range (tuple):
- y_range (tuple):
"""
problem = self.problem_setup(N, x_range, y_range)
solver_options = {'ipopt.print_level': 0,
'print_time': 0,
# 'ipopt.tol': 5,
# 'ipopt.acceptable_tol': 5,
# 'ipopt.acceptable_iter': 10
}
results = self.solve_optimization_problem(problem, initial_guesses, solver_options)
if results['status'] == 'failed':
return None, None, None, None, None
X = results['X']
sol = results['solution']
# 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(circle_obs, num_robots, starts, goals, x_opt, initial_guess, x_range, y_range):
fig, ax = plt.subplots()
......@@ -449,7 +232,7 @@ if __name__ == "__main__":
# load the yaml file
import yaml
with open("tests/initial_guesses.yaml") as file:
with open("guided_mrmp/tests/initial_guesses.yaml") as file:
settings = yaml.load(file, Loader=yaml.FullLoader)
seed = 1123581
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment