HydraulicActuator2Arms.py
You can view and download this file on Github: HydraulicActuator2Arms.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: A two arm mechanism is actuated by the HydraulicActuatorSimple;
5# The actuator contains internal dynamics based on GenericODE1 node
6#
7# Author: Johannes Gerstmayr
8# Date: 2022-06-16
9#
10# 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.
11#
12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
14import exudyn as exu
15from exudyn.utilities import *
16
17#import numpy as np
18from math import sin, cos, sqrt,pi
19
20SC = exu.SystemContainer()
21mbs = SC.AddSystem()
22
23L = 1 #x-dim of arm
24b = 0.1 #y-dim of arm
25addArm2 = True
26
27#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28#one arm mechanism
29background = [GraphicsDataCheckerBoard(point=[L,0,-2*b],size=5)]
30background += [GraphicsDataCylinder(pAxis=[0,-0.25*L-0.5*b,-0.5*b], vAxis= [0,0,1.*b], radius = 0.25*b,
31 color= color4grey, addEdges=True, nTiles=32)]
32
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
38#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
39#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
40#arm1
41#graphics for arm1
42colCyl = color4orange
43colArm = color4dodgerblue
44graphicsList = [GraphicsDataOrthoCubePoint(size= [L,0.75*b,1.4*b], color= colArm, addEdges=True)]
45
46graphicsList += [GraphicsDataCylinder(pAxis=[-0.5*L,0,-0.75*b], vAxis= [0,0,1.5*b], radius = 0.55*b,
47 color= colArm, addEdges=True, nTiles=32)]
48
49graphicsList += [GraphicsDataCylinder(pAxis=[-0.5*L,0,-0.8*b], vAxis= [0,0,1.6*b], radius = 0.25*b,
50 color= color4grey, addEdges=True, nTiles=32)]
51
52#bolt
53graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b,-0.7*b], vAxis= [0,0,1.4*b], radius = 0.15*b,
54 color= color4grey, addEdges=True, nTiles=32)]
55
56graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b,-0.6*b], vAxis= [0,0,0.25*b], radius = 0.3*b,
57 color= colArm, addEdges=True, nTiles=32)]
58graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b, 0.6*b], vAxis= [0,0,-0.25*b], radius = 0.3*b,
59 color= colArm, addEdges=True, nTiles=32)]
60
61if addArm2:
62 graphicsList += [GraphicsDataCylinder(pAxis=[ 0.25*L,-0.5*b,-0.7*b], vAxis= [0,0,1.4*b], radius = 0.15*b,
63 color= color4grey, addEdges=True, nTiles=32)]
64
65 graphicsList += [GraphicsDataCylinder(pAxis=[ 0.25*L,-0.5*b,-0.6*b], vAxis= [0,0,0.25*b], radius = 0.3*b,
66 color= colArm, addEdges=True, nTiles=32)]
67 graphicsList += [GraphicsDataCylinder(pAxis=[ 0.25*L,-0.5*b, 0.6*b], vAxis= [0,0,-0.25*b], radius = 0.3*b,
68 color= colArm, addEdges=True, nTiles=32)]
69
70#+++++++++++++++++++++++++++++++++++++++++++++++++++
71
72#print(graphicsList)
73nRigid = mbs.AddNode(Rigid2D(referenceCoordinates=[0.5*L,0,0], initialVelocities=[0,0,0]));
74oRigid = mbs.AddObject(RigidBody2D(physicsMass=massRigid, physicsInertia=inertiaRigid,nodeNumber=nRigid,
75 visualization=VObjectRigidBody2D(graphicsData= graphicsList)))
76
77mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[-0.5*L,0.,0.])) #support point
78mCOM1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[ 0.,0.,0.]))
79mR1end = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[0.5*L,0.,0.])) #end point
80
81#add joint
82mG0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[0,0,0]))
83mbs.AddObject(RevoluteJoint2D(markerNumbers=[mG0,mR1]))
84
85mbs.AddLoad(Force(markerNumber = mCOM1, loadVector = [0, -massRigid*g, 0]))
86
87#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
88#add hydraulics actuator:
89mGH = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[0,-0.25*L-0.5*b,0.]))
90mRH = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[-0.25*L,-0.5*b,0.]))
91
92
93LH0 = sqrt(2*(0.25*L)**2) #zero length of actuator
94
95#hydraulics parameters:
96V0 = 1. #oil volume (could actually change ...)
97V1 = V0 #oil volume (could actually change ...)
98A=[0.01,0.01] #piston area side 1/2
99Eoil = 1e12
100Av1 = 1 #valve opening (factor)
101Av2 = 0.0 #valve opening (factor)
102Qn = 2e-5 #nominal flow
103pS = 200.*1e5 #system pressure (200bar)
104pT = 1e-16+0.*1e5 #tank pressure;
105actuatorDamping = 2e5
106
107#ODE1 for pressures:
108nODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0,0],
109 initialCoordinates=[2e6,2e6], #initialize with 20 bar
110 numberOfODE1Coordinates=2))
111
112oHA = mbs.AddObject(HydraulicActuatorSimple(markerNumbers=[mGH, mRH],
113 nodeNumbers=[nODE1],
114 offsetLength=LH0, strokeLength=LH0*0.7,
115 chamberCrossSection0=A[0], chamberCrossSection1=A[1],
116 hoseVolume0=V0, hoseVolume1=V1,
117 valveOpening0=0, valveOpening1=0,
118 oilBulkModulus=Eoil, actuatorDamping=actuatorDamping, nominalFlow=Qn,
119 systemPressure=pS, tankPressure=pT,
120 useChamberVolumeChange=False,
121 visualization=VHydraulicActuatorSimple(cylinderRadius= 0.55*b, rodRadius= 0.3*b,
122 baseMountLength = 0.4*b, baseMountRadius = 0.4*b,
123 rodMountRadius = 0.3*b, pistonLength = 0.2*b, pistonRadius = 0.5*b,
124 colorCylinder=colCyl, colorPiston=color4lightgrey),
125 ))
126
127
128#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
129#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
130#arm2
131#graphics for arm2
132oHA2 = -1
133if addArm2:
134 graphicsList = [GraphicsDataOrthoCubePoint(size= [L,0.75*b,1.4*b], color= colArm, addEdges=True)]
135
136 graphicsList += [GraphicsDataCylinder(pAxis=[-0.5*L,0,-0.75*b], vAxis= [0,0,1.5*b], radius = 0.55*b,
137 color= colArm, addEdges=True, nTiles=32)]
138
139 graphicsList += [GraphicsDataCylinder(pAxis=[-0.5*L,0,-0.8*b], vAxis= [0,0,1.6*b], radius = 0.25*b,
140 color= color4grey, addEdges=True, nTiles=32)]
141
142 #bolt
143 graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b,-0.7*b], vAxis= [0,0,1.4*b], radius = 0.15*b,
144 color= color4grey, addEdges=True, nTiles=32)]
145
146 graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b,-0.6*b], vAxis= [0,0,0.25*b], radius = 0.3*b,
147 color= colArm, addEdges=True, nTiles=32)]
148 graphicsList += [GraphicsDataCylinder(pAxis=[-0.25*L,-0.5*b, 0.6*b], vAxis= [0,0,-0.25*b], radius = 0.3*b,
149 color= colArm, addEdges=True, nTiles=32)]
150 #+++++++++++++++++++++++++++++++++++++++++++++++++++
151
152 #print(graphicsList)
153 nRigid2 = mbs.AddNode(Rigid2D(referenceCoordinates=[1.*L,-0.5*L,-0.5*pi], initialVelocities=[0,0,0]));
154 oRigid2 = mbs.AddObject(RigidBody2D(physicsMass=massRigid, physicsInertia=inertiaRigid,nodeNumber=nRigid2,
155 visualization=VObjectRigidBody2D(graphicsData= graphicsList)))
156
157 mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[-0.5*L,0.,0.])) #support point
158 mCOM2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[ 0.,0.,0.]))
159
160 #add joint
161 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR1end,mR1]))
162
163 mbs.AddLoad(Force(markerNumber = mCOM2, loadVector = [0, -massRigid*g, 0]))
164
165 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
166 #add hydraulics actuator:
167 mH12 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid, localPosition=[0.25*L,-0.5*b,0.]))
168 mH2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[-0.25*L,-0.5*b,0.]))
169
170
171 LH02 = sqrt(2*(0.25*L-0.5*b)**2) #zero length of actuator
172
173
174 #ODE1 for pressures:
175 nODE1_2 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0,0],
176 initialCoordinates=[2e6,2e6], #initialize with 20 bar
177 numberOfODE1Coordinates=2))
178
179 oHA2 = mbs.AddObject(HydraulicActuatorSimple(markerNumbers=[mH12, mH2],
180 nodeNumbers=[nODE1_2],
181 offsetLength=LH02, strokeLength=LH02*0.7,
182 chamberCrossSection0=A[0], chamberCrossSection1=A[1],
183 hoseVolume0=V0, hoseVolume1=V1,
184 valveOpening0=0, valveOpening1=0,
185 oilBulkModulus=Eoil, actuatorDamping=actuatorDamping, nominalFlow=Qn,
186 systemPressure=pS, tankPressure=pT,
187 useChamberVolumeChange=False,
188 visualization=VHydraulicActuatorSimple(cylinderRadius= 0.45*b, rodRadius= 0.2*b,
189 baseMountLength = 0.3*b, baseMountRadius = 0.3*b,
190 rodMountRadius = 0.2*b, pistonLength = 0.1*b, pistonRadius = 0.4*b,
191 colorCylinder=colCyl, colorPiston=color4lightgrey),
192 ))
193
194
195
196#add some simpistic trajectory and valve control
197def PreStepUserFunction(mbs, t):
198 LHact = mbs.GetObjectOutput(oHA, variableType=exu.OutputVariableType.Distance)
199 x = (max(0.5, min(1.5,(1-cos(t*pi*2*0.5))) ) - 0.5)*0.15+LH0
200
201 Av0 = (x-LHact)*2 #valve position control ==> penalize set value LH0
202 #print('Av0=',Av0)
203 Av1 = -Av0
204 mbs.SetObjectParameter(oHA, "valveOpening0", Av0)
205 mbs.SetObjectParameter(oHA, "valveOpening1", Av1)
206
207 if oHA2 != -1:
208 LHact2 = mbs.GetObjectOutput(oHA2, variableType=exu.OutputVariableType.Distance)
209 x = (max(0.5, min(1.5,(1-cos(2*t*pi*2*0.5))) ) - 0.5)*0.2+LH02
210 #if t>2: x=LH0
211
212 Av0 = (x-LHact2)*2 #valve position control ==> penalize set value LH0
213 #print('Av0=',Av0)
214 Av1 = -Av0
215 mbs.SetObjectParameter(oHA2, "valveOpening0", Av0)
216 mbs.SetObjectParameter(oHA2, "valveOpening1", Av1)
217
218 return True
219
220mbs.SetPreStepUserFunction(PreStepUserFunction)
221
222
223sForce = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Force))
224sDistance = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Distance))
225sPressures = mbs.AddSensor(SensorNode(nodeNumber=nODE1, storeInternal=True, outputVariableType=exu.OutputVariableType.Coordinates))
226
227sForce2 = mbs.AddSensor(SensorObject(objectNumber=oHA2, storeInternal=True, outputVariableType=exu.OutputVariableType.Force))
228sDistance2 = mbs.AddSensor(SensorObject(objectNumber=oHA2, storeInternal=True, outputVariableType=exu.OutputVariableType.Distance))
229sPressures2 = mbs.AddSensor(SensorNode(nodeNumber=nODE1_2, storeInternal=True, outputVariableType=exu.OutputVariableType.Coordinates))
230
231sVelocity = mbs.AddSensor(SensorObject(objectNumber=oHA, storeInternal=True, outputVariableType=exu.OutputVariableType.Velocity))
232
233mbs.Assemble()
234
235#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
236
237simulationSettings = exu.SimulationSettings() #takes currently set values or default values
238
239
240tEnd = 30
241stepSize = 0.001
242simulationSettings.solutionSettings.sensorsWritePeriod = 2*stepSize
243simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
244simulationSettings.timeIntegration.endTime = tEnd
245simulationSettings.timeIntegration.startTime = 0
246simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100 #10000
247simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
248simulationSettings.timeIntegration.verboseMode = 1
249#simulationSettings.timeIntegration.simulateInRealtime = True #to see what happens ...
250
251simulationSettings.timeIntegration.newton.useModifiedNewton = True
252simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
253simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
254simulationSettings.displayStatistics = True
255
256simulationSettings.solutionSettings.solutionInformation = 'Hydraulic actuator test'
257
258SC.visualizationSettings.openGL.multiSampling = 4
259SC.visualizationSettings.openGL.lineWidth = 2
260SC.visualizationSettings.openGL.shadow = 0.5
261SC.visualizationSettings.window.renderWindowSize = [1600,1200]
262
263exu.StartRenderer()
264mbs.WaitForUserToContinue()
265
266#use %timeit to measure time!
267mbs.SolveDynamic(simulationSettings, showHints=False)
268
269
270if True: #use this to reload the solution and use SolutionViewer
271 SC.visualizationSettings.general.autoFitScene = False
272
273 mbs.SolutionViewer() #can also be entered in IPython ...
274
275exu.StopRenderer() #safely close rendering window!
276
277
278mbs.PlotSensor(sensorNumbers=[sForce,sForce2], components=[exudyn.plot.componentNorm]*2, labels=['connector force arm1','connector force arm1'], yLabel='force (N)', closeAll=True)
279mbs.PlotSensor(sensorNumbers=[sDistance,sDistance2], components=0)
280mbs.PlotSensor(sensorNumbers=[sPressures]*2+[sPressures2]*2, components=[0,1,0,1], labels=['p0 arm1', 'p1 arm1', 'p0 arm2', 'p1 arm2'], yLabel='pressure (N/m^2)')
281
282#p01 = mbs.GetSensorStoredData(sPressures)
283#p01[:,1] = A[0]*p01[:,1] - A[1]*p01[:,2]
284#mbs.PlotSensor(sensorNumbers=p01, components=0, labels=['differential hydraulic force'], yLabel='hydraulic force (N)')