def test_clip(self):
"""
Utilities: Vector clipping
"""
for i in range(10):
x = 100. * (np.random.rand(100) - 0.5)
x_limits = 50. * (np.random.rand(2) - 0.5)
x_clipped = utils.clip(x, a_min=min(x_limits), a_max=max(x_limits))
self.assertLessEqual(max(x_clipped), max(x_limits))
self.assertGreaterEqual(min(x_clipped), min(x_limits))
def vertical(s: np.ndarray,
tgt_z: np.ndarray,
):
e_z = np.array([s[2], s[9], 0., 0., 0., 0.]) - np.array([tgt_z, 0., 0., 0., 0., 0.])
u = -K_Z @ e_z
# scale by vertical component of orientation
q_real = s[3]
u = u / utils.clip(q_real, 0.3)
return u
def step(s: np.ndarray, u: np.ndarray, dt: float, r_scale_dist: float = 1.):
v, n_body, g, dn_body, df_motor = calc_derivative(s, u, r_scale_dist)
# euler integration
s[:3] = s[:3] + v * dt + 0.5 * g * dt**2
s[3:7] = quaternion.integrate(s[3:7], n_body, dt)
s[7:10] = s[7:10] + g * dt
s[10:13] = s[10:13] + dn_body * dt
s[13:17] = utils.clip(s[13:17] + df_motor * dt,
a_min=0.,
a_max=f_motor_max)
return s, g, dn_body
def update(s: np.ndarray,
tgt: np.ndarray,
):
u_attitude = attitude(s, tgt[np.array([0, 1])])
u_vertical = vertical(s, tgt[2])
if max(u_attitude) - min(u_attitude) > f_motor_max:
u = 0.5 * f_motor_max * np.ones(4) + u_attitude
else:
u = U_0 + u_vertical + u_attitude
if max(u) > f_motor_max:
u = u - (max(u) - f_motor_max)
u = utils.clip(u, a_min=0., a_max=f_motor_max)
return u
def to_state(
sensor_state: Sensor,
filter_state: Filter,
u: np.ndarray,
dt: float,
):
s_filter = filter_state.s
if ORIENTATION_FILTER == "Kalman":
q, n_body, p = kalman_orientation(sensor_state, filter_state, u, dt)
elif ORIENTATION_FILTER == "Madgwick":
q, n_body = madgwick(sensor_state, filter_state, dt)
p = np.zeros((11, 11))
else:
q, n_body, p = np.zeros(4), np.zeros(3), np.zeros((11, 11))
x, v, g = filter_translational(sensor_state, filter_state, u, dt)
# calculate motor force
df_motor = drone.calc_motor_derivative(filter_state.s, u)
# update state vector
s_filter[:3] = x
s_filter[3:7] = q
s_filter[7:10] = v
s_filter[10:13] = n_body
s_filter[13:17] = utils.clip(s_filter[13:17] + df_motor * dt,
a_min=0.,
a_max=f_motor_max)
# update filter (can't _replace in numba)
filter_state = Filter(
s=s_filter,
p=p,
r_trans_sensor=filter_state.r_trans_sensor,
r_madgwick_gain=filter_state.r_madgwick_gain,
g=g,
)
return filter_state