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