Pushing changes from SAM

src/smarc_modelling/control/acados_Trajectory_simulator.py

@@ -127,7 +127,7 @@ def main():
     #file_path = "/home/admin/smarc_modelling/src/Trajectories/report_update/easy/trajectories/case_easy0.csv"
     #file_path = "/home/parallels/ros2_ws/src/smarc2/behaviours/sam/sam_diving_controller/sam_diving_controller/trajectoryComplexity3.csv"
     #file_path = "/home/parallels/ros2_ws/src/smarc2/behaviours/sam/sam_diving_controller/sam_diving_controller/trajectories/good_trajectory2.csv"
-    file_path = "/home/parallels/ros2_ws/src/smarc2/behaviours/sam/sam_diving_controller/sam_diving_controller/trajectories/straight_trajectory_1.csv"
+    file_path = "/home/orin/colcon_ws/src/smarc2/behaviours/sam/sam_diving_controller/sam_diving_controller/trajectories/straight_trajectory_1.csv"
 
 
     #file_path = "/home/admin/smarc_modelling/src/Trajectories/resolution01.csv"  
@@ -274,7 +274,7 @@ def main():
     zero_values[:,3] = 1.
     #plot.plot_function(x_axis, trajectory, zero_values, simU)
     #plot.plot_function(x_axis, trajectory, simX[:-1], simU)
-    plot.plot_function(x_axis, ref_0, simX[:-1], simU)
+    #plot.plot_function(x_axis, ref_0, simX[:-1], simU)
     ocp_solver = None
 
 

src/smarc_modelling/control/control.py

@@ -27,29 +27,30 @@ def __init__(self, casadi_model, Ts, N_horizon, update_solver_settings):
         # --------------------------- Cost setup ---------------------------------
         # State weight matrix
         Q_diag = np.ones(self.nx)
-        Q_diag[ 0:2 ] = 100     # Position:         standard 10
-        Q_diag[ 2 ] = 500       # z-Position:         standard 10
-        Q_diag[ 3:7 ] = 10       # Quaternion:       standard 10
-        Q_diag[ 7:10] = 10       # linear velocity:  standard 1
+        Q_diag[ 0 ] = 500     # Position:         standard 10
+        Q_diag[ 1 ] = 10     # Position:         standard 10
+        Q_diag[ 2 ] = 100       # z-Position:         standard 10
+        Q_diag[ 3:7 ] = 1       # Quaternion:       standard 10
+        Q_diag[ 7:10] = 1       # linear velocity:  standard 1
         Q_diag[10:13] = 1       # Angular velocity: standard 1
 
         # Control weight matrix - Costs set according to Bryson's rule
         Q_diag[13] = 1e-5            # VBS:      Standard: 1e-4
         Q_diag[14] = 1e-5            # LCG:      Standard: 1e-4
-        Q_diag[ 15  ] = 5e2             # stern_angle:   Standard: 100
-        Q_diag[ 16  ] = 1e2             # rudder_angle:  Standard: 100
-        Q_diag[17:  ] = 1e-3            # RPM1 And RPM2: Standard: 1e-6
-        Q_diag[13:  ] = Q_diag[13:  ]   # Adjustment to all control weights
+        Q_diag[15] = 5e2             # stern_angle:   Standard: 100
+        Q_diag[16] = 1e2             # rudder_angle:  Standard: 100
+        Q_diag[17:] = 1e-6 #1e-3            # RPM1 And RPM2: Standard: 1e-6
+        #Q_diag[13:] = Q_diag[13:  ]   # Adjustment to all control weights
         Q = np.diag(Q_diag)
 
         # Control rate of change weight matrix - control inputs as [x_vbs, x_lcg, delta_s, delta_r, rpm1, rpm2]
         R_diag = np.ones(self.nu)
-        R_diag[0] = 1e-1        # VBS
+        R_diag[0] = 1e-2 #1e-1        # VBS
         R_diag[1] = 1e-1        # LCG
         R_diag[2] = 1e2
         R_diag[3] = 1e3
         R_diag[4: ] = 1e-5
-        R = np.diag(R_diag)*1e-3
+        R = np.diag(R_diag)#*1e-3
 
         # Stage costs
         self.model.p = ca.MX.sym('ref_param', self.nx+self.nu,1)
