add feedforward control to the simulation

Author: marcomelloni

agents/matlab/matlab_agent/docs/examples/trajectory-planning-robotic-arm/README.md

@@ -8,6 +8,7 @@ This MATLAB simulation generates smooth trajectories for a Universal Robots UR10
 
 - **Quintic Polynomial Trajectory Generation**: Ensures smooth motion with continuous position, velocity, and acceleration
 - **Proportional Feedback Control**: Tracks the desired trajectory using a simple P-controller
+- **Velocity Feedforward**: The controller adds the desired joint velocity to the command to reduce lag and tracking error during motion
 - **Real-time Visualization**: Displays robot motion, joint trajectories, and tracking errors
 - **Configurable Parameters**: Customize start/goal poses, duration, and time step
 
@@ -69,18 +70,25 @@ Boundary conditions enforce:
 
 ### 2. Control Law
 
-A proportional controller tracks the desired trajectory:
+The controller uses **proportional feedback with velocity feedforward**:
 
 ```
-u = K · (qd - q)
+u = dqd + K · (qd - q)
 ```
 
 Where:
 
-- `K = 100·I₆` is the proportional gain matrix
-- `qd` is the desired position from the trajectory
-- `q` is the current joint position
-- `u` is the control input (commanded velocity)
+- `qd` and `dqd` are the desired joint position and velocity from the quintic trajectory;
+- `q` is the current joint position;
+- `K = 100·I₆` is the proportional gain matrix;
+- `u` is the commanded joint velocity.
+
+**Why feedforward?**  
+The proportional term `K(qd - q)` corrects the error, but introduces delay (lag) when the trajectory is moving. Adding the **feedforward** term `dqd` provides the control chain with the "right" velocity that the trajectory requires at that instant, reducing:
+
+- lag during the tracking phase,
+- transient tracking error,
+- error peaks during accelerations/slope changes.
 
 ### 3. Integration
 
@@ -90,6 +98,7 @@ Joint positions are updated using Euler integration:
 q(t+Δt) = q(t) + u·Δt
 ```
 
+With `u = dqd + K(qd - q)`, the `dqd` part anticipates the movement required by the trajectory, while `K(qd - q)` cancels the residual error.
 The simulation continues until the trajectory time is reached **and** the final error is below threshold (`1e-5` rad).
 
 ## Visualization

agents/matlab/matlab_agent/docs/examples/trajectory-planning-robotic-arm/simulation.m

@@ -67,6 +67,7 @@
 
         % Evaluate polynomial at each time step
         Qd(i, :) = polyval(A, time);
+        dQd(i, :) = polyval(polyder(A), time);
     end
 
     %% ========== VISUALIZE START AND GOAL POSES ==========
@@ -96,8 +97,8 @@
         % Compute tracking error
         e = Qd(:, counter) - q;
 
-        % Proportional control law
-        u = K * e;
+        % Feedforward + Proportional Control law
+        u = dQd(:, counter) + K * e;
 
         % Integrate joint positions (Euler method)
         dq = u;
@@ -124,7 +125,9 @@
 
     % Extend desired trajectory to match simulation length
     Qd_extended = [Qd, repmat(Qd(:, end), 1, length(time_sim) - size(Qd, 2))];
-    
+    % Extend desired velocities to match simulation length
+    dQd_extended = [dQd, repmat(dQd(:, end), 1, length(time_sim) - size(dQd, 2))];
+
     % Compute tracking error for each time step
     error_history = zeros(1, length(time_sim));
     for k = 1:length(time_sim)
@@ -139,7 +142,8 @@
     outputs.tracking_error = error_history;         % Tracking error norm over time
     outputs.final_error = norm(q - qg);             % Final tracking error [rad]
     outputs.final_position = q;                     % Final joint configuration [rad]
-    
+    outputs.desired_velocities = dQd;               % Desired Velocities
+
     %% ========== RESULTS VISUALIZATION ==========
 
     if showGui
@@ -166,6 +170,19 @@
         ylabel('Tracking Error (norm) [rad]');
         title('Tracking Error vs Time');
         grid on;
+
+        % Plot joint velocities: commanded vs desired
+        figure;
+        plot(time_sim, dQhistory, 'LineWidth', 1.5);   % commanded/actual (6 curves)
+        hold on;
+        plot(time_sim, dQd_extended, '--', 'LineWidth', 1.2);  % desired (dashed)
+        xlabel('Time [s]');
+        ylabel('Joint Velocity [rad/s]');
+        title('Joint Velocities: Commanded vs Desired');
+        grid on;
+        legend({'\omega_1','\omega_2','\omega_3','\omega_4','\omega_5','\omega_6', ...
+                '\omega_{1,d}','\omega_{2,d}','\omega_{3,d}','\omega_{4,d}','\omega_{5,d}','\omega_{6,d}'}, ...
+               'Location','bestoutside');
 
         % Animate robot motion
         figure;