Modularized simulator

Author: mattia-gramuglia

acsl_pychrono/config/config.py

@@ -0,0 +1,58 @@
+from dataclasses import dataclass
+
+@dataclass
+class SimulationConfig:
+  # Total simulation duration in seconds
+  simulation_duration_seconds: float = 21.5
+  # Run the simulator in Wrapper mode (more simulations automatically run sequentially)
+  wrapper_flag: bool = False
+  # If True, perform real-time rendering of the simulation with Irrlicht
+  visualization_flag: bool = True
+  # Dynamic camera options:
+  # "fixed"
+  # "default",
+  # "side",
+  # "front",
+  # "follow",
+  # "fpv"
+  camera_mode: str = "fixed"
+  # Simulation timestep used by Chrono
+  timestep: float = 0.005 
+
+  # Controller types:
+  # "PID",
+  # "MRAC",
+  controller_type: str = "PID"
+
+  # User-defined trajectory types:
+  # "circular_trajectory",
+  # "hover_trajectory",
+  # "square_trajectory",
+  # "rounded_rectangle_trajectory",
+  # "piecewise_polynomial_trajectory"
+  trajectory_type: str = "piecewise_polynomial_trajectory"
+
+  # If the trajectory_type is "piecewise_polynomial_trajectory", then choose the trajectory file to run
+  # Path relative to 'current_working_directory/params/user_defined_trajectory'
+  trajectory_data_path: str = "bean_trajectory0p6.json"
+
+  # Flag to add or remove the payload from the simulation
+  add_payload_flag: bool = True
+  # Payload types: 
+  # "two_steel_balls"
+  # "ten_steel_balls_in_two_lines"
+  # "many_steel_balls_in_random_position"
+  payload_type: str = "two_steel_balls"
+
+@dataclass
+class VehicleConfig:
+  # Path relative to 'current_working_directory/assets/vehicles'
+  model_relative_path: str = "x8copter/x8copter.py" 
+
+@dataclass
+class EnvironmentConfig:
+  # Include external environment in the simulation
+  include: bool = False
+  # Path relative to 'current_working_directory/assets/environments'
+  model_relative_path: str = "environmentA/environmentA.py" 
+  

acsl_pychrono/control/MRAC/m_mrac.py

@@ -0,0 +1,60 @@
+import math
+import numpy as np  
+
+class M_MRAC:
+  @staticmethod
+  def computeAdaptiveLaw(Gamma_gain, pi_vector, eTranspose_P_B):
+    K_hat_state_dot = Gamma_gain * pi_vector * eTranspose_P_B
+
+    return K_hat_state_dot
+  
+  def reshapeAdaptiveGainsToMatrices(self):
+    """
+    Reshapes all gain parameters to their correct (row, col) shape and converts them to np.matrix.
+    This is intended to be called once after loading or updating gains stored as flat arrays.
+    """
+    self.K_hat_x_tran = np.matrix(self.K_hat_x_tran.reshape(6,3))
+    self.K_hat_r_tran = np.matrix(self.K_hat_r_tran.reshape(3,3))
+    self.Theta_hat_tran = np.matrix(self.Theta_hat_tran.reshape(6,3))
+    self.K_hat_x_rot = np.matrix(self.K_hat_x_rot.reshape(3,3))
+    self.K_hat_r_rot = np.matrix(self.K_hat_r_rot.reshape(3,3))
+    self.Theta_hat_rot = np.matrix(self.Theta_hat_rot.reshape(6,3))
+
+  def compute_eTransposePB_OuterLoop(self):
+    eTranspose_P_B_tran = self.e_tran.T * self.gains.P_tran * self.gains.B_tran
+
+    return eTranspose_P_B_tran
+
+  def computeAllAdaptiveLawsOuterLoop(self, eTranspose_P_B_tran):
+    self.K_hat_x_tran_dot = self.computeAdaptiveLaw(
+      -self.gains.Gamma_x_tran,
+      self.x_tran,
+      eTranspose_P_B_tran
+    )
+    self.K_hat_r_tran_dot = self.computeAdaptiveLaw(
+      -self.gains.Gamma_r_tran,
+      self.r_tran,
+      eTranspose_P_B_tran
+    )
+    self.Theta_hat_tran_dot = self.computeAdaptiveLaw(
+      self.gains.Gamma_Theta_tran,
+      self.Phi_adaptive_tran_augmented,
+      eTranspose_P_B_tran
+    )
+
+  def computeAllAdaptiveLawsInnerLoop(self, eTranspose_P_B_rot):
+    self.K_hat_x_rot_dot = self.computeAdaptiveLaw(
+      -self.gains.Gamma_x_rot,
+      self.odein.angular_velocity,
+      eTranspose_P_B_rot
+    )
+    self.K_hat_r_rot_dot = self.computeAdaptiveLaw(
+      -self.gains.Gamma_r_rot,
+      self.r_rot,
+      eTranspose_P_B_rot
+    )
+    self.Theta_hat_rot_dot = self.computeAdaptiveLaw(
+      self.gains.Gamma_Theta_rot,
+      self.Phi_adaptive_rot_augmented,
+      eTranspose_P_B_rot
+    )

