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