Merge pull request #1 from andrealaffly/main2

Main2

Author: mattia-gramuglia

GainsController.py

@@ -30,7 +30,25 @@ def PID():
         KD_rot = np.matrix(1 * np.diag([50,50,50]))
         KI_rot = np.matrix(1 * np.diag([20,20,10]))
 
-        return [number_of_states,size_DATA,KP_tran,KD_tran,KI_tran,KP_rot,KD_rot,KI_rot]
+        # ----------------------------------------------------------------
+        #                   Safety Mechanism Parameters
+        # ----------------------------------------------------------------
+        
+        # Mu - sphere intersection
+        sphereEpsilon = 1e-2
+        maximumThrust = 85 # [N] 85
+        
+        # Mu - elliptic cone intersection
+        EllipticConeEpsilon = 1e-2
+        maximumRollAngle = math.radians(32) # [rad] 25
+        maximumPitchAngle = math.radians(32) # [rad] 25
+        
+        # Mu - plane intersection
+        planeEpsilon = 1e-2
+        alphaPlane = 0.95 # [-] coefficient for setting the 'height' of the bottom plane. Must be >0 and <1.
+        
+        return [number_of_states,size_DATA,KP_tran,KD_tran,KI_tran,KP_rot,KD_rot,KI_rot,sphereEpsilon,maximumThrust,
+        EllipticConeEpsilon,maximumRollAngle,maximumPitchAngle,planeEpsilon,alphaPlane]
     # ================================================================================================================================================================
     # end of PID
     # ================================================================================================================================================================
