Delete unused code

guided_mrmp/optimizers/optimizer.py

-import cvxpy as opt
-import numpy as np
-
-class Optimizer:
-    def __init__(self):
-        pass
-
-    def solve(self, cost_function, constraints, initial_guess):
-        """
-        Solve the optimization problem.
-        
-        Parameters:
-        - cost_function: Function to minimize
-        - constraints: List of constraints
-        - initial_guess: Initial guess for the optimizer
-        
-        Returns:
-        - optimal_solution: The optimal control input found by the optimizer
-        """
-        raise NotImplementedError("This method should be implemented by a specific optimizer.")
-
-class iLQR(Optimizer):
-    def __init__(self, nx, nu, control_horizon):
-        self.nx = nx
-        self.nu = nu
-        self.control_horizon = control_horizon
-
-    def solve(self, x,u,cost, constraints, initial_guess=None):
-        """
-        Solve the optimization problem.
-        
-        Parameters:
-        - cost_function: Function to minimize
-        - constraints: List of constraints
-        - initial_guess: Optional initial guess for the optimizer
-        
-        Returns:
-        - x_opt: Optimal state trajectory
-        - u_opt: Optimal control trajectory
-        """
-        # Set up variables for the optimization problem
-        # x = opt.Variable((self.nx, self.control_horizon + 1))
-        # u = opt.Variable((self.nu, self.control_horizon))
-        
-        # If initial guess is provided, set the initial values
-        # if initial_guess:
-        #     x.value = initial_guess['x']
-        #     u.value = initial_guess['u']
-
-        
-
-        prob = opt.Problem(opt.Minimize(cost), constraints)
-
-        # Solve the problem
-        prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
-        
-        if prob.status != opt.OPTIMAL:
-            raise ValueError("The optimization problem did not solve successfully.")
-        
-        # print(f"x val = {x.value}")
-        # print(f"u val = {u.value}")
-
-        return x.value, u.value
\ No newline at end of file

guided_mrmp/tests/traj_from_points_test.py

-import time
-import numpy as np
-from guided_mrmp.utils import get_traj_from_points, plot_sim, Env, Roomba
-from guided_mrmp.planners import RRTStar
-
-
-
-def test_traj_from_points():
-    T = 1  # Prediction Horizon [s]
-    DT = 0.2  # discretization step [s]
-    target_velocity = 3.0 # m/s
-    num_robots = 20
-    dynamics = Roomba()
-    radius = 10
-    N = 20
-    env = Env([-10,10], [-10,10], [], [])
-
-    # Generate start and goal positions in a circle
-    angles = np.linspace(0, 2 * np.pi, num_robots, endpoint=False)
-    robot_starts = [(radius * np.cos(angle), radius * np.sin(angle)) for angle in angles]
-    robot_goals = [(radius * np.cos(angle + np.pi), radius * np.sin(angle + np.pi)) for angle in angles]
-
-    success_count = 0
-    total_time = 0
-
-    x_hists, y_hists, h_hists, paths = [], [], [], []
-
-    for i in range(num_robots):
-        start_time = time.time()
-        start = robot_starts[i]
-        goal = robot_goals[i]
-
-        
-        try:
-            rrtstar = RRTStar(env, (start[0], start[1]), (goal[0], goal[1]), 0.5, 0.05, 500, r=2.0)
-            rrtstarpath = rrtstar.run()
-            rrtstarpath = list(reversed(rrtstarpath))
-            xs = []
-            ys = []
-            for node in rrtstarpath:
-                xs.append(node[0])
-                ys.append(node[1])
-
-            dx = xs[1] - xs[0]
-            dy = ys[1] - ys[0]
-            theta = np.arctan2(dy, dx)
-
-            x,y,h,path = get_traj_from_points(np.array([start[0], start[1],theta]), dynamics, target_velocity, T, DT, [xs, ys])
-            x_hists.append(x)
-            y_hists.append(y)
-            h_hists.append(h)
-            paths.append(path)
-            success_count += 1
-            
-        except Exception as e:
-            print(f"Robot {i} failed to find a valid trajectory: {e}")
-
-        end_time = time.time()
-        total_time += (end_time - start_time)
-        print(f"Robot {i} took {end_time - start_time:.2f} seconds to find a trajectory")
-
-    print(f"Success rate: {success_count}/{num_robots}")
-    print(f"Average time per robot: {total_time / num_robots:.2f} seconds")
-
-    plot_sim(x_hists, y_hists, h_hists, paths)
-    
-
-if __name__ == "__main__":
-
-    import os
-    import random
-    seed = 42
-    print(f"***Setting Python Seed {seed}***")
-    os.environ['PYTHONHASHSEED'] = str(seed)
-    np.random.seed(seed)
-    random.seed(seed)
-    test_traj_from_points()
\ No newline at end of file