def _update_anim(self): import vpython as vp g = self.domain_param['g'] m_ball = self.domain_param['m_ball'] r_ball = self.domain_param['r_ball'] m_beam = self.domain_param['m_beam'] l_beam = self.domain_param['l_beam'] d_beam = self.domain_param['d_beam'] ang_offset = self.domain_param['ang_offset'] c_frict = self.domain_param['c_frict'] x = float(self.state[0]) # ball position along the beam axis [m] a = float(self.state[1]) # angle [rad] # Update the animation self._anim['ball'].radius = r_ball self._anim['ball'].pos = vp.vec( vp.cos(a)*x - vp.sin(a)*(d_beam/2. + r_ball), vp.sin(a)*x + vp.cos(a)*(d_beam/2. + r_ball), 0 ) self._anim['beam'].size = vp.vec(l_beam, d_beam, 2*d_beam) self._anim['beam'].rotate( angle=float(self.state[3])*self._dt, axis=vp.vec(0, 0, 1), origin=vp.vec(0, 0, 0) ) # Set caption text self._anim['canvas'].caption = f"""
def main(): ''' Set up Stationary scene ''' scene = vp.canvas(title='Solar system',width=1300,height=600,center=vp.vector(0,0,0)) scene.autoscale = False scene.range = star_radius*model_scale scene.camera.pos = vp.vector(0,100000,star_radius*model_scale) # nice view # scene.camera.pos = vp.vector(100000,0,0) # side view # scene.camera.pos = vp.vector(0,100000,0) # top down view scene.camera.axis = vp.vector(vp.vector(0,0,0) - scene.camera.pos) # Background stars if show_backgound_stars == True: init_stars(star_radius, star_size, num_star) ''' Set up Animated objects ''' # Intialize Asteroid belt if show_asteroid_belt == True: print('Generating asteroid belt') ast_list, ast_radius_list, ast_theta0_list = init_belt(ast_belt_radius, ast_size, num_ast) # Initialize Kuiper belt if show_kuiper_belt == True: print('Generating kuiper belt') kui_list, kui_radius_list, kui_theta0_list = init_belt(kui_belt_radius, kui_size, num_kui) # Initialize planet system object_list = init_system(filename) ''' Animation ''' print('Begin animation') dt = 100 i = 0 # Counter for moving the belts while True: vp.rate(500000) # update planet position for body in object_list: body.motion(dt, i) # rotate asteroid belt and kuiper belt if rotate_belt == True and show_asteroid_belt == True: # shift theta of all belt objects for ast, ast_radius, ast_theta0 in zip(ast_list, ast_radius_list, ast_theta0_list): ast_theta = ast_theta0 + i * 2*np.pi/ast_belt_period # change in theta per unit time = omega = 2pi/T ast.pos = model_scale*ast_radius*vp.vector(vp.cos(ast_theta),0,vp.sin(ast_theta)) if rotate_belt == True and show_kuiper_belt == True: for kui, kui_radius, kui_theta0 in zip(kui_list, kui_radius_list, kui_theta0_list): kui_theta = kui_theta0 + i * 2*np.pi/kui_belt_period kui.pos = model_scale*kui_radius*vp.vector(vp.cos(kui_theta),0,vp.sin(kui_theta)) if i%int(print_step) == 0: print('Celestial object positions update completed - Running next iteration') i += 1
def init_stars(radius, size, num_star): ''' Generate background stars ''' for i in range(int(num_star)): theta = rd.uniform(0, 2 * np.pi) phi = rd.uniform(0, np.pi) vp.sphere(pos=radius * model_scale * vp.vector( vp.sin(theta) * vp.cos(phi), vp.sin(theta) * vp.sin(phi), vp.cos(theta)), radius=model_scale * size, color=vp.color.white)
def motion(self, dt): ''' Rotate rod in SHM ''' self.dt = dt # Update position, omega and alpha self.theta += self.omega*dt self.alpha = - kappa / self.inertia * self.theta self.omega += self.alpha*dt # Update rod position and axis self.rod.pos = vp.vector(self.length/2.*vp.cos(self.theta),self.y_location,-self.length/2.*vp.sin(self.theta)) self.rod.axis = vp.vector(-self.length*vp.cos(self.theta),0,self.length*vp.sin(self.theta))
def draw_graph_lagrange(): # przygotowanie wykresu gd = graph(title='Interpolacja Lagrange\'a') f1 = gcurve(graph=gd, color=color.cyan) dots = gdots(graph=gd, color=color.red) f2 = gcurve(graph=gd, color=color.green) # tablica do przechowywania wylosowanych punktów initial_points = [] # zmienna pomocnicza używana przy losowaniu punktów wielomianu index = 0 # wyliczanie wartości funkcji podstawowej w tym przypadku cos(2*x) * exp(-0.2 * x) field_end = 8.05 for x in arange(0, field_end, 0.1): y = cos(2 * x) * exp(-0.2 * x) f1.plot(x, y) #losowanie punktów do wyliczania interpolacji - odbywa kiedy x znajduje się w przedziale 1..field_end-1 if 1 < x <= field_end - 1 and index % 2 == 0 and bool(getrandbits(1)): initial_points.append({'x': x, 'y': y}) dots.plot(x, y) index = index + 1 # wyliczanie i rysowanie interpolacji for point in lagrange_interpolation(initial_points, field_end): f2.plot(point['x'], point['y'])
def _init_anim(self): import vpython as vp l_pole = float(self.domain_param['l_pole']) r_pole = 0.05 th, _ = self.state # Init render objects on first call self._anim['canvas'] = vp.canvas(width=1000, height=600, title="Pendulum") # Joint self._anim['joint'] = vp.sphere( pos=vp.vec(0, 0, r_pole), radius=r_pole, color=vp.color.white, ) # Pole self._anim['pole'] = vp.cylinder(pos=vp.vec(0, 0, r_pole), axis=vp.vec(2 * l_pole * vp.sin(th), -2 * l_pole * vp.cos(th), 0), radius=r_pole, length=2 * l_pole, color=vp.color.blue, canvas=self._anim['canvas'])
def _init_anim(self): import vpython as vp r_ball = self.domain_param['r_ball'] l_beam = self.domain_param['l_beam'] d_beam = self.domain_param['d_beam'] x = float(self.state[0]) # ball position along the beam axis [m] a = float(self.state[1]) # angle [rad] self._anim['canvas'] = vp.canvas(width=800, height=600, title="Ball on Beam") self._anim['ball'] = vp.sphere( pos=vp.vec(x, d_beam/2. + r_ball, 0), radius=r_ball, color=vp.color.red, canvas=self._anim['canvas'] ) self._anim['beam'] = vp.box( pos=vp.vec(0, 0, 0), axis=vp.vec(vp.cos(a), vp.sin(a), 0), length=l_beam, height=d_beam, width=2*d_beam, color=vp.color.green, canvas=self._anim['canvas'] )
def init_belt(belt_radius, size_range, num_rock): ''' Generate the ring-like belts ''' rock_list = [] radius_list = [] theta0_list = [] # Create objects in a belt at random positions following a normal distribution radius_mean = np.mean(belt_radius) # radius range of belt radius_std = (belt_radius[1] - belt_radius[0]) / 4. # estimated std for i in range(int(num_rock)): loading = (i + 1) / num_rock * 100 if loading % 10 == 0: print('loading... ', int(loading), '%') radius = np.random.normal(radius_mean, radius_std) size = rd.uniform(size_range[0], size_range[1]) / 2. # radius theta0 = rd.uniform(0, 2 * np.pi) rock = vp.sphere( pos=model_scale * vp.vector(radius * vp.cos(theta0), 0, radius * vp.sin(theta0)), radius=model_scale * size, color=vp.color.white) rock_list.append(rock) radius_list.append(radius) theta0_list.append(theta0) return rock_list, radius_list, theta0_list
def __init__(self, pos, ori, rad, thi, I, no_of_seg, turns=1): self.pos = pos self.ori = ori self.rad = rad self.thi = thi self.I = I self.no_of_seg = no_of_seg self.turns = turns self.ring = vp.ring(pos=self.pos, axis=self.ori, radius=self.rad, thickness=self.thi) element_length = (2 * np.pi * self.rad) / self.no_of_seg segments = [0] * (self.no_of_seg) for i in range(self.no_of_seg): theta = i * (2 * np.pi / self.no_of_seg) y = self.rad * np.cos(theta) z = self.rad * np.sin(theta) segments[i] = vp.cylinder( pos=vp.vec(self.pos.x, self.pos.y + y, self.pos.z + z), axis=element_length * vp.vec(0, -vp.sin(theta), vp.cos(theta)), radius=self.thi) segments[i].visible = False self.segments = segments print('done')
def _update_anim(self): import vpython as vp g = self.domain_param['g'] l_plate = self.domain_param['l_plate'] m_ball = self.domain_param['m_ball'] r_ball = self.domain_param['r_ball'] eta_g = self.domain_param['eta_g'] eta_m = self.domain_param['eta_m'] K_g = self.domain_param['K_g'] J_m = self.domain_param['J_m'] J_l = self.domain_param['J_l'] r_arm = self.domain_param['r_arm'] k_m = self.domain_param['k_m'] R_m = self.domain_param['R_m'] B_eq = self.domain_param['B_eq'] c_frict = self.domain_param['c_frict'] V_thold_x_neg = self.domain_param['V_thold_x_neg'] V_thold_x_pos = self.domain_param['V_thold_x_pos'] V_thold_y_neg = self.domain_param['V_thold_y_neg'] V_thold_y_pos = self.domain_param['V_thold_y_pos'] offset_th_x = self.domain_param['offset_th_x'] offset_th_y = self.domain_param['offset_th_y'] d_plate = 0.01 # only for animation # Compute plate orientation a_vp = -self.plate_angs[0] # plate's angle around the y axis (alpha) b_vp = self.plate_angs[1] # plate's angle around the x axis (beta) # Axis runs along the x direction self._anim['plate'].size = vp.vec(l_plate, l_plate, d_plate) self._anim['plate'].axis = vp.vec(vp.cos(a_vp), 0, vp.sin(a_vp)) * float(l_plate) # Up runs along the y direction (vpython coordinate system) self._anim['plate'].up = vp.vec(0, vp.cos(b_vp), vp.sin(b_vp)) # Get ball position x = self.state[2] # along the x axis y = self.state[3] # along the y axis self._anim['ball'].pos = vp.vec( x * vp.cos(a_vp), y * vp.cos(b_vp), r_ball + x * vp.sin(a_vp) + y * vp.sin(b_vp) + vp.cos(a_vp) * d_plate / 2.) self._anim['ball'].radius = r_ball # Set caption text self._anim['canvas'].caption = f"""
def __init__(self, mass, length, y_location, init_theta): ''' Create object ''' self.mass = mass self.length = length self.y_location = y_location # Moment of inertia self.inertia = 1/12. * self.mass * self.length**2 # Initialize angular position theta self.theta = init_theta * vp.pi/180 # convert to radians # Initialize angular velocity omega self.omega = init_omega # Initialize vpython objects self.rod = vp.cylinder(pos=vp.vector(self.length/2.*vp.cos(self.theta),self.y_location,-self.length/2.*vp.sin(self.theta)), axis=vp.vector(-self.length*vp.cos(self.theta),0,self.length*vp.sin(self.theta)), radius=.25,color=vp.color.red) self.string = vp.cylinder(pos=vp.vector(0,-10,0),axis=vp.vector(0,y_step*num_rods+10.,0),radius=.1,color=vp.color.white)
def reset(): global theta, thetadot, psi, psidot, phi, phidot theta = 0.3*vp.pi # initial polar angle of shaft (from vertical) thetadot = 0 # initial rate of change of polar angle psi = 0 # initial spin angle psidot = 30 # initial rate of change of spin angle (spin ang. velocity) phi = -vp.pi/2 # initial azimuthal angle phidot = 0 # initial rate of change of azimuthal angle if pureprecession: # Set to True if you want pure precession, without nutation a = (1-I3/I1)*vp.sin(theta)*vp.cos(theta) b = -(I3/I1)*psidot*vp.sin(theta) c = M*g*r*vp.sin(theta)/I1 phidot = (-b+vp.sqrt(b**2-4*a*c))/(2*a) gyro.axis = gyro.length * \ vp.vector(vp.sin(theta)*vp.sin(phi), vp.cos(theta), vp.sin(theta)*vp.cos(phi)) A = vp.norm(gyro.axis) gyro.pos = 0.5*Lshaft*A tip.pos = Lshaft*A tip.clear_trail()
def _reset_anim(self): import vpython as vp r_ball = self.domain_param['r_ball'] l_beam = self.domain_param['l_beam'] d_beam = self.domain_param['d_beam'] self._anim['ball'].pos = vp.vec( float(self.state[0]), vp.sin(self.state[1]) * float(self.state[0]) + vp.cos(self.state[1]) * d_beam / 2. + r_ball, 0) self._anim['beam'].visible = False self._anim['beam'] = vp.box(pos=vp.vec(0, 0, 0), axis=vp.vec(vp.cos(float(self.state[1])), vp.sin(float(self.state[1])), 0), length=l_beam, height=d_beam, width=2 * d_beam, color=vp.color.green, canvas=self._anim['canvas'])
def _init_anim(self): import vpython as vp l_pole = float(self.domain_param['l_pole']) l_rail = float(self.domain_param['l_rail']) # Only for animation l_cart, h_cart = 0.08, 0.08 r_pole, r_rail = 0.01, 0.005 # Get positions x, th, _, _ = self.state self._anim['canvas'] = vp.canvas(width=1000, height=600, title="Quanser Cartpole") # Rail self._anim['rail'] = vp.cylinder( pos=vp.vec(-l_rail / 2, -h_cart / 2 - r_rail, 0), # a VPython's cylinder origin is at the bottom radius=r_rail, length=l_rail, color=vp.color.white, canvas=self._anim['canvas']) # Cart self._anim['cart'] = vp.box(pos=vp.vec(x, 0, 0), length=l_cart, height=h_cart, width=h_cart / 2, color=vp.color.green, canvas=self._anim['canvas']) self._anim['joint'] = vp.sphere( pos=vp.vec(x, 0, r_pole + h_cart / 4), radius=r_pole, color=vp.color.white, ) # Pole self._anim['pole'] = vp.cylinder(pos=vp.vec(x, 0, r_pole + h_cart / 4), axis=vp.vec(2 * l_pole * vp.sin(th), -2 * l_pole * vp.cos(th), 0), radius=r_pole, length=2 * l_pole, color=vp.color.blue, canvas=self._anim['canvas'])
def motion(self, dt, other_objects, i): ''' Update object positions as a function of timestep ''' self.other_objects = other_objects # Verlocity verlet method # Update body position, velocity and acceleration relative to all other objects self.position, self.velocity, self.acceleration = velocity_verlet( self.centr_body, self.other_objects, self.position, self.velocity, self.acceleration, dt) # Update vpython sphere location self.sphere.pos = self.position * model_scale if self.is_moon == False: # Update text position self.label.pos = model_scale * vp.vector( self.position.x, self.position.y + 5e9 * model_scale, self.position.z) # Update trojan belt position to follow jupiter if self.name == 'Jupiter' and show_trojan_belt == True: for tro_rock, radius, theta0 in zip(self.tro_rock_list, self.tro_radius_list, self.tro_theta0_list): # Change in theta, from original position to updated self.position updated_theta = np.arctan(self.position.x / self.position.z) if self.position.z < 0: updated_theta += np.pi j_del_theta = updated_theta - self.init_j_theta # new theta = starting angle of rock relative to original theta + jupiter's moved theta theta = theta0 + j_del_theta tro_rock.pos = model_scale * vp.vector( radius * vp.sin(theta), 0, radius * vp.cos(theta)) if show_arrows == True: self.arrow.pos = model_scale * self.position self.arrow.axis = self.velocity # Print planet's updated position if i % int(print_step) == 0 and self.is_moon == False: print(self.name, 'at location', self.position)
def _update_anim(self): import vpython as vp g = self.domain_param['g'] m_pole = self.domain_param['m_pole'] l_pole = float(self.domain_param['l_pole']) d_pole = self.domain_param['d_pole'] tau_max = self.domain_param['tau_max'] r_pole = 0.05 th, _ = self.state # Cart self._anim['joint'].pos = vp.vec(0, 0, r_pole) # Pole self._anim['pole'].pos = vp.vec(0, 0, r_pole) self._anim['pole'].axis = vp.vec(2 * l_pole * vp.sin(th), -2 * l_pole * vp.cos(th), 0) # Set caption text self._anim['canvas'].caption = f"""
def _update_anim(self): import vpython as vp g = self.domain_param['g'] m_cart = self.domain_param['m_cart'] m_pole = self.domain_param['m_pole'] l_pole = float(self.domain_param['l_pole']) l_rail = float(self.domain_param['l_rail']) eta_m = self.domain_param['eta_m'] eta_g = self.domain_param['eta_g'] K_g = self.domain_param['K_g'] J_m = self.domain_param['J_m'] R_m = self.domain_param['R_m'] k_m = self.domain_param['k_m'] r_mp = self.domain_param['r_mp'] B_eq = self.domain_param['B_eq'] B_pole = self.domain_param['B_pole'] # Only for animation l_cart, h_cart = 0.08, 0.08 r_pole, r_rail = 0.01, 0.005 # Get positions x, th, _, _ = self.state # Rail self._anim['rail'].pos = vp.vec(-l_rail / 2, -h_cart / 2 - r_rail, 0) self._anim['rail'].length = l_rail # Cart self._anim['cart'].pos = vp.vec(x, 0, 0) self._anim['joint'].pos = vp.vec(x, 0, r_pole + h_cart / 4) # Pole self._anim['pole'].pos = vp.vec(x, 0, r_pole + h_cart / 4) self._anim['pole'].axis = vp.vec(2 * l_pole * vp.sin(th), -2 * l_pole * vp.cos(th), 0) # Set caption text self._anim['canvas'].caption = f"""
def render(self, mode, render_step=10): assert isinstance(self._dyn, BallBalancerDynamics ), "Missing dynamics properties for simulation!" # Cheap printing to console if self._step_count % render_step == 0: print( "time step: {:3} | in bounds: {:1} | state: {} | action: {}" .format(self._step_count, self.state_space.contains(self._state), self._state, self._curr_action)) if not self.state_space.contains(self._state): # State is out of bounds np.set_printoptions(precision=3) print("min state : ", self.state_space.low) print("last state: ", self._state) print("max state : ", self.state_space.high) # Render using VPython import vpython as vp vp.rate(30) d_plate = 0.01 # only for animation # Init render objects on first call if self._anim_canvas is None: self._anim_canvas = vp.canvas(width=800, height=600, title="Quanser Ball Balancer") self._anim_ball = vp.sphere( pos=vp.vector(self._state[2], self._state[3], self._dyn.r_ball + d_plate / 2.), radius=self._dyn.r_ball, mass=self._dyn.m_ball, color=vp.color.red, canvas=self._anim_canvas, ) self._anim_plate = vp.box( pos=vp.vector(0, 0, 0), size=vp.vector(self._dyn.l_plate, self._dyn.l_plate, d_plate), color=vp.color.green, canvas=self._anim_canvas, ) # Compute plate orientation a = -self._plate_angs[0] # plate's angle around the y axis (alpha) b = self._plate_angs[1] # plate's angle around the x axis (beta) # Axis runs along the x direction self._anim_plate.axis = vp.vec( vp.cos(a), 0, vp.sin(a), ) * self._dyn.l_plate # Up runs along the y direction (vpython coordinate system is weird) self._anim_plate.up = vp.vec( 0, vp.cos(b), vp.sin(b), ) # Compute ball position x = self._state[2] # ball position along the x axis y = self._state[3] # ball position along the y axis self._anim_ball.pos = vp.vec( x * vp.cos(a), y * vp.cos(b), self._dyn.r_ball + x * vp.sin(a) + y * vp.sin(b) + vp.cos(a) * d_plate / 2., ) # Set caption text self._anim_canvas.caption = f"""
def simulateDoublePendula(models, func, x0, tEnd=10, deltat=0.005): """ Multiple double pendulum simulation via VPython given an R^4 -> R^4 differential equation (true double pendulum ODEs) and set of trained models Adapted from https://trinket.io/glowscript/9bdab2cf88 Arguments models: list of trained EQL/EQL-div models func: python function, R^4 -> R^4 system of ODEs x0: list with 4 elements, initial condition to integrate from in the form [theta1, omega1, theta2, omega2] tEnd: time to integrate until deltat: spacing between sampled functional values (note: not the step the integrator uses) """ t = 0 theta1, omega1, theta2, omega2 = x0 L1 = 1 L2 = 1 vp.canvas(width=1400, height=750, background=vp.color.white, range=1.75, autoscale=False, userpan=False, userspin=False, userzoom=False) # create the ceiling, masses, and strings ceiling = vp.box(pos=vec(0, 1, 0), size=vec(0.1, 0.05, 0.1), color=vp.color.gray(0.5)) # real double pendulum rball1 = vp.sphere(pos=vec(ceiling.pos.x + L1 * vp.sin(theta1), ceiling.pos.y - L1 * vp.cos(theta1), 0), radius=0.05, color=vp.color.orange) rball1.color = vp.color.black rball1.radius = 0.05 rball2 = vp.sphere(pos=vec( ceiling.pos.x + L1 * vp.sin(theta1) + L2 * vp.sin(theta2), ceiling.pos.y - L1 * vp.cos(theta1) - L2 * vp.cos(theta2), 0), radius=0.05, color=vp.color.black, make_trail=True, interval=10, retain=15) rball2.color = vp.color.black rball2.radius = 0.05 rstring1 = vp.cylinder(pos=ceiling.pos, axis=rball1.pos - ceiling.pos, color=vp.color.gray(0.5), radius=0.008) rstring2 = vp.cylinder(pos=rball1.pos, axis=rball2.pos - rball1.pos, color=vp.color.gray(0.5), radius=0.008) # double pendulums based on learned equations balls = [None for i in range(len(models) * 2)] strings = [None for i in range(len(models) * 2)] colors = [ vec(240 / 255, 133 / 255, 33 / 255), vec(110 / 255, 0, 95 / 255), vec(0, 129 / 255, 131 / 255) ] for i in range(0, len(balls), 2): balls[i] = vp.sphere(pos=vec(ceiling.pos.x + L1 * vp.sin(theta1), ceiling.pos.y - L1 * vp.cos(theta1), 0), radius=0.05, color=colors[int(i / 2)]) balls[i].color = colors[int(i / 2)] balls[i].radius = 0.05 balls[i + 1] = vp.sphere(pos=vec( ceiling.pos.x + L1 * vp.sin(theta1) + L2 * vp.sin(theta2), ceiling.pos.y - L1 * vp.cos(theta1) - L2 * vp.cos(theta2), 0), radius=0.05, color=colors[int(i / 2)], make_trail=True, interval=10, retain=15) balls[i + 1].color = colors[int(i / 2)] balls[i + 1].radius = 0.05 strings[i] = vp.cylinder(pos=ceiling.pos, axis=balls[i].pos - ceiling.pos, color=vp.color.gray(0.5), radius=0.008) strings[i + 1] = vp.cylinder(pos=balls[i].pos, axis=balls[i + 1].pos - balls[i].pos, color=vp.color.gray(0.5), radius=0.008) # generating simulation data actualSol, modelsSol = ode.odeSolve(models, func, x0, [0, tEnd], deltat) # calculation loop while t < tEnd / deltat: vp.rate(1 / deltat) rball1.pos = vec(vp.sin(actualSol.y[0][t]), -vp.cos(actualSol.y[0][t]), 0) + ceiling.pos rstring1.axis = rball1.pos - ceiling.pos rball2.pos = rball1.pos + vec(vp.sin(actualSol.y[2][t]), -vp.cos(actualSol.y[2][t]), 0) rstring2.axis = rball2.pos - rball1.pos rstring2.pos = rball1.pos for i in range(0, len(balls), 2): balls[i].pos = vec(vp.sin(modelsSol[int( i / 2)].y[0][t]), -vp.cos(modelsSol[int(i / 2)].y[0][t]), 0) + ceiling.pos strings[i].axis = balls[i].pos - ceiling.pos balls[i + 1].pos = (balls[i].pos + vec(vp.sin(modelsSol[int(i / 2)].y[2][t]), -vp.cos(modelsSol[int(i / 2)].y[2][t]), 0)) strings[i + 1].axis = balls[i + 1].pos - balls[i].pos strings[i + 1].pos = balls[i].pos t = t + 1 stop_server()
def origin_lagrange_function(x): # return cos(2 * x) * exp(-0.2 * x) return exp(-x) * cos(5 * x)
def origin_newton_function(x): return exp(-x) * cos(5 * x)
def f(x, y): # Return the value of the function of x and y: return 0.7+0.2*vp.sin(10*x)*vp.cos(10*y)*vp.sin(5*t)
def czybyszew_point(k, n, field_start, field_end): return (field_end - field_start) / 2 * cos( ((2 * k + 1) * pi) / (2 * n + 2)) + (field_start + field_end) / 2
def in_strikezone(): return in_boundary(strikezone.pos.x, strikezone.size.x, ball.pos.x) and \ in_boundary(strikezone.pos.y, strikezone.size.y, ball.pos.y) and \ in_boundary(strikezone.pos.z, strikezone.size.z, ball.pos.z) gravity = 0.447 * vec(0, -9.8, 0) ball = sphere() ball.pos = 0.305 * vec(0, 5 + (10 / 12), 0) ball.radius = 0.305 * 1.45 / 12 angle = atan( (strikezone.pos.y - ball.pos.y) / (strikezone.pos.x - ball.pos.x)) + 0.03 ball.velocity = 0.447 * 120 * vec(cos(angle), sin(angle), 0) trail = curve(color=color.blue, pos=ball.pos) t = 0 dt = 0.0001 C = 1.0 A = ball.radius**2 * pi p = 1.2 wind = vec(0, 0, 40) def drag(v, C, p, A): return 0.5 * C * p * A * v.mag * v
from vpython import sphere, vector, sin, cos, rate, pi s = sphere() t = 0 # seconds dt = 0.001 # seconds R = 10 # meters omega = 3.14 # radians/second s.pos = vector(R * cos(omega * t), R * sin(omega * t), 0) s.velocity = omega * R * vector(-sin(t * omega), cos(t * omega), 0) while t < 2 * 2 * pi / omega: s.velocity = omega * R * vector(-sin(t * omega), cos(t * omega), 0) s.pos = s.pos + s.velocity * dt t = t + dt rate(1 / dt)
def __handle_keyboard_inputs(self): """ Pans amount dependent on distance between camera and focus point. Closer = smaller pan amount A = move left (pan) D = move right (pan) W = move up (pan) S = move down (pan) <- = rotate left along camera axes (rotate) -> = rotate right along camera axes (rotate) ^ = rotate up along camera axes (rotate) V = rotate down along camera axes (rotate) Q = roll left (rotate) E = roll right (rotate) ctrl + LMB = rotate (Default Vpython) """ # If camera lock, just skip the function if self.__camera_lock: return # Constants pan_amount = 0.02 # units rot_amount = 1.0 # deg # Current settings cam_distance = self.scene.camera.axis.mag cam_pos = vector(self.scene.camera.pos) cam_focus = vector(self.scene.center) # Weird manipulation to get correct vector directions. (scene.camera.up always defaults to world up) cam_axis = (vector(self.scene.camera.axis)) # X cam_side_axis = self.scene.camera.up.cross(cam_axis) # Y cam_up = cam_axis.cross(cam_side_axis) # Z cam_up.mag = cam_axis.mag # Get a list of keys keys = keysdown() # Userpan uses ctrl, so skip this check to avoid changing camera pose while shift is held if 'shift' in keys: return ################################################################################################################ # PANNING # Check if the keys are pressed, update vectors as required # Changing camera position updates the scene center to follow same changes if 'w' in keys: cam_pos = cam_pos + cam_up * pan_amount if 's' in keys: cam_pos = cam_pos - cam_up * pan_amount if 'a' in keys: cam_pos = cam_pos + cam_side_axis * pan_amount if 'd' in keys: cam_pos = cam_pos - cam_side_axis * pan_amount # Update camera position before rotation (to keep pan and rotate separate) self.scene.camera.pos = cam_pos ################################################################################################################ # Camera Roll # If only one rotation key is pressed if 'q' in keys and 'e' not in keys: # Rotate camera up cam_up = cam_up.rotate(angle=-radians(rot_amount), axis=cam_axis) # Set magnitude as it went to inf cam_up.mag = cam_axis.mag # Set self.scene.up = cam_up # If only one rotation key is pressed if 'e' in keys and 'q' not in keys: # Rotate camera up cam_up = cam_up.rotate(angle=radians(rot_amount), axis=cam_axis) # Set magnitude as it went to inf cam_up.mag = cam_axis.mag # Set self.scene.up = cam_up ################################################################################################################ # CAMERA ROTATION d = cam_distance move_dist = sqrt(d ** 2 + d ** 2 - 2 * d * d * cos(radians(rot_amount))) # SAS Cosine # If only left not right key if 'left' in keys and 'right' not in keys: # Calculate distance to translate cam_pos = cam_pos + norm(cam_side_axis) * move_dist # Calculate new camera axis cam_axis = -(cam_pos - cam_focus) if 'right' in keys and 'left' not in keys: cam_pos = cam_pos - norm(cam_side_axis) * move_dist cam_axis = -(cam_pos - cam_focus) if 'up' in keys and 'down' not in keys: cam_pos = cam_pos + norm(cam_up) * move_dist cam_axis = -(cam_pos - cam_focus) if 'down' in keys and 'up' not in keys: cam_pos = cam_pos - norm(cam_up) * move_dist cam_axis = -(cam_pos - cam_focus) # Update camera position and axis self.scene.camera.pos = cam_pos self.scene.camera.axis = cam_axis
def justOne(func, x0, tEnd=10, deltat=0.005): """ Double pendulum simulation via VPython given an R^4 -> R^4 differential equation (allows the testing of various such ODEs) Adapted from https://trinket.io/glowscript/9bdab2cf88 Arguments func: python function, R^4 -> R^4 system of ODEs x0: list with 4 elements, initial condition to integrate from in the form [theta1, omega1, theta2, omega2] tEnd: time to integrate until deltat: spacing between sampled functional values (note: not the step the integrator uses) """ # constants t = 0 theta1, omega1, theta2, omega2 = x0 L1 = 1 L2 = 1 vp.canvas(width=1400, height=750, background=vp.color.white, range=1.75, autoscale=False, userpan=False, userspin=False, userzoom=False) # create the ceiling, masses, and strings ceiling = vp.box(pos=vec(0, 1, 0), size=vec(0.1, 0.05, 0.1), color=vp.color.gray(0.5)) rball1 = vp.sphere(pos=vec(ceiling.pos.x + L1 * vp.sin(theta1), ceiling.pos.y - L1 * vp.cos(theta1), 0), radius=0.05, color=vp.color.orange) rball1.color = vp.color.black rball1.radius = 0.05 rball2 = vp.sphere(pos=vec( ceiling.pos.x + L1 * vp.sin(theta1) + L2 * vp.sin(theta2), ceiling.pos.y - L1 * vp.cos(theta1) - L2 * vp.cos(theta2), 0), radius=0.05, color=vp.color.black, make_trail=True, interval=10, retain=15) rball2.color = vp.color.black rball2.radius = 0.05 rstring1 = vp.cylinder(pos=ceiling.pos, axis=rball1.pos - ceiling.pos, color=vp.color.gray(0.5), radius=0.008) rstring2 = vp.cylinder(pos=rball1.pos, axis=rball2.pos - rball1.pos, color=vp.color.gray(0.5), radius=0.008) # generating simulation data actualSol = solve_ivp(func, [0, tEnd], x0, t_eval=np.linspace(0, tEnd, int(tEnd / deltat))) # calculation loop while t < tEnd / deltat: vp.rate(1 / deltat) rball1.pos = vec(vp.sin(actualSol.y[0][t]), -vp.cos(actualSol.y[0][t]), 0) + ceiling.pos rstring1.axis = rball1.pos - ceiling.pos rball2.pos = rball1.pos + vec(vp.sin(actualSol.y[2][t]), -vp.cos(actualSol.y[2][t]), 0) rstring2.axis = rball2.pos - rball1.pos rstring2.pos = rball1.pos t = t + 1 stop_server()
import vpython as vp from robot_imu import RobotImu from delta_timer import DeltaTimer import imu_settings imu = RobotImu(mag_offsets=imu_settings.mag_offsets) vp.cylinder(radius=1, axis=vp.vector(0, 0, 1), pos=vp.vector(0, 0, -1)) needle = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red) vp.graph(xmin=0, xmax=60, scroll=True) graph_yaw = vp.gcurve(color=vp.color.blue) timer = DeltaTimer() while True: vp.rate(100) dt, elapsed = timer.update() mag = imu.read_magnetometer() yaw = -vp.atan2(mag.y, mag.x) graph_yaw.plot(elapsed, vp.degrees(yaw)) needle.axis = vp.vector(vp.sin(yaw), vp.cos(yaw), 0)
tip.pos = Lshaft*A tip.clear_trail() reset() vp.scene.waitfor('textures') dt = 0.0001 t = 0 Nsteps = 20 # number of calculational steps between graphics updates while True: vp.rate(200) for step in range(Nsteps): # multiple calculation steps for accuracy # Calculate accelerations of the Lagrangian coordinates: atheta = vp.sin(theta)*vp.cos(theta)*phidot**2+(M*g*r*vp.sin(theta) - I3*(psidot+phidot*vp.cos(theta))*phidot*vp.sin(theta))/I1 aphi = (I3/I1)*(psidot+phidot*vp.cos(theta))*thetadot / \ vp.sin(theta)-2*vp.cos(theta)*thetadot*phidot/vp.sin(theta) apsi = phidot*thetadot*vp.sin(theta)-aphi*vp.cos(theta) # Update velocities of the Lagrangian coordinates: thetadot += atheta*dt phidot += aphi*dt psidot += apsi*dt # Update Lagrangian coordinates: theta += thetadot*dt phi += phidot*dt psi += psidot*dt gyro.axis = gyro.length * \ vp.vector(vp.sin(theta)*vp.sin(phi),
model = virtual_robot.make_robot() virtual_robot.robot_view() compass = vp.canvas(width=400, height=400) vp.cylinder(radius=1, axis=vp.vector(0, 0, 1), pos=vp.vector(0, 0, -1)) needle = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red) vp.graph(xmin=0, xmax=60, scroll=True) graph_roll = vp.gcurve(color=vp.color.red) graph_pitch = vp.gcurve(color=vp.color.green) graph_yaw = vp.gcurve(color=vp.color.blue) timer = DeltaTimer() while True: vp.rate(100) dt, elapsed = timer.update() fusion.update(dt) # reset the model model.up = vp.vector(0, 1, 0) model.axis = vp.vector(1, 0, 0) # Reposition it model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0)) model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0)) model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1)) needle.axis = vp.vector(vp.sin(vp.radians(fusion.yaw)), vp.cos(vp.radians(fusion.yaw)), 0) graph_roll.plot(elapsed, fusion.roll) graph_pitch.plot(elapsed, fusion.pitch) graph_yaw.plot(elapsed, fusion.yaw)