1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
|
import numpy as np
import matplotlib.pyplot as plt
import math
# fonction pour importer facilement le fichier csv contenant les données
def import_csv(filename):
file = open(filename, 'r')
fichier = file.readlines()
file.close()
head = fichier[0].split(', ')
head[6] = head[6].replace("\n", "")
print(head)
data = [[], [], []]
for i in range(1, len(fichier) - 1):
words = fichier[i].split(',')
data[0].append(float(words[0]))
data[1].append([float(words[1]), float(words[2]), float(words[3])])
data[2].append([float(words[4]), float(words[5]), float(words[6].replace("\n", "0"))])
return head, data
######################### Importation du fichier ################################
titre, donnees = import_csv('test.csv')
time = [0.001 * donnees[0][i] for i in range(len(donnees[0]))]
######################### Changement de base grace aux angles d'Euler ################################
lst_yaw = [math.radians(elem[0]) for elem in donnees[2]]
lst_pitch = [math.radians(elem[1]) for elem in donnees[2]]
lst_roll = [math.radians(elem[2]) for elem in donnees[2]]
R = [] #Liste des matrices de passage d'une base à une autre
for i in range(len(donnees[0])):
yaw = lst_yaw[i]
pitch = lst_pitch[i]
roll = lst_roll[i]
yawMatrix = np.array([[math.cos(yaw), -math.sin(yaw), 0],
[math.sin(yaw), math.cos(yaw), 0],
[0, 0, 1]])
pitchMatrix = np.array([[math.cos(pitch), 0, math.sin(pitch)],
[0, 1, 0],
[-math.sin(pitch), 0, math.cos(pitch)]])
rollMatrix = np.array([[1, 0, 0],
[0, math.cos(roll), -math.sin(roll)],
[0, math.sin(roll), math.cos(roll)]])
R.append(np.dot(yawMatrix, np.dot(pitchMatrix, rollMatrix))) # Produit des 3 matrices de rotations
acc_B = donnees[1] #accélérations dans la base orientée selon le barycentre de la centrale
acc_O = [list(np.dot(R[i], acc_B[i])) for i in range(len(acc_B))] # accélération dans la base lié au barycentre de la centrale à l'instant initiale
x = [0.5 * acc_O[i][0] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)] # Création du tableau de l'axe x
y = [0.5 * acc_O[i][1] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)] # Création du tableau de l'axe y
z = [0.5 * acc_O[i][2] * ((time[i+1] - time[i])**2) for i in range(len(acc_O)-1)] # Création du tableau de l'axe z
# Tracé du résultat en 3D
fig = plt.figure()
ax = fig.gca(projection='3d') # Affichage en 3D
ax.scatter(x, y, z, label='Courbe', marker='o') # Tracé des points 3D
plt.title("Points 3D")
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.tight_layout()
plt.show() |
Partager