distanceSensor.py
You can view and download this file on Github: distanceSensor.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: test distance sensor with sphere and ANCFCable2D
5#
6# Author: Johannes Gerstmayr
7# Date: 2021-11-01
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13import exudyn as exu
14from exudyn.utilities import *
15
16useGraphics = True #without test
17#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
19try: #only if called from test suite
20 from modelUnitTests import exudynTestGlobals #for globally storing test results
21 useGraphics = exudynTestGlobals.useGraphics
22except:
23 class ExudynTestGlobals:
24 pass
25 exudynTestGlobals = ExudynTestGlobals()
26#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
28import numpy as np
29from math import sin, cos, sqrt,pi
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33
34#parameters:
35m=1
36L = 2
37a = 0.1
38radius = 0.5*a
39t = 0.1*a
40k = 1e4 #4e3 needs h=1e-4
41d = 0.001*k
42g = 9.81
43
44#integration and contact settings
45stepSize = 1e-4 #step size
46gContact = mbs.AddGeneralContact()
47gContact.verboseMode = 1
48gContact.SetFrictionPairings(0*np.eye(1))
49noc = 8
50gContact.SetSearchTreeCellSize(numberOfCells=[noc,noc,noc])
51
52rRot = 0.2 #rotating table radius
53#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
54# add sphere and ground
55addEdges=False
56gFloor = GraphicsDataOrthoCubePoint([0.45*L,0,-0.5*t],[L,L,t],color4steelblue,False,addEdges=addEdges)
57gFloorAdd = GraphicsDataCylinder([0,1*rRot,0],[0,2*rRot,0], radius=0.5*rRot, color=color4dodgerblue, addEdges=addEdges,
58 #angleRange=[0.5*pi,1.65*pi],lastFace=False,
59 #angleRange=[0.5*pi,1.5*pi],lastFace=False,
60 nTiles=4*8)#,
61gFloor = MergeGraphicsDataTriangleList(gFloorAdd, gFloor)
62gDataList = [gFloor]
63[meshPoints, meshTrigs] = GraphicsData2PointsAndTrigs(gFloor)
64
65
66nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0] ))
67mGround = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nGround))
68
69# [meshPoints, meshTrigs] = RefineMesh(meshPoints, meshTrigs) #just to have more triangles on floor
70gContact.AddTrianglesRigidBodyBased(rigidBodyMarkerIndex=mGround, contactStiffness=k, contactDamping=d,
71 frictionMaterialIndex=0, pointList=meshPoints, triangleList=meshTrigs)
72
73#gDataList = [GraphicsDataFromPointsAndTrigs(meshPoints, meshTrigs, color4green)]
74
75#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
76#add rotating table:
77gTable = [GraphicsDataCylinder([0,0,0],[0,0,0.05*rRot],radius=rRot, color=color4orange,addEdges=True, nTiles=64)]
78gTable+= [GraphicsDataCylinder([0,-rRot,0.05*rRot],[0,0,0.05*rRot],radius=0.1*rRot, color=color4orange,addEdges=True, nTiles=16)]
79nTable = mbs.AddNode(Node1D(referenceCoordinates=[0], initialVelocities=[4*pi]))
80oTable = mbs.AddObject(ObjectRotationalMass1D(physicsInertia=1, nodeNumber=nTable, referencePosition=[0,rRot,rRot],
81 referenceRotation=np.eye(3),
82 visualization=VRotor1D(graphicsData=gTable)))
83mTable = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oTable, localPosition=[0,-rRot,0]))
84
85#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
86#add sphere:
87f=m*g
88p0 = np.array([0,0,radius-f/(k*0.5)]) #stiffness is serial from sphere and trigs ==> 0.5*k ...
89v0 = np.array([0.2,0,0])
90omega0 = np.array([0,0*v0[0]/radius,0])
91
92gObject = [GraphicsDataSphere(radius=radius, color=color4orange, nTiles=20)]
93gObject += [GraphicsDataBasis(length=2*radius)]
94RBinertia = InertiaSphere(m, radius)
95oMass = mbs.CreateRigidBody(referencePosition=p0,
96 initialVelocity=v0,
97 initialAngularVelocity=omega0,
98 inertia=RBinertia,
99 nodeType=exu.NodeType.RotationRotationVector, #for explicit integration
100 gravity=[0,0,-g],
101 graphicsDataList=gObject,
102 )
103nMass = mbs.GetObject(oMass)['nodeNumber']
104
105mThis = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nMass))
106
107gContact.AddSphereWithMarker(mThis, radius=radius, contactStiffness=k, contactDamping=d,
108 frictionMaterialIndex=0)
109
110#put here, such that it is transparent in background
111oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
112 visualization=VObjectGround(graphicsData=gDataList)))
113
114#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
115#add ANCFCable2D
116L=0.5 # length of ANCF element in m
117E=1e7 # Young's modulus of ANCF element in N/m^2
118rho=7800 # density of ANCF element in kg/m^3
119b=0.01 # width of rectangular ANCF element in m
120h=0.05 # height of rectangular ANCF element in m
121A=b*h # cross sectional area of ANCF element in m^2
122I=b*h**3/12 # second moment of area of ANCF element in m^4
123
124
125#generate ANCF beams with utilities function
126cableTemplate = Cable2D(physicsMassPerLength = rho*A,
127 physicsBendingStiffness = E*I,
128 physicsAxialStiffness = E*A,
129 physicsBendingDamping = 0.005*E*I,
130 useReducedOrderIntegration = 2,
131 visualization=VCable2D(drawHeight=h)
132 )
133
134positionOfNode0 = [-2*L, 0, 0.] # starting point of line
135positionOfNode1 = [-L, 0, 0.] # end point of line
136numberOfElements = 4
137
138#alternative to mbs.AddObject(Cable2D(...)) with nodes:
139ancf=GenerateStraightLineANCFCable2D(mbs,
140 positionOfNode0, positionOfNode1,
141 numberOfElements,
142 cableTemplate, #this defines the beam element properties
143 massProportionalLoad = [0,-9.81,0], #optionally add gravity
144 fixedConstraintsNode0 = [1,1,0,1], #add constraints for pos and rot (r'_y)
145 fixedConstraintsNode1 = [0,0,0,0])
146
147# #add all cable elements to contact
148for oIndex in ancf[1]:
149 gContact.AddANCFCable(objectIndex=oIndex, halfHeight=0.5*h,
150 contactStiffness=1, contactDamping=0, frictionMaterialIndex=0)
151
152
153
154#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
155# for i in range(10):
156# sDist0 = mbs.CreateDistanceSensor(positionOrMarker=[i*2*radius,-1.3*radius,4*radius], dirSensor=[0,0,-2*radius], minDistance=0, maxDistance=2*t+4*radius,
157# cylinderRadius=radius*0.5, storeInternal=True, addGraphicsObject=True)
158
159#alternative way:
160# def UFsensor0(mbs, t, sensorNumbers, factors, configuration):
161# p0 = np.array([radius*3,1,0])
162# d = gContact.ShortestDistanceAlongLine(pStart = p0, direction = [0,-1,0],
163# minDistance=0, maxDistance=1.0, cylinderRadius=0.01)
164# return [d]
165# sDistanceSphere = mbs.AddSensor(SensorUserFunction(sensorNumbers=[], factors=[],
166# storeInternal=True,
167# sensorUserFunction=UFsensor0))
168
169#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
170#add sensors
171ngc = mbs.NumberOfGeneralContacts()-1 #index of GeneralContact object that has been added last (0 here ...)
172sDistanceSphere = mbs.CreateDistanceSensor(ngc, positionOrMarker=[2*radius,4*radius,radius], dirSensor=[0,-2*radius,0], minDistance=0, maxDistance=1*t+4*radius, measureVelocity=True,
173 cylinderRadius=radius*0.5, storeInternal=True, addGraphicsObject=True, selectedTypeIndex=exu.ContactTypeIndex.IndexSpheresMarkerBased)
174
175sDistanceSphere2 = mbs.CreateDistanceSensor(ngc, positionOrMarker=[2*radius,0,4*radius], dirSensor=[0,0,-2*radius], minDistance=0, maxDistance=1*t+4*radius, measureVelocity=True,
176 cylinderRadius=radius*0.5, storeInternal=True, addGraphicsObject=True, selectedTypeIndex=exu.ContactTypeIndex.IndexSpheresMarkerBased)
177
178sDistanceTable = mbs.CreateDistanceSensor(ngc, positionOrMarker=mTable, dirSensor=[0,0,-2*radius],
179 minDistance=-10, maxDistance=1*t+4*radius, measureVelocity=True,
180 cylinderRadius=0, storeInternal=True, addGraphicsObject=True)#, selectedTypeIndex=exu.ContactTypeIndex.IndexSpheresMarkerBased)
181
182sANCF = mbs.CreateDistanceSensor(ngc, positionOrMarker=[-L*1.5,0,0], dirSensor=[0,-0.1,0], minDistance=0, maxDistance=L, measureVelocity=True,
183 storeInternal=True, addGraphicsObject=True)
184
185sANCFdist = mbs.CreateDistanceSensor(ngc, positionOrMarker=[-0.6061511314921351,0,0], dirSensor=[0,-0.1,0], minDistance=0, maxDistance=L, measureVelocity=True,
186 storeInternal=True, addGraphicsObject=True)
187
188sANCFdisp = mbs.AddSensor(SensorNode(nodeNumber=ancf[0][-1], storeInternal=True, outputVariableType=exu.OutputVariableType.Displacement))
189
190sVelocitySphere = mbs.AddSensor(SensorMarker(markerNumber=mThis, storeInternal=True,
191 outputVariableType=exu.OutputVariableType.Velocity))
192
193#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
194mbs.Assemble()
195# exu.Print(gContact)
196
197tEnd = 0.25
198#tEnd = h*100
199simulationSettings = exu.SimulationSettings()
200# simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
201simulationSettings.solutionSettings.writeSolutionToFile = False
202simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
203simulationSettings.displayComputationTime = useGraphics
204SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
205
206# simulationSettings.timeIntegration.simulateInRealtime = True
207# simulationSettings.timeIntegration.realtimeFactor = 0.5
208simulationSettings.timeIntegration.verboseMode = 1
209
210# SC.visualizationSettings.loads.show=False
211SC.visualizationSettings.window.renderWindowSize=[1600,1200]
212SC.visualizationSettings.openGL.multiSampling = 4
213# SC.visualizationSettings.openGL.shadow = 0.3
214SC.visualizationSettings.openGL.light0position = [-5,-5,20,0]
215
216simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
217simulationSettings.timeIntegration.endTime = tEnd
218
219if False: #show bounding boxes
220 SC.visualizationSettings.contact.showSearchTree =True
221 SC.visualizationSettings.contact.showSearchTreeCells =True
222 SC.visualizationSettings.contact.showBoundingBoxes = True
223
224if useGraphics:
225 SC.visualizationSettings.general.autoFitScene = False
226 exu.StartRenderer()
227 if 'renderState' in exu.sys:
228 SC.SetRenderState(exu.sys['renderState'])
229 mbs.WaitForUserToContinue()
230
231
232mbs.SolveDynamic(simulationSettings,
233 #solverType=exu.DynamicSolverType.ExplicitEuler,
234 solverType=exu.DynamicSolverType.RK44,
235 )
236
237if useGraphics:
238 SC.WaitForRenderEngineStopFlag()
239 exu.StopRenderer() #safely close rendering window!
240
241x=mbs.GetNodeOutput(ancf[0][-1], variableType=exu.OutputVariableType.Position)
242exu.Print('pLast=',list(x),'\n')
243#[-0.546983567323076, -0.19231209764430873, 0.0]
244
245
246s1 = (mbs.GetSensorValues(sDistanceSphere))
247s2 = (mbs.GetSensorValues(sDistanceSphere2))
248s3 = (mbs.GetSensorValues(sANCF))
249s4 = (mbs.GetSensorValues(sANCFdist))
250s5 = (mbs.GetSensorValues(sVelocitySphere))
251
252exu.Print('sensors=',s1,s2,s3,s4,s5,'\n')
253
254u = NormL2(s1) + NormL2(s2) + NormL2(s3) + NormL2(s4) + NormL2(s5)
255
256exu.Print('solution of distanceSensor=',u)
257exudynTestGlobals.testResult = u
258
259#%%
260if useGraphics:
261
262 mbs.PlotSensor(closeAll=True)
263 mbs.PlotSensor(sDistanceSphere, components=0, colorCodeOffset=0, labels=['y-axis'])
264 mbs.PlotSensor(sDistanceSphere2, components=0, colorCodeOffset=1, newFigure=False, labels=['z-axis'])
265 mbs.PlotSensor(sDistanceTable, components=0, colorCodeOffset=2, newFigure=False, labels=['table z-dist'])
266
267 mbs.PlotSensor(sDistanceSphere, components=1, colorCodeOffset=3, newFigure=False, labels=['LDV'])
268 mbs.PlotSensor(sANCFdist, components=0, colorCodeOffset=5, newFigure=False, labels=['ANCF distance'])
269 mbs.PlotSensor(sANCFdisp, components=1, colorCodeOffset=6, newFigure=False, labels=['ANCF displacement'], factors=[-1])
270 # mbs.PlotSensor(sVelocitySphere, components=0, closeAll=True)