Erstmal möchte ich kurz mein Problem schildern:
Ich habe drei Arrays, mit Verläufen translatorischer Bewegungen (surge, sway und heave) und drei Arrays, die Winkelverläufe (roll, pitch und yaw) enthalten.
Die translatorischen Bewegungen eines bestimmten Punktes x, die aus den zeitlichen Winkeländerungen resultieren sollen auf die drei anderen Arrays addiert werden.
Die Drehmatrix berechne ich momentan für jeden Zeitpunkt neu (siehe Codeschnipsel unten)
Code: Alles auswählen
#!/usr/bin/python
# -*- coding: utf-8 -*-
import numpy as np
import random
t = np.linspace(0,10,100)
tank_pos = np.array([25, 0, -5])
surge = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
sway = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
heave = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
roll = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
pitch = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
yaw = np.cos(0.5 * t + random.uniform(0, 2*np.pi))
for n in range(0, len(surge)):
R = np.array(
[
[
np.cos(np.radians(pitch[n])) * np.cos(np.radians(yaw[n])),
-np.cos(np.radians(roll[n])) * np.sin(np.radians(yaw[n])) + np.sin(np.radians(roll[n])) * np.sin(np.radians(pitch[n])) * np.cos(np.radians(yaw[n])),
np.sin(np.radians(roll[n])) * np.sin(np.radians(yaw[n])) + np.cos(np.radians(roll[n])) * np.sin(np.radians(pitch[n])) * np.cos(np.radians(yaw[n]))
],
[
np.cos(np.radians(pitch[n])) * np.sin(np.radians(yaw[n])),
np.cos(np.radians(roll[n])) * np.cos(np.radians(yaw[n])) + np.sin(np.radians(roll[n])) * np.sin(np.radians(pitch[n])) * np.sin(np.radians(yaw[n])),
-np.sin(np.radians(roll[n])) * np.cos(np.radians(yaw[n])) + np.cos(np.radians(roll[n])) * np.sin(np.radians(pitch[n])) * np.sin(np.radians(yaw[n]))
],
[
-np.sin(np.radians(pitch[n])),
np.sin(np.radians(roll[n])) * np.cos(np.radians(pitch[n])),
np.cos(np.radians(roll[n])) * np.cos(np.radians(pitch[n]))
]
]
)
x = np.dot(R, tank_pos)
surge[n] += x[0]
sway[n] += x[1]
heave[n] += x[2]
Kann ich irgendwie die das Produkt aus der Drehmatrix für die gesamte Zeitreihe und dem Ortsvektor in einem Schritt berechnen?
Besten Dank und viele Grüße
Simon