From 4f6d8d1ac7da4844e61a6b51d39b6cf7914c31eb Mon Sep 17 00:00:00 2001 From: Samuel Ayala Date: Thu, 28 Mar 2024 19:27:13 -0400 Subject: [PATCH] it works needs more testing and then porting to c++ --- data/displacement.py | 57 ++++++++++++++++++++++++++------------------ 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/data/displacement.py b/data/displacement.py index ed5a492..cecb5c4 100644 --- a/data/displacement.py +++ b/data/displacement.py @@ -4,23 +4,18 @@ class Kinematics: joints = [] # list of transformation lambdas - frames = [np.identity(4)] # list of joint frames in global coordinates def add_joint(self, joint, frame): self.joints.append(joint) - self.frames.append(frame) - def displacement(self, t, move_frames=True): + def displacement(self, t): disp = np.identity(4) for i in range(len(self.joints)): - disp = disp @ self.joints[i](t, self.frames[i]) - if move_frames: - self.frames[i+1] = self.joints[i](t, self.frames[i]) @ self.frames[i+1] + disp = disp @ self.joints[i](t) return disp def relative_displacement(self, t0, t1): - base = np.linalg.inv(self.displacement(t0, True)) - return self.displacement(t1, False) @ base + return self.displacement(t1) @ np.linalg.inv(self.displacement(t0)) def skew_matrix(v): return np.array([ @@ -44,33 +39,47 @@ def rotation_matrix(center, axis, theta): mat[:3, 3] = (np.identity(3) - rodrigues) @ center # matvec return mat +def rotation_matrix2(axis, theta): + axis = np.array(axis) + axis = axis / np.sqrt(np.dot(axis, axis)) + a = np.cos(theta / 2.0) + b, c, d = -axis * np.sin(theta / 2.0) + return np.array([[a*a + b*b - c*c - d*d, 2*(b*c - a*d), 2*(b*d + a*c), 0], + [2*(b*c + a*d), a*a + c*c - b*b - d*d, 2*(c*d - a*b), 0], + [2*(b*d - a*c), 2*(c*d + a*b), a*a + d*d - b*b - c*c, 0], + [0, 0, 0, 1]]) + def move_vertices(vertices, displacement): return displacement @ vertices -def displacement_wing(t, frame): return translation_matrix([0, 0, np.sin(0.9 * t)]) -def displacement_freestream(t, frame): return translation_matrix([-1 * t, 0, 0]) -def displacement_rotor(t, frame): return rotation_matrix(frame @ [0, 0, 0, 1], frame @ [0, 0, 1, 0], 1 * t) -#def displacement_rotor(t, frame): return rotation_matrix([0, 0, 0, 1], [0, 0, 1, 0], 1 * t) +def displacement_wing(t): return translation_matrix([0, 0, np.sin(0.9 * t)]) +def displacement_freestream(t): return translation_matrix([-2 * t, 0, 0]) +# def displacement_rotor(t, frame): +# return rotation_matrix(frame @ [0, 0, 0, 1], frame @ [0, 0, 1, 0], 1 * t) +def displacement_rotor(t): + return rotation_matrix([0, 0, 0], [0, 0, 1], 7 * t) +# def displacement_rotor(t, frame): +# return rotation_matrix2([0, 0, 1], 1 * t) kinematics = Kinematics() kinematics.add_joint(displacement_freestream, np.identity(4)) kinematics.add_joint(displacement_wing, np.identity(4)) -# kinematics.add_joint(displacement_rotor, np.identity(4)) +kinematics.add_joint(displacement_rotor, np.identity(4)) -dt = 0.5 -t_final = 15 +dt = 0.01 +t_final = 20 # vertices of a single panel (clockwise) (initial position) (global coordinates) vertices = np.array([ - [0.0, 0.5, 1.0, 1.0], # x + [0.0, 2.0, 4.0, 4.0], # x [0.0, 10.0, 10.0, 0.0], # y [0.0, 0.0, 0.0, 0.0], # z [1.0, 1.0, 1.0, 1.0] # homogeneous coordinates ]) +vertices = np.column_stack((vertices, vertices[:,0])) body_frame = np.identity(4) -vertices = np.column_stack((vertices, vertices[:,0])) wing_frame = np.identity(4) @@ -83,7 +92,7 @@ def displacement_rotor(t, frame): return rotation_matrix(frame @ [0, 0, 0, 1], f ax.set_xlabel('X axis') ax.set_ylabel('Y axis') ax.set_zlabel('Z axis') -ax.set_xlim(-15, 0) +ax.set_xlim(-30, 0) ax.set_ylim(-15, 15) ax.set_zlim(-5, 5) ax.invert_xaxis() # Invert x axis @@ -93,14 +102,16 @@ def displacement_rotor(t, frame): return rotation_matrix(frame @ [0, 0, 0, 1], f line, = ax.plot3D(vertices[0, :], vertices[1, :], vertices[2, :], 'o-') def update(frame): + global vertices, kinematics t = frame * dt - current_displacement = kinematics.relative_displacement(0, t) - moved_vertices = move_vertices(vertices, current_displacement) + print(f"t = {t:.2f}/{t_final}", end='\r') + vertices = move_vertices(vertices, kinematics.relative_displacement(t, t+dt)) # Update the line object for 3D - line.set_data(moved_vertices[0, :], moved_vertices[1, :]) # y and z for 2D part of set_data - line.set_3d_properties(moved_vertices[2, :]) # x for the 3rd dimension + line.set_data(vertices[0, :], vertices[1, :]) # y and z for 2D part of set_data + line.set_3d_properties(vertices[2, :]) # x for the 3rd dimension return line, -ani = animation.FuncAnimation(fig, update, frames=np.arange(0, t_final/dt), blit=False) +ani = animation.FuncAnimation(fig, update, frames=np.arange(0, t_final/dt), blit=False, repeat=False) +# ani.save('animation.mp4', fps=30, extra_args=['-vcodec', 'libx264']) plt.show()