acsl_pychrono/control/MRAC/mrac.py

@@ -6,37 +6,41 @@
 from acsl_pychrono.flight_params import FlightParams
 from acsl_pychrono.control.control import Control
 from acsl_pychrono.control.base_mrac import BaseMRAC
+from acsl_pychrono.control.MRAC.m_mrac import M_MRAC
 
-class MRAC(BaseMRAC, Control):
-  def __init__(self, gains: MRACGains, ode_input: OdeInput, flight_params: FlightParams):
+class MRAC(M_MRAC, BaseMRAC, Control):
+  def __init__(self, gains: MRACGains, ode_input: OdeInput, flight_params: FlightParams, timestep: float):
+    super().__init__(odein=ode_input)
     self.gains = gains
-    self.odein = ode_input
     self.fp = flight_params
+    self.timestep = timestep
     self.safety_mechanism = OuterLoopSafetyMechanism(gains, self.fp.G_acc)
     self.dy = np.zeros((self.gains.number_of_states, 1))
+    # Initial conditions
+    self.y = np.zeros((self.gains.number_of_states, 1))
 
-  def computeControlAlgorithm(self, y, ode_input: OdeInput):
+  def computeControlAlgorithm(self, ode_input: OdeInput):
     """
     Compute all intermediate variables and control inputs once per RK4 step to compute the dy for RK4.
     """
     # Update the vehicle state and user-defined trajectory
     self.odein = ode_input
 
     # ODE state            
-    self.state_phi_ref_diff = y[0:2] # State of the differentiator for phi_ref (roll_ref)
-    self.state_theta_ref_diff = y[2:4] # State of the differentiator for theta_ref (pitch_ref)
-    self.x_ref_tran = y[4:10] # Reference model state
-    self.integral_position_tracking_ref = y[10:13] # Integral of ('translational_position_in_I_ref' - 'translational_position_in_I_user')
-    self.K_hat_x_tran = y[13:31] # \hat{K}_x (translational)
-    self.K_hat_r_tran = y[31:40] # \hat{K}_r (translational)
-    self.Theta_hat_tran = y[40:58] # \hat{\Theta} (translational)
-    self.omega_ref = y[58:61] # Reference model rotational dynamics
-    self.K_hat_x_rot = y[61:70] # \hat{K}_x (rotational)
-    self.K_hat_r_rot = y[70:79] # \hat{K}_r (rotational)
-    self.Theta_hat_rot = y[79:97] # \hat{\Theta} (rotational)
-    self.integral_e_rot = y[97:100] # Integral of 'e_rot' = (angular_velocity - omega_ref) 
-    self.integral_angular_error = y[100:103] # Integral of angular_error = attitude - attitude_ref
-    self.integral_e_omega_ref_cmd = y[103:106] #Integral of (omega_ref - omega_cmd)
+    self.state_phi_ref_diff = self.y[0:2] # State of the differentiator for phi_ref (roll_ref)
+    self.state_theta_ref_diff = self.y[2:4] # State of the differentiator for theta_ref (pitch_ref)
+    self.x_ref_tran = self.y[4:10] # Reference model state
+    self.integral_position_tracking_ref = self.y[10:13] # Integral of ('translational_position_in_I_ref' - 'translational_position_in_I_user')
+    self.K_hat_x_tran = self.y[13:31] # \hat{K}_x (translational)
+    self.K_hat_r_tran = self.y[31:40] # \hat{K}_r (translational)
+    self.Theta_hat_tran = self.y[40:58] # \hat{\Theta} (translational)
+    self.omega_ref = self.y[58:61] # Reference model rotational dynamics
+    self.K_hat_x_rot = self.y[61:70] # \hat{K}_x (rotational)
+    self.K_hat_r_rot = self.y[70:79] # \hat{K}_r (rotational)
+    self.Theta_hat_rot = self.y[79:97] # \hat{\Theta} (rotational)
+    self.integral_e_rot = self.y[97:100] # Integral of 'e_rot' = (angular_velocity - omega_ref) 
+    self.integral_angular_error = self.y[100:103] # Integral of angular_error = attitude - attitude_ref
+    self.integral_e_omega_ref_cmd = self.y[103:106] #Integral of (omega_ref - omega_cmd)
 
     # Reshapes all adaptive gains to their correct (row, col) shape as matrices
     self.reshapeAdaptiveGainsToMatrices()
