def matplotlib_3d(self, rocket):
def axis_equal(X, Y, Z):
max_range = numpy.array([
numpy.array(X).max() - numpy.array(X).min(),
numpy.array(Y).max() - numpy.array(Y).min(),
numpy.array(Z).max() - numpy.array(Z).min()
]).max()
print(max_range)
Xb = 0.5 * max_range * numpy.mgrid[
-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (
numpy.array(X).max() + numpy.array(X).min())
Yb = 0.5 * max_range * numpy.mgrid[
-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (
numpy.array(Y).max() + numpy.array(Y).min())
Zb = 0.5 * max_range * numpy.mgrid[
-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (
numpy.array(Z).max() + numpy.array(Z).min())
# Comment or uncomment following both lines to test the fake bounding box:
return [Xb, Yb, Zb]
fig = plt.figure()
ax = fig.gca(projection='3d')
plt.hold(True)
bodyline_vector = numpy.array([[1000.0], [0.0], [0.0]], dtype="float_")
quat_bodyline = Quaternion()
for i_fig in range(0, rocket.i - 1, self.step):
fig_list = [[rocket.log_r[0, i_fig]], [rocket.log_r[1, i_fig]],
[-rocket.log_r[2, i_fig]]]
print(fig_list)
ax.scatter3D(fig_list[0], fig_list[1], fig_list[2])
ax.set_aspect('equal')
quat_bodyline.__init__(rocket.log_quat[i_fig][:])
dcm_bodyline = numpy.array(quat_bodyline.quat2dcm())
bodyline = numpy.dot(dcm_bodyline, bodyline_vector)
print(numpy.sqrt(bodyline[0]**2 + bodyline[1]**2 + bodyline[2]**2))
r_vec = numpy.array([[rocket.log_r[0][i_fig]],
[rocket.log_r[1][i_fig]],
[rocket.log_r[2][i_fig]]])
bodyline_rear = (-bodyline + r_vec)
fig_list_bodyline = numpy.hstack([r_vec, bodyline_rear])
ax.plot3D(fig_list_bodyline[0], fig_list_bodyline[1],
-fig_list_bodyline[2])
XYZb = axis_equal(rocket.log_r[0][:], rocket.log_r[1][:],
rocket.log_r[2][:])
print(XYZb)
for xb, yb, zb in zip(XYZb[0], XYZb[1], XYZb[2]):
ax.plot([xb], [yb], [zb], "w")
plt.show()
def __init__(self):
#時間初期化
self.t = 0
self.dt = 0.002
self.i = 1
#速度系初期化
self.vel_b = [[0.0], [0.0], [0.0]]
self.vel_i = [[0.0], [0.0], [0.0]]
self.vel_v = [[0.0], [0.0], [0.0]]
#慣性座標での位置初期化
self.r_i = [[0.0], [0.0], [0.0]]
#飛行姿勢初期化
self.quaternion_b2i = Quaternion()
self.quaternion_b2i.rot2quat(88 * numpy.pi / 180, 0, 1, 0)
self.quaternion_b2i.normalize()
self.dcm_b2i = self.quaternion_b2i.quat2dcm()
self.quaternion_i2b = Quaternion(self.quaternion_b2i.inverse())
self.dcm_i2b = self.quaternion_i2b.quat2dcm()
#速度座標への変換行列初期化
self.dcm_b2v = numpy.zeros((3, 3), dtype="float_")
self.dcm_v2b = numpy.zeros((3, 3), dtype="float_")
#迎え角
self.alpha = 0.0
self.beta = 0.0
#空力初期化
self.lift_b = [[0.0], [0.0], [0.0]]
self.drag_b = [[0.0], [0.0], [0.0]]
self.moment_b = [[0.0], [0.0], [0.0]]
self.thrust_b = [[0.0], [0.0], [0.0]]
self.lift_i = [[0.0], [0.0], [0.0]]
self.drag_i = [[0.0], [0.0], [0.0]]
self.thrust_i = [[0.0], [0.0], [0.0]]
self.lift_v = [[0.0], [0.0], [0.0]]
self.drag_v = [[0.0], [0.0], [0.0]]
self.moment_v = [[0.0], [0.0], [0.0]]
self.gyro = [[0.0], [0.0], [0.0]]
#風
self.wind = [[0.0], [0.0], [0.0]]
#減衰係数
self.K = [[0.1], [0.1], [0.1]]
self.Ko = 2.5
#ロガー初期化
self.log_r = [[], [], []]
self.log_vel_i = [[], [], []]
self.log_vel_b = [[], [], []]
self.log_vel_v = [[], [], []]
self.log_euler = [[], [], []]
self.log_force_i = [[], [], []]
self.log_lift_i = [[], [], []]
self.log_quat = []
self.log_t = []
self.log_alpha = []
self.log_beta = []
#ランチャー長さ
self.louncher = 3.0
#積分用ログ初期化
self.integ_dgyro_dt = []
self.integ_dq_dt = []
self.integ_forces_i = []
self.integ_vel_i = []
#パラメータ代入
#TODO : Legionの傘下に入るように
self.rocketlength = 0.32
self.sidesurface = 0.32 * 0.015
self.frontsurface = 0.015**2 * numpy.pi
self.Cd = [[0.0], [0.0], [0.0]]
self.CL = [[0.0], [0.0], [0.0]]
self.mass = 0.0712
self.inatia = [[0.0000198], [0.001293], [0.001293]]
#パラシュート
self.parashoot_t = 20
self.parashoot_surface = 0.1**2 * numpy.pi
def simulate_integrate(self, t_start=0):
def quad_quad_kari(self, fourth, integ):
fourth = numpy.array(fourth, dtype="float_")
integ = numpy.array(integ, dtype="float_")
fst = 65 / 4 * (fourth - 3 * integ[-1] + 3 * integ[-2] -
integ[-3]) / 6
snd = 19 / 3 * (-fourth + 4 * integ[-1] - 5 * integ[-2] +
2 * integ[-3]) / 2
trd = 5 / 2 * (2 * fourth - 9 * integ[-1] + 18 * integ[-2] -
11 * integ[-3]) / 6
quad_value = (fst + snd + trd + integ[-3]) * self.dt
return quad_value
#力に重力の影響付加
lift_i = numpy.array(self.lift_i, dtype="float_")
drag_i = numpy.array(self.drag_i, dtype="float_")
thrust_i = numpy.array(self.thrust_i, dtype="float_")
vel_i = numpy.array(self.vel_i, dtype="float_")
vel_b = numpy.array(self.vel_b, dtype="float_")
dcm_b2i = numpy.array(self.dcm_b2i, dtype="float_")
dcm_i2b = numpy.array(self.dcm_i2b, dtype="float_")
r_i = numpy.array(self.r_i, dtype="float_")
forces_i = self.mass * numpy.array(
[[0.0], [0.0], [9.8]], dtype="float_") + thrust_i + lift_i + drag_i
if numpy.sqrt(r_i[0]**2 + r_i[1]**2 + r_i[2]**2) <= self.louncher:
print(self.i)
force_b = numpy.dot(dcm_i2b, forces_i)
force_b[1] = 0.0
force_b[2] = 0.0
forces_i = numpy.dot(dcm_b2i, force_b)
self.force_i = numpy.ndarray.tolist(forces_i)
inatia = numpy.array(self.inatia)
gyro = numpy.array(self.gyro)
moment_b = numpy.array(self.moment_b)
quat = numpy.array(self.quaternion_b2i.quat)
dt = numpy.array(self.dt)
K = numpy.array(self.K)
if self.i == 1:
vel_i += (forces_i) / self.mass * self.dt
r_i += vel_i * self.dt
self.vel_i = numpy.ndarray.tolist(vel_i)
self.r_i = numpy.ndarray.tolist(r_i)
gyro += (moment_b) / inatia * self.dt
dq_dt = self.quaternion_b2i.dq_dt(gyro[0], gyro[1], gyro[2])
quat += numpy.array(dq_dt, dtype="float_") * self.dt
self.gyro = numpy.ndarray.tolist(gyro)
self.quaternion_b2i.quat = numpy.ndarray.tolist(quat)
self.quaternion_b2i.normalize()
self.dcm_b2i = self.quaternion_b2i.quat2dcm()
self.quaternion_i2b = Quaternion(self.quaternion_b2i.inverse())
self.quaternion_i2b.normalize()
self.dcm_i2b = self.quaternion_i2b.quat2dcm()
self.integ_dgyro_dt.append((moment_b - K * gyro) / inatia)
self.integ_dq_dt.append(dq_dt)
self.integ_forces_i.append(
numpy.ndarray.tolist(forces_i / self.mass))
self.integ_vel_i.append(self.vel_i)
elif self.i == 2 or self.i == 3:
vel_i += ((forces_i) / self.mass +
numpy.array(self.integ_forces_i[-1])) / 2 * self.dt
r_i += (vel_i + self.vel_i[-1]) / 2 * self.dt
self.vel_i = numpy.ndarray.tolist(vel_i)
self.r_i = numpy.ndarray.tolist(r_i)
gyro += ((moment_b - K * gyro) / inatia +
self.integ_dgyro_dt[-1]) / 2 * self.dt
dq_dt = self.quaternion_b2i.dq_dt(gyro[0], gyro[1], gyro[2])
quat += (numpy.array(dq_dt, dtype="float") + numpy.array(
self.integ_dq_dt[-1], dtype="float_")) / 2 * self.dt
self.integ_dgyro_dt.append(
(moment_b - K * numpy.array(self.gyro, dtype="float_")) /
inatia)
self.gyro = numpy.ndarray.tolist(gyro)
self.quaternion_b2i.quat = numpy.ndarray.tolist(quat)
self.quaternion_b2i.normalize()
self.dcm_b2i = self.quaternion_b2i.quat2dcm()
self.quaternion_i2b = Quaternion(self.quaternion_b2i.inverse())
self.quaternion_i2b.normalize()
self.dcm_i2b = self.quaternion_i2b.quat2dcm()
self.integ_dq_dt.append(dq_dt)
self.integ_forces_i.append(
numpy.ndarray.tolist(forces_i / self.mass))
self.integ_vel_i.append(self.vel_i)
else:
vel_i += quad_quad_kari(self, (forces_i) / self.mass,
self.integ_forces_i)
r_i += quad_quad_kari(self, vel_i, self.integ_vel_i)
self.vel_i = numpy.ndarray.tolist(vel_i)
self.r_i = numpy.ndarray.tolist(r_i)
gyro += quad_quad_kari(self, (moment_b - K * gyro) / inatia,
self.integ_dgyro_dt)
dq_dt = self.quaternion_b2i.dq_dt(gyro[0], gyro[1], gyro[2])
quat += quad_quad_kari(self, dq_dt, self.integ_dq_dt)
self.integ_dgyro_dt.append(
(moment_b - K * numpy.array(self.gyro, dtype="float_")) /
inatia)
self.gyro = numpy.ndarray.tolist(gyro)
self.quaternion_b2i.quat = numpy.ndarray.tolist(quat)
self.quaternion_b2i.normalize()
self.dcm_b2i = self.quaternion_b2i.quat2dcm()
self.quaternion_i2b = Quaternion(self.quaternion_b2i.inverse())
self.quaternion_i2b.normalize()
self.dcm_i2b = self.quaternion_i2b.quat2dcm()
self.integ_dq_dt.append(dq_dt)
self.integ_forces_i.append(
numpy.ndarray.tolist(forces_i / self.mass))
self.integ_vel_i.append(self.vel_i)
self.integ_dgyro_dt.pop(0)
self.integ_dq_dt.pop(0)
self.integ_forces_i.pop(0)
self.integ_vel_i.pop(0)
self.i += 1
self.t += self.dt
