diff --git a/src/smarc_modelling/control/acados_Trajectory_simulator.py b/src/smarc_modelling/control/acados_Trajectory_simulator.py index 3bb4637..242527c 100644 --- a/src/smarc_modelling/control/acados_Trajectory_simulator.py +++ b/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 diff --git a/src/smarc_modelling/control/control.py b/src/smarc_modelling/control/control.py index 718fed1..e039bf8 100644 --- a/src/smarc_modelling/control/control.py +++ b/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' diff --git a/src/smarc_modelling/sam_casadi_sim.py b/src/smarc_modelling/sam_casadi_sim.py index 5610bf6..ee67c28 100644 --- a/src/smarc_modelling/sam_casadi_sim.py +++ b/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') diff --git a/src/smarc_modelling/vehicles/SAM_casadi.py b/src/smarc_modelling/vehicles/SAM_casadi.py index 6341040..f2cd6a7 100644 --- a/src/smarc_modelling/vehicles/SAM_casadi.py +++ b/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):