minimum jerk trajectory generator

Author: mattia-gramuglia

MATLAB_files/Minimum_jerk_trajectory_generator/ComputePosition.m

@@ -0,0 +1,14 @@
+function pos = ComputePosition(t, r_x, r_y, r_z)
+%COMPUTEPOSITION Summary of this function goes here
+%   Detailed explanation goes here
+
+
+% Computing tangent vector T
+r_x_ev = polyval(r_x, t);
+r_y_ev = polyval(r_y, t);
+r_z_ev = polyval(r_z, t);
+
+pos = [r_x_ev; r_y_ev; r_z_ev];
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/FrenetSerretFrame.m

@@ -0,0 +1,30 @@
+function R = FrenetSerretFrame(t, r_x, r_y, r_z)
+%FRENETSERRETFRAME Summary of this function goes here
+%   Detailed explanation goes here
+
+% Computing tangent vector T
+dr_x = polyval(polyder(r_x), t);
+dr_y = polyval(polyder(r_y), t);
+dr_z = polyval(polyder(r_z), t);
+
+dr = [dr_x; dr_y; dr_z];
+T = dr / norm(dr);
+
+% Computing binormal vector B
+ddr_x = polyval(polyder(polyder(r_x)), t);
+ddr_y = polyval(polyder(polyder(r_y)), t);
+ddr_z = polyval(polyder(polyder(r_z)), t);
+
+ddr = [ddr_x; ddr_y; ddr_z];
+
+B = cross(dr,ddr) / norm(cross(dr,ddr));
+
+% Computing normal vector N
+
+N = cross(B,T);
+
+R = [T N B];
+
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/GenerateRotMat.m

@@ -0,0 +1,15 @@
+function R = GenerateRotMat(phi, theta, psi)
+%GENERATEROTMAT Rotation matrix deriving from a 321 rotation sequence
+%   roll - phi
+%   pitch - theta
+%   psi - psi
+
+R = [cos(psi)*cos(theta) -cos(phi)*sin(psi) + cos(psi)*sin(theta)*sin(phi)  cos(psi)*cos(phi)*sin(theta) + sin(psi)*sin(phi);
+     cos(theta)*sin(psi)  cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi) -cos(psi)*sin(phi) + cos(phi)*sin(psi)*sin(theta);
+             -sin(theta)                               cos(theta)*sin(phi)                               cos(theta)*cos(phi)];  
+
+
+
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/Norm2D.m

@@ -0,0 +1,9 @@
+function norm_value = Norm2D(poly_coef_x,poly_coef_y,t)
+%NORM2D Computes the 2D norm from polynomial coefficients of the components of the vector 
+%   Detailed explanation goes here
+
+norm_value = sqrt((polyval(poly_coef_x,t))^2 + (polyval(poly_coef_y,t))^2);
+
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/Norm2Dderivative.m

@@ -0,0 +1,16 @@
+function derivative_value = Norm2Dderivative(poly_coef_x,...
+                                             poly_coef_y,...
+                                             poly_coef_x_prime,...
+                                             poly_coef_y_prime,...
+                                             t)
+%NORM2DDERIVATIVE Computes the derivative of the 2D norm from polynomial
+%coefficients of the components of the vector
+%   Detailed explanation goes here
+
+derivative_value = (polyval(poly_coef_x,t)*polyval(poly_coef_x_prime,t) +...
+                    polyval(poly_coef_y,t)*polyval(poly_coef_y_prime,t)) /...
+                    Norm2D(poly_coef_x,poly_coef_y,t);
+
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/Note Feb 12, 2024 at 17_52_15.pdf

@@ -0,0 +1,25 @@
+function [poly_coeff_matrix_out_x,...
+          poly_coeff_matrix_out_y,...
+          poly_coeff_matrix_out_z] = PolyCoefAssigning(poly_coeff_matrix_in)
+%POLYCOEFASSIGNING Assigning polynomial coefficients to designated
+%variables
+%   Detailed explanation goes here
+
+poly_coeff_matrix_in_size = size(poly_coeff_matrix_in);
+
+poly_coeff_matrix_out_size = [poly_coeff_matrix_in_size(1)/3, poly_coeff_matrix_in_size(2)];
+
+poly_coeff_matrix_out_x = zeros(poly_coeff_matrix_out_size(1), poly_coeff_matrix_out_size(2));
+poly_coeff_matrix_out_y = zeros(poly_coeff_matrix_out_size(1), poly_coeff_matrix_out_size(2));
+poly_coeff_matrix_out_z = zeros(poly_coeff_matrix_out_size(1), poly_coeff_matrix_out_size(2));
+
+for i=0:poly_coeff_matrix_out_size(1)-1
+    poly_coeff_matrix_out_x(i+1,:) = poly_coeff_matrix_in(3*i + 1,:);
+    poly_coeff_matrix_out_y(i+1,:) = poly_coeff_matrix_in(3*i + 2,:);
+    poly_coeff_matrix_out_z(i+1,:) = poly_coeff_matrix_in(3*i + 3,:);
+
+end
+
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/PolyCoefAssigning.m

@@ -0,0 +1,23 @@
+function [t_adjusted,segment] = PolyTimeAdjusted(time_waypoint_vector,t)
+%POLYTIMEADJUSTED Adjusts the time to be fed to the various polynomials
+%   Detailed explanation goes here
+
+num_waypoints = length(time_waypoint_vector);
+segment = 0;
+% disp('START')
+for i=1:num_waypoints-1
+    % disp(['i: ', num2str(i)])
+    % disp(['t: ', num2str(t)])
+    if (t >= time_waypoint_vector(i) & t <= time_waypoint_vector(i+1)) % >= <
+        segment = i;
+        % disp(['segment: ', num2str(segment)])
+        % disp(['time_waypoint_vector(segment): ', num2str(time_waypoint_vector(segment))])
+        t_adjusted = t - time_waypoint_vector(segment);
+        % disp(['t_adjusted: ', num2str(t_adjusted)])
+        % break
+        return
+    end
+end
+
+end
+

MATLAB_files/Minimum_jerk_trajectory_generator/PolyTimeAdjusted.m

@@ -0,0 +1,15 @@
+function poly_coeff_matrix_out = PolyderMatrix(poly_coeff_matrix_in)
+%POLYDERMATRIX derivative of the piecewise polynomial coefficient matrix
+%   Detailed explanation goes here
+
+poly_coeff_matrix_in_size = size(poly_coeff_matrix_in);
+
+poly_coeff_matrix_out = zeros(poly_coeff_matrix_in_size(1), poly_coeff_matrix_in_size(2)-1);
+
+for i=1:poly_coeff_matrix_in_size(1)
+    poly_coeff_matrix_out(i,:) = polyder(poly_coeff_matrix_in(i,:));
+end
+
+
+end
+