Planning/friction ellipse

config/planning/eufs.yaml

@@ -25,7 +25,7 @@ planning:
   smoothing_spline_coeffs_ratio: 3.0
   smoothing_min_path_point_distance: 0
   smoothing_use_path_smoothing: true
-  smoothing_use_optimization: false
+  smoothing_use_optimization: true
   smoothing_car_width: 1.2
   smoothing_safety_margin: 0.3
   smoothing_curvature_weight: 100.0
@@ -34,10 +34,10 @@ planning:
   smoothing_max_iterations: 4000
   smoothing_tolerance: 1e-5     
   #---------------------- Velocity Planning (vp_) -----------------------
-  vp_minimum_velocity: 4.0
+  vp_minimum_velocity: 2.0
   vp_braking_acceleration: -3.0
-  vp_acceleration: 8.0
-  vp_normal_acceleration: 6.0
+  vp_acceleration: 4.0
+  vp_lateral_acceleration: 3.0
   vp_use_velocity_planning: true
   vp_desired_velocity: 20.0
   #---------------------- Simulation Config (simulation_) ---------------------------

config/planning/pacsim.yaml

@@ -25,7 +25,7 @@ planning:
   smoothing_spline_coeffs_ratio: 3.0
   smoothing_min_path_point_distance: 0
   smoothing_use_path_smoothing: true
-  smoothing_use_optimization: false
+  smoothing_use_optimization: true
   smoothing_car_width: 1.2
   smoothing_safety_margin: 0.3
   smoothing_curvature_weight: 100.0
@@ -34,16 +34,14 @@ planning:
   smoothing_max_iterations: 4000
   smoothing_tolerance: 1e-5     
   #---------------------- Velocity Planning (vp_) -----------------------
-  vp_minimum_velocity: 4.0
+  vp_minimum_velocity: 2.0
   vp_braking_acceleration: -3.0
-  vp_acceleration: 8.0
-  vp_normal_acceleration: 6.0
+  vp_acceleration: 4.0
+  vp_lateral_acceleration: 3.0
   vp_use_velocity_planning: true
   vp_desired_velocity: 20.0
   #---------------------- Simulation Config (simulation_) ---------------------------
   simulation_publishing_visualization_msgs: true
-
-
 
 
 # Parameters for EBS Test

config/planning/vehicle.yaml

@@ -25,7 +25,7 @@ planning:
   smoothing_spline_coeffs_ratio: 3.0
   smoothing_min_path_point_distance: 0
   smoothing_use_path_smoothing: true
-  smoothing_use_optimization: false
+  smoothing_use_optimization: true
   smoothing_car_width: 1.2
   smoothing_safety_margin: 0.3
   smoothing_curvature_weight: 100.0
@@ -37,7 +37,7 @@ planning:
   vp_minimum_velocity: 2.0
   vp_braking_acceleration: -3.0
   vp_acceleration: 4.0
-  vp_normal_acceleration: 6.0
+  vp_lateral_acceleration: 5.0
   vp_use_velocity_planning: true
   vp_desired_velocity: 20.0
   #---------------------- Simulation Config (simulation_) ---------------------------

src/planning/include/config/planning_config.hpp

@@ -52,7 +52,7 @@ struct PlanningParameters {
   double vp_minimum_velocity_;
   double vp_braking_acceleration_;
   double vp_acceleration_;
-  double vp_normal_acceleration_;
+  double vp_lateral_acceleration_;
   bool vp_use_velocity_planning_;
   double vp_desired_velocity_;
 
@@ -94,7 +94,7 @@ struct PlanningConfig {
                     params.simulation_using_simulated_se_),
         velocity_planning_(params.vp_minimum_velocity_, params.vp_desired_velocity_,
                            params.vp_braking_acceleration_, params.vp_acceleration_,
-                           params.vp_normal_acceleration_, params.vp_use_velocity_planning_),
+                           params.vp_lateral_acceleration_, params.vp_use_velocity_planning_),
         skidpad_(params.skidpad_minimum_cones_, params.skidpad_tolerance_) {}
 };
 

src/planning/include/config/velocity_config.hpp

@@ -25,9 +25,9 @@ struct VelocityPlanningConfig {
   double acceleration_;
 
   /**
-   * @brief The maximum normal acceleration.
+   * @brief Maximum lateral acceleration.
    */
-  double normal_acceleration_;
+  double lateral_acceleration_;
 
   /**
    * @brief Flag to enable/disable velocity planning.
@@ -42,19 +42,19 @@ struct VelocityPlanningConfig {
         desired_velocity_(5.0),
         braking_acceleration_(-4.0),
         acceleration_(7.0),
-        normal_acceleration_(7.0),
+        lateral_acceleration_(7.0),
         use_velocity_planning_(true) {}
 
   /**
    * @brief Parameterized constructor.
    */
   VelocityPlanningConfig(double minimum_velocity, double desired_velocity,
                          double braking_acceleration, double acceleration,
-                         double normal_acceleration, bool use_velocity_planning)
+                         double lateral_acceleration, bool use_velocity_planning)
       : minimum_velocity_(minimum_velocity),
         desired_velocity_(desired_velocity),
         braking_acceleration_(braking_acceleration),
         acceleration_(acceleration),
-        normal_acceleration_(normal_acceleration),
+        lateral_acceleration_(lateral_acceleration),
         use_velocity_planning_(use_velocity_planning) {}
 };

src/planning/include/planning/velocity_planning.hpp

@@ -11,9 +11,13 @@ using PathPoint = common_lib::structures::PathPoint;
  * @brief Computes velocity profiles for a planned path based on curvature and dynamics constraints.
  *
  * The VelocityPlanning class generates a velocity profile along a path by:
