I3 = 1/2*M*R**2 # moment of inertia, I, of gyroscope about its own axis
I1 = M*r**2 + 1/2*I3 # I about a line through the support, perpendicular to axis
phi = psi = thetadot = 0
x = vector(theta, phi, psi) # Lagrangian coordinates
v = vector(thetadot, phidot, psidot)
# ############################################################ the scene
vp = Plotter(verbose=0, axes=3, interactive=0)
shaft = vp.cylinder([[0,0,0], [Lshaft,0,0]], r=.03, c='dg')
rotor = vp.cylinder([[Lshaft/2.2,0,0],[Lshaft/1.8,0,0]], r=R, texture='marble')
base = vp.sphere([ 0, 0, 0], c='dg', r=.03)
tip = vp.sphere([Lshaft, 0, 0], c='dg', r=.03)
gyro = vp.makeAssembly([shaft, rotor, base, tip]) # group relevant actors
pedestal = vp.box([0,-0.63,0], height=.1, length=.1, width=1, texture='wood5')
pedbase = vp.box([0,-1.13,0], height=.5, length=.5, width=.05, texture='wood5')
pedpin = vp.pyramid([0,-.08,0], axis=[0,1,0], s=.05, height=.12, texture='wood5')
formulas = vp.load('data/images/gyro_formulas.png', alpha=.9).scale(.003).pos([-1,-1,-1.1])
# ############################################################ the physics
pb = ProgressBar(0, 4, dt, c='b')
for i, t in enumerate(pb.range()):
st, ct, sp, cp = sin(x[0]), cos(x[0]), sin(x[1]), cos(x[1])
thetadot, phidot, psidot = v # unpack
atheta = st*ct*phidot**2 + (M*g*r*st-I3*(psidot+phidot*ct)*phidot*st)/I1
aphi = (I3/I1)*(psidot+phidot*ct)*thetadot/st - 2*ct*thetadot*phidot/st
apsi = phidot*thetadot*st - aphi*ct
a = vector(atheta, aphi, apsi)
l_rest = 0.1 # spring x position at rest
x0 = 0.85 # initial x-coordinate of the block
k = 25 # spring constant
m = 20 # block mass
b = 0.5 # viscosity friction (proportional to velocity)
dt = 0.1 # time step
#initial conditions
v = vector(0, 0, 0.2)
x = vector(x0, 0, 0)
xr = vector(l_rest, 0, 0)
sx0 = vector(-0.8, 0, 0)
offx = vector(0, 0.3, 0)
vp.box(pos=(0, -0.1, 0), length=2.0, width=0.02, height=0.5) #surface
vp.box(pos=(-.82, .15, 0), length=.04, width=0.50, height=0.3) #wall
block = vp.cube(pos=x, length=0.2, c='t')
spring = vp.helix(sx0, x, r=.06, thickness=.01, texture='metal1')
pb = ProgressBar(0, 500, c='r')
for i in pb.range():
F = -k * (x - xr) - b * v # Force and friction
a = F / m # acceleration
v = v + a * dt # velocity
x = x + v * dt + 1 / 2 * a * dt**2 # position
block.pos(x) # update block position
spring.stretch(sx0, x) # stretch helix accordingly
trace = vp.point(x + offx, c='r/0.5', r=3) # leave a red trace
gaxis = vector(0, 0, 1) # initial orientation of gyroscope
gaxis = norm(gaxis)
I = 1/2*M*R**2 # moment of inertia of gyroscope
Lrot = I*omega*gaxis # angular momentum
cm = gpos + 0.5*Ls*gaxis # center of mass of shaft
# ############################################################ the scene
vp = Plotter(verbose=0, axes=0, interactive=0)
shaft = vp.cylinder([[0,0,0], Ls*gaxis], r=0.03, c='dg')
rotor = vp.cylinder([(Ls-0.55)*gaxis, (Ls-0.45)*gaxis], r=R, c='t')
bar = vp.cylinder([Ls*gaxis/2-R*vector(0,1,0), Ls*gaxis/2+R*vector(0,1,0)], r=R/6, c='r')
gyro = vp.Assembly([shaft, rotor, bar]) # group actors into a single one
spring= vp.helix(top, gpos, r=0.06, thickness=0.01, c='gray')
vp.box(top, length=0.2, width=0.02, height=0.2, c='gray')
vp.box(pos=(0,.5,0), length=2.2, width=3, height=2.2, c='gray', wire=1, alpha=.2)
# ############################################################ the physics
pb = ProgressBar(0, 5, dt, c='b')
for t in pb.range():
Fspring = -ks*norm(gpos-top)*(mag(gpos-top)-Lrest)
torque = cross(-1/2*Ls*norm(Lrot), Fspring) # torque about center of mass
Lrot += torque*dt
precess += (Fgrav+Fspring)*dt # momentum of center of mass
cm += (precess/M)*dt
gpos = cm - 1/2*Ls*norm(Lrot)
# set orientation along gaxis and rotate it around its axis by omega*t degrees
gyro.orientation(Lrot, rotation=omega*t*57.3).pos(gpos)
spring.stretch(top, gpos)
gaxis = vector(0, 0, 1) # initial orientation of gyroscope
gaxis = norm(gaxis)
I = 1/2*M*R**2 # moment of inertia of gyroscope
Lrot = I*omega*gaxis # angular momentum
cm = gpos + 0.5*Ls*gaxis # center of mass of shaft
# ############################################################ the scene
vp = Plotter(verbose=0, axes=3, interactive=0)
shaft = vp.cylinder([[0,0,0], Ls*gaxis], r=0.03, c='dg')
rotor = vp.cylinder([(Ls-0.55)*gaxis, (Ls-0.45)*gaxis], r=R, c='t')
bar = vp.cylinder([Ls*gaxis/2-R*vector(0,1,0), Ls*gaxis/2+R*vector(0,1,0)], r=R/6, c='r')
gyro = vp.makeAssembly([shaft, rotor, bar]) # group actors into a single one
spring= vp.helix(top, gpos, r=0.06, thickness=0.01, c='gray')
box = vp.box(top, length=0.2, width=0.02, height=0.2, c='gray')
# ############################################################ the physics
pb = ProgressBar(0, 5, dt, c='b')
for t in pb.range():
Fspring = -ks*norm(gpos-top)*(mag(gpos-top)-Lrest)
torque = cross(-1/2*Ls*norm(Lrot), Fspring) # torque about center of mass
Lrot += torque*dt
precess += (Fgrav+Fspring)*dt # momentum of center of mass
cm += (precess/M)*dt
gpos = cm - 1/2*Ls*norm(Lrot)
# set orientation along gaxis and rotate it around its axis by omega*t degrees
gyro.orientation(Lrot, rotation=omega*t*57.3).pos(gpos)
spring.stretch(top, gpos)
vp.point(gpos + Ls*norm(Lrot), r=1, c='g') # add trace point to show in the end
class VirtualKeyboard:
def __init__(self, songname=''):
self.KB = dict()
self.vp = None
self.rightHand = None
self.leftHand = None
self.vpRH = None
self.vpLH = None
self.playsounds = True
self.verbose = True
self.songname = songname
self.t0 = 0 # keep track of how many seconds to play
self.dt = 0.1
self.speedfactor = 1
self.engagedfingersR = [False]*6 # element 0 is dummy
self.engagedfingersL = [False]*6
self.engagedkeysR = []
self.engagedkeysL = []
self.build_keyboard()
#######################################################
def makeHandActor(self, f=1):
a1, a2, a3, c = (10*f,0,0), (0,7*f,0), (0,0,3*f), (.7,0.3,0.3)
palm = ellipsoid(pos=(0,-3,0), axis1=a1, axis2=a2, axis3=a3, alpha=0.6, c=c)
wrist= box(pos=(0,-9,0), length=6*f, width=5, height=2, alpha=0.4, c=c)
arm = makeAssembly([palm,wrist])
self.vp.actors.append(arm) # add actor to internal list
f1 = self.vp.cylinder((-2, 1.5,0), axis=(0,1,0), height=5, r=.8*f, c=c)
f2 = self.vp.cylinder((-1, 3 ,0), axis=(0,1,0), height=6, r=.7*f, c=c)
f3 = self.vp.cylinder(( 0, 4 ,0), axis=(0,1,0), height=6.2, r=.75*f, c=c)
f4 = self.vp.cylinder(( 1, 3.5,0), axis=(0,1,0), height=6.1, r=.7*f, c=c)
f5 = self.vp.cylinder(( 2, 2 ,0), axis=(0,1,0), height=5, r=.6*f, c=c)
return [arm, f1,f2,f3,f4,f5]
def build_RH(self, hand):
if self.verbose: print('Building Right Hand..')
self.rightHand = hand
f = utils.handSizeFactor(hand.size)
self.vpRH = self.makeHandActor(f)
for limb in self.vpRH: # initial x positions are superseded later
limb.x( limb.x()* 2.5 )
limb.addpos([16.5*5+1, -7.5, 3] )
def build_LH(self, hand): #########################
if self.verbose: print('Building Left Hand..')
self.leftHand = hand
f = utils.handSizeFactor(hand.size)
self.vpLH = self.makeHandActor(f)
for limb in self.vpLH:
limb.x( limb.x()* -2.5 ) #flip
limb.addpos([16.5*3+1, -7.5, 3] )
#######################################################
def build_keyboard(self):
if self.verbose: print('Building Keyboard..')
nts = ("C","D","E","F","G","A","B")
tol = 0.12
keybsize = 16.5 # in cm, span of one octave
wb = keybsize/7
nr_octaves = 7
span = nr_octaves*wb*7
self.vp = Plotter(title='PianoPlayer '+__version__, axes=0, size=(1400,700), bg='lb', verbose=0)
#wooden top and base
self.vp.box(pos=(span/2+keybsize, 6, 1), length=span+1, height=3, width= 5, texture='wood5') #top
self.vp.box(pos=(span/2+keybsize, 0, -1), length=span+1, height=1, width=17, texture='wood5')
self.vp.text('PianoPlayer '+__version__, pos=(18, 5.5, 2), depth=.7)
self.vp.text('https://github.com/marcomusy/pianoplayer', pos=(105,4.8,2), depth=.7, s=.8)
leggio = self.vp.box(pos=(span/1.55,8,10), length=span/2, height=span/8, width=0.08, c=(1,1,0.9))
leggio.rotateX(-20)
self.vp.text('Playing\n\n'+self.songname, s=1.2).rotateX(70).pos([49,11,9])
for ioct in range(nr_octaves):
for ik in range(7): #white keys
x = ik * wb + (ioct+1)*keybsize +wb/2
tb = self.vp.box(pos=(x,-2,0), length=wb-tol, height=1, width=12, c='white')
self.KB.update({nts[ik]+str(ioct+1) : tb})
if not nts[ik] in ("E","B"): #black keys
tn=self.vp.box(pos=(x+wb/2,0,1), length=wb*.6, height=1, width=8, c='black')
self.KB.update({nts[ik]+"#"+str(ioct+1) : tn})
self.vp.show(interactive=0)
self.vp.camera.Azimuth(4)
self.vp.camera.Elevation(-30)
#####################################################################
def play(self):
printc('Press [0-9] to proceed by one note or for more seconds',1)
printc('Press Esc to exit.',1)
self.vp.keyPressFunction = runTime # enable observer
if self.rightHand:
self.engagedkeysR = [False]*len(self.rightHand.noteseq)
self.engagedfingersR = [False]*6 # element 0 is dummy
if self.leftHand:
self.engagedkeysL = [False]*len(self.leftHand.noteseq)
self.engagedfingersL = [False]*6
t=0.0
while True:
if self.rightHand: self._moveHand( 1, t)
if self.leftHand: self._moveHand(-1, t)
if t > 1000: break
t += self.dt # absolute time flows
if self.verbose: printc('End of note sequence reached.')
self.vp.keyPressFunction = None # disable observer
###################################################################
def _moveHand(self, side, t):############# runs inside play() loop
if side == 1:
c1,c2 = 'tomato', 'orange'
engagedkeys = self.engagedkeysR
engagedfingers = self.engagedfingersR
H = self.rightHand
vpH = self.vpRH
else:
c1,c2 = 'purple', 'mediumpurple'
engagedkeys = self.engagedkeysL
engagedfingers = self.engagedfingersL
H = self.leftHand
vpH = self.vpLH
for i, n in enumerate(H.noteseq):#####################
start, stop, f = n.time, n.time+n.duration, n.fingering
if isinstance(f, str): continue
if f and stop <= t <= stop+self.dt and engagedkeys[i]: #release key
engagedkeys[i] = False
engagedfingers[f] = False
name = nameof(n)
krelease(self.KB[name])
frelease(vpH[f])
self.vp.interactor.Render()
for i, n in enumerate(H.noteseq):#####################
start, stop, f = n.time, n.time+n.duration, n.fingering
if isinstance(f, str):
print('Warning: cannot understand lyrics:',f, 'skip note',i)
continue
if f and start <= t < stop and not engagedkeys[i] and not engagedfingers[f]: #press key
if i >= len(H.fingerseq): return
engagedkeys[i] = True
engagedfingers[f] = True
name = nameof(n)
if t> self.t0 + self.vp.clock:
self.t0 = t
self.vp.show(zoom=2, interactive=True)
for g in [1,2,3,4,5]:
vpH[g].x( side * H.fingerseq[i][g] )
vpH[0].x(vpH[3].x()) # index 0 is arm, put it where middle finger is
fpress(vpH[f], c1)
kpress(self.KB[name], c2)
self.vp.show(zoom=2, interactive=False)
if self.verbose:
msg = 'meas.'+str(n.measure)+' t='+str(round(t,2))
if side==1: printc((msg,'\t\t\t\tRH.finger', f, 'hit', name), 'b')
else: printc((msg, '\tLH.finger', f, 'hit', name), 'm')
if self.playsounds:
playSound(n, self.speedfactor)
else:
time.sleep(n.duration*self.speedfactor)
L = 0.1 # spring x position at rest
x0 = 0.85 # initial x-coordinate of the block
k = 25 # spring constant
m = 20 # block mass
b = 0.5 # viscosity friction (proportional to velocity)
dt= 0.15 # time step
#initial conditions
v = vector(0, 0, 0.2)
x = vector(x0, 0, 0)
xr = vector(L, 0, 0)
sx0 = vector(-0.8, 0, 0)
offx= vector(0, 0.3, 0)
vp.box(pos=(0, -0.1, 0), length=2.0, width=0.02, height=0.5) #surface
vp.box(pos=(-.82,.15,0), length=.04, width=0.50, height=0.3) #wall
block = vp.cube(pos=x, length=0.2, c='tomato')
block.addTrail(offset=[0,0.2,0], alpha=0.6, lw=2, n=500)
spring= vp.helix(sx0, x, r=.06, thickness=.01, texture='metal1')
pb = ProgressBar(0,300, c='r')
for i in pb.range():
F = -k*(x-xr) - b*v # Force and friction
a = F/m # acceleration
v = v + a * dt # velocity
x = x + v*dt + 1/2 * a * dt**2 # position
block.pos(x) # update block position and trail
spring.stretch(sx0, x) # stretch helix accordingly
# Create the initial positions and velocitites (0,0) of the bobs
bob_x = [0]
bob_y = [0]
x_dot = [0] * (N + 1) #velocities
y_dot = [0] * (N + 1)
for k in range(1, N + 1):
alpha = np.pi / 5 * k / 10
bob_x.append(bob_x[k - 1] + np.cos(alpha) + np.random.normal(0, .1))
bob_y.append(bob_y[k - 1] + np.sin(alpha) + np.random.normal(0, .1))
# Create the bobs
vp = Plotter(title="Multiple Pendulum", axes=0, verbose=0)
vp.box(pos=(bob_x[0], bob_y[0], 0),
length=12,
width=12,
height=.7,
c='k',
wire=1)
bob = [vp.sphere(pos=(bob_x[0], bob_y[0], 0), r=R / 2, c='gray')]
for k in range(1, N + 1):
bob.append(vp.cylinder(pos=(bob_x[k], bob_y[k], 0), r=R, height=.3, c=k))
# Create the springs out of N links
link = [0] * N
for k in range(N):
p0 = bob[k].pos()
p1 = bob[k + 1].pos()
link[k] = vp.helix(p0, p1, thickness=.015, r=R / 3, c='gray')
# Create some auxiliary variables
x_dot_m = [0] * (N + 1)
