added turn rate and accel limits to planner in

trajectory_generation_dynamics

Author: diegoasfmravpp

GEMstack/onboard/planning/racing_planning.py

@@ -295,6 +295,150 @@ 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.