@@ -136,13 +140,14 @@ def computeControlAlgorithm(self, y, ode_input: OdeInput):
 
     (self.u2,
      self.u3,
-     self.u4
+     self.u4,
+     _
     ) = self.computeU2_U3_U4()
 
     # Compute individual motor thrusts
     self.motor_thrusts = Control.computeMotorThrusts(self.fp, self.u1, self.u2, self.u3, self.u4)
 
-  def ode(self, t, y, ode_input: OdeInput):
+  def ode(self, t, y):
     """
     Function called by RK4. Assumes `computeControlAlgorithm` was called
     at the beginning of the integration step to update internal state.
@@ -162,50 +167,4 @@ def ode(self, t, y, ode_input: OdeInput):
     self.dy[100:103] = self.angular_error
     self.dy[103:106] = self.omega_ref - self.omega_cmd
 
-    return np.array(self.dy)
-  
-  def reshapeAdaptiveGainsToMatrices(self):
-    """
-    Reshapes all gain parameters to their correct (row, col) shape and converts them to np.matrix.
-    This is intended to be called once after loading or updating gains stored as flat arrays.
-    """
-    self.K_hat_x_tran = np.matrix(self.K_hat_x_tran.reshape(6,3))
-    self.K_hat_r_tran = np.matrix(self.K_hat_r_tran.reshape(3,3))
-    self.Theta_hat_tran = np.matrix(self.Theta_hat_tran.reshape(6,3))
-    self.K_hat_x_rot = np.matrix(self.K_hat_x_rot.reshape(3,3))
-    self.K_hat_r_rot = np.matrix(self.K_hat_r_rot.reshape(3,3))
-    self.Theta_hat_rot = np.matrix(self.Theta_hat_rot.reshape(6,3))
-
-  def computeAllAdaptiveLawsOuterLoop(self, eTranspose_P_B_tran):
-    self.K_hat_x_tran_dot = self.computeAdaptiveLawMRAC(
-      -self.gains.Gamma_x_tran,
-      self.x_tran,
-      eTranspose_P_B_tran
-    )
-    self.K_hat_r_tran_dot = self.computeAdaptiveLawMRAC(
-      -self.gains.Gamma_r_tran,
-      self.r_tran,
-      eTranspose_P_B_tran
-    )
-    self.Theta_hat_tran_dot = self.computeAdaptiveLawMRAC(
-      self.gains.Gamma_Theta_tran,
-      self.Phi_adaptive_tran_augmented,
-      eTranspose_P_B_tran
-    )
-
-  def computeAllAdaptiveLawsInnerLoop(self, eTranspose_P_B_rot):
-    self.K_hat_x_rot_dot = self.computeAdaptiveLawMRAC(
-      -self.gains.Gamma_x_rot,
-      self.odein.angular_velocity,
-      eTranspose_P_B_rot
-    )
-    self.K_hat_r_rot_dot = self.computeAdaptiveLawMRAC(
-      -self.gains.Gamma_r_rot,
-      self.r_rot,
-      eTranspose_P_B_rot
-    )
-    self.Theta_hat_rot_dot = self.computeAdaptiveLawMRAC(
-      self.gains.Gamma_Theta_rot,
-      self.Phi_adaptive_rot_augmented,
-      eTranspose_P_B_rot
-    )
+    return np.array(self.dy)

acsl_pychrono/control/MRAC/mrac_gains.py

@@ -1,7 +1,6 @@
 import math
 import numpy as np
 from numpy import linalg as LA
-import scipy
 from scipy import linalg
 from acsl_pychrono.flight_params import FlightParams
 

acsl_pychrono/control/MRAC/mrac_logger.py

@@ -1,19 +1,16 @@
-import math
 import numpy as np  
 from acsl_pychrono.control.MRAC.mrac_gains import MRACGains
 from acsl_pychrono.control.MRAC.mrac import MRAC
-from acsl_pychrono.ode_input import OdeInput
-from acsl_pychrono.flight_params import FlightParams
 
 class MRACLogger:
   def __init__(self, gains: MRACGains) -> None:
     self.gains = gains
     self.data_list = []
 
-  def collect_data(self, controller: MRAC, time_now: float, simulation_time: float):
+  def collectData(self, controller: MRAC, simulation_time: float):
     DATA_vector = np.zeros((self.gains.size_DATA, 1))
 
-    DATA_vector[0] = time_now
+    DATA_vector[0] = controller.odein.time_now
     DATA_vector[1] = simulation_time
     DATA_vector[2:5] = controller.odein.translational_position_in_I
     DATA_vector[5:8] = controller.odein.translational_velocity_in_I
@@ -58,7 +55,7 @@ def collect_data(self, controller: MRAC, time_now: float, simulation_time: float
 
     self.data_list.append(DATA_vector.flatten())
 
-  def to_dictionary(self):
+  def toDictionary(self):
     DATA_np = np.array(self.data_list)
 
     log_dict = {

acsl_pychrono/control/PID/pid.py

@@ -7,25 +7,28 @@
 from acsl_pychrono.control.control import Control
 
 class PID(Control):
-  def __init__(self, gains: PIDGains, ode_input: OdeInput, flight_params: FlightParams):
+  def __init__(self, gains: PIDGains, ode_input: OdeInput, flight_params: FlightParams, timestep: float):
+    super().__init__(odein=ode_input)
     self.gains = gains
-    self.odein = ode_input
     self.fp = flight_params
+    self.timestep = timestep
     self.safety_mechanism = OuterLoopSafetyMechanism(gains, self.fp.G_acc)
     self.dy = np.zeros((self.gains.number_of_states, 1))
+    # Initial conditions
+    self.y = np.zeros((self.gains.number_of_states, 1))
 
-  def computeControlAlgorithm(self, y, ode_input: OdeInput):
+  def computeControlAlgorithm(self, ode_input: OdeInput):
     """
     Compute all intermediate variables and control inputs once per RK4 step to compute the dy for RK4.
     """
     # Update the vehicle state and user-defined trajectory
     self.odein = ode_input
 
     # ODE state
-    self.state_phi_ref_diff = y[0:2]
-    self.state_theta_ref_diff = y[2:4]
-    self.integral_position_tracking = y[4:7]
-    self.integral_angular_error = y[7:10]
+    self.state_phi_ref_diff = self.y[0:2]
+    self.state_theta_ref_diff = self.y[2:4]
+    self.integral_position_tracking = self.y[4:7]
+    self.integral_angular_error = self.y[7:10]
 
     # Compute translational position error
     self.translational_position_error = Control.computeTranslationalPositionError(
@@ -86,7 +89,7 @@ def computeControlAlgorithm(self, y, ode_input: OdeInput):
     # Compute individual motor thrusts
     self.motor_thrusts = Control.computeMotorThrusts(self.fp, self.u1, self.u2, self.u3, self.u4)
 
-  def ode(self, t, y, ode_input: OdeInput):
+  def ode(self, t, y):
     """
     Function called by RK4. Assumes `computeControlAlgorithm` was called
     at the beginning of the integration step to update internal state.

