added piecewise polynomial trajectory

Author: mattia-gramuglia

GainsController.py

@@ -1449,8 +1449,8 @@ def MRACwithBASELINE_SafetyMechanism(mass_total_estimated,air_density_estimated,
 
         # Mu - elliptic cone intersection
         EllipticConeEpsilon = 1e-2
-        maximumRollAngle = math.radians(80) # [rad] 25 - 32
-        maximumPitchAngle = math.radians(80) # [rad] 25 - 32
+        maximumRollAngle = math.radians(60) # [rad] 25 - 32
+        maximumPitchAngle = math.radians(60) # [rad] 25 - 32
 
         # Mu - plane intersection
         planeEpsilon = 1e-2

drone_bb_simulator.py

@@ -57,7 +57,8 @@ def WrapperMain_function(target_folder, controller_type, wrapper_control_paramet
     global angular_error_dot, u2, u3, u4, mu_baseline_tran, mu_adaptive_tran, Moment_baseline, Moment_adaptive
     global mu_PD_baseline_tran, Moment_baseline_PI, e_tran, integral_eQe_tran, e_rot, integral_eQe_rot
     global epsilon_tran, integral_epsQeps_tran, epsilon_rot, integral_epsQeps_rot, e_transient_tran
-    global integral_etQet_tran, e_transient_rot, integral_etQet_rot, omega_ref_dot
+    global integral_etQet_tran, e_transient_rot, integral_etQet_rot, omega_ref_dot, omega_cmd, omega_cmd_dot
+    global Jacobian_matrix, Jacobian_matrix_dot
 
     #%% File settings
 
@@ -336,7 +337,8 @@ def WrapperMain_function(target_folder, controller_type, wrapper_control_paramet
     mfloor = chrono.ChBodyEasyBox(50, 0.1, 50, 1000,True,True, contact_material_floor)
     mfloor.SetName('Floor')
     mfloor.SetBodyFixed(True)
-    mfloor.SetPos(chrono.ChVectorD(0,-0.3,0))
+    mfloor_Yposition = 0.3
+    mfloor.SetPos(chrono.ChVectorD(0,-mfloor_Yposition,0))
     # mfloor.SetPos(chrono.ChVectorD(0,-0.5,0))
     mfloor.GetVisualShape(0).SetTexture(chrono.GetChronoDataFile("textures/light_gray.png"))
     # mfloor.GetVisualShape(0).SetTexture(chrono.GetChronoDataFile("textures/concrete.jpg"))
@@ -900,13 +902,34 @@ def WrapperMain_function(target_folder, controller_type, wrapper_control_paramet
         ####################################################################################################################################################################
 
     elif trajectory_type == 'piecewisePolynomial_trajectory':
-        pp_coefficients = np.loadtxt(open("trajectory_PolynomialCoefficientMatrix.csv",
-                                          "rb"), delimiter=",", skiprows=0)
+        pp_coefficients = np.loadtxt(open(
+                                  "trajectory_PolynomialCoefficientMatrix.csv",
+                                  "rb"), delimiter=",", skiprows=0)
         waypointTimes = np.loadtxt(open("trajectory_WaypointTimes.csv", "rb"),
                                    delimiter=",", skiprows=0)
 
         trajectory_instance = piecewisePolynomial_trajectory()
-        trajectory_instance.set_parameters(pp_coefficients)
+        trajectory_instance.set_parameters(pp_coefficients, waypointTimes)
+        
+        # Create a ChLinePath geometry, and insert sub-paths
+        mpath = chrono.ChLinePath()
+        [pos_x, pos_y, pos_z] = trajectory_instance.ComputePositionVector(0.01)
+        
+        for i in range(pos_x.size - 1):
+          mpath.AddSubLine(chrono.ChLineSegment(
+            chrono.ChVectorD(pos_x[i],
+                             -pos_z[i] + mfloor_Yposition,
+                             pos_y[i]),
+            chrono.ChVectorD(pos_x[i+1],
+                             -pos_z[i+1] + mfloor_Yposition,
+                             pos_y[i+1])))
+        
+        # Create a ChLineShape, a visualization asset for lines.
+        # The ChLinePath is a special type of ChLine and it can be visualized.
+        mpathasset = chrono.ChLineShape()
+        mpathasset.SetLineGeometry(mpath)
+        mpathasset.SetColor(chrono.ChColor(0,0,0))
+        mfloor.AddVisualShape(mpathasset)
 
     #%% Controller Inizialization
 
@@ -926,6 +949,11 @@ def WrapperMain_function(target_folder, controller_type, wrapper_control_paramet
     Moment_adaptive = np.zeros((3,1))
 
     omega_ref_dot = np.zeros((3,1))
+    omega_ref = np.zeros((3,1))
+    omega_cmd = np.zeros((3,1))
+    omega_cmd_dot = np.zeros((3,1))
+    Jacobian_matrix = np.matrix(np.zeros((3,3)))
+    Jacobian_matrix_dot = np.matrix(np.zeros((3,3)))
 
 
     # Maximum thrust produced by a single motor = 1355 grams = 1.355 kg = 1.355 kg * 9.81 m/s^2 = 13.28 N
@@ -1260,7 +1288,8 @@ def MRACwithBASELINE(t, y):
             global mu_x, mu_y, mu_z, u1, roll_ref, pitch_ref, roll_ref_dot, pitch_ref_dot, roll_ref_ddot, pitch_ref_ddot
             global angular_position_ref_dot, angular_position_ref_ddot, Jacobian_matrix_inverse, angular_position_dot
             global angular_error_dot, u2, u3, u4, mu_baseline_tran, mu_adaptive_tran, Moment_baseline, Moment_adaptive
-            global mu_PD_baseline_tran, Moment_baseline_PI, omega_ref_dot
+            global mu_PD_baseline_tran, Moment_baseline_PI, omega_ref_dot, omega_cmd, omega_cmd_dot, Jacobian_matrix
+            global Jacobian_matrix_dot
 
 
             state_phi_ref_diff = y[0:2] # State of the differentiator for phi_ref (roll_ref)
@@ -3104,7 +3133,8 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
             global mu_x, mu_y, mu_z, u1, roll_ref, pitch_ref, roll_ref_dot, pitch_ref_dot, roll_ref_ddot, pitch_ref_ddot
             global angular_position_ref_dot, angular_position_ref_ddot, Jacobian_matrix_inverse, angular_position_dot
             global angular_error_dot, u2, u3, u4, mu_baseline_tran, mu_adaptive_tran, Moment_baseline, Moment_adaptive
-            global mu_PD_baseline_tran, Moment_baseline_PI, omega_ref_dot
+            global mu_PD_baseline_tran, Moment_baseline_PI, omega_ref_dot, omega_cmd, omega_cmd_dot, Jacobian_matrix
+            global Jacobian_matrix_dot
 
 
             state_phi_ref_diff = y[0:2] # State of the differentiator for phi_ref (roll_ref)
@@ -3263,7 +3293,7 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
             Jacobian_matrix = np.matrix([[1,               0,                 -math.sin(pitch)],
                                           [0,  math.cos(roll), math.sin(roll) * math.cos(pitch)],
                                           [0, -math.sin(roll), math.cos(roll) * math.cos(pitch)]])
-            
+            print('HELLO Jacobian_matrix')
             omega_cmd = Jacobian_matrix * (-KP_rot*angular_error + angular_position_ref_dot)
             omega_cmd_dot = Jacobian_matrix_dot * (-KP_rot*angular_error + angular_position_ref_dot) + Jacobian_matrix * (-KP_rot*angular_error_dot + angular_position_ref_ddot)
 
@@ -3673,8 +3703,7 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
             [translational_position_in_I_user,
              translational_velocity_in_I_user,
              translational_acceleration_in_I_user] = (trajectory_instance.
-                    user_defined_trajectory(time_now - controller_start_time,
-                                            waypointTimes))
+                     user_defined_trajectory(time_now - controller_start_time))
             [yaw_ref,
              yaw_ref_dot,
              yaw_ref_ddot] = trajectory_instance.user_defined_yaw()
@@ -4657,7 +4686,16 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
         print ('\nSimulation time: ', time_now)
 
 
-        print('omega_ref_dot: ', omega_ref_dot)
+        print('omega_ref_dot: ', '%.4f'%omega_ref_dot[0], '%.4f'%omega_ref_dot[1], '%.4f'%omega_ref_dot[2])
+        print('omega_ref: ', '%.4f'%omega_ref[0], '%.4f'%omega_ref[1], '%.4f'%omega_ref[2])
+        print('omega_cmd_dot: ', '%.8f'%omega_cmd_dot[0],
+                                 '%.8f'%omega_cmd_dot[1],
+                                 '%.8f'%omega_cmd_dot[2])
+        print('omega_cmd: ', '%.4f'%omega_cmd[0], '%.4f'%omega_cmd[1], '%.4f'%omega_cmd[2])
+        print('Jacobian_matrix_dot: ', Jacobian_matrix_dot)
+        print('Jacobian_matrix: ', Jacobian_matrix)
+        
+        
         # print('z - z_ref: ', pos_pixhawk_LOC_to_GLOB_NED.z - z_ref)
         print('Z_onboard: ', '%.4f'%pos_pixhawk_LOC_to_GLOB_NED.z)
         # print('(G_acc - controller_z_output)*mass_total: ', (G_acc + controller_z_output)*mass_total)
@@ -4709,11 +4747,13 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
 
 
         # print('Pixhawk LOCAL Position: ', chvector_to_list(pos_pixhawk_LOC))
-        print('Pixhawk GLOBAL Position: ', chcoordsys_to_list(coord_pixhawk_GLOB)[0])
+        print('Pixhawk GLOBAL Position: ', '%.4f'%chcoordsys_to_list(coord_pixhawk_GLOB)[0][0],
+                                           '%.4f'%chcoordsys_to_list(coord_pixhawk_GLOB)[0][1],
+                                           '%.4f'%chcoordsys_to_list(coord_pixhawk_GLOB)[0][2])
         # print('Pixhawk GLOBAL Velocity: ', chcoordsys_to_list(coord_dt_pixhawk_GLOB)[0])
         # print('Pixhawk LOCAL  Velocity_T: ', chvector_to_list(vel_pixhawk_LOC_T))
         # print('Pixhawk LOCAL  Velocity: ', chvector_to_list(vel_pixhawk_LOC))
-        print('Pixhawk GLOBAL Velocity NORM: ', LA.norm(np.asarray(chcoordsys_to_list(coord_dt_pixhawk_GLOB)[0])))
+        print('Pixhawk GLOBAL Velocity NORM: ', '%.4f'%LA.norm(np.asarray(chcoordsys_to_list(coord_dt_pixhawk_GLOB)[0])))
         # print('Pixhawk LOCAL Velocity_T NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC_T))))
         # print('Pixhawk LOCAL Velocity NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC))),'\n')
 

loop.py

@@ -33,7 +33,7 @@
 
 # controller_type = 'PID' # Check mass_total_estimated
 
-controller_type = 'MRACwithBASELINE'
+# controller_type = 'MRACwithBASELINE'
 
 # controller_type = 'TwoLayerMRACwithBASELINE'
 
@@ -53,7 +53,7 @@
 
 # controller_type = 'FunnelTwoLayerMRACwithBASELINE'
 
-# controller_type = 'MRACwithBASELINE_SafetyMechanism'
+controller_type = 'MRACwithBASELINE_SafetyMechanism'
 
 # ----------------------------------------------------------------
 #                     %%%%%%%%%%%%%%%%%%%%%%

trajectory.py

@@ -418,7 +418,9 @@ def __init__(self):
     self.psi_ref_ddot = 0
     self.psi_ref_previous = 0
 
-  def set_parameters(self, pp_coefficients):
+  def set_parameters(self, pp_coefficients, waypointTimes):
+    
+    self.waypointTimes = waypointTimes
 
     [self.position_coef_x,
      self.position_coef_y,
@@ -436,7 +438,7 @@ def set_parameters(self, pp_coefficients):
     self.jerk_coef_y = traj_functions.PolyderMatrix(self.acceleration_coef_y)
     self.jerk_coef_z = traj_functions.PolyderMatrix(self.acceleration_coef_z)
 
-  def user_defined_trajectory(self, t, waypointTimes):
+  def user_defined_trajectory(self, t):
       """Piecewise polynomial trajectory.
       t: current simulation time
       pp_coefficients: matrix of the coefficients of the piecewise polynomial
@@ -445,37 +447,37 @@ def user_defined_trajectory(self, t, waypointTimes):
       """
 
       [self.t_adjusted,
-       self.segment] = traj_functions.PolyTimeAdjusted(waypointTimes, t)
+       self.segment] = traj_functions.PolyTimeAdjusted(self.waypointTimes, t)
 
-      self.translational_position_in_I_user[0] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.position_coef_x[self.segment,:]))
-      self.translational_position_in_I_user[1] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.position_coef_y[self.segment,:]))
-      self.translational_position_in_I_user[2] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.position_coef_z[self.segment,:]))
+      self.translational_position_in_I_user[0] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.position_coef_x[self.segment,:])
+      self.translational_position_in_I_user[1] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.position_coef_y[self.segment,:])
+      self.translational_position_in_I_user[2] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.position_coef_z[self.segment,:])
 
