lugreFrictionTest.py
You can view and download this file on Github: lugreFrictionTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: This model reproduces the results of Canudas de Wit et al. (1995),
5# A New Model for Control of Systems with Friction,
6# IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 40, NO. 3, MARCH 1995
7# uses exactly same ODE1 model, and compares to position based friction model
8#
9# Author: Johannes Gerstmayr
10# Date: 2022-03-01
11#
12# 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.
13#
14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
16import exudyn as exu
17from exudyn.itemInterface import *
18from exudyn.utilities import *
19
20import numpy as np
21from math import sin, cos, exp, sqrt, pi
22
23SC = exu.SystemContainer()
24mbs = SC.AddSystem()
25exu.Print('EXUDYN version='+exu.GetVersionString())
26
27#++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28#Lugre friction text model: Canudas de Wit et al. (1995):
29M=1
30K=2
31sigma0=1e5
32sigma1=sqrt(sigma0)
33sigma2=0.4
34Fc=1
35Fs=1.5
36Vs=0.001
37
38useLugre = True #compute ODE1 Lugre model
39useLugreRef = False #store as reference solution (with small step size)
40useLugrePos = True #alternative: uses a position level approach, being much more efficient for implicit solvers
41useLugreFast = False #with higher stiffness, but shorter time; shows good agreement, but requires extremely small time steps
42doImplicit = True #use implicit time integration
43
44#faster version with higher spring stiffness and "friction" stiffness sigma0 ==> gives closer results to idealized case:
45if useLugreFast:
46 K=100
47 sigma0 = 1e7 #for LugrePos works also well with 1e6 and (with some step reductions) with 1e5
48 sigma1=sqrt(sigma0)
49
50if useLugre:
51 nODE1=3 #U,V,Z
52 qInit = [0]*nODE1
53 #qInit[0] = 1
54 nodeODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0]*nODE1,
55 initialCoordinates=qInit,
56 numberOfODE1Coordinates=nODE1))
57
58 def UFode1(mbs, t, itemNumber, q):
59 qt=np.zeros(nODE1)
60 U=0.1*t
61 FL=0
62 X=q[0]
63 V=q[1]
64 Z=q[2]
65 G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
66
67 Z_t=V-Z*abs(V)/G
68 FL=sigma0*Z+sigma1*Z_t+sigma2*V
69
70 qt[0] = V
71 qt[1] = (K*(U-X) - FL)/M
72 qt[2] = Z_t
73 #print('qt=',qt)
74 return qt
75
76 oGenericODE1 = mbs.AddObject(ObjectGenericODE1(nodeNumbers=[nodeODE1],
77 rhsUserFunction=UFode1))
78
79
80 sCoords1 = mbs.AddSensor(SensorNode(nodeNumber = nodeODE1,
81 storeInternal=True,
82 fileName='solution/lugreCoords'+'Ref'*useLugreRef+'.txt',
83 outputVariableType=exu.OutputVariableType.Coordinates))
84
85 def UFsensorFrictionForce(mbs, t, sensorNumbers, factors, configuration):
86 q = mbs.GetSensorValues(sensorNumbers[0])
87 X=q[0]
88 V=q[1]
89 Z=q[2]
90 G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
91
92 Z_t=V-Z*abs(V)/G
93 FL=sigma0*Z+sigma1*Z_t+sigma2*V
94 return [FL]
95
96 sFriction1 = mbs.AddSensor(SensorUserFunction(sensorNumbers=[sCoords1],
97 fileName='solution/lugreForce'+'Ref'*useLugreRef+'.txt',
98 storeInternal=True,sensorUserFunction=UFsensorFrictionForce))
99 #ODE23 integrator, aTol=rTol=1e-8:
100 #h=2e-4:
101 #coords1= [1.9088392241941983, 9.424153111977732e-06, 1.1816794956539981e-05]
102 #h=2.5e-5:
103 #coords1= [1.9088391993013991, 9.424154586579873e-06, 1.1816795454370936e-05]
104 #DOPRI5:
105 #h=5e-5:
106 #coords1= [1.908839199226505, 9.424154590959904e-06, 1.1816795455868868e-05]
107 #h=1e-3:
108 #coords1= [1.9088391995380227, 9.424154572220395e-06, 1.181679544963896e-05]
109
110if useLugrePos:
111 node1D = mbs.AddNode(Node1D(referenceCoordinates = [0],
112 initialCoordinates=[0.],
113 initialVelocities=[0.]))
114 mass1D = mbs.AddObject(Mass1D(nodeNumber = node1D, physicsMass=M,
115 visualization=VMass1D(graphicsData=[GraphicsDataSphere(radius=0.05, color=color4dodgerblue)])))
116
117 #+++++++++++++++++++++++++++++++++++++++++++
118 #friction model:
119
120 #data[0]: 0=slip, 1=stick; start with sticking at last position=0!
121 #data[1]: last sticking position
122 nData = mbs.AddNode(NodeGenericData(initialCoordinates=[1,0], numberOfDataCoordinates=2))
123
124 #sigma1=0 #this does not work without damping!!!
125 #markers for friction point (does not change)
126 nGroundFric = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
127 groundMarkerFric=mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= nGroundFric, coordinate = 0))
128 nodeMarker =mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= node1D, coordinate = 0))
129
130 def springForce(mbs, t, itemNumber, u, v, k, d, offset, velocityOffset,
131 dynamicFriction, staticFrictionOffset, exponentialDecayStatic, viscousFriction, frictionProportionalZone):
132 #offset, dryFriction, dryFrictionProportionalZone):
133
134 data = mbs.GetNodeOutput(nData,variableType=exu.OutputVariableType.Coordinates)
135 if data[0] == 1:
136 F = sigma0*(u-data[1])+sigma1*v
137 else:
138 F = np.sign(v)*(Fc+(Fs-Fc)*exp(-(v/Vs)**2))
139
140 return d*v + F
141
142 #Spring-Damper between two marker coordinates
143 oCSD=mbs.AddObject(CoordinateSpringDamperExt(markerNumbers = [groundMarkerFric, nodeMarker],
144 stiffness = sigma0, damping = sigma2,
145 frictionProportionalZone=1e-16, #0 not possible right now
146 springForceUserFunction = springForce,
147 visualization=VCoordinateSpringDamper(show=False)))
148
149 #+++++++++++++++++++++++++++++++++++++++++++
150 #spring
151 #reference point for spring:
152 nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
153 groundMarker=mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= nGround, coordinate = 0))
154
155 oCSD2=mbs.AddObject(CoordinateSpringDamper(markerNumbers = [groundMarker, nodeMarker],
156 stiffness = K, damping = 0))
157
158 cnt=0
159 def PreStepUserFunction(mbs, t):
160 # global cnt
161 U=0.1*t #displacement
162 mbs.SetNodeParameter(nGround, 'referenceCoordinates', [U,0.,0.])
163 mbs.SetObjectParameter(oCSD2, 'offset', U)
164
165 #F = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Force)
166 u = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Displacement)
167 v = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Velocity)
168 F = (Fc+(Fs-Fc)*exp(-(v/Vs)**2))
169 #data = mbs.GetNodeOutput(nData,variableType=exu.OutputVariableType.Coordinates)
170 data = mbs.systemData.GetDataCoordinates()
171 u0 = data[1]
172
173 # cnt+=1
174 # if t>0 and cnt%5000==0:
175 # print('friction spring force=',abs(sigma0*(u-u0)+sigma1*v), ', Ffric=', F)
176 #stick->slip:
177 if data[0] == 1 and abs(sigma0*(u-u0)+sigma1*v) > F:
178 data[0] = 0
179 #slip->stick:
180 #elif data[0] == 0 and abs(sigma0*(u-u0)+sigma1*v) < F:
181 # elif data[0] == 0 and np.sign(v) != np.sign(F): #this seems to be the best choice for larger Vs, also for Fc~Fs
182 elif data[0] == 0 and (np.sign(v) != np.sign(F) or abs(sigma0*(u-u0)+sigma1*v) < F):
183 data[0] = 1
184
185 if data[0] == 0:
186 data[1] = u #always update sticking position during slipping
187
188 mbs.systemData.SetDataCoordinates(data)
189
190 return True
191
192 mbs.SetPreStepUserFunction(PreStepUserFunction)
193
194 #sensors
195 sCoords2 = mbs.AddSensor(SensorNode(nodeNumber = node1D, storeInternal=True,
196 outputVariableType=exu.OutputVariableType.Coordinates))
197 sCoords2_t = mbs.AddSensor(SensorNode(nodeNumber = node1D, storeInternal=True,
198 outputVariableType=exu.OutputVariableType.Coordinates_t))
199 sCSD2 = mbs.AddSensor(SensorObject(objectNumber = oCSD,storeInternal=True,
200 outputVariableType=exu.OutputVariableType.Force))
201 sData2 = mbs.AddSensor(SensorNode(nodeNumber = nData, storeInternal=True,
202 outputVariableType=exu.OutputVariableType.Coordinates))
203
204#assemble and solve system for default parameters
205mbs.Assemble()
206
207# exu.Print(mbs.systemData.GetObjectLTGODE1(0))
208# exu.Print(mbs.systemData.GetObjectLTGODE2(1))
209
210sims=exu.SimulationSettings()
211tEnd = 25
212h=1e-4
213sims.timeIntegration.absoluteTolerance = 1e-6
214
215if useLugreFast:
216 tEnd = 2
217 h=1e-4
218 if useLugre:
219 h=1e-6
220 sims.timeIntegration.absoluteTolerance = 1e-6
221
222sims.timeIntegration.relativeTolerance = sims.timeIntegration.absoluteTolerance
223
224sims.timeIntegration.endTime = tEnd
225sims.solutionSettings.writeSolutionToFile = False
226#sims.solutionSettings.sensorsWritePeriod = h
227sims.solutionSettings.sensorsWritePeriod = 1e-3
228sims.timeIntegration.verboseMode = 1
229
230# solverType=exu.DynamicSolverType.ExplicitEuler
231solverType=exu.DynamicSolverType.ODE23
232#solverType=exu.DynamicSolverType.DOPRI5
233#solverType=exu.DynamicSolverType.RK67
234
235if doImplicit:
236 solverType=exu.DynamicSolverType.TrapezoidalIndex2
237 h=0.5e-3 #works quite well with 2e-2
238
239if useLugreRef:
240 sims.solutionSettings.sensorsWritePeriod = 2e-3
241 solverType=exu.DynamicSolverType.DOPRI5
242
243
244
245sims.timeIntegration.numberOfSteps = int(tEnd/h)
246sims.timeIntegration.endTime = tEnd
247#sims.timeIntegration.initialStepSize = 1e-5
248
249
250useGraphics = True
251if useGraphics:
252 SC.visualizationSettings.general.autoFitScene = False
253 exu.StartRenderer()
254 if 'renderState' in exu.sys:
255 SC.SetRenderState(exu.sys['renderState'])
256 mbs.WaitForUserToContinue()
257
258
259
260if True:
261 sims.timeIntegration.numberOfSteps = int(tEnd/h)
262 mbs.SolveDynamic(solverType=solverType, simulationSettings=sims)
263
264
265if useGraphics:
266 SC.WaitForRenderEngineStopFlag()
267 exu.StopRenderer() #safely close rendering window!
268
269if useLugre:
270 exu.Print('coords1=', list(mbs.GetSensorValues(sCoords1)) )
271
272#+++++++++++++++++++++++++++++++++++++++++++++++++++++
273if True:
274
275 mbs.PlotSensor([], closeAll=True)
276
277 if useLugre:
278 mbs.PlotSensor(sCoords1,[0,1,2])
279 mbs.PlotSensor(sFriction1,0, colorCodeOffset=3, newFigure=False)
280 else:
281 if useLugreFast:
282 mbs.PlotSensor('solution/lugreCoordsRef2.txt',[0,1,2],
283 labels=['LuGre pos','LuGre vel','Lugre Z'])
284 mbs.PlotSensor('solution/lugreForceRef2.txt',0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])
285 else:
286 mbs.PlotSensor('solution/lugreCoordsRef1e7Impl.txt',[0,1,2],
287 labels=['LuGre pos','LuGre vel','Lugre Z'])
288 mbs.PlotSensor('solution/lugreForceRef1e7Impl.txt',0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])
289 if useLugrePos:
290 mbs.PlotSensor([sCoords2,sCoords2_t,sCSD2,sData2,sData2],[0,0,0,0,1], lineStyles='--', yLabel='coordinates, force', newFigure=False,
291 labels=['pos','vel','spring force','stick','last sticking pos'],
292 markerStyles=['','','','x','o '], markerDensity=200)