""" TODO: run multiple RK updates per frame? to decouple timestep from framerate """ import taichi as ti import numpy as np ti.init(arch=ti.cuda, default_fp=ti.f32) """ Coordinate system: theta = 0: pendulum is vertical, hanging at equilibrium below platform theta = PI/2: pendulum is at 3pm theta' > 0: clockwise motion Vertical force * sin(theta) gives the angular component Vertical force * cos(theta) gives the radial component Pendulum_x = l*sin(theta) Pendulum_z = z(t) -l*cos(theta) z(t) = A*sin(Bt + C) + D for driving pendulum Origin: Starting location of platform centroid Initial Conditions: horizontal to the right (PI/2) (to the right) angular velocity = 0 (released from free fall) """ """ System Parameters """ g = ti.field(float, shape=()) # Gravity N_PENDULUMS = 4 """ Render Buffers """ N_PARTICLES = 2*N_PENDULUMS bar_pos = ti.Vector.field(3, dtype=float, shape = N_PARTICLES) pendulum_pos = ti.Vector.field(3, dtype=float, shape = N_PENDULUMS) platform_pos = ti.Vector.field(3, dtype=float, shape = N_PENDULUMS) """ Types """ mat2x1 = ti.types.matrix(2,1,float) """ Pendulum Dataclass """ @ti.dataclass class Pendulum: l: float a: float b: float c: float d: float Theta: mat2x1 Cart: ti.math.vec2 PlatformZ: float @ti.func def Z(self, t: float) -> float: """ Compute the plaform's position at time T """ return self.a * ti.sin( self.b * t + self.c) + self.d @ti.func def Zpp(self, t: float) -> float: """ Compute the Second derivative of the platform's oscillation explicitly """ return -self.a * (self.b**2) * ti.sin( self.b * t + self.c) + self.d @ti.func def M(self, t: float) -> ti.math.mat2: """ Build the matrix which turns the DiffEq into a system of DiffEqs and evaluate the driving force z(t) """ gravity = g[None] return ti.Matrix([ [ 0, 1 ], [-(gravity + self.Zpp(t))/l , 0] ]) @ti.func def to_cartesian(self, t) -> float: """ Convert Theta to XY """ theta = self.Theta[0,0] #TODO: check how secod coordinate sign affects gravity return ti.Vector([self.l*ti.sin(theta), self.Z(t) - self.l*ti.cos(theta)]) @ti.func def compute_dTheta_RK(self, t:float, dt: float) -> mat2x1: """ Use Runge Kutta to estimate dTheta for a t and dt value """ # Compute K values K1 = dt * self.M(t) @ Theta_to_T(self.Theta) K2 = dt * self.M(t + dt/2) @ Theta_to_T(self.Theta + 0.5 * K1) K3 = dt * self.M(t + dt/2) @ Theta_to_T(self.Theta + 0.5 * K2) K4 = dt * self.M(t + dt) @ Theta_to_T(self.Theta + K3) # Compute dY dTheta = (1/6)*K1 + (1/3)*K2 + (1/3)*K3 + (1/6)*K4 return dTheta @ti.func def update(self, t: float, timestep: float): dTheta = self.compute_dTheta_RK(t, timestep) self.Theta += dTheta self.PlatformZ = self.Z(t) self.Cart = self.to_cartesian(t) pendulums = Pendulum.field(shape=(N_PENDULUMS)) l = 1.2 # length a = 1.75 # Platform amplitude b = 1.0 # Platform frequency c = 0.0 # Platform initial phase d = 5.0 # Platform Offst """ Initial Coditions """ theta_0 = np.pi /2 thetaP_0 = 0 Theta_0 = ti.Matrix.cols([[ theta_0, thetaP_0 ]]) """ Time Management """ now = ti.field(float, shape=()) timestep = 0.01 spacing = 2.5 """ Kernel """ @ti.kernel def init_globals(): now[None] = 0 g[None] = 1.9 @ti.kernel def init_fields(): """ Populate Mutable State """ for i in pendulums: pendulums[i].l = l+ti.random(float)*l pendulums[i].a = a+ti.random(float)*a/2 pendulums[i].b = b+ti.random(float)*b pendulums[i].c = c pendulums[i].d = d pendulums[i].Theta = Theta_0 pendulums[i].PlatformZ = pendulums[i].Z(now[None]) pendulums[i].Cart = pendulums[i].to_cartesian(now[None]) @ti.kernel def update_pendulums(timestep: float): for i in pendulums: pendulums[i].update(now[None], timestep) update_graphics_objects(i, pendulums[i]) now[None] += timestep @ti.func def update_graphics_objects(i: int, pendulum: Pendulum): """ Update Graphics Objects """ # TODO: move pendulum_pos[i].y = pendulum.Cart.x + spacing * i pendulum_pos[i].z = pendulum.Cart.y platform_pos[i].y = spacing*i platform_pos[i].z = pendulum.PlatformZ bar_pos[2*i].y = spacing*i bar_pos[2*i].z = pendulum.PlatformZ bar_pos[2*i+1].y = pendulum.Cart.x + spacing*i bar_pos[2*i+1].z = pendulum.Cart.y @ti.func def Theta_to_T(theta: mat2x1) -> mat2x1: """ Convert Theta, Theta' Vector into SinTheta, Theta' vector """ return ti.Matrix.cols([ [ ti.sin(theta[0,0]), theta[1,0] ] ]) """ GUI / Rendering """ def init_gui_scene(): """ Init GUI """ window = ti.ui.Window('Window Title', res = (1920, 1080), pos = (150, 150)) gui = window.get_gui() canvas = window.get_canvas() scene = ti.ui.Scene() camera = ti.ui.Camera() camera.position(10, spacing * (N_PENDULUMS-1)/2, 5) camera.lookat(0, spacing * (N_PENDULUMS-1)/2, 5) camera.up(0,0,1) return window, canvas, scene, camera, gui def build_cam_and_lights(camera, scene): """ Add cameras and lights """ camera.track_user_inputs(window, movement_speed=0.03, hold_key=ti.ui.RMB) scene.set_camera(camera) scene.ambient_light((0.8, 0.8, 0.8)) scene.point_light(pos=(0.5, 1.5, 1.5), color=(1, 1, 1)) def build_controls(gui): # Buid GUI for i in range(N_PENDULUMS): with gui.sub_window(f"Pendulum {i} Controls", 0.025, 0.2 + 0.2*i, 0.2, 0.15) as guiwindow: guiwindow.text("Oscillatig Driver") new_amp =guiwindow.slider_float("Oscillator Amplitude", pendulums[i].a, 0.0, 5.0) new_freq = guiwindow.slider_float("Oscillator Freq", pendulums[i].b, 0.0, 6) guiwindow.text("Pendulum") new_l = guiwindow.slider_float("Pendulum L", pendulums[i].l, 0.0, 3) pendulums[i].a = new_amp pendulums[i].b = new_freq pendulums[i].l = new_l with gui.sub_window("Physics Controls", 0.025, 0.05, 0.2, 0.1) as guiwndow: new_g = guiwindow.slider_float("Gravity", g[None], 0.01, 10) g[None] = new_g """ Main """ if __name__ == '__main__': init_globals() init_fields() window, canvas, scene, camera, gui = init_gui_scene() while window.running: update_pendulums(timestep) build_cam_and_lights(camera, scene) # Render Nodes scene.particles(platform_pos, color = (0.26, 0.8, 0.19), radius = 0.15) scene.particles(pendulum_pos, color = (0.68, 0.26, 0.19), radius = 0.1) # Render Bars scene.lines(bar_pos, color = (0.28, 0.68, 0.99), width = 5.0) # Add scene to canvas canvas.scene(scene) build_controls(gui) window.show()