From 59eed3310c65f2011965201f7331ee21e2a4da02 Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:33:27 -0700 Subject: [PATCH 1/6] moved to new directory --- uavf_2024/__init__.py | 0 uavf_2024/conversions.py | 65 ------- uavf_2024/sqp_nlmpc.py | 401 --------------------------------------- 3 files changed, 466 deletions(-) delete mode 100644 uavf_2024/__init__.py delete mode 100644 uavf_2024/conversions.py delete mode 100644 uavf_2024/sqp_nlmpc.py diff --git a/uavf_2024/__init__.py b/uavf_2024/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/uavf_2024/conversions.py b/uavf_2024/conversions.py deleted file mode 100644 index 3eda0d1a..00000000 --- a/uavf_2024/conversions.py +++ /dev/null @@ -1,65 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation - - -def convert_quaternion_to_euler_angles(quat:np.ndarray) -> np.ndarray: - ''' Converts an orientation represented as a quaternion into a - 3D vector of Euler angles. - ''' - assert len(quat) == 4 - rot = Rotation.from_quat(quat) - eul_ang = np.float32(rot.as_euler('xyz')) - return eul_ang - - -def convert_NED_ENU_in_inertial(x) -> np.ndarray: - ''' Converts a state between NED or ENU inertial frames. - This operation is commutative. - ''' - assert len(x) == 3 - new_x = np.float32( - [x[1], x[0], -x[2]]) - return new_x - - -def convert_NED_ENU_in_body(x) -> np.ndarray: - ''' Converts a state between NED or ENU body frames. - (More formally known as FRD or RLU body frames) - This operation is commutative. - ''' - assert len(x) == 3 - new_x = np.float32( - [x[0], -x[1], -x[2]]) - return new_x - - -def convert_body_to_inertial_frame(ang_rate_body, ang) -> np.ndarray: - ''' Converts body euler angle rates to their inertial frame counterparts. - ''' - assert len(ang_rate_body) == 3 - assert len(ang) == 3 - - phi, theta, psi = ang - W_inv = np.float32([ - [1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)], - [0, np.cos(phi), -np.sin(phi)], - [0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)], - ]) - ang_rate_inertial = W_inv @ ang_rate_body - return ang_rate_inertial - - -def convert_inertial_to_body_frame(ang_rate_inertial, ang) -> np.ndarray: - ''' Converts inertial euler angle rates to their body frame counterparts. - ''' - assert len(ang_rate_inertial) == 3 - assert len(ang) == 3 - - phi, theta, psi = ang - W = np.float32([ - [1, 0, -np.sin(theta)], - [0, np.cos(phi), np.cos(theta)*np.sin(phi)], - [0, -np.sin(phi), np.cos(theta)*np.cos(phi)] - ]) - ang_rate_body = W @ ang_rate_inertial - return ang_rate_body \ No newline at end of file diff --git a/uavf_2024/sqp_nlmpc.py b/uavf_2024/sqp_nlmpc.py deleted file mode 100644 index 52096d1d..00000000 --- a/uavf_2024/sqp_nlmpc.py +++ /dev/null @@ -1,401 +0,0 @@ -''' Installing acados: - https://docs.acados.org/installation/index.html#windows-10-wsl - Installing python interface: - https://docs.acados.org/python_interface/index.html - May need to install qpOASES version 3.1 as well. -''' - -from acados_template import AcadosOcpSolver, AcadosOcp, AcadosModel -import casadi as cs -import numpy as np -from numpy import pi -import scipy.linalg as la -import matplotlib.pyplot as plt -import time - -import atexit -import shutil -import os - - -class SQP_NLMPC(): - ''' SQP approximation of nonlinear MPC using Acados's OCP solver. - ''' - - def __init__(self, model, Q, R, - time_step=0.1, num_nodes=20, u_max=40000): - ''' Initialize the MPC with dynamics as casadi namespace, - Q & R cost matrices, time-step, - number of shooting nodes (length of prediction horizon), - square of maximum motor frequency. - ''' - self.DT = time_step - self.N = num_nodes - model, self.u_hover = self.get_acados_model(model) - self.solver = self.formulate_ocp(model, Q, R, u_max) - - # deleting acados compiled files when script is terminated. - atexit.register(self.delete_compiled_files) - return - - - def get_acados_model(self, model_cs): - ''' Acados model format: - f_imp_expr/f_expl_expr, x, xdot, u, name - ''' - - u_hover = model_cs.hover * np.ones(model_cs.u.shape[0]) - model_ac = AcadosModel() - model_ac.f_expl_expr = model_cs.f_expl_expr - model_ac.x = model_cs.x - model_ac.xdot = model_cs.xdot - model_ac.u = model_cs.u - model_ac.name = model_cs.name - return model_ac, u_hover - - - def formulate_ocp(self, model, Q, R, u_max): - ''' Guide to acados OCP formulation: - https://github.com/acados/acados/blob/master/docs/problem_formulation/problem_formulation_ocp_mex.pdf - ''' - - nx = model.x.shape[0] - nu = model.u.shape[0] - ny = nx + nu # combine x and u into y - - ocp = AcadosOcp() - ocp.model = model - ocp.dims.N = self.N - ocp.dims.nu = nu - ocp.dims.nx = nx - ocp.dims.ny = ny - ocp.dims.nbx_0 = nx - ocp.dims.nbu = nu - ocp.dims.nbx = 4 # number of states being constrained - - # total horizon in seconds - ocp.solver_options.tf = self.DT*self.N - - # formulate the default least-squares cost as a quadratic cost - ocp.cost.cost_type = 'LINEAR_LS' - ocp.cost.cost_type_e = 'LINEAR_LS' - - # W is a block diag matrix of Q and R costs from standard QP - ocp.cost.W = la.block_diag(Q, R) - - # use V coeffs to map x & u to y - ocp.cost.Vx = np.zeros((ny, nx)) - ocp.cost.Vx[:nx,:nx] = np.eye(nx) - ocp.cost.Vu = np.zeros((ny, nu)) - ocp.cost.Vu[-nu:,-nu:] = np.eye(nu) - - # Initialize reference trajectory (will be overwritten) - ocp.cost.yref = np.zeros(ny) - - # Initial state (will be overwritten) - ocp.constraints.x0 = np.zeros(nx) - - # control input constraints (square of motor freq) - ocp.constraints.lbu = -u_max * np.ones(nu) - ocp.constraints.ubu = u_max * np.ones(nu) - ocp.constraints.idxbu = np.arange(nu) - - # state constraints: z, roll, pitch, yaw - inf = 1000000000 - ocp.constraints.lbx = np.array([ - 0, -pi/2, -pi/2, 0 - ]) - ocp.constraints.ubx = np.array([ - inf, pi/2, pi/2, 2*pi - ]) - ocp.constraints.idxbx = np.array([ - 2, 3, 4, 5 - ]) - - # not sure what this is, but this paper say partial condensing HPIPM - # is fastest: https://cdn.syscop.de/publications/Frison2020a.pdf - ocp.solver_options.hpipm_mode = 'SPEED_ABS' - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' - ocp.solver_options.qp_solver_iter_max = 1 - ocp.solver_options.qp_solver_warm_start = 1 - ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'ERK' - ocp.solver_options.nlp_solver_type = 'SQP' - ocp.solver_options.print_level = 0 - - # compile acados ocp - solver = AcadosOcpSolver(ocp) - return solver - - - def run_optimization(self, x0, x_set, timer) -> np.ndarray: - ''' Set initial state and setpoint, - then solve the optimization once. - ''' - if timer: st = time.time() - - assert len(x0) == 12 - assert len(x_set) == 12 - - # bound x0 to initial state - self.solver.set(0, 'lbx', x0) - self.solver.set(0, 'ubx', x0) - - # the reference input will be the hover input - y_ref = np.concatenate((x_set, self.u_hover)) - for k in range(self.N): - self.solver.set(k, 'yref', y_ref) - - # solve for the next ctrl input - self.solver.solve() - if timer: print(time.time() - st) - return - - - def get_next_control(self, x0, x_set, timer=False): - ''' Get the first control action from the optimization. - ''' - self.run_optimization(x0, x_set, timer) - nxt_ctrl = self.solver.get(0, 'u') - return nxt_ctrl - - - def get_next_state(self, x0, x_set, timer=False, visuals=False): - ''' Get the next state from the optimization. - ''' - self.run_optimization(x0, x_set, timer) - nxt_state = self.solver.get(1, 'x') - - if visuals: - opt_us = np.zeros((self.N, self.u_hover.shape[0])) - opt_xs = np.zeros((self.N, x0.shape[0])) - for k in range(self.N): - opt_us[k] = self.solver.get(k, 'u') - opt_xs[k] = self.solver.get(k, 'x') - self.vis_plots(opt_us, opt_xs) - return nxt_state - - - def vis_plots(self, ctrl_inputs:np.ndarray, trajectory:np.ndarray): - ''' Displaying the series of control inputs - and trajectory over prediction horizon. - ''' - t = self.DT * np.arange(self.N) - - u1 = ctrl_inputs[:,0] - u2 = ctrl_inputs[:,1] - u3 = ctrl_inputs[:,2] - u4 = ctrl_inputs[:,3] - - x = trajectory[:,0] - y = trajectory[:,1] - z = trajectory[:,2] - - phi = trajectory[:,3] - theta = trajectory[:,4] - psi = trajectory[:,5] - - x_dot = trajectory[:,6] - y_dot = trajectory[:,7] - z_dot = trajectory[:,8] - - phi_dot = trajectory[:,9] - theta_dot = trajectory[:,10] - psi_dot = trajectory[:,11] - - fig, axs = plt.subplots(5, figsize=(12, 10)) - - axs[0].set_ylabel('ctrl inputs (1/s^2)') - axs[0].plot(t,u1, label='u1') - axs[0].plot(t,u2, label='u2') - axs[0].plot(t,u3, label='u3') - axs[0].plot(t,u4, label='u4') - axs[0].legend() - - axs[1].set_ylabel('position (m)') - axs[1].plot(t,x, label='x') - axs[1].plot(t,y, label='y') - axs[1].plot(t,z, label='z') - axs[1].legend() - - axs[2].set_ylabel('orientation (rad)') - axs[2].plot(t,phi, label='phi') - axs[2].plot(t,theta, label='theta') - axs[2].plot(t,psi, label='psi') - axs[2].legend() - - axs[3].set_ylabel('velocity (m/s)') - axs[3].plot(t,x_dot,label='x_dot') - axs[3].plot(t,y_dot,label='y_dot') - axs[3].plot(t,z_dot,label='z_dot') - axs[3].legend() - - axs[4].set_ylabel('angular vel (rad/s)') - axs[4].plot(t,phi_dot,label='phi_dot') - axs[4].plot(t,theta_dot,label='theta_dot') - axs[4].plot(t,psi_dot,label='psi_dot') - axs[4].legend() - - for ax in axs.flat: - ax.set(xlabel='time (s)') - ax.label_outer() - - plt.show() - return - - - def delete_compiled_files(self): - ''' Clean up the acados generated files. - ''' - try: shutil.rmtree('c_generated_code') - except: print('failed to delete c_generated_code') - - try: os.remove('acados_ocp_nlp.json') - except: print('failed to delete acados_ocp_nlp.json') - - -def derive_quad_dynamics(mass, arm_len, Ix, Iy, Iz, thrust_coeff, torque_coeff): - ''' Returns casadi struct containing explicit dynamics, - state, state_dot, control input, and name. - Nonlinear continuous-time quadcopter dynamics. - The cartesian states are in ENU. - ''' - # State Variables: position, rotation, and their time-derivatives - x = cs.SX.sym('x') - y = cs.SX.sym('y') - z = cs.SX.sym('z') - phi = cs.SX.sym('phi') # roll - theta = cs.SX.sym('theta') # pitch - psi = cs.SX.sym('psi') # yaw - x_d = cs.SX.sym('x_d') # time-derivatives - y_d = cs.SX.sym('y_d') - z_d = cs.SX.sym('z_d') - phi_d = cs.SX.sym('phi_d') - theta_d = cs.SX.sym('theta_d') - psi_d = cs.SX.sym('psi_d') - # state - X = cs.vertcat(x, y, z, phi, theta, psi,\ - x_d, y_d, z_d, phi_d, theta_d, psi_d) - - - # Inertial parameters - m = mass #1.287 # kg #1282 iris3d g #0.027 crazyfly kg - l = arm_len #0.040 # m - Ixx = Ix #2.3951 * 10**(-5) - Iyy = Iy #2.3951 * 10**(-5) - Izz = Iz #3.2347 * 10**(-5) - - - # Aerodynamic Parameters - kf = thrust_coeff #0.005022 - km = torque_coeff #1.858 * 10**(-5) - Ax = 0 - Ay = 0 - Az = 0 - - - # rotation matrix from body frame to inertial frame - Rx = cs.SX(np.array([ - [1, 0, 0], - [0, cs.cos(phi), -cs.sin(phi)], - [0, cs.sin(phi), cs.cos(phi)] - ])) - Ry = cs.SX(np.array([ - [cs.cos(theta), 0, cs.sin(theta)], - [0, 1, 0], - [-cs.sin(theta), 0, cs.cos(theta)] - ])) - Rz = cs.SX(np.array([ - [cs.cos(psi), -cs.sin(psi), 0], - [cs.sin(psi), cs.cos(psi), 0], - [0, 0, 1] - ])) - R = Rz @ Ry @ Rx - - - # calculation of jacobian matrix that converts body frame vels to inertial frame - W = cs.SX(np.array([ - [1, 0, -cs.sin(theta)], - [0, cs.cos(phi), cs.cos(theta)*cs.sin(phi)], - [0, -cs.sin(phi), cs.cos(theta)*cs.cos(phi)] - ])) - I = np.diag([Ixx,Iyy,Izz]) - J = W.T @ I @ W - - - # Coriolis matrix for defining angular equations of motion - C11 = 0 - - C12 = (Iyy-Izz)*(theta_d*cs.cos(phi)*cs.sin(phi) + psi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ - (Izz-Iyy)*psi_d*(cs.cos(phi)**2)*cs.cos(theta) -\ - Ixx*psi_d*cs.cos(theta) - - C13 = (Izz-Iyy)*psi_d*cs.cos(phi)*cs.sin(phi)*(cs.cos(theta)**2) - - C21 = (Izz-Iyy)*(theta_d*cs.cos(phi)*cs.sin(phi) + psi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ - (Iyy-Izz)*psi_d*(cs.cos(phi)**2)*cs.cos(theta) +\ - Ixx*psi_d*cs.cos(theta) - - C22 = (Izz-Iyy)*phi_d*cs.cos(phi)*cs.sin(phi) - - C23 = -Ixx*psi_d*cs.sin(theta)*cs.cos(theta) +\ - Iyy*psi_d*(cs.sin(phi)**2)*cs.sin(theta)*cs.cos(theta) +\ - Izz*psi_d*(cs.cos(phi)**2)*cs.sin(theta)*cs.cos(theta) - - C31 = (Iyy-Izz)*psi_d*(cs.cos(theta)**2)*cs.sin(phi)*cs.cos(phi) -\ - Ixx*theta_d*cs.cos(theta) - - C32 = (Izz-Iyy)*(theta_d*cs.cos(phi)*cs.sin(phi)*cs.sin(theta) + phi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ - (Iyy-Izz)*phi_d*(cs.cos(phi)**2)*cs.cos(theta) +\ - Ixx*psi_d*cs.sin(theta)*cs.cos(theta) -\ - Iyy*psi_d*(cs.sin(phi)**2)*cs.sin(theta)*cs.cos(theta) -\ - Izz*psi_d*(cs.cos(phi)**2)*cs.sin(theta)*cs.cos(theta) - - C33 = (Iyy-Izz)*phi_d*cs.cos(phi)*cs.sin(phi)*(cs.cos(theta)**2) -\ - Iyy*theta_d*(cs.sin(phi)**2)*cs.cos(theta)*cs.sin(theta) -\ - Izz*theta_d*(cs.cos(phi)**2)*cs.cos(theta)*cs.sin(theta) +\ - Ixx*theta_d*cs.cos(theta)*cs.sin(theta) - - C = cs.SX(np.array([ - [C11, C12, C13], - [C21, C22, C23], - [C31, C32, C33] - ])) - - - # Control Input is square of rotor frequency - u1 = cs.SX.sym('u1') - u2 = cs.SX.sym('u2') - u3 = cs.SX.sym('u3') - u4 = cs.SX.sym('u4') - U = cs.vertcat(u1, u2, u3, u4) - - - # actuation dynamics - tau_beta = cs.SX(np.array([ - [l*kf*(-u2 + u4)], - [l*kf*(-u1 + u3)], - [km*(-u1 + u2 - u3 + u4)] - ])) - thrust = kf*(u1 + u2 + u3 + u4) - g = 9.81 # acceleration due to gravity - - - # continuous-time dynamics - Xdot = cs.vertcat( - x_d, y_d, z_d, phi_d, theta_d, psi_d, - cs.vertcat(0,0,-g) + R @ cs.vertcat(0,0,thrust) / m, - cs.inv(J) @ (tau_beta - C @ cs.vertcat(phi_d, theta_d, psi_d)) - ) - - - # store variables in casadi struct - model_cs = cs.types.SimpleNamespace() - model_cs.f_expl_expr = Xdot - model_cs.x = X - model_cs.xdot = Xdot - model_cs.u = U - model_cs.hover = (m*g) / (kf*U.shape[0]) # min. control for hover - model_cs.name = 'nonlin_quadcopter' - return model_cs - From 3b86721ffdbe615099ff7463de313363576c002b Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:34:01 -0700 Subject: [PATCH 2/6] moved to gnc subdirectory --- uavf_2024/gnc/__init__.py | 0 uavf_2024/gnc/conversions.py | 65 ++++++ uavf_2024/gnc/sqp_nlmpc.py | 401 +++++++++++++++++++++++++++++++++++ 3 files changed, 466 insertions(+) create mode 100644 uavf_2024/gnc/__init__.py create mode 100644 uavf_2024/gnc/conversions.py create mode 100644 uavf_2024/gnc/sqp_nlmpc.py diff --git a/uavf_2024/gnc/__init__.py b/uavf_2024/gnc/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/gnc/conversions.py b/uavf_2024/gnc/conversions.py new file mode 100644 index 00000000..3eda0d1a --- /dev/null +++ b/uavf_2024/gnc/conversions.py @@ -0,0 +1,65 @@ +import numpy as np +from scipy.spatial.transform import Rotation + + +def convert_quaternion_to_euler_angles(quat:np.ndarray) -> np.ndarray: + ''' Converts an orientation represented as a quaternion into a + 3D vector of Euler angles. + ''' + assert len(quat) == 4 + rot = Rotation.from_quat(quat) + eul_ang = np.float32(rot.as_euler('xyz')) + return eul_ang + + +def convert_NED_ENU_in_inertial(x) -> np.ndarray: + ''' Converts a state between NED or ENU inertial frames. + This operation is commutative. + ''' + assert len(x) == 3 + new_x = np.float32( + [x[1], x[0], -x[2]]) + return new_x + + +def convert_NED_ENU_in_body(x) -> np.ndarray: + ''' Converts a state between NED or ENU body frames. + (More formally known as FRD or RLU body frames) + This operation is commutative. + ''' + assert len(x) == 3 + new_x = np.float32( + [x[0], -x[1], -x[2]]) + return new_x + + +def convert_body_to_inertial_frame(ang_rate_body, ang) -> np.ndarray: + ''' Converts body euler angle rates to their inertial frame counterparts. + ''' + assert len(ang_rate_body) == 3 + assert len(ang) == 3 + + phi, theta, psi = ang + W_inv = np.float32([ + [1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)], + [0, np.cos(phi), -np.sin(phi)], + [0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)], + ]) + ang_rate_inertial = W_inv @ ang_rate_body + return ang_rate_inertial + + +def convert_inertial_to_body_frame(ang_rate_inertial, ang) -> np.ndarray: + ''' Converts inertial euler angle rates to their body frame counterparts. + ''' + assert len(ang_rate_inertial) == 3 + assert len(ang) == 3 + + phi, theta, psi = ang + W = np.float32([ + [1, 0, -np.sin(theta)], + [0, np.cos(phi), np.cos(theta)*np.sin(phi)], + [0, -np.sin(phi), np.cos(theta)*np.cos(phi)] + ]) + ang_rate_body = W @ ang_rate_inertial + return ang_rate_body \ No newline at end of file diff --git a/uavf_2024/gnc/sqp_nlmpc.py b/uavf_2024/gnc/sqp_nlmpc.py new file mode 100644 index 00000000..52096d1d --- /dev/null +++ b/uavf_2024/gnc/sqp_nlmpc.py @@ -0,0 +1,401 @@ +''' Installing acados: + https://docs.acados.org/installation/index.html#windows-10-wsl + Installing python interface: + https://docs.acados.org/python_interface/index.html + May need to install qpOASES version 3.1 as well. +''' + +from acados_template import AcadosOcpSolver, AcadosOcp, AcadosModel +import casadi as cs +import numpy as np +from numpy import pi +import scipy.linalg as la +import matplotlib.pyplot as plt +import time + +import atexit +import shutil +import os + + +class SQP_NLMPC(): + ''' SQP approximation of nonlinear MPC using Acados's OCP solver. + ''' + + def __init__(self, model, Q, R, + time_step=0.1, num_nodes=20, u_max=40000): + ''' Initialize the MPC with dynamics as casadi namespace, + Q & R cost matrices, time-step, + number of shooting nodes (length of prediction horizon), + square of maximum motor frequency. + ''' + self.DT = time_step + self.N = num_nodes + model, self.u_hover = self.get_acados_model(model) + self.solver = self.formulate_ocp(model, Q, R, u_max) + + # deleting acados compiled files when script is terminated. + atexit.register(self.delete_compiled_files) + return + + + def get_acados_model(self, model_cs): + ''' Acados model format: + f_imp_expr/f_expl_expr, x, xdot, u, name + ''' + + u_hover = model_cs.hover * np.ones(model_cs.u.shape[0]) + model_ac = AcadosModel() + model_ac.f_expl_expr = model_cs.f_expl_expr + model_ac.x = model_cs.x + model_ac.xdot = model_cs.xdot + model_ac.u = model_cs.u + model_ac.name = model_cs.name + return model_ac, u_hover + + + def formulate_ocp(self, model, Q, R, u_max): + ''' Guide to acados OCP formulation: + https://github.com/acados/acados/blob/master/docs/problem_formulation/problem_formulation_ocp_mex.pdf + ''' + + nx = model.x.shape[0] + nu = model.u.shape[0] + ny = nx + nu # combine x and u into y + + ocp = AcadosOcp() + ocp.model = model + ocp.dims.N = self.N + ocp.dims.nu = nu + ocp.dims.nx = nx + ocp.dims.ny = ny + ocp.dims.nbx_0 = nx + ocp.dims.nbu = nu + ocp.dims.nbx = 4 # number of states being constrained + + # total horizon in seconds + ocp.solver_options.tf = self.DT*self.N + + # formulate the default least-squares cost as a quadratic cost + ocp.cost.cost_type = 'LINEAR_LS' + ocp.cost.cost_type_e = 'LINEAR_LS' + + # W is a block diag matrix of Q and R costs from standard QP + ocp.cost.W = la.block_diag(Q, R) + + # use V coeffs to map x & u to y + ocp.cost.Vx = np.zeros((ny, nx)) + ocp.cost.Vx[:nx,:nx] = np.eye(nx) + ocp.cost.Vu = np.zeros((ny, nu)) + ocp.cost.Vu[-nu:,-nu:] = np.eye(nu) + + # Initialize reference trajectory (will be overwritten) + ocp.cost.yref = np.zeros(ny) + + # Initial state (will be overwritten) + ocp.constraints.x0 = np.zeros(nx) + + # control input constraints (square of motor freq) + ocp.constraints.lbu = -u_max * np.ones(nu) + ocp.constraints.ubu = u_max * np.ones(nu) + ocp.constraints.idxbu = np.arange(nu) + + # state constraints: z, roll, pitch, yaw + inf = 1000000000 + ocp.constraints.lbx = np.array([ + 0, -pi/2, -pi/2, 0 + ]) + ocp.constraints.ubx = np.array([ + inf, pi/2, pi/2, 2*pi + ]) + ocp.constraints.idxbx = np.array([ + 2, 3, 4, 5 + ]) + + # not sure what this is, but this paper say partial condensing HPIPM + # is fastest: https://cdn.syscop.de/publications/Frison2020a.pdf + ocp.solver_options.hpipm_mode = 'SPEED_ABS' + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + ocp.solver_options.qp_solver_iter_max = 1 + ocp.solver_options.qp_solver_warm_start = 1 + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' + ocp.solver_options.print_level = 0 + + # compile acados ocp + solver = AcadosOcpSolver(ocp) + return solver + + + def run_optimization(self, x0, x_set, timer) -> np.ndarray: + ''' Set initial state and setpoint, + then solve the optimization once. + ''' + if timer: st = time.time() + + assert len(x0) == 12 + assert len(x_set) == 12 + + # bound x0 to initial state + self.solver.set(0, 'lbx', x0) + self.solver.set(0, 'ubx', x0) + + # the reference input will be the hover input + y_ref = np.concatenate((x_set, self.u_hover)) + for k in range(self.N): + self.solver.set(k, 'yref', y_ref) + + # solve for the next ctrl input + self.solver.solve() + if timer: print(time.time() - st) + return + + + def get_next_control(self, x0, x_set, timer=False): + ''' Get the first control action from the optimization. + ''' + self.run_optimization(x0, x_set, timer) + nxt_ctrl = self.solver.get(0, 'u') + return nxt_ctrl + + + def get_next_state(self, x0, x_set, timer=False, visuals=False): + ''' Get the next state from the optimization. + ''' + self.run_optimization(x0, x_set, timer) + nxt_state = self.solver.get(1, 'x') + + if visuals: + opt_us = np.zeros((self.N, self.u_hover.shape[0])) + opt_xs = np.zeros((self.N, x0.shape[0])) + for k in range(self.N): + opt_us[k] = self.solver.get(k, 'u') + opt_xs[k] = self.solver.get(k, 'x') + self.vis_plots(opt_us, opt_xs) + return nxt_state + + + def vis_plots(self, ctrl_inputs:np.ndarray, trajectory:np.ndarray): + ''' Displaying the series of control inputs + and trajectory over prediction horizon. + ''' + t = self.DT * np.arange(self.N) + + u1 = ctrl_inputs[:,0] + u2 = ctrl_inputs[:,1] + u3 = ctrl_inputs[:,2] + u4 = ctrl_inputs[:,3] + + x = trajectory[:,0] + y = trajectory[:,1] + z = trajectory[:,2] + + phi = trajectory[:,3] + theta = trajectory[:,4] + psi = trajectory[:,5] + + x_dot = trajectory[:,6] + y_dot = trajectory[:,7] + z_dot = trajectory[:,8] + + phi_dot = trajectory[:,9] + theta_dot = trajectory[:,10] + psi_dot = trajectory[:,11] + + fig, axs = plt.subplots(5, figsize=(12, 10)) + + axs[0].set_ylabel('ctrl inputs (1/s^2)') + axs[0].plot(t,u1, label='u1') + axs[0].plot(t,u2, label='u2') + axs[0].plot(t,u3, label='u3') + axs[0].plot(t,u4, label='u4') + axs[0].legend() + + axs[1].set_ylabel('position (m)') + axs[1].plot(t,x, label='x') + axs[1].plot(t,y, label='y') + axs[1].plot(t,z, label='z') + axs[1].legend() + + axs[2].set_ylabel('orientation (rad)') + axs[2].plot(t,phi, label='phi') + axs[2].plot(t,theta, label='theta') + axs[2].plot(t,psi, label='psi') + axs[2].legend() + + axs[3].set_ylabel('velocity (m/s)') + axs[3].plot(t,x_dot,label='x_dot') + axs[3].plot(t,y_dot,label='y_dot') + axs[3].plot(t,z_dot,label='z_dot') + axs[3].legend() + + axs[4].set_ylabel('angular vel (rad/s)') + axs[4].plot(t,phi_dot,label='phi_dot') + axs[4].plot(t,theta_dot,label='theta_dot') + axs[4].plot(t,psi_dot,label='psi_dot') + axs[4].legend() + + for ax in axs.flat: + ax.set(xlabel='time (s)') + ax.label_outer() + + plt.show() + return + + + def delete_compiled_files(self): + ''' Clean up the acados generated files. + ''' + try: shutil.rmtree('c_generated_code') + except: print('failed to delete c_generated_code') + + try: os.remove('acados_ocp_nlp.json') + except: print('failed to delete acados_ocp_nlp.json') + + +def derive_quad_dynamics(mass, arm_len, Ix, Iy, Iz, thrust_coeff, torque_coeff): + ''' Returns casadi struct containing explicit dynamics, + state, state_dot, control input, and name. + Nonlinear continuous-time quadcopter dynamics. + The cartesian states are in ENU. + ''' + # State Variables: position, rotation, and their time-derivatives + x = cs.SX.sym('x') + y = cs.SX.sym('y') + z = cs.SX.sym('z') + phi = cs.SX.sym('phi') # roll + theta = cs.SX.sym('theta') # pitch + psi = cs.SX.sym('psi') # yaw + x_d = cs.SX.sym('x_d') # time-derivatives + y_d = cs.SX.sym('y_d') + z_d = cs.SX.sym('z_d') + phi_d = cs.SX.sym('phi_d') + theta_d = cs.SX.sym('theta_d') + psi_d = cs.SX.sym('psi_d') + # state + X = cs.vertcat(x, y, z, phi, theta, psi,\ + x_d, y_d, z_d, phi_d, theta_d, psi_d) + + + # Inertial parameters + m = mass #1.287 # kg #1282 iris3d g #0.027 crazyfly kg + l = arm_len #0.040 # m + Ixx = Ix #2.3951 * 10**(-5) + Iyy = Iy #2.3951 * 10**(-5) + Izz = Iz #3.2347 * 10**(-5) + + + # Aerodynamic Parameters + kf = thrust_coeff #0.005022 + km = torque_coeff #1.858 * 10**(-5) + Ax = 0 + Ay = 0 + Az = 0 + + + # rotation matrix from body frame to inertial frame + Rx = cs.SX(np.array([ + [1, 0, 0], + [0, cs.cos(phi), -cs.sin(phi)], + [0, cs.sin(phi), cs.cos(phi)] + ])) + Ry = cs.SX(np.array([ + [cs.cos(theta), 0, cs.sin(theta)], + [0, 1, 0], + [-cs.sin(theta), 0, cs.cos(theta)] + ])) + Rz = cs.SX(np.array([ + [cs.cos(psi), -cs.sin(psi), 0], + [cs.sin(psi), cs.cos(psi), 0], + [0, 0, 1] + ])) + R = Rz @ Ry @ Rx + + + # calculation of jacobian matrix that converts body frame vels to inertial frame + W = cs.SX(np.array([ + [1, 0, -cs.sin(theta)], + [0, cs.cos(phi), cs.cos(theta)*cs.sin(phi)], + [0, -cs.sin(phi), cs.cos(theta)*cs.cos(phi)] + ])) + I = np.diag([Ixx,Iyy,Izz]) + J = W.T @ I @ W + + + # Coriolis matrix for defining angular equations of motion + C11 = 0 + + C12 = (Iyy-Izz)*(theta_d*cs.cos(phi)*cs.sin(phi) + psi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ + (Izz-Iyy)*psi_d*(cs.cos(phi)**2)*cs.cos(theta) -\ + Ixx*psi_d*cs.cos(theta) + + C13 = (Izz-Iyy)*psi_d*cs.cos(phi)*cs.sin(phi)*(cs.cos(theta)**2) + + C21 = (Izz-Iyy)*(theta_d*cs.cos(phi)*cs.sin(phi) + psi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ + (Iyy-Izz)*psi_d*(cs.cos(phi)**2)*cs.cos(theta) +\ + Ixx*psi_d*cs.cos(theta) + + C22 = (Izz-Iyy)*phi_d*cs.cos(phi)*cs.sin(phi) + + C23 = -Ixx*psi_d*cs.sin(theta)*cs.cos(theta) +\ + Iyy*psi_d*(cs.sin(phi)**2)*cs.sin(theta)*cs.cos(theta) +\ + Izz*psi_d*(cs.cos(phi)**2)*cs.sin(theta)*cs.cos(theta) + + C31 = (Iyy-Izz)*psi_d*(cs.cos(theta)**2)*cs.sin(phi)*cs.cos(phi) -\ + Ixx*theta_d*cs.cos(theta) + + C32 = (Izz-Iyy)*(theta_d*cs.cos(phi)*cs.sin(phi)*cs.sin(theta) + phi_d*(cs.sin(phi)**2)*cs.cos(theta)) +\ + (Iyy-Izz)*phi_d*(cs.cos(phi)**2)*cs.cos(theta) +\ + Ixx*psi_d*cs.sin(theta)*cs.cos(theta) -\ + Iyy*psi_d*(cs.sin(phi)**2)*cs.sin(theta)*cs.cos(theta) -\ + Izz*psi_d*(cs.cos(phi)**2)*cs.sin(theta)*cs.cos(theta) + + C33 = (Iyy-Izz)*phi_d*cs.cos(phi)*cs.sin(phi)*(cs.cos(theta)**2) -\ + Iyy*theta_d*(cs.sin(phi)**2)*cs.cos(theta)*cs.sin(theta) -\ + Izz*theta_d*(cs.cos(phi)**2)*cs.cos(theta)*cs.sin(theta) +\ + Ixx*theta_d*cs.cos(theta)*cs.sin(theta) + + C = cs.SX(np.array([ + [C11, C12, C13], + [C21, C22, C23], + [C31, C32, C33] + ])) + + + # Control Input is square of rotor frequency + u1 = cs.SX.sym('u1') + u2 = cs.SX.sym('u2') + u3 = cs.SX.sym('u3') + u4 = cs.SX.sym('u4') + U = cs.vertcat(u1, u2, u3, u4) + + + # actuation dynamics + tau_beta = cs.SX(np.array([ + [l*kf*(-u2 + u4)], + [l*kf*(-u1 + u3)], + [km*(-u1 + u2 - u3 + u4)] + ])) + thrust = kf*(u1 + u2 + u3 + u4) + g = 9.81 # acceleration due to gravity + + + # continuous-time dynamics + Xdot = cs.vertcat( + x_d, y_d, z_d, phi_d, theta_d, psi_d, + cs.vertcat(0,0,-g) + R @ cs.vertcat(0,0,thrust) / m, + cs.inv(J) @ (tau_beta - C @ cs.vertcat(phi_d, theta_d, psi_d)) + ) + + + # store variables in casadi struct + model_cs = cs.types.SimpleNamespace() + model_cs.f_expl_expr = Xdot + model_cs.x = X + model_cs.xdot = Xdot + model_cs.u = U + model_cs.hover = (m*g) / (kf*U.shape[0]) # min. control for hover + model_cs.name = 'nonlin_quadcopter' + return model_cs + From 97045616407fc211b4b978a5c04646f15500f376 Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:34:40 -0700 Subject: [PATCH 3/6] new files --- include/uavf_2024/gnc/README.md | 0 include/uavf_2024/imaging/README.md | 0 requirements.txt | 0 uavf_2024/gnc/README.md | 0 uavf_2024/imaging/README.md | 0 uavf_2024/imaging/__init__.py | 0 6 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 include/uavf_2024/gnc/README.md create mode 100644 include/uavf_2024/imaging/README.md create mode 100644 requirements.txt create mode 100644 uavf_2024/gnc/README.md create mode 100644 uavf_2024/imaging/README.md create mode 100644 uavf_2024/imaging/__init__.py diff --git a/include/uavf_2024/gnc/README.md b/include/uavf_2024/gnc/README.md new file mode 100644 index 00000000..e69de29b diff --git a/include/uavf_2024/imaging/README.md b/include/uavf_2024/imaging/README.md new file mode 100644 index 00000000..e69de29b diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/gnc/README.md b/uavf_2024/gnc/README.md new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/imaging/README.md b/uavf_2024/imaging/README.md new file mode 100644 index 00000000..e69de29b diff --git a/uavf_2024/imaging/__init__.py b/uavf_2024/imaging/__init__.py new file mode 100644 index 00000000..e69de29b From 099326a78db4393c63cc024f530cff335810aa04 Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:35:25 -0700 Subject: [PATCH 4/6] moved "pip install -e ." to the front --- README.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index b972df97..834a3f54 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,16 @@ ## Install instructions +### Install required and local Python libraries + +1. cd into this repo's root directory. + +2. Run: + ``` + pip install -e . + ``` + + ### [ROS 2 Foxy for Ubuntu 20.04 Focal](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) 1. Uninstall ROS Noetic. @@ -159,13 +169,3 @@ 1. [Follow these instructions to install Acados.](https://docs.acados.org/installation/) 2. [Install the Python interface afterwards.](https://docs.acados.org/python_interface/index.html) - - -### Install required and local Python libraries (again, for MPC usage) - -1. cd into this repo's root directory. - -2. Run: - ``` - pip install -e . - ``` From e9403e7373d8954c304e6f16006201e40be524dc Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:36:07 -0700 Subject: [PATCH 5/6] updated imported module name --- scripts/commander_node.py | 37 +++++++++++++++++++----------- scripts/trajectory_planner_node.py | 2 +- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/scripts/commander_node.py b/scripts/commander_node.py index 4d4e3b7c..b727bdd1 100644 --- a/scripts/commander_node.py +++ b/scripts/commander_node.py @@ -7,7 +7,7 @@ OffboardControlMode, VehicleCommand, VehicleStatus, VehicleOdometry, VehicleGlobalPosition from uavf_ros2_msgs.msg import GpsAltitudePosition, NedEnuOdometry, NedEnuSetpoint, NedEnuWaypoint -from uavf_2024.conversions import convert_quaternion_to_euler_angles, convert_NED_ENU_in_inertial,\ +from uavf_2024.gnc.conversions import convert_quaternion_to_euler_angles, convert_NED_ENU_in_inertial,\ convert_NED_ENU_in_body, convert_body_to_inertial_frame, convert_inertial_to_body_frame import numpy as np from scipy.spatial.transform import Rotation @@ -74,7 +74,7 @@ def __init__(self) -> None: VehicleOdometry, '/fmu/out/vehicle_odometry', self.odom_cb, qos_profile) - self.traj_planner_is_ENU = True + #self.traj_planner_is_ENU = True self.all_wp_reached = False self.waypoint_is_ENU = False self.waypoint = np.zeros(3, dtype=np.float32) @@ -88,7 +88,9 @@ def make_decision(self) -> None: ''' self.publish_heartbeat() if not self.all_wp_reached: - self.publish_trajectory_planner_command(self, self.waypoint, self.waypoint_is_ENU) + self.publish_trajectory_planner_command( + pos_decision=self.waypoint, + pos_is_ENU=self.waypoint_is_ENU) else: self.land() self.disarm() @@ -97,11 +99,20 @@ def make_decision(self) -> None: def traj_plan_cb(self, ned_enu_setpt:NedEnuSetpoint) -> None: ''' Reads the setpoint calculated by the trajectory planner and publishes the corresponding setpoints to PX4. - Incomplete. ''' - # assert something - self.traj_planner_is_ENU = ned_enu_setpt.is_enu - pass + #self.traj_planner_is_ENU = ned_enu_setpt.is_enu + self.publish_trajectory_setpoint( + pos=ned_enu_setpt.position_setpoint, + vel=ned_enu_setpt.velocity_setpoint, + is_ENU=ned_enu_setpt.is_enu) + self.publish_euler_angle_setpoint( + ang=ned_enu_setpt.euler_angle_setpoint, + is_ENU=ned_enu_setpt.is_enu) + self.publish_euler_angle_rate_setpoint( + ang_rate=ned_enu_setpt.euler_angle_rate_setpoint, + is_ENU=ned_enu_setpt.is_enu, + is_inertial=ned_enu_setpt.is_inertial + ) def wp_tracker_cb(self, ned_enu_wp:NedEnuWaypoint) -> None: @@ -110,7 +121,7 @@ def wp_tracker_cb(self, ned_enu_wp:NedEnuWaypoint) -> None: ''' if not ned_enu_wp.all_waypoints_reached: self.waypoint_is_ENU = ned_enu_wp.is_enu - self.waypoint = ned_enu_wp.position_waypoint + self.waypoint = np.float32(ned_enu_wp.position_waypoint) else: self.destroy_subscription(self.wp_tracker_sub) self.all_wp_reached = ned_enu_wp.all_waypoints_reached @@ -265,8 +276,8 @@ def publish_trajectory_setpoint(self, pos:np.ndarray, vel:np.ndarray, is_ENU:boo vel_f32 = np.float32(vel) if is_ENU: - pos_f32 = self.convert_NED_ENU_in_inertial(pos_f32) - vel_f32 = self.convert_NED_ENU_in_inertial(vel_f32) + pos_f32 = convert_NED_ENU_in_inertial(pos_f32) + vel_f32 = convert_NED_ENU_in_inertial(vel_f32) msg = TrajectorySetpoint() msg.position = pos_f32 @@ -286,7 +297,7 @@ def publish_euler_angle_setpoint(self, ang:np.ndarray, is_ENU:bool) -> None: assert type(is_ENU) == bool ang_f32 = np.float32(ang) if is_ENU: - ang_f32 = self.convert_NED_ENU_in_inertial(ang_f32) + ang_f32 = convert_NED_ENU_in_inertial(ang_f32) msg = VehicleAttitudeSetpoint() msg.roll_body = ang_f32[0] @@ -309,9 +320,9 @@ def publish_euler_angle_rate_setpoint(self, ang_rate:float, is_ENU:bool, is_iner assert type(is_inertial) == bool ang_rate_f32 = np.float32(ang_rate) if is_ENU: - ang_rate_f32 = self.convert_NED_ENU_in_body(ang_rate_f32) + ang_rate_f32 = convert_NED_ENU_in_body(ang_rate_f32) if is_inertial: - ang_rate_f32 = self.convert_inertial_to_body_frame(ang_rate_f32, is_ENU=is_ENU) + ang_rate_f32 = convert_inertial_to_body_frame(ang_rate_f32, is_ENU=is_ENU) msg = VehicleRatesSetpoint() msg.roll = ang_rate_f32[0] diff --git a/scripts/trajectory_planner_node.py b/scripts/trajectory_planner_node.py index 21223dde..0a0b0b4c 100644 --- a/scripts/trajectory_planner_node.py +++ b/scripts/trajectory_planner_node.py @@ -5,7 +5,7 @@ from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy from uavf_ros2_msgs.msg import NedEnuOdometry, NedEnuSetpoint -from uavf_2024.sqp_nlmpc import derive_quad_dynamics, SQP_NLMPC +from uavf_2024.gnc.sqp_nlmpc import derive_quad_dynamics, SQP_NLMPC import numpy as np import atexit From 75c606067afd76fe326900fdc75e9ecce3e3db5e Mon Sep 17 00:00:00 2001 From: Derek Fan Date: Wed, 30 Aug 2023 21:44:40 -0700 Subject: [PATCH 6/6] edited readme title --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 834a3f54..3fd3f903 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Sample repository showcasing model predictive control with PX4's offboard mode and ROS 2. +# UAV Forge's ROS2 package for GN&C and Aerial Imagery Object Detection. ## Usage