@@ -138,12 +139,14 @@ def __init__(self, casadi_model, Ts, N_horizon, update_solver_settings):
         self.ocp.solver_options.sim_method_newton_iter = 2 #3 default
 
         self.ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+        #self.ocp.solver_options.nlp_solver_type = 'SQP'
         #self.ocp.solver_options.nlp_solver_type = 'SQP_WITH_FEASIBLE_QP'
         #self.ocp.solver_options.search_direction_mode = 'BYRD_OMOJOKUN'
         #self.ocp.solver_options.allow_direction_mode_switch_to_nominal = False
         self.ocp.solver_options.nlp_solver_max_iter = 1 #80
         self.ocp.solver_options.tol    = 1e-6       # NLP tolerance. 1e-6 is default for tolerances
         self.ocp.solver_options.qp_tol = 1e-6       # QP tolerance
+        #self.ocp.solver_options.qp_mu0 = 5e-1       # QP initial barrier 
 
         self.ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
         #self.ocp.solver_options.regularize_method = 'NO_REGULARIZE'

src/smarc_modelling/sam_casadi_sim.py

@@ -21,7 +21,7 @@
 
 # Simulation timespan
 dt = 0.01 #0.01 
-t_span = (0, 10)  # 20 seconds simulation
+t_span = (0, 3)  # 20 seconds simulation
 n_sim = int(t_span[1]/dt)
 t_eval = np.linspace(t_span[0], t_span[1], n_sim)
 
@@ -47,7 +47,7 @@ def rk4(x, u, dt, fun):
     x_t = x + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
 
     np.set_printoptions(precision=3)
-    print(f"vbs: {x_t.full().flatten()[13]}; vbs_dot: {k1.full().flatten()[13]}")
+    #print(f"vbs: {x_t.full().flatten()[13]}; vbs_dot: {k1.full().flatten()[13]}")
 
     return x_t.full().flatten()
 
