Pythonで人工衛星の姿勢運動を可視化する
人工衛星の運動は、剛体の運動としてオイラーの運動方程式によって記述することができますが、姿勢表現がクオータニオンだったり角速度だったりでいまいち今どういう運動をしているか直感的にわからなかったりします。この問題を解決するために、衛星の動きをグラフィカルに表現できるように、pythonのOpenGL module, pygameを使って表現してみました。
Install
下記のサイトが参考になりそう
https://stackabuse.com/advanced-opengl-in-python-with-pygame-and-pyopengl/
ひとまずOutputだけ
Pythonで下記のようなグラフィックを生成できます。
グラフィックは、姿勢に外乱が加わった際に、三軸姿勢で元に戻す様子です。
コード (一部)
SatelliteやSat_VisualizerなどのClassはどこかで提示しますが、メインのループは下記のようなコードになります。
import numpy as np
from math import *
# from quat import *
import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
from GainSetting import GainSetting
from dynamics import Dynamics
from Satellite import Satellite, Sat_Visualizer
from GyroSetting import GyroSetting
from SttSetting import SttSetting
from Timer import Timer
from PIL import Image
from PIL import ImageOps
axis_verts = (
(-7.5, 0.0, 0.0),
(7.5, 0.0, 0.0),
(0.0, -7.5, 0.0),
(0.0, 7.5, 0.0),
(0.0, 0.0, -7.5),
(0.0, 0.0, 7.5),
)
axes = ((0, 1), (2, 3), (4, 5))
axis_colors = (
(1.0, 0.0, 0.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
) # Red # Green # Blue
"""
5____________6
/ /|
/ / |
1/__________2/ |
| | |
| | |
| | 7
| | /
| | /
0___________3/
"""
cube_verts = (
(-0.15, -0.2, 0.25),
(-0.15, 0.2, 0.25),
(0.15, 0.2, 0.25),
(0.15, -0.2, 0.25),
(-0.15, -0.2, -0.25),
(-0.15, 0.2, -0.25),
(0.15, 0.2, -0.25),
(0.15, -0.2, -0.25),
(-0.15, 0.7, 0.25),
(0.15, 0.7, 0.25),
(-0.15, -0.7, 0.25),
(0.15, -0.7, 0.25),
)
cube_edges = (
(0, 1),
(0, 3),
(0, 4),
(2, 1),
(2, 3),
(2, 6),
(5, 1),
(5, 4),
(5, 6),
(7, 3),
(7, 4),
(7, 6),
(8, 1),
(8, 9),
(9, 2),
(10, 0),
(10, 11),
(11, 3),
)
cube_surfaces = (
(0, 1, 2, 3), # Front
(3, 2, 6, 7), # Right
(7, 6, 5, 4), # Left
(4, 5, 1, 0), # Back
(1, 5, 6, 2), # Top
(4, 0, 3, 7), # Bottom
(1, 8, 9, 2), # Sap1
(0, 10, 11, 3), # Sap2
)
cube_colors = (
(1.0, 1.0, 1.0), # White
(1.0, 1.0, 1.0), # White
(1.0, 1.0, 1.0), # White
(0.769, 0.118, 0.227), # Red
(1.0, 1.0, 1.0), # White
(1.0, 1.0, 1.0), # White
(1.0, 1.0, 1.0), # White
(1.0, 1.0, 1.0), # White
(0.3, 0.835, 0.3), # Yellow
(0.3, 0.835, 0.3), # Yellow
)
def Axis():
glBegin(GL_LINES)
for color, axis in zip(axis_colors, axes):
glColor3fv(color)
for point in axis:
glVertex3fv(axis_verts[point])
glEnd()
def Cube():
glBegin(GL_QUADS)
for color, surface in zip(cube_colors, cube_surfaces):
glColor3fv(color)
for vertex in surface:
glVertex3fv(cube_verts[vertex])
glEnd()
glBegin(GL_LINES)
glColor3fv((0.0, 0.0, 0.0))
for edge in cube_edges:
for vertex in edge:
glVertex3fv(cube_verts[vertex])
glEnd()
def main():
step = -1
intvl = 10
imgs = []
inertia = np.array([0.5, 0.32, 0.62])
init_q = -np.array([1, 0.0, 0, 0])
init_omega = 0.000001 * np.array([1, 1, 1]) * np.pi / 180 # 1 / 3.2])
init_rw_vel = -0 * np.array([0.35, 0.35, 0.35]) / 0.000734
k_omega = 0.8 * np.array([[0.5, 0, 0], [0, 0.32, 0], [0, 0, 0.62]])
kq = 0.006 * np.array([[0.5, 0, 0], [0, 0.32, 0], [0, 0, 0.62]])
kd_omega = 0.000001 * np.array([[0.5, 0, 0], [0, 0.32, 0], [0, 0, 0.62]])
kqi = 0.00002 * np.array([[0.5, 0, 0], [0, 0.32, 0], [0, 0, 0.62]])
gainsetting = GainSetting(kq, kqi, k_omega, kd_omega)
gyrosetting = GyroSetting(
0,
0.3 / 60 / 60 * 3 * np.random.randn(3) * 2 * np.pi / 180,
0.15 / 60 * 2 * np.pi / 180,
0.05,
)
sttsetting = SttSetting(2 / 3600, 10 / 3600, 0.1)
dt = 0.0025
timer = Timer(dt)
sat1 = Satellite(
inertia,
init_q,
init_omega,
init_rw_vel,
timer,
gainsetting,
gyrosetting,
sttsetting,
)
time = 0
calc_timer = 0
dynamics = Dynamics(dt, sat1)
pygame.init()
display = (1000, 818)
pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
# Using depth test to make sure closer colors are shown over further ones
glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LESS)
# Default view
glMatrixMode(GL_PROJECTION)
gluPerspective(40, (display[0] / display[1]), 0.1, 50.0)
glTranslatef(-0, -1, -8)
glRotatef(90.0, 0.0, 1.0, 0.0)
glRotatef(-90.0, 1.0, 0.0, 0.0)
glRotatef(45.0, 0.0, 0.0, 1.0)
time = 0
while True:
dynamics.update_time()
sat1.reserve_current_state()
sat1.update_true_vec()
sat1.update_observe_vec()
sat1.estimate_att_by_triad()
sat1.estimate_att_by_foam()
sat1.observe_omega()
sat1.observe_quat()
sat1.update_rw(dt)
timer.update_time()
sat1.calc_torque()
time = time + dt
calc_timer += dt
step += 1
for event in pygame.event.get():
if event.type == pygame.QUIT:
vis = Sat_Visualizer(dt, sat1)
vis.visualize_sat_motion()
vis.visualize_omega()
vis.visualize_quat()
vis.compare_observation()
return imgs
if event.type == pygame.KEYDOWN:
# Rotating about the x axis
if event.key == pygame.K_DOWN:
sat1.omega += np.array([0, 0.3, 0.3 / 3.2])
x = sat1.q[1] # -time*0.1
y = sat1.q[2] # time*0.1
z = sat1.q[3] # np.cos(time*0.1) #0.72-time*0.01
w = sat1.q[0] # +time*0.01
mat4 = np.array(
[
[
1 - 2 * y * y - 2 * z * z,
2 * x * y - 2 * z * w,
2 * x * z + 2 * y * w,
0,
],
[
2 * x * y + 2 * z * w,
1 - 2 * x * x - 2 * z * z,
2 * y * z - 2 * x * w,
0,
],
[
2 * x * z - 2 * y * w,
2 * y * z + 2 * x * w,
1 - 2 * x * x - 2 * y * y,
0,
],
[0, 0, 0, 1],
],
"f",
)
glMatrixMode(GL_MODELVIEW)
glLoadMatrixf(mat4)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
Cube()
Axis()
pygame.display.flip()
if step % intvl != 0:
continue
else:
pad_step = "{0:04d}".format(step)
savepath = "img/tutorial3_" + pad_step + ".png"
width = 1000 # glutGet(GLUT_WINDOW_WIDTH)
height = 800 # glutGet(GLUT_WINDOW_HEIGHT)
glReadBuffer(GL_FRONT)
glPixelStorei(GL_UNPACK_ALIGNMENT, 1)
data = glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE)
# image = Image.fromstring("RGBA", (width, height), data)
image = Image.frombytes("RGBA", (width, height), data)
image = ImageOps.flip(image)
# image.save( savepath )
imgs.append(image)
# pygame.time.wait(10)
if __name__ == "__main__":
imgs = main()
imgs[0].save('gif/no_momentum.gif'
, save_all=True
, append_images=imgs[1:]
, optimize=False
, duration=100 #40
, loop=0)
pygame.quit()
quit()
こんな感じで運動が見れる
