HydraulicsUserFunction.py
You can view and download this file on Github: HydraulicsUserFunction.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: A one arm mechanism is actuated by a hydraulics actuator;
5# Hydraulics is emulated by a GenericODE1 object for hydraulics pressure equations,
6# a spring-damper user function applies the hydraulic force
7#
8# Author: Johannes Gerstmayr
9# Date: 2022-05-23
10#
11# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
12#
13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
15import exudyn as exu
16from exudyn.itemInterface import *
17from exudyn.utilities import *
18
19import numpy as np
20from math import sin, cos, sqrt,pi
21
22SC = exu.SystemContainer()
23mbs = SC.AddSystem()
24
25L = 1 #x-dim of arm
26b = 0.1 #y-dim of arm
27
28
29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30#one arm mechanism
31background = GraphicsDataCheckerBoard(point=[0,0.5*L*0,-2*b],size=2)
32oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], visualization=VObjectGround(graphicsData= [background])))
33massRigid = 12*10
34inertiaRigid = massRigid/12*(L)**2
35g = 9.81 # gravity
36
37graphicsList = [GraphicsDataOrthoCubePoint(size= [L,b,b], color= color4dodgerblue, addEdges=True)]
38
39graphicsList += [GraphicsDataCylinder(pAxis=[-0.5*L,0,-0.7*b], vAxis= [0,0,1.4*b], radius = 0.55*b,
40 color= color4lightgrey, addEdges=True, nTiles=32)]
41#print(graphicsList[2])
42nRigid = mbs.AddNode(Rigid2D(referenceCoordinates=[0.5*L,0,0], initialVelocities=[0,0,0]));
43oRigid = mbs.AddObject(RigidBody2D(physicsMass=massRigid, physicsInertia=inertiaRigid,nodeNumber=nRigid,
44 visualization=VObjectRigidBody2D(graphicsData= graphicsList)))
45
46mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[-0.5*L,0.,0.])) #support point
47mR2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[ 0.,0.,0.])) #end point
48
49#add joint
50mG0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[0,0,0]))
51mbs.AddObject(RevoluteJoint2D(markerNumbers=[mG0,mR1]))
52
53mbs.AddLoad(Force(markerNumber = mR2, loadVector = [0, -massRigid*g, 0]))
54
55#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
56#add hydraulics actuator:
57mGH = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[0,-0.25*L-0.5*b*0,0.]))
58mRH = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[-0.25*L,-0.5*b*0,0.]))
59
60
61LH0 = sqrt(2*(0.25*L)**2) #zero length of actuator
62
63#hydraulics parameters:
64V0 = 1. #oil volume (could actually change ...)
65V1 = V0 #oil volume (could actually change ...)
66A=[0.01,0.01] #piston area side 1/2
67Eoil = 1e11
68Av1 = 1 #valve opening (factor)
69Av2 = 0.0 #valve opening (factor)
70Qn = 2e-5 #nominal flow
71pS = 200.*1e5 #system pressure (200bar)
72pT = 0.*1e5 #tank pressure;
73dampingHA = 2e5
74
75Av0 = 0
76Av1 = 0
77
78#defines relative displacement, relative velocity, stiffness k, damping d, and additional spring force f0
79def springForce(mbs, t, itemIndex, u, v, k, d, f0):
80
81 p = mbs.GetObjectOutput(oGenericODE1, variableType=exu.OutputVariableType.Coordinates)
82 F = -p[0]*A[0] + p[1]*A[1] + v*d #tension force is positive, p0>0 acts as compression force, p1>0 is a tension force
83
84 return F
85
86def SignedSqrt(x):
87 return np.sign(x)*np.sqrt(abs(x))
88
89#compute pressure updates
90def UFrhs(mbs, t, itemNumber, q):
91 LHact = mbs.GetObjectOutput(oHA, variableType=exu.OutputVariableType.Distance)
92 uSD = mbs.GetObjectOutput(oHA, variableType=exu.OutputVariableType.Displacement)
93 vSD = mbs.GetObjectOutput(oHA, variableType=exu.OutputVariableType.Velocity)
94 vAct = 1/LHact*uSD@vSD
95 #print('v=',vAct)
96
97
98 #print(Av1)
99 p = q #p is pressure
100 p_t = np.zeros(2) #time derivatives of pressure
101
102 #Av0 and Av1 set in PreStepUserFunction
103 if Av0 >= 0:
104 p_t[0] = Eoil/V0*(-A[0]*vAct + Av0*Qn*SignedSqrt(pS-p[0])) #abs just for safety
105 else:
106 p_t[0] = Eoil/V0*(-A[0]*vAct + Av0*Qn*SignedSqrt(p[0]-pT)) #abs just for safety
107
108 if Av1 >= 0:
109 p_t[1] = Eoil/V1*( A[1]*vAct + Av1*Qn*SignedSqrt(pS-p[1])) #abs just for safety
110 else:
111 p_t[1] = Eoil/V1*( A[1]*vAct + Av1*Qn*SignedSqrt(p[1]-pT)) #abs just for safety
112
113 # print('p_t=',p_t)
114 return p_t
115
116
117
118
119#add spring damper which emulates hydraulic cylinder with user function; stiffness is only used if user function=0
120oHA = mbs.AddObject(ObjectConnectorSpringDamper(markerNumbers=[mGH, mRH], stiffness=2e6,
121 damping=dampingHA, force=0, referenceLength=LH0,
122 springForceUserFunction = springForce,
123 visualization=VSpringDamper(drawSize = 0.5*b),
124 ))
125
126
127#hydraulics objects:
128#ODE1 for pressure:
129nODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0,0],
130 initialCoordinates=[2e6,2e6], #initialize with 20 bar
131 numberOfODE1Coordinates=2))
132
133#add some simpistic trajectory and valve control
134def PreStepUserFunction(mbs, t):
135 LHact = mbs.GetObjectOutput(oHA, variableType=exu.OutputVariableType.Distance)
136 x = (max(0.5, min(1.5,(1-cos(t*pi*2*0.5))) ) - 0.5)*0.1+LH0
137 #if t>2: x=LH0
138 global Av0, Av1
139
140 Av0 = (x-LHact)*2 #valve position control ==> penalize set value LH0
141 #print('Av0=',Av0)
142 Av1 = -Av0
143 return True
144
145mbs.SetPreStepUserFunction(PreStepUserFunction)
146
147
148#now add object instead of object in mini-example:
149oGenericODE1 = mbs.AddObject(ObjectGenericODE1(nodeNumbers=[nODE1],rhsUserFunction=UFrhs))
150
151
152
153sForce = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Force))
154sDistance = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Distance))
155sVelocity = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Velocity))
156sPressures = mbs.AddSensor(SensorNode(nodeNumber=nODE1, storeInternal=True, outputVariableType=exu.OutputVariableType.Coordinates))
157
158mbs.Assemble()
159
160#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
161
162simulationSettings = exu.SimulationSettings() #takes currently set values or default values
163
164
165tEnd = 0.4
166stepSize = 1e-3
167simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
168simulationSettings.timeIntegration.endTime = tEnd
169simulationSettings.timeIntegration.startTime = 0
170simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100 #10000
171simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
172simulationSettings.timeIntegration.verboseMode = 1
173# simulationSettings.timeIntegration.simulateInRealtime = True #to see what happens ...
174
175simulationSettings.timeIntegration.newton.useModifiedNewton = True
176simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
177simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
178simulationSettings.displayStatistics = True
179
180simulationSettings.solutionSettings.solutionInformation = 'Hydraulics user function test'
181
182SC.visualizationSettings.openGL.multiSampling = 4
183SC.visualizationSettings.openGL.lineWidth = 2
184
185exu.StartRenderer()
186mbs.WaitForUserToContinue()
187
188mbs.SolveDynamic(simulationSettings, showHints=False)
189
190SC.WaitForRenderEngineStopFlag()
191exu.StopRenderer() #safely close rendering window!
192
193print('hydraulics user function:')
194print('pressures=', mbs.GetSensorValues(sPressures))
195print('velocity=', mbs.GetSensorValues(sVelocity))
196#for 1e-6: with initialVelocities=[0,0,2]
197# hydraulics user function:
198# pressures= [6441369.55769344 3008417.92678142]
199# velocity= [-0.00500595 0.20338301 0. ]
200
201
202mbs.PlotSensor(sensorNumbers=sForce, components=exudyn.plot.componentNorm, labels=['connector force norm'], yLabel='force (N)', closeAll=False)
203
204
205mbs.PlotSensor(sensorNumbers=sDistance, components=0)
206mbs.PlotSensor(sensorNumbers=[sPressures]*2, components=[0,1], labels=['p1', 'p2'], yLabel='pressure (N/m^2)')
207
208p01 = mbs.GetSensorStoredData(sPressures)
209p01[:,1] = A[0]*p01[:,1] - A[1]*p01[:,2]
210mbs.PlotSensor(sensorNumbers=p01, components=0, labels=['differential hydraulic force'], yLabel='hydraulic force (N)')