-      self.translational_velocity_in_I_user[0] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.velocity_coef_x[self.segment,:]))
-      self.translational_velocity_in_I_user[1] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.velocity_coef_y[self.segment,:]))
-      self.translational_velocity_in_I_user[2] = (polynomial.
-                                polyval(self.t_adjusted,
-                                        self.velocity_coef_z[self.segment,:]))
+      self.translational_velocity_in_I_user[0] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.velocity_coef_x[self.segment,:])
+      self.translational_velocity_in_I_user[1] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.velocity_coef_y[self.segment,:])
+      self.translational_velocity_in_I_user[2] = polynomial.polyval(
+                                          self.t_adjusted,
+                                          self.velocity_coef_z[self.segment,:])
 
-      self.translational_acceleration_in_I_user[0] = (polynomial.
-                            polyval(self.t_adjusted,
-                                    self.acceleration_coef_x[self.segment,:]))
-      self.translational_acceleration_in_I_user[1] = (polynomial.
-                            polyval(self.t_adjusted,
-                                    self.acceleration_coef_y[self.segment,:]))
-      self.translational_acceleration_in_I_user[2] = (polynomial.
-                            polyval(self.t_adjusted,
-                                    self.acceleration_coef_z[self.segment,:]))
+      self.translational_acceleration_in_I_user[0] = polynomial.polyval(
+                                      self.t_adjusted,
+                                      self.acceleration_coef_x[self.segment,:])
+      self.translational_acceleration_in_I_user[1] = polynomial.polyval(
+                                      self.t_adjusted,
+                                      self.acceleration_coef_y[self.segment,:])
+      self.translational_acceleration_in_I_user[2] = polynomial.polyval(
+                                      self.t_adjusted,
+                                      self.acceleration_coef_z[self.segment,:])
 
       return [self.translational_position_in_I_user,
               self.translational_velocity_in_I_user,
@@ -484,10 +486,10 @@ def user_defined_trajectory(self, t, waypointTimes):
   def user_defined_yaw(self):
     "User-defined reference yaw angle"
 
-    self.velocity_norm2D = (traj_functions.
-                            Norm2D(self.velocity_coef_x[self.segment,:],
-                                   self.velocity_coef_y[self.segment,:],
-                                   self.t_adjusted))
+    self.velocity_norm2D = traj_functions.Norm2D(
+                                          self.velocity_coef_x[self.segment,:],
+                                          self.velocity_coef_y[self.segment,:],
+                                          self.t_adjusted)
 
     if self.t_adjusted == 0:
       self.psi_ref = 0
@@ -500,33 +502,51 @@ def user_defined_yaw(self):
       self.psi_ref_ddot = 0
 
     else:
-      self.psi_ref = (traj_functions.
-                      YawComputation(self.velocity_coef_x[self.segment,:],
-                                     self.velocity_coef_y[self.segment,:],
-                                     self.t_adjusted))
-      self.psi_ref_dot = (traj_functions.
-                          YawDotComputation(
+      self.psi_ref = traj_functions.YawComputation(
+                                          self.velocity_coef_x[self.segment,:],
+                                          self.velocity_coef_y[self.segment,:],
+                                          self.t_adjusted)
+      self.psi_ref_dot = traj_functions.YawDotComputation(
                                       self.velocity_coef_x[self.segment,:],
                                       self.velocity_coef_y[self.segment,:],
                                       self.acceleration_coef_x[self.segment,:],
                                       self.acceleration_coef_y[self.segment,:],
-                                      self.t_adjusted))
-      self.psi_ref_ddot = (traj_functions.
-                           YawDotDotComputation(
+                                      self.t_adjusted)
+      self.psi_ref_ddot = traj_functions.YawDotDotComputation(
                                       self.velocity_coef_x[self.segment,:],
                                       self.velocity_coef_y[self.segment,:],
                                       self.acceleration_coef_x[self.segment,:],
                                       self.acceleration_coef_y[self.segment,:],
                                       self.jerk_coef_x[self.segment,:],
                                       self.jerk_coef_y[self.segment,:],
-                                      self.t_adjusted))
+                                      self.t_adjusted)
 
     self.psi_ref_previous = self.psi_ref
 
     return [self.psi_ref,
             self.psi_ref_dot,
             self.psi_ref_ddot]
-
+  
+  def ComputePositionVector(self, samplingTime = 0.01):
+    "Drawing ChLineSegement for visualization of the trajectory in simulation"
+    
+    self.samplingTime = samplingTime
+    
+    sampling_time_vector = traj_functions.SamplingTimeVector(self.waypointTimes,
+                                                             self.samplingTime)
+    pos_x = np.zeros(sampling_time_vector.size)
+    pos_y = np.zeros(sampling_time_vector.size)
+    pos_z = np.zeros(sampling_time_vector.size)
+    
+    for i in range(sampling_time_vector.size):
+      
+      [position, _ , _]= self.user_defined_trajectory(sampling_time_vector[i])
+      
+      pos_x[i] = position[0]
+      pos_y[i] = position[1]
+      pos_z[i] = position[2]
+    
+    return [pos_x, pos_y, pos_z]