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