Add up-to-date version of racing planning

Author: alo-20

GEMstack/onboard/planning/racing_planning.py

@@ -295,150 +295,6 @@ def dynamics_constraints(p):
 
     return x_full, y_full, psi_full, c_full, v_full, eps_, final_error
 
-######## more dynamics version
-def trajectory_generation_dynamics(init_state, final_state, N=30, Lr=1.5,
-                          a_min=-3.0, a_max=3.0,
-                          eps_min=-0.2, eps_max=0.2,
-                          v_min=2.0, v_max=11.0,
-                          T_min = 0.5, T_max = 1000.0,
-                          waypoints=None, waypoint_penalty_weight=10, waypoint_headings=None):
-    """
-    Generate a dynamically feasible trajectory between init_state and final_state
-    using curvature-based vehicle dynamics and nonlinear optimization.
-    
-    Now supports multiple waypoints.
-
-    Parameters:
-    - init_state (dict): Initial vehicle state with keys 'x', 'y', 'psi', 'c', 'v'.
-    - final_state (dict): Target vehicle state with keys 'x', 'y', 'psi', 'c'.
-    - N (int): Number of discrete time steps in the trajectory.
-    - T (float): Duration of each time step (in seconds).
-    - Lr (float): Distance from the vehicle's center to the rear axle.
-    - w_c (float): Weight for penalizing curvature (smoothness of turns).
-    - w_eps (float): Weight for penalizing curvature rate (reduces sharp steering changes).
-    - w_vvar (float): Weight for penalizing speed variance (encourages speed smoothness).
-    - w_terminal (float): Weight for penalizing final state deviation (soft constraint).
-    - v_min (float): Minimum allowed speed (in m/s).
-    - v_max (float): Maximum allowed speed (in m/s).
-    - waypoints (list or None): Optional list of (x, y) coordinates that the trajectory should pass near.
-    - waypoint_penalty_weight (float): Penalty weight for distance from waypoints (soft constraint).
-
-    Returns:
-    - x, y, psi, c, v, eps (np.ndarray): Arrays of optimized state and control values.
-    - final_error (dict): Final state errors in x, y, psi, and c.
-    """
-    def cost(p):
-        T_total = p[-1]
-        return T_total
-    
-    def dynamics_constraints(p):
-        # x_, y_, psi_, c_, v_, eps_ = np.split(p, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)])
-        # print("x_: " +str(x_))
-        split_idx = 5 *(N - 1)
-        x_, y_, psi_, c_, v_= np.split(p[:split_idx], 5)
-        
-        a = p[split_idx:split_idx + (N - 1)]
-        eps = p[split_idx + (N - 1):-1]
-        T_total = p[-1]
-        T_k = T_total / (N - 1)
-        
-        constraints = []
-        x_prev, y_prev, psi_prev, c_prev, v_prev = init_state['x'], init_state['y'], init_state['psi'], init_state['c'], init_state['v']
-        
-        for k in range(N - 1):
-            dx = v_prev * np.cos(psi_prev + c_prev * Lr) * T_k
-            dy = v_prev * np.sin(psi_prev + c_prev * Lr) * T_k
-            dpsi = v_prev * c_prev * T_k
-            dv = a[k] * T_k
-            dc = eps[k] * T_k
-            constraints.extend([
-                x_[k] - (x_prev + dx),
-                y_[k] - (y_prev + dy),
-                psi_[k] - (psi_prev + dpsi),
-                v_[k] - (v_prev + dv),
-                c_[k] - (c_prev + dc)
-            ])
-            x_prev, y_prev, psi_prev, c_prev, v_prev = x_[k], y_[k], psi_[k], c_[k], v_[k]
-        return constraints
-    
-    def waypoint_penalty(p):
-        if waypoints is None or len(waypoints) == 0:
-            return 0.0
-    
-        split_idx = 5 *(N - 1)
-        x_, y_, psi_, c_, v_= np.split(p[:split_idx], 5)
-        # x_ = p[:N -1]
-        # y_ = p[N - 1:2 * (N-1)]
-        # psi_ = [ 3 * (N - 1)]
-        # Calculate equally spaced indices for each waypoint
-        num_waypoints = len(waypoints)
-        indices = [int((i + 1) * (N - 1) / (num_waypoints + 1)) for i in range(num_waypoints)]
-        penalty = 0.0
-        alignment_threshold = 0.3
-        # Sum penalties for each waypoint at its corresponding index
-        for wp_idx, waypoint in enumerate(waypoints):
-            traj_idx = indices[wp_idx]
-            penalty += waypoint_penalty_weight * (
-                (x_[traj_idx] - waypoint[0])**2 + (y_[traj_idx] - waypoint[1])**2
-            )
-        return penalty
-
-    # Initial guesses
-    x_vals = np.linspace(init_state['x'], final_state['x'], N)
-    y_vals = np.linspace(init_state['y'], final_state['y'], N)
-    psi_vals = np.linspace(init_state['psi'], final_state['psi'], N)
-    c_vals = np.linspace(init_state['c'], final_state['c'], N)
-    v_vals = np.ones(N) * init_state['v']
-
-    a_vals = np.zeros(N - 1)
-    eps_vals = np.zeros(N - 1)
-    T_guess = 60.0
-
-    p0 = np.concatenate([x_vals[1:], y_vals[1:], psi_vals[1:], c_vals[1:], v_vals[1:], 
-                         a_vals, eps_vals, [T_guess]])
-    num_vars = len(p0)
-    bounds = ([(None, None)] * (4 * (N - 1)) + 
-              [(v_min, v_max)] * (N - 1) + 
-              [(a_min, a_max)] * (N - 1) +
-              [(eps_min, eps_max)] * (N - 1) +
-              [(T_min, T_max)]
-    )
-    def total_cost(p):
-        return cost(p) + waypoint_penalty(p)
-    
-    result = minimize(total_cost, 
-                      p0, 
-                      bounds=bounds,
-                      constraints={'type': 'eq', 'fun': dynamics_constraints},
-                      options={'maxiter': 1000})
-    
-    # print("result.x:" + str(result.x))
-    if not result.success:
-        raise RuntimeError("Optimization failed")
-
-    split_idx = 5 *(N - 1)
-    x_, y_, psi_, c_, v_= np.split(result.x[:split_idx], 5)
-    
-    a = result.x[split_idx:split_idx + (N - 1)]
-    eps = result.x[split_idx + (N - 1):-1]
-    T_total = result.x[-1]
-    
-    x_full = np.concatenate(([init_state['x']], x_))
-    y_full = np.concatenate(([init_state['y']], y_))
-    psi_full = np.concatenate(([init_state['psi']], psi_))
-    c_full = np.concatenate(([init_state['c']], c_))
-    v_full = np.concatenate(([init_state['v']], v_))
-
-    final_error = {
-        'x_error': abs(x_full[-1] - final_state['x']),
-        'y_error': abs(y_full[-1] - final_state['y']),
-        'psi_error': abs(psi_full[-1] - final_state['psi']),
-        'c_error': abs(c_full[-1] - final_state['c']),
-    }
-
-    return x_full, y_full, psi_full, c_full, v_full, a, eps, T_total, final_error
-#########
-
 def feasibility_check(trajectory, cone_map, car_width=2.0, safety_margin=0.3, v=10.0, Lr=1.5, T=0.1):
     """
     Check if the car trajectory collides with any cones.
@@ -833,127 +689,91 @@ def test_4_cone_slalom():
     def plot_trajectory(x, y, v, c, eps, waypoint=None):
         plt.figure(figsize=(12, 10))
 
-    # Trajectory plot
-    plt.subplot(4, 1, 1)
-    plt.plot(x, y, label="Trajectory")
-    plt.scatter([x[0], x[-1]], [y[0], y[-1]], color='red', label="Start/End")
-    
-    if waypoint is not None:
-        plt.scatter(*waypoint, color='purple', s=60, marker='X', label="Waypoint")
-        plt.annotate("Waypoint", (waypoint[0], waypoint[1]), textcoords="offset points", xytext=(5,5), color='purple')
+        # Trajectory plot
+        plt.subplot(4, 1, 1)
+        plt.plot(x, y, label="Trajectory")
+        plt.scatter([x[0], x[-1]], [y[0], y[-1]], color='red', label="Start/End")
+        
+        if waypoint is not None:
+            plt.scatter(*waypoint, color='purple', s=60, marker='X', label="Waypoint")
+            plt.annotate("Waypoint", (waypoint[0], waypoint[1]), textcoords="offset points", xytext=(5,5), color='purple')
 
-    plt.axis('equal')
-    plt.ylabel("y (m)")
-    plt.title("Trajectory")
-    plt.grid(True)
-    plt.legend()
+        plt.axis('equal')
+        plt.ylabel("y (m)")
+        plt.title("Trajectory")
+        plt.grid(True)
+        plt.legend()
 
-    # Speed plot
-    plt.subplot(4, 1, 2)
-    plt.plot(v, label="Speed (v)", color="blue")
-    plt.ylabel("Speed (m/s)")
-    plt.grid(True)
-    plt.legend()
+        # Speed plot
+        plt.subplot(4, 1, 2)
+        plt.plot(v, label="Speed (v)", color="blue")
+        plt.ylabel("Speed (m/s)")
+        plt.grid(True)
+        plt.legend()
 
-    # Curvature plot
-    plt.subplot(4, 1, 3)
-    plt.plot(c, label="Curvature (c)", color="orange")
-    plt.ylabel("Curvature (1/m)")
-    plt.grid(True)
-    plt.legend()
+        # Curvature plot
+        plt.subplot(4, 1, 3)
+        plt.plot(c, label="Curvature (c)", color="orange")
+        plt.ylabel("Curvature (1/m)")
+        plt.grid(True)
+        plt.legend()
 
-    # Curvature rate plot
-    plt.subplot(4, 1, 4)
-    plt.plot(eps, label="Curvature Rate (ε)", color="green")
-    plt.xlabel("Step")
-    plt.ylabel("ε (1/m²)")
-    plt.grid(True)
-    plt.legend()
+        # Curvature rate plot
+        plt.subplot(4, 1, 4)
+        plt.plot(eps, label="Curvature Rate (ε)", color="green")
+        plt.xlabel("Step")
+        plt.ylabel("ε (1/m²)")
+        plt.grid(True)
+        plt.legend()
 
         plt.tight_layout()
         plt.show()
 
-###### Test case 1: pass a cone in slalom
-def trajectory_generation_test1():
-    # Init and final
-    init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0}
-    final_state = {'x': 15.0, 'y': 0.0, 'psi': np.pi / 20000000, 'c': np.pi / 20000000}
-    waypoint = (8.0, 6.0)
-
-    # Solve
-    x, y, psi, c, v, eps, final_error = trajectory_generation(
-        init_state, final_state, waypoint=waypoint
-    )
-    plot_trajectory(x, y, v, c, eps, waypoint)
+    # --- Test Case 1: pass a cone in slalom ---
+    def trajectory_generation_test1():
+        # Init and final
+        init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0}
+        final_state = {'x': 15.0, 'y': 0.0, 'psi': np.pi / 20000000, 'c': np.pi / 20000000}
+        waypoint = (8.0, 6.0)
 
-    # Error
-    print("\nFinal State Errors:")
-    for k, e in final_error.items():
-        print(f"{k}: {e:.6f}")
-
-###### Test case 2: 90 degree turn
-def trajectory_generation_test2():
-    # Init and final
-    init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0}
-    final_state = {'x': 15.0, 'y': 15.0, 'psi': np.pi / 2, 'c': np.pi / 2}
-    waypoint = (13.0, 3.0)
-
-    # Solve
-    x, y, psi, c, v, eps, final_error = trajectory_generation(
-        init_state, final_state, waypoint=waypoint
-    )
-    plot_trajectory(x, y, v, c, eps, waypoint)
-
-    # Error
-    print("\nFinal State Errors:")
-    for k, e in final_error.items():
-        print(f"{k}: {e:.6f}")
-
-
-
-def feasibility_check(trajectory, cone_map, car_width=2.0, safety_margin=0.3, v=10.0, Lr=1.5, T=0.1):
-    """
-    Check if the car trajectory collides with any cones.
-
-    Parameters:
-    - trajectory: list of (y, psi, c) states
-    - cone_map: list of (x, y) cone positions
-    - car_width: width of the vehicle in meters
-    - safety_margin: buffer around the vehicle
-    - v: vehicle constant speed (used for x position estimation)
-    - Lr: distance to rear axle
-    - T: time step
-
-    Returns:
-    - feasible: True if no collisions
-    - collisions: list of indices of cones that were collided with (for plotting purpose)
-    - x_vals, y_vals: trajectory positions for plotting (for plotting purpose)
-    """
-    y_vals, psi_vals, c_vals = zip(*trajectory)
-    x_vals = [0.0]
-    for i in range(1, len(trajectory)):
-        dx = v * np.cos(psi_vals[i-1] + c_vals[i-1] * Lr) * T
-        x_vals.append(x_vals[-1] + dx)
-
-    collision_radius = (car_width / 2.0) + safety_margin
-    collisions = []
-
-    for j, (cone_x, cone_y) in enumerate(cone_map):
-        for x, y in zip(x_vals, y_vals):
-            if np.hypot(x - cone_x, y - cone_y) < collision_radius:
-                collisions.append(j)
-                break
+        # Solve
+        x, y, psi, c, v, eps, final_error = trajectory_generation(
+            init_state, final_state, waypoint=waypoint
+        )
+        plot_trajectory(x, y, v, c, eps, waypoint)
 
-    feasible = len(collisions) == 0
-    return feasible, collisions, x_vals, y_vals
+        # Error
+        print("\nFinal State Errors:")
+        for k, e in final_error.items():
+            print(f"{k}: {e:.6f}")
+
+    # --- Test case 2: 90 degree turn ---
+    def trajectory_generation_test2():
+        # Init and final
+        init_state = {'x': 0.0, 'y': 0.0, 'psi': 0.0, 'c': 0.0, 'v': 5.0}
+        final_state = {'x': 15.0, 'y': 15.0, 'psi': np.pi / 2, 'c': np.pi / 2}
+        waypoint = (13.0, 3.0)
+
+        # Solve
+        x, y, psi, c, v, eps, final_error = trajectory_generation(
+            init_state, final_state, waypoint=waypoint
+        )
+        plot_trajectory(x, y, v, c, eps, waypoint)
 
-# --- Test Case ---
-def test_feasibility_check():
-    N = 50
-    y_traj = np.linspace(0, 10, N)
-    psi_traj = np.linspace(0, 0.1, N)
-    c_traj = np.linspace(0, 0.2, N)
-    trajectory = list(zip(y_traj, psi_traj, c_traj))
+        # Error
+        print("\nFinal State Errors:")
+        for k, e in final_error.items():
+            print(f"{k}: {e:.6f}")
+
+    #############
+    #########################
+    # --- Test Case ---
+    def test_feasibility_check():
+        N = 50
+        y_traj = np.linspace(0, 10, N)
+        psi_traj = np.linspace(0, 0.1, N)
+        c_traj = np.linspace(0, 0.2, N)
+        trajectory = list(zip(y_traj, psi_traj, c_traj))
 
         # Cone map near the path
         cone_map = [(5.0, 1.0), (10.0, 1.5), (15.0, 2.0), (25.0, 4.0), (25.0, 10.0), (16.0, 9.0), (40.0, 5.0)]
@@ -1206,8 +1026,8 @@ def test_planning(case='slalom', test_loop=2):
             final_state = {'y': wpt_fixed[1], 'psi': 0.0, 'c': 0.0}
 
             y_traj, psi_traj, c_traj, eps_traj = trajectory_generation(init_state, final_state)
-            plot_trajectory(y_traj, psi_traj, c_traj, label="Generated trajectory")
-            plot_dynamics(psi_traj, c_traj, eps_traj)
+            # plot_trajectory(y_traj, psi_traj, c_traj, label="Generated trajectory")
+            # plot_dynamics(psi_traj, c_traj, eps_traj)
 
             # Iterate
             vehicle_state = drive(vehicle_state)