- * - Estimating curvature (via circle fitting between consecutive path points),
+ * - Estimating curvature using the Menger curvature formula (circle fitting through three points),
  * - Deriving maximum allowable velocities from lateral acceleration limits,
- * - Propagating braking constraints backward along the path.
+ * - Applying a friction ellipse model to account for combined longitudinal and lateral tire forces,
+ * - Propagating acceleration constraints forward and braking constraints backward along the path.
+ *
+ * The velocity planner respects the friction circle constraint: a_x² + a_y² ≤ a_max²,
+ * ensuring that the vehicle stays within tire grip limits during combined cornering and acceleration/braking.
  */
 class VelocityPlanning {
 public:
@@ -29,14 +33,14 @@ class VelocityPlanning {
   explicit VelocityPlanning(VelocityPlanningConfig config) : config_(config) {}
 
   /**
-   * @brief Assigns an ideal velocity to each point of the path.
+   * @brief Assigns an velocity to each point of the path.
    *
    * @param final_path Vector of path points to update with planned velocities.
    */
   void set_velocity(std::vector<PathPoint> &final_path);
 
   /**
-   * @brief Computes velocity for track driving scenarios with repeated smoothing.
+   * @brief Computes velocity for trackdrive scenarios.
    *
    * @param final_path Vector of path points to update with planned velocities.
    */
@@ -60,39 +64,51 @@ class VelocityPlanning {
   VelocityPlanningConfig config_;
 
   /**
-   * @brief function to calculate the radius of the circle that passes through 3 points
-   *
-   * @param point1 first point
-   * @param point2 second point
-   * @param point3 third point
-   * @return radius of the circle
+   * @brief Numerical tolerance for floating-point comparisons
+   */
+  static constexpr double epsilon = 1e-9;
+
+  /**
+   * @brief Gravitational acceleration constant (m/s²)
    */
-  double find_circle_center(const PathPoint &point1, const PathPoint &point2,
-                            const PathPoint &point3);
+  static constexpr double gravity = 9.81;
 
   /**
-   * @brief Applies a forward acceleration constraint to the velocity profile.
+   * @brief Computes the curvature at a point using the Menger curvature formula.
    *
+   * @param p1 Previous point
+   * @param p2 Current point where curvature is computed
+   * @param p3 Next point
+   * @return double Curvature value. Returns 0.0 for straight sections
+   *               or nearly collinear points.
+   */
+  double find_curvature(const PathPoint &p1, const PathPoint &p2, const PathPoint &p3);
+
+  /**
+   * @brief Computes the maximum velocity at each point based on curvature constraints.
    *
-   * @param points       The sequence of path points containing positions.
-   * @param velocities   The velocity vector to be modified in-place.
+   * @param curvatures Vector of curvature values for each path point
+   * @param velocities Output vector to store the computed maximum velocities
    */
-  void acceleration_limiter(const std::vector<PathPoint> &points,
-                                              std::vector<double> &velocities);
+  void point_speed(const std::vector<double> &curvatures, std::vector<double> &velocities);
 
   /**
-   * @brief function to limit the speed of the car according to braking constraints
+   * @brief Limits velocities based on forward acceleration constraints and friction ellipse.
    *
-   * @param points path points
-   * @param velocities velocities of the path points
+   * @param points Vector of path points (used for distance calculations)
+   * @param velocities Vector of velocities to be updated (input/output)
+   * @param curvatures Vector of curvature values (used to compute lateral acceleration)
    */
-  void braking_limiter(std::vector<PathPoint> &points, std::vector<double> &velocities);
+  void acceleration_limiter(const std::vector<PathPoint> &points, std::vector<double> &velocities,
+                            const std::vector<double> &curvatures);
 
   /**
-   * @brief function to calculate the speed of the car according to the curvature of the path
+   * @brief Limits velocities based on backward braking constraints and friction ellipse.
    *
-   * @param radiuses radiuses vector of the path points
-   * @param velocities velocities vector of the path points
+   * @param points Vector of path points (used for distance calculations)
+   * @param velocities Vector of velocities to be updated (input/output)
+   * @param curvatures Vector of curvature values (used to compute lateral acceleration)
    */
-  void point_speed(const std::vector<double> &radiuses, std::vector<double> &velocities);
+  void braking_limiter(std::vector<PathPoint> &points, std::vector<double> &velocities,
+                       const std::vector<double> &curvatures);
 };

src/planning/src/planning/planning.cpp

@@ -72,7 +72,7 @@ PlanningParameters Planning::load_config(std::string &adapter) {
   params.vp_minimum_velocity_ = planning_config["vp_minimum_velocity"].as<double>();
   params.vp_braking_acceleration_ = planning_config["vp_braking_acceleration"].as<double>();
   params.vp_acceleration_ = planning_config["vp_acceleration"].as<double>();
-  params.vp_normal_acceleration_ = planning_config["vp_normal_acceleration"].as<double>();
+  params.vp_lateral_acceleration_ = planning_config["vp_lateral_acceleration"].as<double>();
   params.vp_use_velocity_planning_ = planning_config["vp_use_velocity_planning"].as<bool>();
   params.vp_desired_velocity_ = planning_config["vp_desired_velocity"].as<double>();
 
@@ -307,7 +307,7 @@ void Planning::run_trackdrive() {
 
         double avg_vel = sum / smoothed_path_.size();
 
-        RCLCPP_DEBUG(get_logger(),
+        RCLCPP_INFO(get_logger(),
                      "[TRACKDRIVE] Velocity Stats - Avg: %.2f m/s | Max: %.2f m/s | Min: %.2f m/s",
                      avg_vel, max_vel, min_vel);
       }