acsl_pychrono/control/PID/pid_logger.py

@@ -10,10 +10,10 @@ def __init__(self, gains: PIDGains) -> None:
     self.gains = gains
     self.data_list = []
 
-  def collect_data(self, controller: PID, time_now: float, simulation_time: float):
+  def collectData(self, controller: PID, simulation_time: float):
     DATA_vector = np.zeros((self.gains.size_DATA, 1))
 
-    DATA_vector[0] = time_now
+    DATA_vector[0] = controller.odein.time_now
     DATA_vector[1] = simulation_time
     DATA_vector[2:5] = controller.odein.translational_position_in_I
     DATA_vector[5:8] = controller.odein.translational_velocity_in_I
@@ -45,7 +45,7 @@ def collect_data(self, controller: PID, time_now: float, simulation_time: float)
 
     self.data_list.append(DATA_vector.flatten())
 
-  def to_dictionary(self):
+  def toDictionary(self):
     DATA_np = np.array(self.data_list)
 
     log_dict = {

acsl_pychrono/control/__init__.py

@@ -1,4 +1,20 @@
 # acsl_pychrono/control/__init__.py
 
 from .PID import PID, PIDGains, PIDLogger
-from .MRAC import MRAC, MRACGains, MRACLogger
+from .MRAC import MRAC, MRACGains, MRACLogger
+
+controller_classes = {
+  'PID': (PIDGains, PID, PIDLogger),
+  'MRAC': (MRACGains, MRAC, MRACLogger),
+}
+
+def instantiateController(controller_type: str, ode_input, flight_params, timestep):
+  if controller_type not in controller_classes:
+    raise ValueError(f"Unknown controller type: {controller_type}")
+  
+  GainsClass, ControllerClass, LoggerClass = controller_classes[controller_type]
+  gains = GainsClass(flight_params)
+  controller = ControllerClass(gains, ode_input, flight_params, timestep)
+  logger = LoggerClass(gains)
+
+  return gains, controller, logger