• Sign Up
• Login
• API
• FAQ
• Tools
• Archive
daily pastebin goal
75%
SHARE
TWEET

# Untitled

a guest Mar 26th, 2019 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. # Mechanical and geometric constants of the woodpecker model
2. # according to
3. # Ch. Glocker: Dynamik von Starrkörpersystemen mit Reibung und Stössen
4. # PhD thesis TU München, July 1995, pp. 162ff
5. #
6. import numpy as np
7.
8. from assimulo.problem import Implicit_Problem
9. from assimulo.solvers import IDA
10.
11.
12. mS = 3.0e-4 # Mass of sleeve [kg]
13. JS = 5.0e-9 # Moment of inertia of the sleeve [kgm]
14. mB = 4.5e-3 # Mass of bird [kg]
15. masstotal=mS+mB # total mass
16. JB = 7.0e-7 # Moment of inertia of bird [kgm]
17. r0 = 2.5e-3 # Radius of the bar [m]
18. rS = 3.1e-3 # Inner Radius of sleeve [m]
19. hS = 5.8e-3 # 1/2 height of sleeve [m]
20. lS = 1.0e-2 # verical distance sleeve origin to spring origin [m]
21. lG = 1.5e-2 # vertical distance spring origin to bird origin [m]
22. hB = 2.0e-2 # y coordinate beak (in bird coordinate system) [m]
23. lB = 2.01e-2 # -x coordinate beak (in bird coordinate system) [m]
24. cp = 5.6e-3 # rotational spring constant [N/rad]
25. g  = 9.81 #  [m/s^2]
26.
27. to_state=[0,0,0]
28. beak_hits=0
29.
30.
31. #x=[z
32. #   ps
33. #   pb
34. #   zd
35. #   psd
36. #   pbd
37. #   l1
38. #   l2]
39.
40. #sw = [state1,state2,state3]
41.
42. def residual(t,x,xd,sw):
43. #    print("t: ",t)
44.     M = np.array([[masstotal, mB*lS, mB*lG],
45.                [mB*lS, JS+mB*lS**2, mB*lS*lG],
46.                [mB*lG, mB*lS*lG, JB+mB*lG**2]])
47.     ps=x[1]
48.     pb=x[2]
49.     l1=x[6]
50.     l2=x[7]
51.     biss=xd[3:6]
52.     res=np.zeros(8)
53.     if sw[0]: #State 1
54.         #print(1)
55.         res[0:5] = np.hstack((np.dot(M,biss)-[-(mS+mB)*g,cp*(pb-ps)-mB*lS*g,cp*(pb-ps)-mB*lG*g],l1,l2))
56.     if sw[1]: #State 2
57.         #print(2)
58.         res=np.zeros(8)
59.         res[0]=mB*lG*biss[2]+(mS+mB)*g+l2
60.         res[1]=mB*lS*lG*biss[2] - (cp*(pb-ps)-mB*lS*g-hS*l1-rS*l2)
61.         res[2]=(JB+mB*lG*lG)*biss[2] - (cp*(ps-pb)-mB*lG*g)
62. #        res[0:3] = np.hstack((np.dot(M,biss)-[-(mS+mB)*g-l2,cp*(pb-ps)-mB*lS*g-hS*l1-rS*l2,cp*(ps-pb)-mB*lG*g]))
63.         res[3]=(rS-r0)+hS*ps
64.         res[4]=x[3]+rS*x[4]
65.     if sw[2]: #State 3
66. #        print(3)
67.         res[0]=mB*lG*biss[2]+(mS+mB)*g+l2
68.         res[1]=mB*lS*lG*biss[2] - (cp*(pb-ps)-mB*lS*g+hS*l1-rS*l2)
69.         res[2]=(JB+mB*lG*lG)*biss[2] - (cp*(ps-pb)-mB*lG*g)
70. #        res[0:3] = np.hstack((np.dot(M,biss)-[-(mS+mB)*g-l2,cp*(pb-ps)-mB*lS*g+hS*l1-rS*l2,cp*(ps-pb)-mB*lG*g]))
71.         res[3]=(rS-r0)-hS*ps
72.         res[4]=x[3]#+rS*x[4]
73.
74.     res[5]=x[3]-xd[0]
75.     res[6]=x[4]-xd[1]
76.     res[7]=x[5]-xd[2]
77.     #print("res: ",res)
78.     #print("x: ", x)
79.     return res
80.
81. def state_events(t,x, xd,sw):
82.     """
83.     This is the function that keeps track of  events. When the sign
84.     of any of the functions changed, we have an event.
85.     """
86.     state_event=np.zeros(2)
87.     if sw[0]: #state 1:
88.         ps=x[1]
89.         state_event[0]=hS*ps+rS-r0
90.         state_event[1]=hS*ps-rS+r0
91.     if sw[1]: #state 2:
92.         l1=x[6]
93.         state_event[0]=l1
94.         state_event[1]=l1 # It has to be something ;)
95.     if sw[2]: #state 3:
96.         l1=x[6]
97.         pb=x[2]
98.         state_event[0]=l1
99.         state_event[1]=hB*pb-(lS+lG-lB-r0)
100.     return state_event
101.
102.
103. def handle_event(solver, event_info):
104.     """
105.     Event handling. This functions is called when Assimulo finds an event as
106.     specified by the event functions.
107.     """
108.     global to_state
109.     global beak_hits
110.
111.     state_info = event_info[0]
112.     print("Handle event, state info: ", state_info, ", t: ", solver.t)
113.     if solver.sw[0]: #State 1:
114.         zd,psd,pbd=solver.y[3:6]
115.         if state_info[0]!=0 and pbd<0:
116.             print("Switch to state 2")
117.             solver.sw[1]=True
118.             solver.sw[0]=False
119.             solver.y[1]=-(rS-r0)/hS
120.             solver.y[3]=0
121.             solver.y[4]=0
122.             solver.y[5]=(mB*lG*zd+mB*lS*lG*psd)/(JB+mB*lG*lG)+pbd
123.             to_state[1]+=1
124.         elif state_info[1]!=0 and pbd>0: #Switch to state 3
125.             print("Switch to state 3")
126.             solver.sw[2]=True
127.             solver.sw[0]=False
128.             solver.y[1]=(rS-r0)/hS
129.             solver.y[3]=0
130.             solver.y[4]=0
131.             solver.y[5]=(mB*lG*zd+mB*lS*lG*psd)/(JB+mB*lG*lG)+pbd
132.             to_state[2]+=1
133.     elif solver.sw[1]: #State 2:
134.         print("Switch to state 1")
135.         solver.sw[0]=True
136.         solver.sw[1]=False
137.         to_state[0]+=1
138.     elif solver.sw[2]: #State 3:
139.         pbd=solver.y[5]
140.         if state_info[0]!=0 and pbd<0: #Switch to state 1
141.             print("Switch to state 1")
142.             solver.sw[0]=True
143.             solver.sw[2]=False
144.             to_state[0]+=1
145.         elif state_info[1]!=0 and pbd>0: #change sign of pbd
146.             print("Change sign of pbd")
147.             solver.y[5]=-solver.y[5]
148.             solver.yd[2]=solver.y[5]
149.             beak_hits+=1
150.     solver.yd[0:3]=solver.y[3:6]
151.
152. #Initial values
153. x0 = np.zeros(8)
154. x0[0]=1
155. x0[5]=0.1
156. x0[2]=0.3
157. xd0 = np.zeros(8)
158. xd0[0:3]=x0[3:6]
159. t0 = 0.0
160. switches0=[True]+2*[False]
161.
162. #Create an Assimulo Problem
163. problem = Implicit_Problem(residual, x0, xd0, t0, sw0=switches0)
164.
165. problem.state_events = state_events #Sets the state events to the problem
166. problem.handle_event = handle_event #Sets the event handling to the problem
167. problem.name = 'Wood Pecker'   #Sets the name of the problem
168.
169. #Create an Assimulo solver (CVode)
170. sim = IDA(problem)
171. sim.atol=6*[1e-5]+2*[1]
172. sim.algvar=6*[0]+2*[1]
173. #sim.suppress_alg=True
174.
175. #Simulation
176. tfinal = 0.5#Final time
177.
178. sim.simulate(tfinal) #Simulate
179. plotvar=8*[0]
180. plotvar[0]=1
181. sim.plot(plotvar)
182. print("Beak hits: ", beak_hits)
183. print("To state count:", to_state)
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top