From 0b22b30085495f4fd87488c000145569b1bf5193 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Tue, 29 Oct 2024 09:23:09 -0500
Subject: [PATCH] delete the old traj opt class in test file, use the one in
the main part of code
---
guided_mrmp/tests/initial_guesses.yaml | 2 +-
guided_mrmp/tests/test_traj_opt.py | 219 +------------------------
2 files changed, 2 insertions(+), 219 deletions(-)
diff --git a/guided_mrmp/tests/initial_guesses.yaml b/guided_mrmp/tests/initial_guesses.yaml
index 204a670..24fa3f5 100644
--- a/guided_mrmp/tests/initial_guesses.yaml
+++ b/guided_mrmp/tests/initial_guesses.yaml
@@ -22,7 +22,7 @@ cost_weights:
N: 30
-num_trials: 10
+num_trials: 1
control_point_distance: -.5
diff --git a/guided_mrmp/tests/test_traj_opt.py b/guided_mrmp/tests/test_traj_opt.py
index dad0288..7421b16 100644
--- a/guided_mrmp/tests/test_traj_opt.py
+++ b/guided_mrmp/tests/test_traj_opt.py
@@ -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
--
GitLab