def __init__(self, config):
Ti = config.Ti
# Quad Params
# ---------------------------
self.params = sys_params(config)
# Command for initial stable hover
# ---------------------------
ini_hover = init_cmd(self.params)
self.params["FF"] = ini_hover[0] # Feed-Forward Command for Hover
self.params["w_hover"] = ini_hover[1] # Motor Speed for Hover
self.params["thr_hover"] = ini_hover[2] # Motor Thrust for Hover
self.thr = np.ones(4) * ini_hover[2]
self.tor = np.ones(4) * ini_hover[3]
# Initial State
# ---------------------------
self.state = init_state(self.params, config)
self.pos = self.state[0:3]
self.quat = self.state[3:7]
self.vel = self.state[7:10]
self.omega = self.state[10:13]
self.wMotor = np.array(
[self.state[13], self.state[15], self.state[17], self.state[19]])
self.vel_dot = np.zeros(3)
self.omega_dot = np.zeros(3)
self.acc = np.zeros(3)
self.extended_state()
self.forces()
# Set Integrator
# ---------------------------
self.integrator = ode(self.state_dot).set_integrator(
'dopri5', first_step='0.00005', atol='10e-6', rtol='10e-6')
self.integrator.set_initial_value(self.state, Ti)
def __init__(self, states0, Ti=0):
# Quad Params
# ---------------------------
self.params = sys_params()
# Initial State
# ---------------------------
self.state = init_state(states0)
self.pos = self.state[0:3]
self.angle = self.state[3:6]
self.vel = self.state[6:9]
self.angular = self.state[9:12]
self.backstepping = Control(quad, traj.yawType)
self.wind_direct = 0.
# Set Integrator
# ---------------------------
self.integrator = ode(self.state_dot).set_integrator(
'dopri5', first_step='0.00005', atol='10e-6', rtol='10e-6')
self.integrator.set_initial_value(self.state, Ti)
def backstepping(self, quad):
self.params = sys_params()
m = self.params["m"]
g = self.params["g"]
Ixx = self.params["Ixx"]
Iyy = self.params["Iyy"]
Izz = self.params["Izz"]
I1 = self.params["I1"]
I2 = self.params["I2"]
I3 = self.params["I3"]
Jr = self.params["Jr"]
l = self.params["l"]
b = self.params["b"]
d = self.params["d"]
U1, U2, U3, U4 = self.U_list
#states: [x,y,z,phi,theta,psi,x_dot,y_dot,z_dot,phi_dot,theta_dot,psi_dot]
x, y, z = quad.state[0], quad.state[1], quad.state[2]
phi, theta, psi = quad.state[3], quad.state[4], quad.state[5]
x_dot, y_dot, z_dot = quad.state[6], quad.state[7], quad.state[8]
phi_dot, theta_dot, psi_dot = quad.state[9], quad.state[
10], quad.state[11]
# ref_traj = [xd[i], yd[i], zd[i], xd_dot[i], yd_dot[i], zd_dot[i],
# xd_ddot[i], yd_ddot[i], zd_ddot[i], xd_dddot[i], yd_dddot[i],
# xd_ddddot[i], yd_ddddot[i], psid[i], psid_dot[i], psid_ddot[i]]
xd, yd, zd = self.pos_sp[0], self.pos_sp[1], self.pos_sp[2],
xd_dot, yd_dot, zd_dot = self.vel_sp[0], self.vel_sp[1], self.vel_sp[2]
xd_ddot, yd_ddot, zd_ddot = self.acc_sp[0], self.acc_sp[
1], self.acc_sp[2]
xd_dddot, yd_dddot = 0, 0
xd_ddddot, yd_ddddot = 0, 0
psid, psid_dot, psid_ddot = self.eul_sp[2], self.pqr_sp[2], 0
x1, x2, x3 = array([[x],
[y]]), array([[x_dot], [y_dot]]), array([[phi],
[theta]])
x4, x5, x6 = array([[phi_dot],
[theta_dot]]), array([[psi],
[z]]), array([[psi_dot],
[z_dot]])
g0 = array([[np.cos(psi), np.sin(psi)], [np.sin(psi), -np.cos(psi)]])
g0_inv = array([[np.cos(psi), np.sin(psi)],
[np.sin(psi), -np.cos(psi)]])
g1 = array([[theta_dot * psi_dot * I1], [phi_dot * psi_dot * I2]])
g2 = array([[phi_dot * theta_dot * I3], [-g]])
l0 = array([[np.cos(phi) * np.sin(theta)], [np.sin(phi)]]) * U1 / m
dl0_dx3 = array(
[[-np.sin(phi) * np.sin(theta),
np.cos(phi) * np.cos(theta)], [np.cos(phi), 0]]) * U1 / m
dl0_dx3_inv = array([[0, 1 / np.cos(phi)],
[
1 / np.cos(theta) * 1 / np.cos(phi),
1 / np.cos(phi) * np.tan(theta) * np.tan(phi)
]]) * m / U1
dl0_dx3_inv_dot = array(
[[0, 1 / np.cos(phi) * np.tan(phi) * phi_dot],
[
1 / np.cos(theta) * 1 / np.cos(phi) *
(np.tan(theta) * theta_dot + np.tan(phi) * phi_dot),
1 / np.cos(phi) *
((1 / np.cos(theta))**2 * np.tan(phi) * theta_dot +
(-1 + 2 * (1 / np.cos(phi))**2) * np.tan(theta) * phi_dot)
]]) * m / U1
# Omega_square = Omega_coef_inv * abs([U1/b U2/b U3/b U4/d])
# Omega_param = sqrt(Omega_square)
# omega = Omega_param(2) + Omega_param[3] - Omega_param(1) - Omega_param(3)
# h1 = [-Jr/Ixx*theta_dot*omega Jr/Iyy*phi_dot*omega]
h1 = 0
k1 = diag([l / Ixx, l / Iyy])
k1_inv = diag([Ixx / l, Iyy / l])
k2 = diag([1 / Izz, np.cos(phi) * np.cos(theta) / m])
k2_inv = diag([Izz, m / (np.cos(phi) * np.cos(theta))])
x1d = array([[xd], [yd]])
x1d_dot = array([[xd_dot], [yd_dot]])
x1d_ddot = array([[xd_ddot], [yd_ddot]])
x1d_dddot = array([[xd_dddot], [yd_dddot]])
x1d_ddddot = array([[xd_ddddot], [yd_ddddot]])
x5d = array([[psid], [zd]])
x5d_dot = array([[psid_dot], [zd_dot]])
x5d_ddot = array([[psid_ddot], [zd_ddot]])
z1 = x1d - x1
v1 = x1d_dot + dot(self.A1, z1)
z2 = v1 - x2
z1_dot = -dot(self.A1, z1) + z2
v1_dot = x1d_ddot + dot(self.A1, z1_dot)
v2 = dot(g0_inv, z1 + v1_dot + dot(self.A2, z2))
z3 = v2 - l0
z2_dot = -z1 - dot(self.A2, z2) + dot(g0, z3)
z1_ddot = -dot(self.A1, z1_dot) + z2_dot
v1_ddot = x1d_dddot + dot(self.A1, z1_ddot)
v2_dot = dot(g0_inv, z1_dot + v1_ddot + dot(self.A2, z2_dot))
v3 = dot(dl0_dx3_inv, dot(g0.T, z2) + v2_dot + dot(self.A3, z3))
z4 = v3 - x4
z3_dot = -dot(g0.T, z2) - dot(self.A3, z3) + dot(dl0_dx3, z4)
z2_ddot = -z1_dot - dot(self.A2, z2_dot) + dot(g0, z3_dot)
z1_dddot = -dot(self.A1, z1_ddot) + z2_ddot
v1_dddot = x1d_ddddot + dot(self.A1, z1_dddot)
v2_ddot = dot(g0_inv, z1_ddot + v1_dddot + dot(self.A2, z2_ddot))
v3_dot = dot(dl0_dx3_inv,
dot(g0.T, z2_dot) + v2_ddot + dot(self.A3, z3_dot)) + dot(
dl0_dx3_inv_dot,
dot(g0.T, z2) + v2_dot + dot(self.A3, z3))
l1 = dot(k1_inv,
dot(dl0_dx3.T, z3) + v3_dot - g1 - h1 +
dot(self.A4, z4)).ravel()
z5 = x5d - x5
v5 = x5d_dot + dot(self.A5, z5)
z6 = v5 - x6
z5_dot = -dot(self.A5, z5) + z6
v5_dot = x5d_ddot + dot(self.A5, z5_dot)
l2 = dot(k2_inv, z5 + v5_dot - g2 + dot(self.A6, z6)).ravel()
U1, U2, U3, U4 = l2[1], l1[0], l1[1], l2[0]
U1 = np.clip(U1, 1.0, 1e2)
U2 = np.clip(U2, -1e3, 1e3)
U3 = np.clip(U3, -1e3, 1e3)
U4 = np.clip(U4, -1e2, 1e2)
U = np.array([U1, U2, U3, U4])
self.U_list = U
return U
def __init__(self,
Ti,
Tf,
ctrlType,
trajSelect,
numTimeStep,
Ts,
quad_id=None,
mode='ennemy',
id_targ=-1,
color='blue',
pos_ini=[0, 0, 0],
pos_goal=[1, 1, -1],
pos_obs=None):
'''
It is used to store in the basic attributes the inputs already mentioned in the previous
sections.
In the case of the one employed by Python simulation environment, the command for an initial
stable hover and the initial state of the drone are initialized. For that, the dynamics,
kinetics, kinematics and control are also loaded. Finally, the storage of drone data is
pre-allocated.
'''
# Quad Params
# ---------------------------
self.quad_id = quad_id
self.pos_ini = pos_ini
self.pos_goal = pos_goal
self.pos_goal_ini = pos_goal
self.pos_obs = pos_obs
self.id_targ = id_targ
self.pos_quads = None
self.mode = mode
self.mode_ini = mode
self.neutralize = False
self.ctrlType = ctrlType
self.trajSelect = trajSelect
self.params = sys_params()
self.t_update = Ti
self.t_track = 0
self.Ti = Ti
self.Ts = Ts
self.Tf = Tf
# Command for initial stable hover
# ---------------------------
ini_hover = init_cmd(self.params)
self.params["FF"] = ini_hover[0] # Feed-Forward Command for Hover
self.params["w_hover"] = ini_hover[1] # Motor Speed for Hover
self.params["thr_hover"] = ini_hover[2] # Motor Thrust for Hover
self.thr = np.ones(4) * ini_hover[2]
self.tor = np.ones(4) * ini_hover[3]
# Initial State
# ---------------------------
self.state = init_state(self.params, pos_ini)
self.pos = self.state[0:3]
self.quat = self.state[3:7]
self.vel = self.state[7:10]
self.omega = self.state[10:13]
self.wMotor = np.array(
[self.state[13], self.state[15], self.state[17], self.state[19]])
self.vel_dot = np.zeros(3)
self.omega_dot = np.zeros(3)
self.acc = np.zeros(3)
self.previous_pos_targ = self.pos
self.previous_pos_us = self.pos
self.extended_state()
self.forces()
# Set Integrator
# ---------------------------
self.integrator = ode(self.state_dot).set_integrator(
'dopri5', first_step='0.00005', atol='10e-6', rtol='10e-6')
self.integrator.set_initial_value(self.state, Ti)
self.traj = Trajectory(self.psi,
ctrlType,
trajSelect,
pos_ini=self.pos,
pos_goal=pos_goal,
pos_obs=pos_obs,
Tf=self.Tf,
t_ini=0)
self.ctrl = Control(self.params["w_hover"], self.traj.yawType)
self.t_all = np.zeros(numTimeStep)
self.s_all = np.zeros([numTimeStep, len(self.state)])
self.pos_all = np.zeros([numTimeStep, len(self.pos)])
self.vel_all = np.zeros([numTimeStep, len(self.vel)])
self.quat_all = np.zeros([numTimeStep, len(self.quat)])
self.omega_all = np.zeros([numTimeStep, len(self.omega)])
self.euler_all = np.zeros([numTimeStep, len(self.euler)])
self.sDes_traj_all = np.zeros([numTimeStep, len(self.traj.sDes)])
self.sDes_calc_all = np.zeros([numTimeStep, len(self.ctrl.sDesCalc)])
self.w_cmd_all = np.zeros([numTimeStep, len(self.ctrl.w_cmd)])
self.wMotor_all = np.zeros([numTimeStep, len(self.wMotor)])
self.thr_all = np.zeros([numTimeStep, len(self.thr)])
self.tor_all = np.zeros([numTimeStep, len(self.tor)])
self.color = color
self.pf_map_all = np.empty(numTimeStep, dtype=list)