1+ import math
2+ import numpy as np
3+ from acsl_pychrono .control .outerloop_safetymech import OuterLoopSafetyMechanism
4+ from acsl_pychrono .control .MRAC .mrac_gains import MRACGains
5+ from acsl_pychrono .ode_input import OdeInput
6+ from acsl_pychrono .flight_params import FlightParams
7+ from acsl_pychrono .control .control import Control
8+ from acsl_pychrono .control .base_mrac import BaseMRAC
9+
10+ class MRAC (BaseMRAC , Control ):
11+ def __init__ (self , gains : MRACGains , ode_input : OdeInput , flight_params : FlightParams ):
12+ self .gains = gains
13+ self .odein = ode_input
14+ self .fp = flight_params
15+ self .safety_mechanism = OuterLoopSafetyMechanism (gains , self .fp .G_acc )
16+ self .dy = np .zeros ((self .gains .number_of_states , 1 ))
17+
18+ def computeControlAlgorithm (self , y , ode_input : OdeInput ):
19+ """
20+ Compute all intermediate variables and control inputs once per RK4 step to compute the dy for RK4.
21+ """
22+ # Update the vehicle state and user-defined trajectory
23+ self .odein = ode_input
24+
25+ # ODE state
26+ self .state_phi_ref_diff = y [0 :2 ] # State of the differentiator for phi_ref (roll_ref)
27+ self .state_theta_ref_diff = y [2 :4 ] # State of the differentiator for theta_ref (pitch_ref)
28+ self .x_ref_tran = y [4 :10 ] # Reference model state
29+ self .integral_position_tracking_ref = y [10 :13 ] # Integral of ('translational_position_in_I_ref' - 'translational_position_in_I_user')
30+ self .K_hat_x_tran = y [13 :31 ] # \hat{K}_x (translational)
31+ self .K_hat_r_tran = y [31 :40 ] # \hat{K}_r (translational)
32+ self .Theta_hat_tran = y [40 :58 ] # \hat{\Theta} (translational)
33+ self .omega_ref = y [58 :61 ] # Reference model rotational dynamics
34+ self .K_hat_x_rot = y [61 :70 ] # \hat{K}_x (rotational)
35+ self .K_hat_r_rot = y [70 :79 ] # \hat{K}_r (rotational)
36+ self .Theta_hat_rot = y [79 :97 ] # \hat{\Theta} (rotational)
37+ self .integral_e_rot = y [97 :100 ] # Integral of 'e_rot' = (angular_velocity - omega_ref)
38+ self .integral_angular_error = y [100 :103 ] # Integral of angular_error = attitude - attitude_ref
39+ self .integral_e_omega_ref_cmd = y [103 :106 ] #Integral of (omega_ref - omega_cmd)
40+
41+ # Reshapes all adaptive gains to their correct (row, col) shape as matrices
42+ self .reshapeAdaptiveGainsToMatrices ()
43+
44+ # compute translational and rotational trajectory tracking error
45+ self .computeTrajectoryTrackingErrors (self .odein )
46+
47+ self .r_tran = self .computeReferenceCommandInputOuterLoop ()
48+
49+ self .x_ref_tran_dot = self .computeReferenceModelOuterLoop ()
50+
51+ self .mu_PD_baseline_tran = self .computeMuPDbaselineOuterLoop ()
52+
53+ (self .Phi_adaptive_tran_augmented ,
54+ self .Theta_tran_adaptive_bar_augmented
55+ ) = self .computeRegressorVectorAndThetaBarOuterLoop ()
56+
57+ self .mu_baseline_tran = self .computeMuBaselineBarOuterLoop ()
58+
59+ self .mu_adaptive_tran = self .computeMuAdaptiveOuterLoop ()
60+
61+ self .mu_tran_raw = self .computeMuRawOuterLoop ()
62+
63+ # Precompute e^T*P*B for outer loop
64+ eTranspose_P_B_tran = self .compute_eTransposePB_OuterLoop ()
65+
66+ # Outer Loop Adaptive Laws
67+ self .computeAllAdaptiveLawsOuterLoop (eTranspose_P_B_tran )
68+
69+ # Outer Loop Safety Mechanism
70+ self .mu_x , self .mu_y , self .mu_z = self .safety_mechanism .apply (self .mu_tran_raw )
71+
72+ # Compute total thrust, desired roll angle, desired pitch angle
73+ (
74+ self .u1 ,
75+ self .roll_ref ,
76+ self .pitch_ref
77+ ) = Control .computeU1RollPitchRef (
78+ self .mu_x ,
79+ self .mu_y ,
80+ self .mu_z ,
81+ self .gains .mass_total_estimated ,
82+ self .fp .G_acc ,
83+ self .odein .yaw_ref
84+ )
85+
86+ # Computes roll/pitch reference dot and ddot using state-space differentiators.
87+ (
88+ self .internal_state_differentiator_phi_ref_diff ,
89+ self .internal_state_differentiator_theta_ref_diff ,
90+ self .angular_position_ref_dot ,
91+ self .angular_position_ref_ddot
92+ ) = Control .computeAngularReferenceSignals (
93+ self .fp ,
94+ self .odein ,
95+ self .roll_ref ,
96+ self .pitch_ref ,
97+ self .state_phi_ref_diff ,
98+ self .state_theta_ref_diff
99+ )
100+
101+ # Computes angular error and its derivative
102+ (
103+ self .angular_error ,
104+ self .angular_position_dot ,
105+ self .angular_error_dot
106+ ) = Control .computeAngularErrorAndDerivative (
107+ self .odein ,
108+ self .roll_ref ,
109+ self .pitch_ref ,
110+ self .angular_position_ref_dot
111+ )
112+
113+ (self .omega_cmd ,
114+ self .omega_cmd_dot
115+ ) = self .computeOmegaCmdAndOmegaCmdDotInnerLoop ()
116+
117+ self .omega_ref_dot = self .computeReferenceModelInnerLoop ()
118+
119+ self .r_rot = self .computeReferenceCommandInputInnerLoop ()
120+
121+ self .Moment_baseline_PI = self .computeMomentPIbaselineInnerLoop ()
122+
123+ (self .Phi_adaptive_rot ,
124+ self .Phi_adaptive_rot_augmented
125+ ) = self .computeRegressorVectorInnerLoop ()
126+
127+ # Precompute e^T*P*B for inner loop
128+ eTranspose_P_B_rot = self .compute_eTransposePB_InnerLoop ()
129+
130+ # Inner Loop Adaptive Laws
131+ self .computeAllAdaptiveLawsInnerLoop (eTranspose_P_B_rot )
132+
133+ self .Moment_baseline = self .computeMomentBaselineInnerLoop ()
134+
135+ self .Moment_adaptive = self .computeMomentAdaptiveInnerLoop ()
136+
137+ (self .u2 ,
138+ self .u3 ,
139+ self .u4
140+ ) = self .computeU2_U3_U4 ()
141+
142+ # Compute individual motor thrusts
143+ self .motor_thrusts = Control .computeMotorThrusts (self .fp , self .u1 , self .u2 , self .u3 , self .u4 )
144+
145+ def ode (self , t , y , ode_input : OdeInput ):
146+ """
147+ Function called by RK4. Assumes `computeControlAlgorithm` was called
148+ at the beginning of the integration step to update internal state.
149+ """
150+ self .dy [0 :2 ] = self .internal_state_differentiator_phi_ref_diff
151+ self .dy [2 :4 ] = self .internal_state_differentiator_theta_ref_diff
152+ self .dy [4 :10 ] = self .x_ref_tran_dot
153+ self .dy [10 :13 ] = self .translational_position_in_I_ref - self .odein .translational_position_in_I_user
154+ self .dy [13 :31 ] = self .K_hat_x_tran_dot .reshape (18 ,1 )
155+ self .dy [31 :40 ] = self .K_hat_r_tran_dot .reshape (9 ,1 )
156+ self .dy [40 :58 ] = self .Theta_hat_tran_dot .reshape (18 ,1 )
157+ self .dy [58 :61 ] = self .omega_ref_dot
158+ self .dy [61 :70 ] = self .K_hat_x_rot_dot .reshape (9 ,1 )
159+ self .dy [70 :79 ] = self .K_hat_r_rot_dot .reshape (9 ,1 )
160+ self .dy [79 :97 ] = self .Theta_hat_rot_dot .reshape (18 ,1 )
161+ self .dy [97 :100 ] = self .odein .angular_velocity - self .omega_ref
162+ self .dy [100 :103 ] = self .angular_error
163+ self .dy [103 :106 ] = self .omega_ref - self .omega_cmd
164+
165+ return np .array (self .dy )
166+
167+ def reshapeAdaptiveGainsToMatrices (self ):
168+ """
169+ Reshapes all gain parameters to their correct (row, col) shape and converts them to np.matrix.
170+ This is intended to be called once after loading or updating gains stored as flat arrays.
171+ """
172+ self .K_hat_x_tran = np .matrix (self .K_hat_x_tran .reshape (6 ,3 ))
173+ self .K_hat_r_tran = np .matrix (self .K_hat_r_tran .reshape (3 ,3 ))
174+ self .Theta_hat_tran = np .matrix (self .Theta_hat_tran .reshape (6 ,3 ))
175+ self .K_hat_x_rot = np .matrix (self .K_hat_x_rot .reshape (3 ,3 ))
176+ self .K_hat_r_rot = np .matrix (self .K_hat_r_rot .reshape (3 ,3 ))
177+ self .Theta_hat_rot = np .matrix (self .Theta_hat_rot .reshape (6 ,3 ))
178+
179+ def computeAllAdaptiveLawsOuterLoop (self , eTranspose_P_B_tran ):
180+ self .K_hat_x_tran_dot = self .computeAdaptiveLawMRAC (
181+ - self .gains .Gamma_x_tran ,
182+ self .x_tran ,
183+ eTranspose_P_B_tran
184+ )
185+ self .K_hat_r_tran_dot = self .computeAdaptiveLawMRAC (
186+ - self .gains .Gamma_r_tran ,
187+ self .r_tran ,
188+ eTranspose_P_B_tran
189+ )
190+ self .Theta_hat_tran_dot = self .computeAdaptiveLawMRAC (
191+ self .gains .Gamma_Theta_tran ,
192+ self .Phi_adaptive_tran_augmented ,
193+ eTranspose_P_B_tran
194+ )
195+
196+ def computeAllAdaptiveLawsInnerLoop (self , eTranspose_P_B_rot ):
197+ self .K_hat_x_rot_dot = self .computeAdaptiveLawMRAC (
198+ - self .gains .Gamma_x_rot ,
199+ self .odein .angular_velocity ,
200+ eTranspose_P_B_rot
201+ )
202+ self .K_hat_r_rot_dot = self .computeAdaptiveLawMRAC (
203+ - self .gains .Gamma_r_rot ,
204+ self .r_rot ,
205+ eTranspose_P_B_rot
206+ )
207+ self .Theta_hat_rot_dot = self .computeAdaptiveLawMRAC (
208+ self .gains .Gamma_Theta_rot ,
209+ self .Phi_adaptive_rot_augmented ,
210+ eTranspose_P_B_rot
211+ )
0 commit comments