# 小型四旋翼飞机的仿真以及实物操作 ------- Python matplotlib仿真篇（一）画出飞机

``````import matplotlib.pyplot as plt
``````

``````plt.ion()
fig = plt.figure()
``````

plt.ion():

http://blog.miskcoo.com/2016/12/rotation-in-3d-space

from math import cos, sin

import numpy as np

``````def transformation_matrix(self):
x = self.x
y = self.y
z = self.z
roll = self.roll
pitch = self.pitch
yaw = self.yaw
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
])
``````

1.np.matmul() 两个数组的矩阵乘积。
https://docs.scipy.org/doc/numpy-1.13.0/reference/generated/numpy.matmul.html

2.plt.xlim 获取或设置当前x轴的限制。
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.xlim.html

3.plt.ylim 获取或设置当前y轴的限制。
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.xlim.html

4.plt.pause(0.001) 暂停间隔秒
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.pause.html

6.plt.cla() 清除当前轴
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.cla.html

7.self.ax.plot 绘制y与x作为线和/或标记，也就是绘制图像
https://matplotlib.org/api/_as_gen/matplotlib.axes.Axes.plot.html

8.self.ax.set_zlim(0, 10) 设置z轴视图限制。
https://matplotlib.org/api/_as_gen/matplotlib.axes.Axes.set_xlim.html

``````def plot(self):
T = self.transformation_matrix()

p1_t = np.matmul(T, self.p1)
p2_t = np.matmul(T, self.p2)
p3_t = np.matmul(T, self.p3)
p4_t = np.matmul(T, self.p4)

plt.cla()

self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]],
[p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
[p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')

self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]],
[p1_t[2], p2_t[2]], 'r-')
self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]],
[p3_t[2], p4_t[2]], 'r-')

self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:')

plt.xlim(-5, 5)
plt.ylim(-5, 5)
self.ax.set_zlim(0, 10)

plt.pause(0.001)
``````

``````class Quadrotor():
def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=1, show_animation=True):
self.p1 = np.array([size / 2, 0, 0, 1]).T
self.p2 = np.array([-size / 2, 0, 0, 1]).T
self.p3 = np.array([0, size / 2, 0, 1]).T
self.p4 = np.array([0, -size / 2, 0, 1]).T

self.x_data = []
self.y_data = []
self.z_data = []
self.show_animation = show_animation

if self.show_animation:
plt.ion()
fig = plt.figure()

self.update_pose(x, y, z, roll, pitch, yaw)

def update_pose(self, x, y, z, roll, pitch, yaw):
self.x = x
self.y = y
self.z = z
self.roll = roll
self.pitch = pitch
self.yaw = yaw
self.x_data.append(x)
self.y_data.append(y)
self.z_data.append(z)

if self.show_animation:
self.plot()
``````

``````from math import cos, sin
import numpy as np
import matplotlib.pyplot as plt

#size=0.25
def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=1, show_animation=True):
self.p1 = np.array([size / 2, 0, 0, 1]).T
self.p2 = np.array([-size / 2, 0, 0, 1]).T
self.p3 = np.array([0, size / 2, 0, 1]).T
self.p4 = np.array([0, -size / 2, 0, 1]).T

self.x_data = []
self.y_data = []
self.z_data = []
self.show_animation = show_animation

if self.show_animation:
plt.ion()
fig = plt.figure()

self.update_pose(x, y, z, roll, pitch, yaw)

def update_pose(self, x, y, z, roll, pitch, yaw):
self.x = x
self.y = y
self.z = z
self.roll = roll
self.pitch = pitch
self.yaw = yaw
self.x_data.append(x)
self.y_data.append(y)
self.z_data.append(z)

if self.show_animation:
self.plot()

def transformation_matrix(self):
x = self.x
y = self.y
z = self.z
roll = self.roll
pitch = self.pitch
yaw = self.yaw
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
])

def plot(self):
T = self.transformation_matrix()

p1_t = np.matmul(T, self.p1)
p2_t = np.matmul(T, self.p2)
p3_t = np.matmul(T, self.p3)
p4_t = np.matmul(T, self.p4)

plt.cla()

self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]],
[p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
[p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')

self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]],
[p1_t[2], p2_t[2]], 'r-')
self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]],
[p3_t[2], p4_t[2]], 'r-')

self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:')

plt.xlim(-5, 5)
plt.ylim(-5, 5)
self.ax.set_zlim(0, 10)

plt.pause(0.001)

q = Quadrotor(x=-5, y=5, z=5, roll=0,pitch=0, yaw=0, size=1, show_animation=True)
#q.plot()
``````

本文转自网络文章，转载此文章仅为分享知识，如有侵权，请联系博主进行删除。