Skip to content

Instantly share code, notes, and snippets.

@martinegginton
Created July 25, 2016 13:16
Show Gist options
  • Save martinegginton/f09db0a089c3a7ce0d61267812c46785 to your computer and use it in GitHub Desktop.
Save martinegginton/f09db0a089c3a7ce0d61267812c46785 to your computer and use it in GitHub Desktop.
pend.py
# coding: utf-8
from scene import *
import sound
import random
import math
import time
import ui
import numpy as np
import pendulum
reload(pendulum)
A = Action
class Pend (Node):
def __init__(self,*args,**kwargs):
Node.__init__(self)
self.x0 = args[0]
self.y0 = args[1]
self.p = pendulum.Pendulum()
pth = ui.Path.rounded_rect(0,0,80,30,2)
self.cart = ShapeNode(path=pth)
self.add_child(self.cart)
self.l = self.p.l*phy2pix
self.rpth = ui.Path()
self.rod = ShapeNode(path=pth)
self.rod.stroke_color = '#ffffff'
self.add_child(self.rod)
pth = ui.Path.oval(0,0,30,30)
self.pend = ShapeNode(path=pth)
self.add_child(self.pend)
pth = ui.Path.oval(0,0,10,10)
self.pnt = ShapeNode(path=pth)
self.add_child(self.pnt)
def init_position(self,theta,x=0):
self.p.init(theta,x/phy2pix)
self.update_position()
def set_cart_position(self,x):
swt = True # method to control
x = (x - self.x0)/phy2pix
if swt:
# back calculate required accleration
self.p.set_pos(x)
else:
# Direct force input - not practical
self.p.set_force(x/0.001)
def update_position(self):
x0 = self.p.x*phy2pix + self.x0
y0 = self.y0
x = x0
c = self.cart.path.bounds
self.cart.position = (x,y0)
c = self.pend.path.bounds
t = self.p.theta
x = x + np.sin(t)*self.l
y = y0 + np.cos(t)*self.l
self.pend.position = (x,y)
# position is center
pth = ui.Path()
pth.line_width = 5
dx = np.sin(t)*self.l
dy = np.cos(t)*self.l
#print (dx,dy)
pth.line_to(dx,-dy)
pth.stroke()
self.rod.path = pth
self.rod.position = (x0+dx/2,y0+dy/2)
self.pnt.position = (x0+dx/2,y0+dy/2)
self.p.set_force(0)
class IP (Scene):
def setup(self):
self.root = Node(parent=self)
self.updt = 0
(w,h) = self.size
#print (w,h)
global phy2pix
phy2pix = w/0.3 # physical conv
(x0,y0) = (w/2,h/3) # center
self.pend = Pend(x0,y0)
self.pend.init_position(0,0)
self.root.add_child(self.pend)
def did_change_size(self):
pass
def update(self):
dt = self.dt
self.pend.p.update_states(dt)
if self.updt > 0.01:
self.pend.update_position()
self.updt = 0
else:
self.updt += dt
def touch_began(self, touch):
pass
def touch_moved(self, touch):
loc = touch.prev_location
if self.pend.cart.frame.contains_point(loc):
# previously on the cart
loc = touch.location
#print loc
self.pend.set_cart_position(loc.x)
def touch_ended(self, touch):
pass
if __name__ == '__main__':
run(IP(), show_fps=False)
# coding: utf-8
import time
import numpy as np
import matplotlib.pyplot as plt
g = 9.81
class Pendulum (object):
def __init__(self,dt=0.001):
# inputs
self.u = 0 # cart force
# states
self.x = 0
self.v = 0
self.theta = 0
self.omega = 0
# parameters - SI
self.M = 10
self.m = 0.5
self.l = 0.1
self.coefv = 100
# variables
self.V = 0
self.H = 0
self.I = self.m * self.l**2
def set_force(self,F):
# Use function to allow for checks on range etc
self.u = F
print self.u
def set_pos(self,x,dt=0.01):
# Move cart manually - target accel
a = ((x-self.x)/dt-self.v)/dt
print 'accel {} m/s2'.format(a)
a = max(min(a,10),-10)
self.u = self.M*a # F=ma
print self.u
def update_states(self,dt=0.001):
# Should be called each time step
alpha = self.calc_alpha()
a = self.calc_acc()
# update states
self.omega += alpha*dt
self.theta += self.omega*dt
self.v += a*dt
self.x += self.v*dt
def calc_alpha(self):
cos_t = np.cos(self.theta) # rads
sin_t = np.sin(self.theta)
num = self.u*cos_t - (self.M+self.m)*g*sin_t + (self.m*self.l)*(cos_t*sin_t)*self.omega
den = (self.m*self.l)*cos_t**2 - (self.M+self.m)*self.l
return num/den
def calc_acc(self):
cos_t = np.cos(self.theta) # rads
sin_t = np.sin(self.theta)
num = self.u + (self.m*self.l*sin_t)*self.omega**2 -(self.m*g*cos_t*sin_t) - self.v*self.coefv
den = self.M + self.m -(self.m*cos_t**2)
return num/den
def init(self,theta,x=0,v=0,omega=0):
self.x = x
self.v = v
self.theta = theta
self.omega = omega
if __name__=='__main__':
p = Pendulum()
p.init(0.01)
x_data = [0.0]
y_data1 = [0.0]
y_data2 = [0.0]
tmStep = 0.001
dt = tmStep
mx = dt
t0 = time.time()
tm = 0
while tm <= 5:
if tm > 3:
p.set_force(0)
elif tm > 3.05:
p.set_force(0)
tm += dt
p.update_states(dt)
t1 = time.time()
dt = t1 - t0
mx = max(dt,mx)
t0 = t1
x_data.append(tm)
y_data1.append(p.theta*180/np.pi)
y_data2.append(p.x)
#print 'Position of Cart: {} at {} s'.format(p.x, tm)
print 'Max dt: {}ms'.format(np.floor(mx*1e3))
fig = plt.figure()
ax = fig.add_subplot(211)
ax.plot(x_data,y_data1)
ax = fig.add_subplot(212)
ax.plot(x_data,y_data2)
plt.show()
# coding: utf-8
#!/usr/bin/python
import time
def procedure():
time.sleep(2.5)
# measure process time
t0 = time.clock()
procedure()
print time.clock() - t0, "seconds process time"
# measure wall time
t0 = time.time()
procedure()
print time.time() - t0, "seconds wall time"
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment