updated firmware

firmware/MotionControllerRP/src/motion_control/path_planner.cpp

@@ -4,9 +4,14 @@
 #include "path_planner.h"
 #include "utilities/logging.h"
 
+#include <algorithm>
+
 PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
   segment_time_step = time_step;
   kinematic_model = kinematic_model;
+
+  junction_deviation.linear = 0.0001;  // mm
+  junction_deviation.angular = 0.001; // rad
 }
 
 PathPlanner::~PathPlanner() {
@@ -23,9 +28,6 @@ bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_se
     return false;
   }
 
-  // TODO: do look ahead planning of queue
-  new_segment->compute_motion_profile(); // for testing
-
   return true;
 }
 
@@ -34,9 +36,15 @@ void PathPlanner::process(bool disable_interrupts_for_queue_update) {
   // the segment stays in the queue until it is completed
   if(segment_generator == nullptr && ct_path_segment_queue.empty() == false) {
     auto* current_segment = ct_path_segment_queue.peek();
+
+    // compute motion profile ( active segment can not change anymore )
+    current_segment->compute_motion_profile();
+
+    // create path segment generator
     segment_generator = new JointSpacePathSegmentGenerator(current_segment, 
                                                            kinematic_model,
                                                            segment_time_step);
+
     /*LOG_INFO("Starting segment: duration=%fs, (%f, %f, %f)->(%f, %f, %f) | queue size: %i", 
       current_segment->get_duration(),
       current_segment->start_pose.translation.x,
@@ -97,5 +105,111 @@ int PathPlanner::input_queue_size() {
 }
 
 void PathPlanner::run_look_ahead_planning() {
+  if(ct_path_segment_queue.empty())
+    return;
 
+  int n = ct_path_segment_queue.size();
+  auto* last_segment = ct_path_segment_queue.get(n - 1);
+  if(segment_generator != nullptr && last_segment == segment_generator->get_path_segment())
+    return;
+
+  // --- Reverse pass ---
+  // Start from the last segment, set its end velocity to zero (or target)
+  last_segment->end_velocity = LinearAngular{0, 0};
+  
+  // Propagate backward the feasible start velocities
+  for (int i = n - 2; i >= 0; i--) {
+    CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
+    CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
+
+    // dont change active segment
+    if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
+      s2->start_velocity = s1->end_velocity;
+      continue;
+    }
+  
+    // Get minimum acceleration capability at junction
+    float acc = std::min(s1->max_acceleration.linear, s2->max_acceleration.linear);
+
+    // Compute max junction velocity feasible at junction between s1 and s2
+    float max_junction_velocity_linear = compute_max_junction_velocity(
+      s1->translation_delta_normalized,
+      s2->translation_delta_normalized,
+      acc,
+      junction_deviation.linear
+    );
+
+    // no angular junction velocity limit yet
+    float max_junction_velocity_angular = 1e10;
+
+    // compute max velocity delta
+    float v_start_max_linear = std::sqrt(std::max(0.0f, powf(s1->end_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
+    float v_start_max_angular = std::sqrt(std::max(0.0f, powf(s1->end_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
+
+    // compute final junction velocity
+    LinearAngular junction_velocity;
+    junction_velocity.linear = std::min(max_junction_velocity_linear, std::min(s2->target_velocity.linear, v_start_max_linear));
+    junction_velocity.angular = std::min(max_junction_velocity_angular, std::min(s2->target_velocity.angular, v_start_max_angular));
+    
+    // set calculated velocity to both path segments
+    s1->end_velocity = junction_velocity;
+    s2->start_velocity = junction_velocity;
+  }
+
+  // --- Forward pass ---
+  for (int i = 0; i < n - 1; i++) {
+    CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
+    CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
+
+    // dont change active segment
+    if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
+      s2->start_velocity = s1->end_velocity;
+      continue;
+    }
+
+    float v_start = s1->start_velocity.linear;
+    float v_end_max_linear = std::sqrt(std::max(0.0f, powf(s1->start_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
+    float v_end_max_angular = std::sqrt(std::max(0.0f, powf(s1->start_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
+
+    LinearAngular junction_velocity;
+    if(s1->end_velocity.linear > v_end_max_linear) s1->end_velocity.linear = v_end_max_linear;
+    if(s1->end_velocity.angular > v_end_max_angular) s1->end_velocity.angular = v_end_max_angular;
+
+    s2->start_velocity = s1->end_velocity;
+  }
+
+  // debug
+  // print_cartesian_path_segments();
 }
+
+float PathPlanner::compute_max_junction_velocity(const Vec3F& dir_in_normalized, const Vec3F& dir_out_normalized, float acceleration, float junction_deviation) {
+    const float EPSILON = 1e-6f;
+    const float COS_NEAR_STRAIGHT = 0.9999f;
+    const float COS_NEAR_OPPOSITE = -0.9999f;
+
+    // Compute the cosine of the angle between the directions (negative dot product)
+    float cos_theta = -dir_in_normalized.dot(dir_out_normalized);
+
+    // Compute sin(θ/2) using half-angle identity: sin²(θ/2) = (1 - cosθ) / 2
+    float sin_theta_d2 = std::sqrt(std::max(0.0f, 0.5f * (1.0f - cos_theta)));
+
+    // Compute vmax using classical junction deviation formula
+    float denom = std::max(1.0f - sin_theta_d2, EPSILON);
+    float vmax = std::sqrt(acceleration * junction_deviation * sin_theta_d2 / denom);
+
+    return vmax;
+}
+
+void PathPlanner::print_cartesian_path_segments() {
+  LOG_INFO("Cartesian Path Segment Info");
+  int n = ct_path_segment_queue.size();
+  for (int i = 0; i < n; i++) {
+    CartesianPathSegment* s = ct_path_segment_queue.get(i);
+    LOG_INFO("  Segment %02i: [%f, %f, %f]->[%f, %f, %f]  l=%f  vs=%fmm/s   ve=%fmm/s", i, 
+      s->start_pose.translation.x, s->start_pose.translation.y, s->start_pose.translation.z,  
+      s->end_pose.translation.x, s->end_pose.translation.y, s->end_pose.translation.z,  
+      s->travel_distance.linear,
+      s->start_velocity.linear, s->end_velocity.linear);
+  }
+}
+