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