def attitude_control(self, quad, Ts):
# Current thrust orientation e_z and desired thrust orientation e_z_d
e_z = quad.dcm[:, 2]
e_z_d = -utils.vectNormalize(self.thrust_sp)
if (config.orient == "ENU"):
e_z_d = -e_z_d
# Quaternion error between the 2 vectors
qe_red = np.zeros(4)
qe_red[0] = np.dot(e_z, e_z_d) + sqrt(norm(e_z)**2 * norm(e_z_d)**2)
qe_red[1:4] = np.cross(e_z, e_z_d)
qe_red = utils.vectNormalize(qe_red)
# Reduced desired quaternion (reduced because it doesn't consider the desired Yaw angle)
self.qd_red = utils.quatMultiply(qe_red, quad.quat)
# Mixed desired quaternion (between reduced and full) and resulting desired quaternion qd
q_mix = utils.quatMultiply(utils.inverse(self.qd_red), self.qd_full)
q_mix = q_mix * np.sign(q_mix[0])
q_mix[0] = np.clip(q_mix[0], -1.0, 1.0)
q_mix[3] = np.clip(q_mix[3], -1.0, 1.0)
self.qd = utils.quatMultiply(
self.qd_red,
np.array([
cos(self.yaw_w * np.arccos(q_mix[0])), 0, 0,
sin(self.yaw_w * np.arcsin(q_mix[3]))
]))
# Resulting error quaternion
self.qe = utils.quatMultiply(utils.inverse(quad.quat), self.qd)
# Create rate setpoint from quaternion error
self.rate_sp = (2.0 * np.sign(self.qe[0]) * self.qe[1:4]) * att_P_gain
# Limit yawFF
self.yawFF = np.clip(self.yawFF, -rateMax[2], rateMax[2])
# Add Yaw rate feed-forward
self.rate_sp += utils.quat2Dcm(utils.inverse(
quad.quat))[:, 2] * self.yawFF
# Limit rate setpoint
self.rate_sp = np.clip(self.rate_sp, -rateMax, rateMax)
def updateLines(i):
time = t_all[i * numFrames]
pos = pos_all[i * numFrames]
x = pos[0]
y = pos[1]
z = pos[2]
x_from0 = pos_all[0:i * numFrames, 0]
y_from0 = pos_all[0:i * numFrames, 1]
z_from0 = pos_all[0:i * numFrames, 2]
dxm = params["dxm"]
dym = params["dym"]
dzm = params["dzm"]
quat = quat_all[i * numFrames]
if config.orient == "NED":
z = -z
z_from0 = -z_from0
quat = np.array([quat[0], -quat[1], -quat[2], quat[3]])
R = utils.quat2Dcm(quat)
motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm],
[-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym,
dzm]])
motorPoints = np.dot(R, np.transpose(motorPoints))
motorPoints[0, :] += x
motorPoints[1, :] += y
motorPoints[2, :] += z
line1.set_data(motorPoints[0, 0:3], motorPoints[1, 0:3])
line1.set_3d_properties(motorPoints[2, 0:3])
line2.set_data(motorPoints[0, 3:6], motorPoints[1, 3:6])
line2.set_3d_properties(motorPoints[2, 3:6])
line3.set_data(x_from0, y_from0)
line3.set_3d_properties(z_from0)
titleTime.set_text(u"Time = {:.2f} s".format(time))
return line1, line2
def updateLines(i):
# first vehicle
# -------------
time = t_all[i * numFrames]
pos = pos_all[i * numFrames]
x = pos[0]
y = pos[1]
z = pos[2]
x_from0 = pos_all[0:i * numFrames, 0]
y_from0 = pos_all[0:i * numFrames, 1]
z_from0 = pos_all[0:i * numFrames, 2]
dxm = params["dxm"]
dym = params["dym"]
dzm = params["dzm"]
quat = quat_all[i * numFrames]
if (config.orient == "NED"):
z = -z
z_from0 = -z_from0
quat = np.array([quat[0], -quat[1], -quat[2], quat[3]])
R = utils.quat2Dcm(quat)
motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm],
[-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym,
dzm]])
motorPoints = np.dot(R, np.transpose(motorPoints))
motorPoints[0, :] += x
motorPoints[1, :] += y
motorPoints[2, :] += z
line1.set_data(motorPoints[0, 0:3], motorPoints[1, 0:3])
line1.set_3d_properties(motorPoints[2, 0:3])
line2.set_data(motorPoints[0, 3:6], motorPoints[1, 3:6])
line2.set_3d_properties(motorPoints[2, 3:6])
line3.set_data(x_from0, y_from0)
line3.set_3d_properties(z_from0)
titleTime.set_text(u"Time = {:.2f} s".format(time))
# second vehicle
# -------------
time2 = t_all2[i * numFrames]
pos2 = pos_all2[i * numFrames]
x2 = pos2[0]
y2 = pos2[1]
z2 = pos2[2]
x_from02 = pos_all2[0:i * numFrames, 0]
y_from02 = pos_all2[0:i * numFrames, 1]
z_from02 = pos_all2[0:i * numFrames, 2]
dxm2 = params2["dxm"]
dym2 = params2["dym"]
dzm2 = params2["dzm"]
quat2 = quat_all2[i * numFrames]
if (config.orient == "NED"):
z2 = -z2
z_from02 = -z_from02
quat2 = np.array([quat2[0], -quat2[1], -quat2[2], quat2[3]])
R2 = utils.quat2Dcm(quat2)
motorPoints2 = np.array([[dxm2, -dym2, dzm2], [0, 0, 0],
[dxm2, dym2, dzm2], [-dxm2, dym2, dzm2],
[0, 0, 0], [-dxm2, -dym2, dzm2]])
motorPoints2 = np.dot(R2, np.transpose(motorPoints2))
motorPoints2[0, :] += x2
motorPoints2[1, :] += y2
motorPoints2[2, :] += z2
line1b.set_data(motorPoints2[0, 0:3], motorPoints2[1, 0:3])
line1b.set_3d_properties(motorPoints2[2, 0:3])
line2b.set_data(motorPoints2[0, 3:6], motorPoints2[1, 3:6])
line2b.set_3d_properties(motorPoints2[2, 3:6])
line3b.set_data(x_from02, y_from02)
line3b.set_3d_properties(z_from02)
#titleTime.set_text(u"Time = {:.2f} s".format(time))
return line1, line2, line1b, line2b
def updateLines(i):
# # first vehicle
# # -------------
# time = t_all[i*numFrames]
# pos = pos_all[i*numFrames]
# x = pos[0]
# y = pos[1]
# z = pos[2]
# x_from0 = pos_all[0:i*numFrames,0]
# y_from0 = pos_all[0:i*numFrames,1]
# z_from0 = pos_all[0:i*numFrames,2]
# dxm = params["dxm"]
# dym = params["dym"]
# dzm = params["dzm"]
# quat = quat_all[i*numFrames]
# if (config.orient == "NED"):
# z = -z
# z_from0 = -z_from0
# quat = np.array([quat[0], -quat[1], -quat[2], quat[3]])
# R = utils.quat2Dcm(quat)
# motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm], [-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym, dzm]])
# motorPoints = np.dot(R, np.transpose(motorPoints))
# motorPoints[0,:] += x
# motorPoints[1,:] += y
# motorPoints[2,:] += z
# line1.set_data(motorPoints[0,0:3], motorPoints[1,0:3])
# line1.set_3d_properties(motorPoints[2,0:3])
# line2.set_data(motorPoints[0,3:6], motorPoints[1,3:6])
# line2.set_3d_properties(motorPoints[2,3:6])
# line3.set_data(x_from0, y_from0)
# line3.set_3d_properties(z_from0)
#titleTime.set_text(u"Time = {:.2f} s".format(time))
# # second vehicle
# # -------------
# time2 = t_all2[i*numFrames]
# pos2 = pos_all2[i*numFrames]
# x2 = pos2[0]
# y2 = pos2[1]
# z2 = pos2[2]
# x_from02 = pos_all2[0:i*numFrames,0]
# y_from02 = pos_all2[0:i*numFrames,1]
# z_from02 = pos_all2[0:i*numFrames,2]
# dxm2 = params2["dxm"]
# dym2 = params2["dym"]
# dzm2 = params2["dzm"]
# quat2 = quat_all2[i*numFrames]
# if (config.orient == "NED"):
# z2 = -z2
# z_from02 = -z_from02
# quat2 = np.array([quat2[0], -quat2[1], -quat2[2], quat2[3]])
# R2 = utils.quat2Dcm(quat2)
# motorPoints2 = np.array([[dxm2, -dym2, dzm2], [0, 0, 0], [dxm2, dym2, dzm2], [-dxm2, dym2, dzm2], [0, 0, 0], [-dxm2, -dym2, dzm2]])
# motorPoints2 = np.dot(R2, np.transpose(motorPoints2))
# motorPoints2[0,:] += x2
# motorPoints2[1,:] += y2
# motorPoints2[2,:] += z2
# line1b.set_data(motorPoints2[0,0:3], motorPoints2[1,0:3])
# line1b.set_3d_properties(motorPoints2[2,0:3])
# line2b.set_data(motorPoints2[0,3:6], motorPoints2[1,3:6])
# line2b.set_3d_properties(motorPoints2[2,3:6])
# line3b.set_data(x_from02, y_from02)
# line3b.set_3d_properties(z_from02)
# #titleTime.set_text(u"Time = {:.2f} s".format(time))
# n'th vehicle
# -------------
for iVeh in range(0, config.nVeh):
time2 = t_all[iVeh][i * numFrames]
pos2 = pos_all[iVeh][i * numFrames]
x2 = pos2[0]
y2 = pos2[1]
z2 = pos2[2]
x_from02 = pos_all[iVeh][0:i * numFrames, 0]
y_from02 = pos_all[iVeh][0:i * numFrames, 1]
z_from02 = pos_all[iVeh][0:i * numFrames, 2]
dxm2 = params[iVeh]["dxm"]
dym2 = params[iVeh]["dym"]
dzm2 = params[iVeh]["dzm"]
quat2 = quat_all[iVeh][i * numFrames]
if (config.orient == "NED"):
z2 = -z2
z_from02 = -z_from02
quat2 = np.array([quat2[0], -quat2[1], -quat2[2], quat2[3]])
R2 = utils.quat2Dcm(quat2)
motorPoints2 = np.array([[dxm2, -dym2, dzm2], [0, 0, 0],
[dxm2, dym2, dzm2], [-dxm2, dym2, dzm2],
[0, 0, 0], [-dxm2, -dym2, dzm2]])
motorPoints2 = np.dot(R2, np.transpose(motorPoints2))
motorPoints2[0, :] += x2
motorPoints2[1, :] += y2
motorPoints2[2, :] += z2
line1[iVeh].set_data(motorPoints2[0, 0:3], motorPoints2[1, 0:3])
line1[iVeh].set_3d_properties(motorPoints2[2, 0:3])
line2[iVeh].set_data(motorPoints2[0, 3:6], motorPoints2[1, 3:6])
line2[iVeh].set_3d_properties(motorPoints2[2, 3:6])
line3[iVeh].set_data(x_from02, y_from02)
line3[iVeh].set_3d_properties(z_from02)
titleTime.set_text(u"Time = {:.2f} s".format(time2))
#return line1, line2, line1b, line2b
return line1, line2
def update(ev):
if canvas.idx_prev == 0:
# Start time
canvas.startTime = time.perf_counter()
# Get time and find the index of the simulation
currentTime = time.perf_counter()-canvas.startTime
idx_now = np.argmax(t_all > currentTime)
if (idx_now < canvas.idx_prev):
# Stop animation
canvas.timer.stop()
canvas.figs_displayed = True
figures()
else:
# Get drone state for current time
Simtime = t_all[idx_now]
pos = pos_all[idx_now]
x = pos[0]
y = pos[1]
z = pos[2]
x_from0 = pos_all[0:idx_now+1, 0]
y_from0 = pos_all[0:idx_now+1, 1]
z_from0 = pos_all[0:idx_now+1, 2]
quat = quat_all[idx_now]
# Determine by how much yaw (psi) has changed since last frame
if (idx_now==0):
psi_diff = 0
else:
psi_diff = (euler_all[idx_now,2] - euler_all[canvas.idx_prev,2])*rad2deg
# Normal NED frame changes
if (config.orient == "NED"):
z = -z
z_from0 = -z_from0
quat = np.array([quat[0], -quat[1], -quat[2], quat[3]])
psi_diff = -psi_diff
# Find motor positions
R = utils.quat2Dcm(quat)
motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm], [-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym, dzm]])
motorPoints = np.dot(R, np.transpose(motorPoints))
motorPoints[0,:] = motorPoints[0,:] + x
motorPoints[1,:] = motorPoints[1,:] + y
motorPoints[2,:] = motorPoints[2,:] + z
# Draw quadrotor and past trajectory
line1.set_data(np.array([motorPoints[0, 0:3], motorPoints[1, 0:3], motorPoints[2, 0:3]]).T, marker_size=0,)
line2.set_data(np.array([motorPoints[0, 3:6], motorPoints[1, 3:6], motorPoints[2, 3:6]]).T, marker_size=0,)
line3.set_data(np.array([x_from0, y_from0, z_from0]).T, marker_size=0)
# Change pointcloud colors
yellowPoints = np.where(np.logical_or(notInRange_all[idx_now,:], inRange_all[idx_now,:]))[0]
redPoints = np.where(inField_all[idx_now,:])[0]
if np.setdiff1d(redPoints, canvas.redPoints).size != 0:
canvas.yellowPoints = yellowPoints
canvas.redPoints = redPoints
colors[canvas.yellowPoints] = color_points
colors[canvas.redPoints] = color_field
scatter.set_color(face_color=colors)
# Change camera angle and position
view.camera.azimuth = view.camera.azimuth + psi_diff
view.camera.center = [x, y, z]
# Update view
view.camera.view_changed()
# Set previous index
canvas.idx_prev = idx_now