From 20f4a5b45f6909e159b7e85875a95705f0beda34 Mon Sep 17 00:00:00 2001 From: Rakesh Vivekanandan Date: Wed, 28 Aug 2024 23:22:34 +0000 Subject: [PATCH] minor changes done; TODO: fix MPC --- .../config/mpc_bluerov2_heavy.yaml | 16 +- docking_control/launch/mission_control.launch | 1 + docking_control/src/mission_control.py | 10 +- docking_control/src/mpc_acados.py | 241 ++++++++---------- docking_control/src/mpc_external_cost.py | 8 +- 5 files changed, 120 insertions(+), 156 deletions(-) diff --git a/docking_control/config/mpc_bluerov2_heavy.yaml b/docking_control/config/mpc_bluerov2_heavy.yaml index 7b3a03b..b66b8ef 100644 --- a/docking_control/config/mpc_bluerov2_heavy.yaml +++ b/docking_control/config/mpc_bluerov2_heavy.yaml @@ -32,7 +32,7 @@ penalty: # Q: [0., 0., 0., 0., 0., 0.] Q: [1, 1, 1, 1, 1, 1] - R: [0.01, 0.01, 0.1, 0.01, 0.01, 0.01] + R: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] # R: [1, 0.01, 0.01, 10, 10, 0.01] # R: [0.01, 0.01, 0.01, 0.1, 0.1, 0.1] # R: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] @@ -44,8 +44,8 @@ penalty: # R: [100, 100, 100, 100, 100, 100] bounds: - xmin: [-100, -100, -100., -3.14, -3.14, -3.14, -1, -1, -1, -0.087, -0.087, -0.087] - xmax: [100, 100, 100, 3.14, 3.14, 3.14, 1, 1, 1, 0.087, 0.087, 0.087] + xmin: [-15., -15., -15., -3.14, -3.14, -3.14, -1, -1, -1, -0.087, -0.087, -0.087] + xmax: [15., 15., 15., 3.14, 3.14, 3.14, 1, 1, 1, 0.087, 0.087, 0.087] # umin: [-0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1] # umax: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] @@ -53,10 +53,12 @@ bounds: # umax: [250.0, 250.0, 250.0, 250.0, 250.0, 250.0] # umin: [-20.0, -20.0, -20.0, -20.0, -20.0, -20.0] # umax: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0] - # umin: [-35.0, -20.0, -25.0, -1.0, -1.0, -15] - # umax: [35.0, 20.0, 25.0, 1.0, 1.0, 15] - umin: [-35.0, -35.0, -35.0, -35.0, -35.0, -35.0] - umax: [35.0, 35.0, 35.0, 35.0, 35.0, 35.0] + umin: [-35.0, -20.0, -25.0, -1.0, -1.0, -15] + umax: [35.0, 20.0, 25.0, 1.0, 1.0, 15] + # umin: [-35.0, -35.0, -35.0, -10.0, -10.0, -10.0] + # umax: [35.0, 35.0, 35.0, 10.0, 10.0, 10.0] + # umin: [-35.0, -35.0, -35.0, -35.0, -35.0, -35.0] + # umax: [35.0, 35.0, 35.0, 35.0, 35.0, 35.0] # umin: [-100.0, -100.0, -100.0, -100.0, -100.0, -100.0] # umax: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] diff --git a/docking_control/launch/mission_control.launch b/docking_control/launch/mission_control.launch index 65a980e..67dad25 100644 --- a/docking_control/launch/mission_control.launch +++ b/docking_control/launch/mission_control.launch @@ -50,6 +50,7 @@ + diff --git a/docking_control/src/mission_control.py b/docking_control/src/mission_control.py index bd6374b..a918e2d 100755 --- a/docking_control/src/mission_control.py +++ b/docking_control/src/mission_control.py @@ -441,6 +441,8 @@ def auto_control(self, joy): # xr = np.array([[2.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T # xr = np.array([[x0[0, 0], 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T xr = np.array([[-5.0, 5.0, 5.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T + # xr = np.array([[x0[0, 0], 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T + # xr = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T try: forces, wrench, converge_flag = self.mpc.run_mpc(x0, xr) @@ -491,14 +493,12 @@ def auto_control(self, joy): # pwm.append(self.neutral_pwm) for i in range(len(pwm)): - pwm[i] = max( - min(pwm[i], self.max_pwm_auto), self.min_pwm_auto - ) + pwm[i] = max(min(pwm[i], self.max_pwm_auto), self.min_pwm_auto) - if all(element == 0.0 for element in np.round(wrench[1:6],2)): + if all(element == 0.0 for element in np.round(wrench[1:6], 2)): pwm[0:4] = [1800, 1800, 1800, 1800] - pwm[8] = 1250 + pwm[8] = 1300 self.mpc_pwm_pub.publish(pwm) self.control_pub.publish(pwm) diff --git a/docking_control/src/mpc_acados.py b/docking_control/src/mpc_acados.py index 77bd54f..a611679 100755 --- a/docking_control/src/mpc_acados.py +++ b/docking_control/src/mpc_acados.py @@ -1,7 +1,7 @@ import numpy as np import yaml from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel -from casadi import evalf, SX, mtimes, pinv, hessian, vertcat +from casadi import evalf, SX, mtimes, pinv import sys sys.path.insert(0, "/home/ros/ws_dock/src/underwater_docking/docking_control/src") @@ -10,6 +10,12 @@ class MPC: def __init__(self, auv, mpc_params): + """This class is used to control the AUV using the MPC controller. + + Args: + auv: Vehicle object + mpc_params: Dictionary containing the MPC parameters + """ self.auv = auv self.dt = mpc_params["dt"] self.log_quiet = mpc_params["quiet"] @@ -20,19 +26,13 @@ def __init__(self, auv, mpc_params): self.P = np.diag(mpc_params["penalty"]["P"]) self.Q = np.diag(mpc_params["penalty"]["Q"]) self.R = np.diag(mpc_params["penalty"]["R"]) - self.xmin = np.array(mpc_params["bounds"]["xmin"], dtype=np.float64) - self.xmax = np.array(mpc_params["bounds"]["xmax"], dtype=np.float64) - self.umin = np.array(mpc_params["bounds"]["umin"], dtype=np.float64) - self.umax = np.array(mpc_params["bounds"]["umax"], dtype=np.float64) - self.dumin = np.array(mpc_params["bounds"]["dumin"], dtype=np.float64) - self.dumax = np.array(mpc_params["bounds"]["dumax"], dtype=np.float64) + self.xmin = np.array(mpc_params["bounds"]["xmin"], dtype=np.float) + self.xmax = np.array(mpc_params["bounds"]["xmax"], dtype=np.float) + self.umin = np.array(mpc_params["bounds"]["umin"], dtype=np.float) + self.umax = np.array(mpc_params["bounds"]["umax"], dtype=np.float) # self.previous_control = np.random.uniform(-0.1, 0.1, size=(self.thrusters,1)) - # self.previous_control = np.zeros((self.thrusters, 1)) - - self.previous_control = [] - self.previous_states = [] - self.flag = False + self.previous_control = np.zeros((self.thrusters, 1)) self.acados_ocp = AcadosOcp() self.acados_model = AcadosModel() @@ -53,215 +53,176 @@ def load_params(cls, auv_filename, mpc_filename): return cls(auv, mpc_params) def auv_model(self): + """Create the AUV model for the MPC controller.""" x = SX.sym("x", (12, 1)) x_dot = SX.sym("x_dot", (12, 1)) u = SX.sym("u", (self.thrusters, 1)) - p = SX.sym("p", 18, 1) + SX.sym("nu_w", (3, 1)) + SX.sym("nu_w_dot", (3, 1)) + # f_expl_expr = self.nonlinear_dynamics(x, u, nu_w, nu_w_dot, self.model_type) f_expl_expr = self.nonlinear_dynamics(x, u, self.model_type) f_impl_expr = x_dot - f_expl_expr - # p[:] = vertcat(xr, u_prev) - # p = [xr, u_prev] - # p = np.arange(0, 18) - # p = [] + p = [] self.acados_model.name = "bluerov2" self.acados_model.f_expl_expr = f_expl_expr self.acados_model.f_impl_expr = f_impl_expr - # self.acados_model.disc_dyn_expr = f_expl_expr + # self.acados_model.disc_dyn_expr = self.forward_dyn(x, u, self.model_type) self.acados_model.x = x self.acados_model.xdot = x_dot self.acados_model.u = u self.acados_model.p = p + # def nonlinear_dynamics(self, x, u, nu_w, nu_w_dot, full_body): + # x_dot = self.auv.compute_nonlinear_dynamics( + # x, u, nu_w=nu_w, nu_w_dot=nu_w_dot, complete_model=full_body + # ) + # return x_dot + def nonlinear_dynamics(self, x, u, full_body): x_dot = self.auv.compute_nonlinear_dynamics(x, u, complete_model=full_body) return x_dot def create_ocp_solver_description(self): + """Create the optimal control problem (OCP) solver description""" self.acados_ocp.model = self.acados_model x = self.acados_model.x - u = self.acados_model.u nx = self.acados_model.x.shape[0] nu = self.acados_model.u.shape[0] - self.acados_ocp.dims.N = self.horizon - self.acados_ocp.parameter_values = np.zeros(18) - xr = self.acados_model.p[0:12] - u_prev = self.acados_model.p[12:] - - self.acados_ocp.cost.cost_type = "EXTERNAL" - self.acados_ocp.cost.cost_type_e = "EXTERNAL" - - # self.acados_ocp.model.cost_expr_ext_cost = x.T @ self.P @ x + u.T @ self.R @ u - # self.acados_ocp.model.cost_expr_ext_cost_e = x.T @ self.P @ x - - cost = (xr - x).T @ self.P @ (xr - x) + (u - u_prev).T @ self.R @ (u - u_prev) - # cost = (xr - x).T @ self.P @ (xr - x) + u.T @ self.R @ u - cost_e = (xr - x).T @ self.P @ (xr - x) - hessian_cost, grad_cost = hessian(cost, vertcat(u, x)) - hessian_cost_e, grad_cost_e = hessian(cost_e, x) + self.acados_ocp.dims.N = self.horizon + self.acados_ocp.cost.cost_type = "NONLINEAR_LS" + self.acados_ocp.cost.cost_type_e = "NONLINEAR_LS" + self.acados_ocp.cost.W_e = self.P + self.acados_ocp.cost.W = self.P + # self.acados_ocp.cost.W = block_diag(self.P, self.R) - # pdb.set_trace() - self.acados_ocp.model.p = vertcat(xr, u_prev) + # self.acados_ocp.model.cost_y_expr = vertcat(x, u) + self.acados_ocp.model.cost_y_expr = x + self.acados_ocp.model.cost_y_expr_e = x - self.acados_ocp.model.cost_expr_ext_cost = cost - self.acados_ocp.model.cost_expr_ext_cost_e = cost_e + self.acados_ocp.cost.yref = np.zeros((nx,)) + self.acados_ocp.cost.yref_e = np.zeros((nx,)) - self.acados_ocp.model.cost_expr_ext_cost_custom_hess = hessian_cost - self.acados_ocp.model.cost_expr_ext_cost_custom_hess_e = hessian_cost_e + # self.acados_ocp.cost.Vu_0 = self.R + # self.acados_ocp.cost.Vu = self.R self.acados_ocp.constraints.lbu = self.umin self.acados_ocp.constraints.ubu = self.umax self.acados_ocp.constraints.idxbu = np.arange(nu) - # nonlinear_constraint = u - u_prev - # self.acados_model.con_h_expr = nonlinear_constraint - # self.acados_ocp.dims.nh = nu - # self.acados_ocp.constraints.lh = self.dumin - # self.acados_ocp.constraints.uh = self.dumax - - self.acados_ocp.constraints.lbx_0 = self.xmin - self.acados_ocp.constraints.ubx_0 = self.xmax - self.acados_ocp.constraints.idxbx_0 = np.arange(nx) + # self.acados_ocp.constraints.lsbu = self.umin + # self.acados_ocp.constraints.usbu = self.umax + # self.acados_ocp.constraints.idxsbu = np.arange(nu) self.acados_ocp.constraints.lbx = self.xmin self.acados_ocp.constraints.ubx = self.xmax self.acados_ocp.constraints.idxbx = np.arange(nx) - self.acados_ocp.constraints.lbx_e = self.xmin - self.acados_ocp.constraints.ubx_e = self.xmax - self.acados_ocp.constraints.idxbx_e = np.arange(nx) - - # self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" - # self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_OSQP" - # self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_QPOASES" - self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" + self.acados_ocp.constraints.lbx_0 = self.xmin + self.acados_ocp.constraints.ubx_0 = self.xmax + self.acados_ocp.constraints.idxbx_0 = np.arange(nx) - self.acados_ocp.solver_options.hessian_approx = "EXACT" - # self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + # set options - # self.acados_ocp.solver_options.regularize_method = "CONVEXIFY" + # self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" + self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" + # self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_DAQP" + # self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_QPOASES" + # self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_OSQP" - # self.acados_ocp.solver_options.integrator_type = "DISCRETE" + self.acados_ocp.solver_options.qp_solver_cond_N = self.horizon + self.acados_ocp.solver_options.hessian_approx = ( + "GAUSS_NEWTON" # 'GAUSS_NEWTON', 'EXACT' + ) # self.acados_ocp.solver_options.integrator_type = "ERK" self.acados_ocp.solver_options.integrator_type = "IRK" self.acados_ocp.solver_options.sim_method_num_stages = 4 self.acados_ocp.solver_options.sim_method_num_steps = 1 - - self.acados_ocp.solver_options.nlp_solver_type = "SQP" - # self.acados_ocp.solver_options.nlp_solver_type = "SQP_RTI" + # self.acados_ocp.solver_options.nlp_solver_type = "SQP" + self.acados_ocp.solver_options.nlp_solver_type = "SQP_RTI" # self.acados_ocp.solver_options.nlp_solver_type = "DDP" - - self.acados_ocp.solver_options.exact_hess_cost = 1 - - # self.acados_ocp.solver_options.nlp_solver_max_iter = 500 - self.acados_ocp.solver_options.nlp_solver_max_iter = 1000 + self.acados_ocp.solver_options.nlp_solver_max_iter = 500 + # self.acados_ocp.solver_options.nlp_solver_max_iter = 1000 + # self.acados_ocp.solver_options.with_adaptive_levenberg_marquardt = True # self.acados_ocp.solver_options.levenberg_marquardt = 1e-5 - # self.acados_ocp.solver_options.line_search_use_sufficient_descent = 1 + self.acados_ocp.solver_options.tf = self.T_horizon # self.acados_ocp.solver_options.globalization = "MERIT_BACKTRACKING" + # self.acados_ocp.solver_options.ext_cost_num_hess = 1 - self.acados_ocp.solver_options.qp_solver_warm_start = 1 - - self.acados_ocp.solver_options.qp_solver_cond_N = self.horizon - self.acados_ocp.solver_options.tf = self.T_horizon + # self.acados_ocp.solver_options.qp_solver_warm_start = 1 def reset(self): """Reset the previous control to None.""" - self.previous_control = [] - self.previous_states = [] - self.flag = False - - def warm_start(self, x0): - # self.previous_control = self.acados_ocp_solver.solve_for_x0(x0, fail_on_nonzero_status=False) - # self.previous_control = np.array([self.previous_control]).T - self.acados_ocp_solver.set(0, "lbx", x0) - self.acados_ocp_solver.set(0, "ubx", x0) - - status = self.acados_ocp_solver.solve() - if status != 0: - print("acados returned status {}".format(status)) - - self.previous_control = [] - self.previous_states = [] - - for k in range(self.horizon): - wrench = self.acados_ocp_solver.get(k, "u") - self.previous_control.append(wrench) - - for k in range(self.horizon + 1): - state = self.acados_ocp_solver.get(k, "x") - self.previous_states.append(state) - - self.previous_control = np.array(self.previous_control).T - self.previous_states = np.array(self.previous_states).T + self.previous_control = None def run_mpc(self, x, x_ref): return self.optimize(x, x_ref) def optimize(self, x, x_ref): + """Optimize the control input using the MPC controller. + + Args: + x: Initial vehicle state + x_ref: Desired vehicle state + + Returns: + u_next: Control input + """ xr = x_ref[0:12, :] x0 = x[0:12, :] N = self.horizon - if not self.flag: - print("Warm starting") - self.warm_start(x0) + nx = self.acados_ocp.model.x.shape[0] + nu = self.acados_ocp.model.u.shape[0] - self.flag = True + # Preparation phase + # self.acados_ocp_solver.options_set('rti_phase', 1) + # u_init = self.acados_ocp_solver.solve_for_x0(x0, fail_on_nonzero_status=False) # initialize solver for stage in range(N + 1): - self.acados_ocp_solver.set(stage, "x", self.previous_states[:, stage]) + self.acados_ocp_solver.set(stage, "x", np.zeros((nx,))) for stage in range(N): - self.acados_ocp_solver.set(stage, "u", self.previous_control[:, stage]) + self.acados_ocp_solver.set(stage, "u", np.zeros((nu,))) # set initial state constraint self.acados_ocp_solver.set(0, "lbx", x0) self.acados_ocp_solver.set(0, "ubx", x0) - for k in range(N): - if k == 0: - u_prev = np.array([self.previous_control[:, k]]).T - else: - u_prev = np.array([self.acados_ocp_solver.get(k-1, "u")]).T + # for k in range(N): + # self.acados_ocp_solver.set(k, "lbu", self.umin) + # self.acados_ocp_solver.set(k, "ubu", self.umax) - self.acados_ocp_solver.set(k, "p", np.vstack((xr, u_prev))) - self.acados_ocp_solver.set(N, "p", np.vstack((xr, u_prev))) + # self.acados_ocp_solver.set(0, "u", u_init) - self.acados_ocp_solver.options_set("qp_warm_start", 1) + for k in range(N): + self.acados_ocp_solver.set(k, "yref", xr) + self.acados_ocp_solver.set(N, "yref", xr) + + # self.acados_ocp_solver.options_set("qp_warm_start", 1) + # self.acados_ocp_solver.options_set("warm_start_first_qp", 1) + # Feedback phase + # self.acados_ocp_solver.options_set('rti_phase', 2) status = self.acados_ocp_solver.solve() if status != 0: print("acados returned status {}".format(status)) - # Store the control inputs and states for the next iteration - self.previous_control = [] - self.previous_states = [] - - for k in range(self.horizon): - wrench = self.acados_ocp_solver.get(k, "u") - self.previous_control.append(wrench) - - for k in range(self.horizon + 1): - state = self.acados_ocp_solver.get(k, "x") - self.previous_states.append(state) - - self.previous_control = np.array(self.previous_control).T - self.previous_states = np.array(self.previous_states).T - - wrench_next = self.acados_ocp_solver.get(0, "u") + wrench = self.acados_ocp_solver.get(0, "u") + # self.previous_control = np.array([wrench]).T - # dist_err = abs(x0[0, :] - xr[0, :]) - # yaw_err = abs((((x0[5, :] - xr[5, :]) + np.pi) % (2 * np.pi)) - np.pi)[0] + dist_err = abs(x0[0, :] - xr[0, :]) + yaw_err = abs((((x0[5, :] - xr[5, :]) + np.pi) % (2 * np.pi)) - np.pi)[0] - # if dist_err < 0.25 and yaw_err > 0.035: - # wrench[0:5] = 0.0 + if dist_err < 0.30 and yaw_err > 0.05: + wrench[0:5] = 0.0 - # if dist_err < 0.15 and yaw_err < 0.035: - # wrench[1:6] = 0.0 + if dist_err < 0.15 and yaw_err < 0.035: + wrench[1:6] = 0.0 - u_next = evalf(mtimes(pinv(self.auv.tcm), wrench_next)).full() + u_next = evalf(mtimes(pinv(self.auv.tcm), wrench)).full() - return u_next, wrench_next + return u_next, wrench diff --git a/docking_control/src/mpc_external_cost.py b/docking_control/src/mpc_external_cost.py index 77bd54f..c8994e9 100644 --- a/docking_control/src/mpc_external_cost.py +++ b/docking_control/src/mpc_external_cost.py @@ -225,7 +225,7 @@ def optimize(self, x, x_ref): if k == 0: u_prev = np.array([self.previous_control[:, k]]).T else: - u_prev = np.array([self.acados_ocp_solver.get(k-1, "u")]).T + u_prev = np.array([self.acados_ocp_solver.get(k - 1, "u")]).T self.acados_ocp_solver.set(k, "p", np.vstack((xr, u_prev))) self.acados_ocp_solver.set(N, "p", np.vstack((xr, u_prev))) @@ -253,14 +253,14 @@ def optimize(self, x, x_ref): wrench_next = self.acados_ocp_solver.get(0, "u") - # dist_err = abs(x0[0, :] - xr[0, :]) + # dist_err = abs(x0[0, :] - xr[0, :])[0] # yaw_err = abs((((x0[5, :] - xr[5, :]) + np.pi) % (2 * np.pi)) - np.pi)[0] # if dist_err < 0.25 and yaw_err > 0.035: - # wrench[0:5] = 0.0 + # wrench_next[0:5] = 0.0 # if dist_err < 0.15 and yaw_err < 0.035: - # wrench[1:6] = 0.0 + # wrench_next[1:6] = 0.0 u_next = evalf(mtimes(pinv(self.auv.tcm), wrench_next)).full()