pend.py

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)

pendulum.py

# 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()
	
	
		

tm.py

# 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"