planner with turn rate

Author: Demon16th

GEMstack/onboard/planning/racing_planning.py

@@ -175,6 +175,146 @@ def waypoint_generate(vehicle_state, cones, cone_idx):
             target_heading -= 2 * np.pi
 
     return scenario, flex_wps_list, fixed_wp, target_heading
+
+def waypoint_generate_relaxed(vehicle_state, cones, cone_idx):
+    """
+    Generate waypoints based on the scenario.
+    Args:
+        vehicle_state: The state of the vehicle.
+        cone_state: The state of the cones.
+    Returns:
+        Tuple[str, Tuple[float, float], Tuple[float, float]]:
+            scenario: detected scenario type
+            flex_wp: flexible waypoint (used to maneuver)
+            fixed_wp: fixed waypoint (goal position)
+    """
+    scenario, cone_position = scenario_check(vehicle_state, [cones[cone_idx]])
+    car_position = np.array(vehicle_state['position'])
+    car_heading = vehicle_state['heading']  # in radians
+    current_heading = car_heading
+    target_heading = car_heading
+
+    # ===== Parameters =====
+    u_turn_radius = 11.5      # Radius for U-turn
+    offset = 2.0                # Offset for left/right pass
+    lookahead_distance = 10.0   # Distance ahead for fixed point
+    # ======================
+
+    # Direction vector based on heading
+    heading_vector = np.array([np.cos(car_heading), np.sin(car_heading)])
+    
+    # Vector perpendicular to heading (to determine left/right)
+    perpendicular_vector = np.array([-np.sin(car_heading), np.cos(car_heading)])
+
+    if cone_idx > 0:
+        prev_cone_position = np.array((cones[cone_idx - 1]['x'], cones[cone_idx - 1]['y']))
+        cone_position = np.array((cones[cone_idx]['x'], cones[cone_idx]['y']))
+        target_heading = np.arctan2(cone_position[1] - prev_cone_position[1],
+                                    cone_position[0] - prev_cone_position[0])
+
+    if scenario == 'standing':
+        # U-turn: Generate points in a semi-circular arc around the cone
+        cone = np.array(cone_position)
+        
+        # Number of waypoints to generate for the arc
+        num_arc_points = 9
+        
+        # Generate waypoints in a smooth semi-circular pattern on the RIGHT side
+        flex_wps_list = []
+        
+        # Create a semi-circular arc from 0 to π
+        # But negate perpendicular_vector to go to the right side
+        for i in range(num_arc_points):
+            # Calculate angle in the semi-circle (0 to π)
+            angle = i * np.pi * 1 / (num_arc_points - 1)
+            
+            # MODIFIED: Negative perpendicular_vector to go to right side of cone
+            # This creates a counter-clockwise turn around the cone
+            wp = cone - u_turn_radius * (
+                perpendicular_vector * np.cos(angle) - 
+                heading_vector * np.sin(angle)
+            )
+            
+            flex_wps_list.append(wp)
+            
+        # Fixed waypoint: behind the cone after U-turn (also on right side)
+        fixed_wp = cone + u_turn_radius * perpendicular_vector
+
+        target_heading = target_heading + np.pi       
+
+    elif scenario == 'left':
+        cone = np.array(cone_position)
+
+        # Flexible waypoint: go to the left of the cone
+        flex_wp1 = cone + offset * perpendicular_vector
+        flex_wps_list = [flex_wp1]
+
+        # Fixed waypoint: forward after passing the cone
+        # fixed_wp = flex_wp + heading_vector * lookahead_distance - offset * perpendicular_vector * 2
+        fixed_wp = flex_wp1 + heading_vector * lookahead_distance - offset * perpendicular_vector
+
+    elif scenario == 'right':
+        cone = np.array(cone_position)
+
+        # Flexible waypoint: go to the right of the cone
+        flex_wp1 = cone - offset * perpendicular_vector
+        flex_wps_list = [flex_wp1]
+
+        # Fixed waypoint: forward after passing the cone
+        # fixed_wp = flex_wp + heading_vector * lookahead_distance + offset * perpendicular_vector * 2
+        fixed_wp = flex_wp1 + heading_vector * lookahead_distance + offset * perpendicular_vector
+
+    # TODO: move to a different setting instead of as a scenario
+    elif scenario == '90turn':
+        turn_vector = np.array([-np.sin(car_heading + np.pi / 6), np.cos(car_heading + np.pi / 6)])
+        cone = np.array(cone_position)
+        distance_to_cone = np.linalg.norm(car_position - cone)
+
+        flex_wps_list = []
+        if distance_to_cone > 7:
+            steps = int(distance_to_cone // 6)
+            print("steps:", steps)
+
+            final_flex_wp = cone + 1.0 * perpendicular_vector + heading_vector * 1.0
+            fixed_wp = cone - turn_vector * 2.5 + heading_vector * 0.0
+
+            cone_direction = (final_flex_wp - car_position) / np.linalg.norm(final_flex_wp - car_position)
+            for i in range(steps - 2):
+                intermediate_wp = car_position + cone_direction * ((i + 1) * 6)
+                flex_wps_list.append(intermediate_wp)
+
+            flex_wps_list.append(final_flex_wp)
+        else:
+            flex_wp = cone + 1.0 * perpendicular_vector + heading_vector * 1.0
+            fixed_wp = cone - turn_vector * 2.5 + heading_vector * 0.0
+            flex_wps_list = [flex_wp]
+
+    else:
+        flex_wps_list = None
+        fixed_wp = None
+
+    target_heading = (target_heading + np.pi) % (2 * np.pi) - np.pi
+    current_heading = (current_heading + np.pi) % (2 * np.pi) - np.pi
+
+    if abs(target_heading - current_heading) > np.pi: 
+        if target_heading < current_heading:
+            target_heading += 2 * np.pi
+        else:
+            target_heading -= 2 * np.pi
+
+    # For visualization only
+    for i, wp in enumerate(flex_wps_list):
+        plt.plot(wp[0], wp[1], 'ro')  # Plot all waypoints in red
+        plt.text(wp[0], wp[1], f'wp{i}', fontsize=9)
+        
+    plt.plot(fixed_wp[0], fixed_wp[1], 'bo')  # Plot fixed waypoint in blue
+    plt.plot(cone[0], cone[1], 'gx')  # Plot cone in green
+    plt.plot(car_position[0], car_position[1], 'ks')  # Plot car position
+    plt.axis('equal')  # Equal aspect ratio for better visualization
+    plt.grid(True)
+    plt.show()
+
+    return scenario, flex_wps_list, fixed_wp, target_heading
 
 def velocity_profiling(path, acceleration, deceleration, max_speed, current_speed, lateral_acc_limit):
     """
@@ -431,6 +571,172 @@ 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=3.0, v_max=11.0,
+                          T_min = 0.5, T_max = 500.0,
+                          waypoints=None, waypoint_penalty_weight=1.0):
+    """
+    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):
+    #     x_, y_, psi_, c_, v_, eps_ = np.split(p, [N - 1, 2 * (N - 1), 3 * (N - 1), 4 * (N - 1), 5 * (N - 1)])
+    #     c_seq = np.concatenate(([init_state['c']], c_))
+    #     v_seq = np.concatenate(([init_state['v']], v_))
+    #     x_final, y_final, psi_final, c_final = x_[-1], y_[-1], psi_[-1], c_[-1]
+
+    #     cost_c = w_c * np.sum(c_seq ** 2)
+    #     cost_eps = w_eps * np.sum(eps_ ** 2)
+    #     v_mean = np.mean(v_seq)
+    #     cost_vvar = w_vvar * np.mean((v_seq - v_mean) ** 2)
+
+    #     cost_terminal = w_terminal * (
+    #         (x_final - final_state['x']) ** 2 +
+    #         (y_final - final_state['y']) ** 2 +
+    #         (psi_final - final_state['psi']) ** 2 * 100 + 
+    #         (c_final - final_state['c']) ** 2 * 100
+    #     )
+
+    #     cost_waypoints = 0.0
+    #     
+
+    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)
+        # print("x_: " +str(x_))
+        
+        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
+    
+        x_ = p[:N -1]
+        y_ = p[N - 1:2 * (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 
+        # 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 = 10.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.
@@ -796,7 +1102,8 @@ def plan_full_slalom_trajectory(vehicle_state, cones):
     current_heading = vehicle_state['heading']
 
     for cone_idx, cone in enumerate(cones):
-        scenario, flex_wps, fixed_wp, target_heading = waypoint_generate(vehicle_state, cones, cone_idx)
+        # scenario, flex_wps, fixed_wp, target_heading = waypoint_generate(vehicle_state, cones, cone_idx)
+        scenario, flex_wps, fixed_wp, target_heading = waypoint_generate_relaxed(vehicle_state, cones, cone_idx)
         if not flex_wps or fixed_wp is None:
             continue
         # flex_wp = flex_wps[0]
@@ -810,7 +1117,8 @@ def plan_full_slalom_trajectory(vehicle_state, cones):
             'x': fixed_wp[0], 'y': fixed_wp[1], 'psi': target_heading, 'c': 0.0
         }
 
-        x, y, psi, c, v, eps, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps)
+        # x, y, psi, c, v, eps, _ = trajectory_generation(init_state, final_state, waypoints=flex_wps)
+        x, y, psi, c, v, a, eps, T_total, final_error = trajectory_generation_dynamics(init_state, final_state, waypoints=flex_wps)
         x_all.extend(x)
         y_all.extend(y)
         v_all.extend(v)
@@ -827,22 +1135,22 @@ def plan_full_slalom_trajectory(vehicle_state, cones):
 
         current_pos = np.array([x[-1], y[-1]])
 
-    # # Plot overall trajectory
-    # plt.figure()
-    # plt.plot(x_all, y_all, label='Overall Trajectory')
-
-    # # Plot cones
-    # for i, cone in enumerate(cones):
-    #     plt.scatter(cone['x'], cone['y'], color='orange', s=10, label='Cone' if i == 0 else "")
-    #     plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange')
-
-    # plt.title('4-Cone Full Course Trajectory')
-    # plt.xlabel('X')
-    # plt.ylabel('Y')
-    # plt.legend()
-    # plt.axis('equal')
-    # plt.grid(True)
-    # plt.show()
+    # Plot overall trajectory
+    plt.figure()
+    plt.plot(x_all, y_all, label='Overall Trajectory')
+
+    # Plot cones
+    for i, cone in enumerate(cones):
+        plt.scatter(cone['x'], cone['y'], color='orange', s=10, label='Cone' if i == 0 else "")
+        plt.text(cone['x'], cone['y'] + 0.5, f'C{i+1}', ha='center', fontsize=9, color='darkorange')
+
+    plt.title('4-Cone Full Course Trajectory')
+    plt.xlabel('X')
+    plt.ylabel('Y')
+    plt.legend()
+    plt.axis('equal')
+    plt.grid(True)
+    plt.show()
 
     combined_xy = [[x, y] for x, y in zip(x_all, y_all)]
     path = Path(ObjectFrameEnum.START,combined_xy)

GEMstack/onboard/planning/velocity_profile.py

@@ -278,9 +278,9 @@ def compute_velocity_profile(points, plot=True):
     wheelbase = settings.get('vehicle.geometry.wheelbase')
 
     points = np.array(points)
-    # print(points)
+    print(points)
     xs, ys = points[:,0], points [:,1]
-    # print(xs)
+    print(xs)
 
     dx = np.diff(xs)
     dy = np.diff(ys)