createSphereQuadContact.py

You can view and download this file on Github: createSphereQuadContact.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for CreateSphereQuadContact, combining sphere-sphere and 2 sphere-triangle contacts
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2025-06-29
  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 *
 15import exudyn.graphics as graphics
 16import numpy as np
 17
 18useGraphics = True #without test
 19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 20#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 21try: #only if called from test suite
 22    from modelUnitTests import exudynTestGlobals #for globally storing test results
 23    useGraphics = exudynTestGlobals.useGraphics
 24except:
 25    class ExudynTestGlobals:
 26        pass
 27    exudynTestGlobals = ExudynTestGlobals()
 28#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29#useGraphics = False
 30
 31testSolution = 0
 32
 33SC = exu.SystemContainer()
 34mbs = SC.AddSystem()
 35
 36methodList = ['trigs','quad'] #0=trigs, 1=quad; both tests should gave same results!
 37listSolutions = []
 38
 39for methodNum, method in enumerate(methodList):
 40    mbs.Reset()
 41
 42    exu.Print('\n\n***********************************')
 43    exu.Print('*** Test contact create method: '+method+' ***')
 44    exu.Print('***********************************\n')
 45
 46    radius=0.1
 47    mass = 0.2                              #mass in kg
 48    contactStiffness = 2e4                  #stiffness of spring-damper in N/m
 49    contactDamping = 0*5e-4*contactStiffness  #damping constant in N/(m/s)
 50    dynamicFriction = 0.2
 51    restitutionCoefficient = 0.75
 52    impactModel = 2
 53
 54    isExplicitSolver = False
 55    tEnd = 0.65     #end time of simulation
 56    stepSize = 2e-4 #*10
 57
 58    g = 9.81
 59
 60    size = 1
 61
 62
 63    oGround = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard(point=[0,0,0],size=2*size,
 64                                                                       color=graphics.color.lightgrey[0:3]+[graphics.material.indexChrome],
 65                                                                       alternatingColor=graphics.color.lightgrey2[0:3]+[graphics.material.indexChrome],
 66                                                                       ), ])
 67    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround))
 68    mGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,-radius]))
 69
 70    listMasses=[]
 71    sPosList=[]
 72
 73    ny = 4
 74    cnt = -1
 75    for jy in range(ny):
 76
 77        for ix in range(max(1,jy)):
 78            cnt+=1
 79            x = (ix-(jy-1)*0.5)*2*radius
 80            y = -4*radius + jy*radius*np.sqrt(3)
 81
 82            vy = 2*sin(cnt*pi/3)
 83            vx = 2*cos(cnt*pi/3)
 84
 85            massFact = 1
 86
 87            #for explicit solver, we need Lie group node:
 88            nodeType=exu.NodeType.RotationRotationVector if isExplicitSolver else exu.NodeType.RotationEulerParameters
 89
 90            oMass = mbs.CreateRigidBody(referencePosition=[x,y,radius],
 91                                        initialVelocity=[vx,vy,0],
 92                                        initialAngularVelocity=[0,0,0],
 93                                        nodeType=nodeType,
 94                                        inertia=InertiaSphere(mass=massFact*mass, radius=radius),
 95                                        gravity = [0,0,-g],
 96                                        graphicsDataList=[graphics.Sphere(radius=radius,
 97                                                                          color=graphics.colorList[cnt][0:3]+[graphics.material.indexDefault],
 98                                                                          nTiles=48)],
 99                                        )
