lugreFrictionODE1.py
You can view and download this file on Github: lugreFrictionODE1.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 = False
39useLugreRef = False
40
41nODE1=3 #U,V,Z
42qInit = [0]*nODE1
43#qInit[0] = 1
44nodeODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0]*nODE1,
45 initialCoordinates=qInit,
46 numberOfODE1Coordinates=nODE1))
47
48#this user function represents the RHS of the first order differential equation for LuGre model:
49#q_t = f(q,t)
50def UFode1(mbs, t, itemNumber, q):
51 q_t=np.zeros(nODE1)
52 U=0.1*t
53 FL=0
54 X=q[0]
55 V=q[1]
56 Z=q[2]
57 G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
58
59 Z_t=V-Z*abs(V)/G
60 FL=sigma0*Z+sigma1*Z_t+sigma2*V #force
61
62 q_t[0] = V
63 q_t[1] = (K*(U-X) - FL)/M
64 q_t[2] = Z_t
65
66 return q_t
67
68oGenericODE1 = mbs.AddObject(ObjectGenericODE1(nodeNumbers=[nodeODE1],
69 rhsUserFunction=UFode1))
70
71
72sCoords1 = mbs.AddSensor(SensorNode(nodeNumber = nodeODE1,
73 storeInternal=True,
74 fileName='solution/lugreCoords'+'Ref'*useLugreRef+'.txt',
75 outputVariableType=exu.OutputVariableType.Coordinates))
76
77#user force which computes the friction force
78def UFsensorFrictionForce(mbs, t, sensorNumbers, factors, configuration):
79 q = mbs.GetSensorValues(sensorNumbers[0])
80 X=q[0]
81 V=q[1]
82 Z=q[2]
83 G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
84
85 Z_t=V-Z*abs(V)/G
86 FL=sigma0*Z+sigma1*Z_t+sigma2*V
87 return [FL]
88
89sFriction1 = mbs.AddSensor(SensorUserFunction(sensorNumbers=[sCoords1],
90 fileName='solution/lugreForce'+'Ref'*useLugreRef+'.txt',
91 storeInternal=True,sensorUserFunction=UFsensorFrictionForce))
92#ODE23 integrator, aTol=rTol=1e-8:
93#h=2e-4:
94#coords1= [1.9088392241941983, 9.424153111977732e-06, 1.1816794956539981e-05]
95#h=2.5e-5:
96#coords1= [1.9088391993013991, 9.424154586579873e-06, 1.1816795454370936e-05]
97#DOPRI5:
98#h=5e-5:
99#coords1= [1.908839199226505, 9.424154590959904e-06, 1.1816795455868868e-05]
100#h=1e-3:
101#coords1= [1.9088391995380227, 9.424154572220395e-06, 1.181679544963896e-05]
102
103
104#assemble and solve system for default parameters
105mbs.Assemble()
106
107sims=exu.SimulationSettings()
108tEnd = 25
109h=1e-4
110sims.timeIntegration.absoluteTolerance = 1e-8
111sims.timeIntegration.relativeTolerance = sims.timeIntegration.absoluteTolerance
112
113sims.timeIntegration.endTime = tEnd
114sims.solutionSettings.writeSolutionToFile = False
115sims.solutionSettings.sensorsWritePeriod = 1e-3
116sims.timeIntegration.verboseMode = 1
117
118solverType=exu.DynamicSolverType.ODE23 #adaptive
119#solverType=exu.DynamicSolverType.DOPRI5
120#solverType=exu.DynamicSolverType.RK67
121
122sims.timeIntegration.numberOfSteps = int(tEnd/h)
123sims.timeIntegration.endTime = tEnd
124#sims.timeIntegration.initialStepSize = 1e-5
125
126
127
128sims.timeIntegration.numberOfSteps = int(tEnd/h)
129mbs.SolveDynamic(solverType=solverType, simulationSettings=sims)
130
131
132#+++++++++++++++++++++++++++++++++++++++++++++++++++++
133if True:
134
135 mbs.PlotSensor([], closeAll=True)
136
137 mbs.PlotSensor(sCoords1,[0,1,2], labels=['LuGre pos','LuGre vel','Lugre Z'])
138 mbs.PlotSensor(sFriction1,0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])