consistency validation

consistency_validation.py

@@ -8,24 +8,6 @@
 if __name__ == '__main__':
     np.random.seed(42)  # For reproducibility
 
-    # for i in tqdm(range(1_000_000)):
-    #     # Sample random boundary conditions
-    #     start_pos = np.random.uniform(-100, 100)
-    #     end_pos = np.random.uniform(-100, 100)
-    #     start_vel = np.random.uniform(-10, 10)
-    #     end_vel = np.random.uniform(-10, 10)
-    #     vmax = np.random.uniform(10, 20)
-    #     amax = np.random.uniform(2, 10)
-
-    #     trajectories, optimal_label = univariate_time_optimal_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, amax)
-    #     T = trajectories[optimal_label][0]
-
-    #     trajectories, optimal_label = minimum_acceleration_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, T, amax)
-    #     amin = trajectories[optimal_label][0]
-
-    #     assert np.isclose(amin, amax)
-    #     assert amin <= amax
-
     for i in tqdm(range(1_000_000)):
         # Sample random boundary conditions
         start_pos = np.random.uniform(-100, 100)
@@ -36,12 +18,23 @@
         amax = np.random.uniform(2, 10)
 
         trajectories, optimal_label = univariate_time_optimal_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, amax)
-        T = trajectories[optimal_label][0] * np.random.uniform(1.0, 10.0) 
-
-        if i == 281:
-            print("")
+        T = trajectories[optimal_label][0]
 
         trajectories, optimal_label = minimum_acceleration_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, T, amax)
         amin = trajectories[optimal_label][0]
 
-        assert amin <= amax
+        assert np.isclose(amin, amax)
+        assert amin <= amax
+
+    # There is an example demonstrating that as the total time increases, the required acceleration can increase rather than decrease.
+    try:
+        start_pos, end_pos = np.array([0]), np.array([5])
+        start_vel, end_vel = np.array([10]), np.array([10])
+        vmax = np.array([10])
+        amax = np.array([5])    
+        trajectories, optimal_label = univariate_time_optimal_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, amax)
+        T = trajectories[optimal_label][0] * 5
+        trajectories, optimal_label = minimum_acceleration_interpolants(start_pos, end_pos, start_vel, end_vel, vmax, T, amax)
+        amin = trajectories[optimal_label][0]
+    except:
+        print("The minimum required acceleration can exceed the maximum available acceleration although the total time increase.")

minimum_acceleration.py

@@ -80,7 +80,7 @@ def compute_p_minus_l_plus_p_plus():
     valid_trajectories = {k: v for k, v in trajectories.items() if v is not None}
     if not valid_trajectories:
         # NOTE: This problem is possible to have no solution.
-        return None
+        return None, None
 
     optimal_label = min(valid_trajectories, key=lambda k: valid_trajectories[k][0])
     amin = valid_trajectories[optimal_label][0]
@@ -90,9 +90,9 @@ def compute_p_minus_l_plus_p_plus():
         amin = np.clip(amin, 0, a_threshold)
         trajectories[optimal_label] = (amin, trajectories[optimal_label][1], trajectories[optimal_label][2])
     else:
-        return None
-        raise ValueError("Required acceleration exceeds the a threshold.")
-
+        # NOTE: The minimum required acceleration can exceed the maximum available acceleration.
+        return None, None
+        
     return trajectories, optimal_label
 
 # ------------------ Testing and Main Code ------------------
@@ -136,4 +136,4 @@ def compute_p_minus_l_plus_p_plus():
     )
 
     # Plot the trajectory based on the selected candidate.
-    plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax, T)
+    plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax, T=T, solution_type='min_accel')

plot.py

@@ -45,7 +45,7 @@ def plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax,
                 states = [get_motion_state_at_local_t(t, candidate, start_pos, start_vel, end_vel,
                                                         vmax, amax, switch_time1, switch_time2, total_time)
                           for t in t_samples]
-                title_text = f"{candidate} (Total Time: {list(total_time)[0]:.2f} s)"
+                title_text = f"{candidate} (Total Time: {float(total_time):.2f} s)"
             elif solution_type == 'min_accel':
                 # Candidate solution: (acceleration, switch_time1, switch_time2)
                 acc, switch_time1, switch_time2 = trajectories[candidate]
@@ -54,7 +54,7 @@ def plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax,
                 states = [get_motion_state_at_local_t(t, candidate, start_pos, start_vel, end_vel,
                                                         vmax, acc, switch_time1, switch_time2, T)
                           for t in t_samples]
-                title_text = f"{candidate} (Min Accel: {list(acc)[0]:.3f} m/s^2)"
+                title_text = f"{candidate} (Min Accel: {float(acc):.3f} m/s^2)"
             else:
                 raise ValueError("Unknown solution_type. Use 'time_optimal' or 'min_accel'.")
 

univariate_time_optimal.py

@@ -134,4 +134,4 @@ def compute_p_minus_l_plus_p_plus():
     )
 
     # Plot the optimal trajectory.
-    plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax, amax)
+    plot_trajectory(trajectories, start_pos, end_pos, start_vel, end_vel, vmax, amax=amax, solution_type='time_optimal')