@@ -57,11 +57,11 @@ def run_simulation(t_span, x0, dt, sam):
     """
 
     u = np.zeros(6)
-    u[0] = 12000#*np.sin((i/(20/0.02))*(3*np.pi/4))        # VBS
+    u[0] = 50 #*np.sin((i/(20/0.02))*(3*np.pi/4))        # VBS
     u[1] = 50 # LCG
     u[2] = 0 #np.deg2rad(7)    # Vertical (stern)
-    u[3] = 0 #-np.deg2rad(7)   # Horizontal (rudder)
-    u[4] = 0 #1000     # RPM 1
+    u[3] = -np.deg2rad(7)   # Horizontal (rudder)
+    u[4] = 1000 #1000     # RPM 1
     u[5] = u[4]     # RPM 2
 
     # Run integration
@@ -106,12 +106,12 @@ def quaternion_to_euler_vec(sol):
     _, axs = plt.subplots(8, 3, figsize=(12, 10))
 
     # Position plots
-    axs[0,0].plot(sol.t, sol.y[1], label='x')
-    axs[0,1].plot(sol.t, sol.y[0], label='y')
-    axs[0,2].plot(sol.t, -sol.y[2], label='z')
+    axs[0,0].plot(sol.t, sol.y[0], label='x')
+    axs[0,1].plot(sol.t, sol.y[1], label='y')
+    axs[0,2].plot(sol.t, sol.y[2], label='z')
     axs[0,0].set_ylabel('x Position [m]')
     axs[0,1].set_ylabel('y Position [m]')
-    axs[0,2].set_ylabel('-z Position [m]')
+    axs[0,2].set_ylabel('z Position [m]')
 
     # Euler plots
     axs[1,0].plot(sol.t, np.rad2deg(phi_vec), label='roll')
@@ -200,6 +200,7 @@ def anim_function(num, dataSet, line):
 
     # Line/trajectory plot
     line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b')[0] 
+    start = plt.plot(dataSet[0,0], dataSet[1,0], dataSet[2,0], lw=2, c='g', marker='o')[0]
 
     # Setting the axes properties
     ax.set_xlabel('X / East')

src/smarc_modelling/vehicles/SAM_casadi.py

@@ -207,7 +207,7 @@ def __init__(
 
         # Some factors to make sim agree with real life data, these are eyeballed from sim vs gt data
         #self.vbs_factor = 0.5 # How sensitive the vbs is
-        #self.inertia_factor = 2 # Adjust how quickly we can change direction
+        self.inertia_factor = 10 # Adjust how quickly we can change direction
         #self.damping_factor = 60 # Adjust how much the damping affect acceleration high number = move less
         #self.damping_rot = 5 # Adjust how much the damping affects the rotation high number = less rotation should be tuned on bag where we turn without any control inputs
         #self.thruster_rot_strength = 1  # Just making the thruster a bit stronger for rotation
@@ -245,8 +245,8 @@ def __init__(
 
         # Rigid-body mass matrix expressed in CO
         u_init = np.zeros(6)
-        u_init[0] = 72 
-        u_init[1] = 75 #45
+        u_init[0] = 50 #72 
+        u_init[1] = 50 #75 #45
         self.x_vbs_init = self.calculate_vbs_position(u_init)
         # Update actuators
         self.x_vbs = self.calculate_vbs_position(u_init) 
@@ -284,19 +284,19 @@ def __init__(
 
         # NOTE: These need to be identified properly
         # Damping coefficients
-        self.Xuu = 1e-0 * 50 # default: 3 #100     # x-damping
-        self.Yvv = 1e-1 * 50 # default: 50    # y-damping
-        self.Zww = 1e1 * 150 # default: 50    # z-damping
-        self.Kpp = 1e-1 * 40 # default: 40    # Roll damping
-        self.Mqq = 1e-2 * 150 # default: 200    # Pitch damping
-        self.Nrr = 1e1 * 150 # default: 10    # Yaw dampin
-
-        #self.Xuu = 3 #100     # x-damping
-        #self.Yvv = 50    # y-damping
-        #self.Zww = 50    # z-damping
-        #self.Kpp = 40    # Roll damping
-        #self.Mqq = 200    # Pitch damping
-        #self.Nrr = 10    # Yaw damping
+        #self.Xuu = 1e-0 * 50 # default: 3 #100     # x-damping
+        #self.Yvv = 1e-1 * 50 # default: 50    # y-damping
+        #self.Zww = 1e1 * 150 # default: 50    # z-damping
+        #self.Kpp = 1e-1 * 40 # default: 40    # Roll damping
+        #self.Mqq = 1e-2 * 150 # default: 200    # Pitch damping
+        #self.Nrr = 1e1 * 150 # default: 10    # Yaw dampin
+
+        self.Xuu = 3 #100     # x-damping
+        self.Yvv = 50    # y-damping
+        self.Zww = 50    # z-damping
+        self.Kpp = 40    # Roll damping
+        self.Mqq = 200    # Pitch damping
+        self.Nrr = 10    # Yaw damping
 
         # Center of effort -> where the thrust force acts?
         self.x_cp = 0.1
@@ -325,8 +325,9 @@ def init_vehicle(self):
             d_ss=0.19,
             #m_ss=12.225-1,
             #p_CSsg_O = np.array([0.74+0.05, 0, 0.06]),
-            p_CSsg_O = np.array([0.74+0.0175, 0, 0.0]),
-            m_ss=14.9-1,
+            #p_CSsg_O = np.array([0.74+0.0175, 0, 0.0]),
+            p_CSsg_O = np.array([0.74, 0, 0.0]),
+            m_ss=14.9,
             #p_CSsg_O = np.array([0.74, 0, 0.06]),
             p_OC_O=self.p_OC_O
         )
@@ -341,9 +342,9 @@ def init_vehicle(self):
 
         self.lcg = LongitudinalCenterOfGravityControl(
             l_lcg_l=0.223,
-            l_lcg_r=0.1,
-            #l_lcg_r=0.06,
-            m_lcg=2.6+1,
+            #l_lcg_r=0.1,
+            l_lcg_r=0.06,
+            m_lcg=2.6,
             h_lcg_dim=0.08,
             p_OC_O=self.p_OC_O
         )
@@ -532,8 +533,8 @@ def update_inertias(self):
         # everythign is more or less symmetric etc. That's not true. Hence, we
         # adjust the inertias to match the tank experiments until we actually
         # measured them.
-        #self.J_total[0, 0] *= self.inertia_factor
-        self.J_total[1,1] *= 100
+        self.J_total[0, 0] *= self.inertia_factor
+        #self.J_total[1,1] *= 100
 
 
     def calculate_M(self):