SHARE
TWEET

Untitled

a guest Mar 26th, 2019 72 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. OK, I Understand
 
Top