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