@@ -1336,10 +1354,11 @@ def FunnelTwoLayerMRACwithBASELINE(mass_total_estimated,air_density_estimated,su
     def MRACwithBASELINE_SafetyMechanism(mass_total_estimated,air_density_estimated,surface_area_estimated,drag_coefficient_matrix_estimated):
 
         # Number of states to be integrated by RK4
-        number_of_states = 100
+        # number_of_states = 100
+        number_of_states = 106
         # Length of the array vector that will be exported 
-        size_DATA = 74 
-        # size_DATA = 94 # OLD data format
+        # size_DATA = 74 
+        size_DATA = 86 
 
         # ----------------------------------------------------------------
         #                     Baseline Parameters
@@ -1355,14 +1374,23 @@ def MRACwithBASELINE_SafetyMechanism(mass_total_estimated,air_density_estimated,
         KD_tran_PD_baseline = np.matrix(1 * np.diag([8,8,3]))
 
         # **Rotational** baseline parameters
-        KP_rot = np.matrix(1e2 * np.diag([1,1,0.5]))
+        # KP_rot = np.matrix(1e2 * np.diag([1,1,0.5]))
+        # KP_rot = np.matrix(5e1 * np.diag([0.1,0.1,0.5]))
+        KP_rot = np.matrix(5e1 * np.diag([0.05,0.05,0.5]))
+        # KI_rot = np.matrix(1e2 * np.diag([1,1,1]))
+        KI_rot = np.matrix(1e2 * np.diag([1,1,1]))
 
         # **Rotational** parameters for the PI baseline controller (Moment_baseline_PI)       
-        KP_rot_PI_baseline = np.matrix(40 * np.diag([1,1,1]))
+        # KP_rot_PI_baseline = np.matrix(40 * np.diag([1,1,1]))
+        # KD_rot_PI_baseline = np.matrix(0 * np.diag([1,1,0.5]))
+        # KI_rot_PI_baseline = np.matrix(1e-1 * np.diag([1,1,0.5]))
+        KP_rot_PI_baseline = np.matrix(200 * np.diag([1,1,0.3]))
         KD_rot_PI_baseline = np.matrix(0 * np.diag([1,1,0.5]))
-        KI_rot_PI_baseline = np.matrix(0.1 * np.diag([1,1,0.5]))
+        KI_rot_PI_baseline = np.matrix(1e2 * np.diag([10,10,0.5]))
 
-        K_P_omega_ref = np.matrix(1.5e-1 * np.diag([5,5,10]))
+        # K_P_omega_ref = np.matrix(1.5e-1 * np.diag([5,5,10]))
+        K_P_omega_ref = np.matrix(1.5e0 * np.diag([50,50,10]))
+        K_I_omega_ref = np.matrix(1e2 * np.diag([1,1,1]))
 
         # ----------------------------------------------------------------
         #                   Translational Parameters MRAC
@@ -1413,13 +1441,17 @@ def MRACwithBASELINE_SafetyMechanism(mass_total_estimated,air_density_estimated,
         B_ref_rot = np.matrix(np.eye(3))
 
         # **Rotational** parameters Lyapunov equation
-        Q_rot = np.matrix(7e-3 * np.diag([1,1,2]))
+        # Q_rot = np.matrix(7e-3 * np.diag([1,1,2]))
+        Q_rot = np.matrix(7e-3 * np.diag([2,2,2]))
         P_rot = np.matrix(linalg.solve_continuous_lyapunov(A_ref_rot.T, -Q_rot))
 
         # **Rotational** adaptive parameters
-        Gamma_x_rot = np.matrix(1e1 * np.diag([1,1,10])) # Adaptive rates
-        Gamma_r_rot = np.matrix(1e-4 * np.diag([1,1,1])) # Adaptive rates
-        Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+        # Gamma_x_rot = np.matrix(1e1 * np.diag([1,1,10])) # Adaptive rates
+        # Gamma_r_rot = np.matrix(1e-4 * np.diag([1,1,1])) # Adaptive rates
+        # Gamma_Theta_rot = np.matrix(1e0 * np.diag([1,1,1,1,1,1])) # Adaptive rates
+        Gamma_x_rot = np.matrix(1e4 * np.diag([1,1,1])) # Adaptive rates
+        Gamma_r_rot = np.matrix(1e1 * np.diag([1,1,1])) # Adaptive rates
+        Gamma_Theta_rot = np.matrix(1e2 * np.diag([1,1,1,1,1,1])) # Adaptive rates
 
         # ----------------------------------------------------------------
         #                   Safety Mechanism Parameters
@@ -1431,16 +1463,16 @@ def MRACwithBASELINE_SafetyMechanism(mass_total_estimated,air_density_estimated,
 
         # Mu - elliptic cone intersection
         EllipticConeEpsilon = 1e-2
-        maximumRollAngle = math.radians(20) # [rad] 25
-        maximumPitchAngle = math.radians(20) # [rad] 25
+        maximumRollAngle = math.radians(60) # [rad] 25 - 32
+        maximumPitchAngle = math.radians(60) # [rad] 25 - 32
 
         # Mu - plane intersection
         planeEpsilon = 1e-2
         alphaPlane = 0.95 # [-] coefficient for setting the 'height' of the bottom plane. Must be >0 and <1.
 
 
-        return [number_of_states,size_DATA,KP_tran,KD_tran,KI_tran,KP_tran_PD_baseline,KD_tran_PD_baseline,KP_rot,KP_rot_PI_baseline,
-                KD_rot_PI_baseline,KI_rot_PI_baseline,K_P_omega_ref,A_tran,B_tran,A_tran_bar,Lambda_bar,Theta_tran_adaptive_bar,
+        return [number_of_states,size_DATA,KP_tran,KD_tran,KI_tran,KP_tran_PD_baseline,KD_tran_PD_baseline,KP_rot,KI_rot,KP_rot_PI_baseline,
+                KD_rot_PI_baseline,KI_rot_PI_baseline,K_P_omega_ref,K_I_omega_ref,A_tran,B_tran,A_tran_bar,Lambda_bar,Theta_tran_adaptive_bar,
                 A_ref_tran,B_ref_tran,Gamma_x_tran,Gamma_r_tran,Gamma_Theta_tran,Gamma_Theta_tran,Q_tran,P_tran,K_x_tran_bar,K_r_tran_bar,A_rot,
                 B_rot,A_ref_rot,B_ref_rot,Q_rot,P_rot,Gamma_x_rot,Gamma_r_rot,Gamma_Theta_rot,sphereEpsilon,maximumThrust,
                 EllipticConeEpsilon,maximumRollAngle,maximumPitchAngle,planeEpsilon,alphaPlane]

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
+