100            listMasses.append(oMass)
101            mMass = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oMass))
102
103            if False: #object mode
104                for oMass2 in listMasses[:-1]:
105                    mMass2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oMass2))
106                    nData1 = mbs.AddNode(NodeGenericData(initialCoordinates=[0.1,0,0,0],
107                                                        numberOfDataCoordinates=4))
108                    oSSC = mbs.AddObject(ObjectContactSphereSphere(markerNumbers=[mMass, mMass2],
109                                                                    nodeNumber=nData1,
110                                                                    spheresRadii=[radius, radius],
111                                                                    contactStiffness = contactStiffness,
112                                                                    dynamicFriction=dynamicFriction,
113                                                                    impactModel = impactModel,
114                                                                    restitutionCoefficient = restitutionCoefficient,
115                                                                    visualization=VObjectContactSphereSphere(show=True),
116                                                                    ))
117            else:
118                #create contact between each sphere:
119                for oMass2 in listMasses[:-1]:
120                    oSSC = mbs.CreateSphereSphereContact(bodyNumbers=[oMass, oMass2],
121                                                         spheresRadii=[radius, radius],
122                                                         contactStiffness = contactStiffness,
123                                                         dynamicFriction = dynamicFriction,
124                                                         impactModel = impactModel,
125                                                         restitutionCoefficient = restitutionCoefficient,
126                                                         show=True,
127                                                         )
128
129            if method=='trigs': #create 2 triangles
130                trianglePoints0 = exu.Vector3DList([[-size,-size,0],[size,-size,0],[-size,size,0]])
131                trianglePoints1 = exu.Vector3DList([[size,-size,0],[size,size,0],[-size,size,0]])
132                includeEdgesList = [5,3]
133
134                trigList = [trianglePoints0,trianglePoints1]
135                for k, trianglePoints in enumerate(trigList):
136                    oSSC = mbs.CreateSphereTriangleContact(bodyNumbers=[oMass, oGround],
137                                                           trianglePoints=trianglePoints,
138                                                           includeEdges=includeEdgesList[k],
139                                                           radiusSphere=radius,
140                                                           contactStiffness = contactStiffness,
141                                                           dynamicFriction=dynamicFriction,
142                                                           impactModel = impactModel,
143                                                           restitutionCoefficient = restitutionCoefficient,
144                                                           show = True
145                                                           )
146            else: #alternative with quads
147                quadPoints = exu.Vector3DList([[-size,-size,0],[size,-size,0],[size,size,0],[-size,size,0]])
148                oSSC = mbs.CreateSphereQuadContact(bodyNumbers=[oMass, oGround],
149                                                   quadPoints=quadPoints,
150                                                   includeEdges=15, #all edges
151                                                   radiusSphere=radius,
152                                                   contactStiffness = contactStiffness,
153                                                   dynamicFriction=dynamicFriction,
154                                                   impactModel = impactModel,
155                                                   restitutionCoefficient = restitutionCoefficient,
156                                                   show = True
157                                                   )
158
159
160            sPos=mbs.AddSensor(SensorBody(bodyNumber=oMass, storeInternal=True,
161                                          outputVariableType=exu.OutputVariableType.Position))
162            sPosList.append(sPos)
163
164    #exu.Print(mbs)
165    mbs.Assemble()
166
167    simulationSettings = exu.SimulationSettings()
168    simulationSettings.solutionSettings.writeSolutionToFile = True
169    simulationSettings.solutionSettings.solutionWritePeriod = 0.005
170    simulationSettings.solutionSettings.sensorsWritePeriod = 0.001  #output interval
171    simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
172    simulationSettings.timeIntegration.endTime = tEnd
173    #simulationSettings.timeIntegration.simulateInRealtime = True
174    simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-6
175    simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
176
177    simulationSettings.timeIntegration.stepInformation = 3 #remove flag 64 which shows step reduction warnings
178
179
180    simulationSettings.timeIntegration.newton.useModifiedNewton = True
181    simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
182
183    simulationSettings.displayStatistics = True
184    simulationSettings.timeIntegration.verboseMode = 1
185    SC.visualizationSettings.general.drawCoordinateSystem = False
186    SC.visualizationSettings.general.showSolverInformation = False
187
188
189    if useGraphics:
190        SC.renderer.Start()              #start graphics visualization
191        if methodNum == 0:
192            SC.renderer.DoIdleTasks()    #wait for pressing SPACE bar to continue
193
194    mbs.SolveDynamic(simulationSettings)
195
196    if useGraphics:
197        #SC.renderer.DoIdleTasks()
198        SC.renderer.Stop()               #safely close rendering window!
199
200        if False:
201            mbs.PlotSensor(sPosList, components=[2]*len(sPosList))
202
203    #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
204    ode2 = mbs.systemData.GetODE2Coordinates()
205    listSolutions.append(ode2)
206
207    testSolution += 0.1*np.linalg.norm(ode2)
208
209diff = listSolutions[0] - listSolutions[1]
210exu.Print('solution diff=',np.linalg.norm(diff))
211
212for i, sol in enumerate(listSolutions):
213    exu.Print('solver=',str(methodList[i]),'\nsol=',sol[0:6])
214
215#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
216exu.Print('solution of createSphereQuadContact=',testSolution)
217exudynTestGlobals.testResult = testSolution
218#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
219
220
221if useGraphics:
222    mbs.SolutionViewer()