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)
