createSphereQuadContact2.py

You can view and download this file on Github: createSphereQuadContact2.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
 36
 37radius=0.1
 38mass = 0.2                              #mass in kg
 39contactStiffness = 2e4*10               #stiffness of spring-damper in N/m
 40contactDamping = 1e-4*contactStiffness  #damping constant in N/(m/s); as we have always contact, we only need some damping for initial effects
 41dynamicFriction = 0.2
 42
 43isExplicitSolver = False
 44tEnd = 1     #end time of simulation
 45if useGraphics:
 46    tEnd = 10
 47
 48stepSize = 5e-4 #*10
 49
 50g = 9.81
 51
 52size = 1
 53
 54oGround = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard(point=[0,0,0],size=2*size,
 55                                                                   color=graphics.color.lightgrey[0:3]+[graphics.material.indexChrome],
 56                                                                   alternatingColor=graphics.color.lightgrey2[0:3]+[graphics.material.indexChrome],
 57                                                                   ), ])
 58mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround))
 59mGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,-radius]))
 60
 61#for explicit solver, we need Lie group node:
 62nodeType=exu.NodeType.RotationRotationVector if isExplicitSolver else exu.NodeType.RotationEulerParameters
 63
 64
 65nSpheres = 5
 66listMasses=[]
 67sPosList=[]
 68
 69for iPhi in range(nSpheres):
 70
 71    phi = iPhi / nSpheres * (2*pi)
 72    x = 2*radius*cos(phi)
 73    y = 2*radius*sin(phi)
 74
 75    vv = 0.5*2
 76    vx = -vv*sin(phi)
 77    vy = vv*cos(phi)
 78
 79    oMass = mbs.CreateRigidBody(referencePosition=[x,y,radius],
 80                                initialVelocity=[vx,vy,0],
 81                                initialAngularVelocity=[0,0,0],
 82                                nodeType=nodeType,
 83                                inertia=InertiaSphere(mass=mass, radius=radius),
 84                                gravity = [0,0,-g],
 85                                graphicsDataList=[graphics.Sphere(radius=radius, color=graphics.colorList[iPhi],
 86                                                                  nTiles=32)],
 87                                )
 88    listMasses.append(oMass)
 89    mMass = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oMass))
 90
 91    if False:
 92        #create contact between each sphere:
 93        for oMass2 in listMasses[:-1]:
 94            restitutionCoefficient = 0.75
 95            impactModel = 2
 96            oSSC = mbs.CreateSphereSphereContact(bodyNumbers=[oMass, oMass2],
 97                                                 spheresRadii=[radius, radius],
 98                                                 contactStiffness = contactStiffness,
 99                                                 dynamicFriction = dynamicFriction,
100                                                 impactModel = impactModel,
101                                                 restitutionCoefficient = restitutionCoefficient,
102                                                 show=True,
103                                                 )
104
105    quadPoints = exu.Vector3DList([[-size,-size,0],[size,-size,0],[size,size,0],[-size,size,0]])
106    oSSC = mbs.CreateSphereQuadContact(bodyNumbers=[oMass, oGround],
107                                       quadPoints=quadPoints,
108                                       includeEdges=15, #all edges
109                                       radiusSphere=radius,
110                                       contactStiffness = contactStiffness,
111                                       contactDamping = contactDamping,
112                                       dynamicFriction=dynamicFriction,
113                                       show = True
114                                       )
115    sPos=mbs.AddSensor(SensorBody(bodyNumber=oMass, storeInternal=True,
116                                  outputVariableType=exu.OutputVariableType.Position))
117    sPosList.append(sPos)
118
119
120sizeTop = 8*radius
121zTop = 0.5*radius
122oBodyTop = mbs.CreateRigidBody(referencePosition=[0,0,2*radius+0.5*zTop],
123                               initialVelocity=[0,0,0],
124                               initialAngularVelocity=[0,0,0], #not initialized and will follow initial velocity of spheres
125                               nodeType=nodeType,
126                               inertia=InertiaCuboid(0.01*2800, sideLengths=[sizeTop,sizeTop,zTop]),
127                               gravity = [0,0,-g],
128                               color = graphics.color.red[0:3]+[0.3],
129                            )
130sBodyAngVel = mbs.AddSensor(SensorBody(bodyNumber=oBodyTop,
131                                    storeInternal=True,
132                                    outputVariableType=exu.OutputVariableType.AngularVelocity))
133# mbs.CreateGenericJoint(bodyNumbers=[oGround, oBodyTop], position = [0,0,2*radius+0.5*zTop])
134
135
136for oMass in listMasses:
137    quadPoints = exu.Vector3DList([[-0.5*sizeTop,-0.5*sizeTop,-0.5*zTop],
138                                   [0.5*sizeTop,-0.5*sizeTop,-0.5*zTop],
139                                   [0.5*sizeTop,0.5*sizeTop,-0.5*zTop],
140                                   [-0.5*sizeTop,0.5*sizeTop,-0.5*zTop]])
141    oSSC = mbs.CreateSphereQuadContact(bodyNumbers=[oMass,oBodyTop],
142                                       quadPoints=quadPoints,
143                                       includeEdges=15, #all edges
144                                       radiusSphere=radius*1,
145                                       contactStiffness = contactStiffness,
146                                       contactDamping = contactDamping,
147                                       dynamicFriction=dynamicFriction,
148                                       show = True
149                                       )
150
151
152#exu.Print(mbs)
153mbs.Assemble()
154
155simulationSettings = exu.SimulationSettings()
156simulationSettings.solutionSettings.writeSolutionToFile = True
157simulationSettings.solutionSettings.solutionWritePeriod = 0.005
158simulationSettings.solutionSettings.sensorsWritePeriod = 0.001  #output interval
159simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
160simulationSettings.timeIntegration.endTime = tEnd
161#simulationSettings.timeIntegration.simulateInRealtime = True
162simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-6
163simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
164
165simulationSettings.timeIntegration.stepInformation = 3 #remove flag 64 which shows step reduction warnings
166
167
168simulationSettings.timeIntegration.newton.useModifiedNewton = True
169simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
170
171simulationSettings.displayStatistics = True
172simulationSettings.timeIntegration.verboseMode = 1
173SC.visualizationSettings.general.drawCoordinateSystem = False
174SC.visualizationSettings.general.showSolverInformation = False
175SC.visualizationSettings.connectors.showContact = False
176SC.visualizationSettings.nodes.showBasis = True
177SC.visualizationSettings.nodes.basisSize = 1.5*radius
178#SC.visualizationSettings.openGL.depthSorting = True
179
180if useGraphics:
181    SC.renderer.Start()              #start graphics visualization
182    SC.renderer.DoIdleTasks()    #wait for pressing SPACE bar to continue
183
184mbs.SolveDynamic(simulationSettings)
185
186if useGraphics:
187    SC.renderer.Stop()               #safely close rendering window!
188
189    if False:
190        mbs.PlotSensor(sPosList, components=[2]*len(sPosList))
191
192#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
193ode2 = mbs.systemData.GetODE2Coordinates()
194
195testSolution += 0.1*np.linalg.norm(ode2)
196
197
198#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
199exu.Print('solution of createSphereQuadContact2=',testSolution)
200exudynTestGlobals.testResult = testSolution
201#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
202
203mbs.PlotSensor(sBodyAngVel,components=2)
204
205
206if useGraphics and False:
207    mbs.SolutionViewer()