createSphereTriangleContact.py
You can view and download this file on Github: createSphereTriangleContact.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Test for SphereQuadContact, combining sphere-sphere and sphere-triangle contact
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 *
15from math import sin, cos
16import exudyn.graphics as graphics
17import numpy as np
18
19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
22# import exudyn
23# from exudyn.advancedUtilities import RaiseTypeError, IsVector, IsReal, ExpectedType, IsValidObjectIndex, IsValidNodeIndex, \
24# IsValidRealInt, IsValidPRealInt, IsValidURealInt, IsIntVector, \
25# IsValidBool, IsSquareMatrix, IsNone, IsNotNone, IsInteger
26# import exudyn.itemInterface as eii
27
28# #**function: return True, if x is int, np.integer or similar types that can be automatically casted to pybind11
29# def IsValidInt(x):
30# if (isinstance(x, int)
31# or isinstance(x, np.integer)
32# ):
33# return True
34# return False
35
36# #internal function, which checks bodyList and bodyOrNodeList and returns appropriate bodyOrNodeList
37# def ProcessBodyNodeLists(bodyNumbers, bodyOrNodeList, localPosition0, localPosition1, where, bodyList=[None,None]):
38# if not exudyn.__useExudynFast:
39# if not isinstance(bodyList, list) or len(bodyList) != 2:
40# RaiseTypeError(where=where, argumentName='bodyList', received = bodyList, expectedType = 'list of 2 body numbers')
41# if not isinstance(bodyNumbers, list) or len(bodyNumbers) != 2:
42# RaiseTypeError(where=where, argumentName='bodyNumbers', received = bodyNumbers, expectedType = 'list of 2 body numbers')
43
44# causingArgName = 'bodyOrNodeList'
45# if IsNotNone(bodyNumbers[0]) or IsNotNone(bodyNumbers[1]):
46# bodyOrNodeList = [bodyNumbers[0],bodyNumbers[1]] #flat copy, but otherwise would lead to change of args (mutable args!)
47# causingArgName = 'bodyNumbers'
48# elif IsNotNone(bodyList[0]) or IsNotNone(bodyList[1]):
49# exu.Print('WARNING: bodyList in MainSystem Create functions is deprecated; use bodyNumbers instead!')
50# bodyOrNodeList = [bodyList[0],bodyList[1]] #flat copy, but otherwise would lead to change of args (mutable args!)
51# causingArgName = 'bodyList'
52
53# if not exudyn.__useExudynFast:
54# if not isinstance(bodyOrNodeList, list) or len(bodyOrNodeList) != 2:
55# RaiseTypeError(where=where, argumentName='bodyOrNodeList', received = bodyOrNodeList, expectedType = 'list of 2 body or node numbers')
56
57# if not (isinstance(bodyOrNodeList[0], exudyn.ObjectIndex) or (isinstance(bodyOrNodeList[0], exudyn.NodeIndex) and localPosition0==[0.,0.,0.])):
58# RaiseTypeError(where=where, argumentName=''+causingArgName+'[0]', received = bodyOrNodeList[0],
59# expectedType = 'expected either ObjectIndex, or NodeIndex AND localPosition0=[0.,0.,0.]')
60
61# if not (isinstance(bodyOrNodeList[1], exudyn.ObjectIndex) or (isinstance(bodyOrNodeList[1], exudyn.NodeIndex) and localPosition1==[0.,0.,0.])):
62# RaiseTypeError(where=where, argumentName=''+causingArgName+'[1]', received = bodyOrNodeList[1],
63# expectedType = 'expected either ObjectIndex, or NodeIndex AND localPosition1=[0.,0.,0.]')
64
65# return bodyOrNodeList
66
67
68
69# exudyn.MainSystem.CreateSphereQuadContact = MainSystemCreateSphereQuadContact
70
71
72
73testIsActive = exu.sys.get('testIsActive', False)
74#testIsActive = True
75
76testSolution = 0
77
78SC = exu.SystemContainer()
79mbs = SC.AddSystem()
80
81methodList = ['trigs','quad'] #0=trigs, 1=quad
82listSolutions = []
83
84for methodNum, method in enumerate(methodList):
85 mbs.Reset()
86
87 exu.Print('\n\n***********************************')
88 exu.Print('*** Test contact create method: '+method+' ***')
89 exu.Print('***********************************\n')
90
91 radius=0.1
92 mass = 0.2 #mass in kg
93 contactStiffness = 2e4 #stiffness of spring-damper in N/m
94 contactDamping = 0*5e-4*contactStiffness #damping constant in N/(m/s)
95 dynamicFriction = 0.2
96 restitutionCoefficient = 0.75
97 impactModel = 2
98
99 isExplicitSolver = False
100 tEnd = 0.25 #shortened for the test suite, with the step size unchanged (#2494)
101 stepSize = 2e-4 #*10
102
103 g = 9.81
104
105 size = 1
106
107
108 oGround = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard(point=[0,0,0],size=2*size,
109 color=graphics.color.lightgrey[0:3]+[graphics.material.indexChrome],
110 alternatingColor=graphics.color.lightgrey2[0:3]+[graphics.material.indexChrome],
111 ), ])
112 mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround))
113 mGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,-radius]))
114
115 listMasses=[]
116 sPosList=[]
117
118 ny = 4
119 cnt = -1
120 for jy in range(ny):
121
122 for ix in range(max(1,jy)):
123 cnt+=1
124 x = (ix-(jy-1)*0.5)*2*radius
125 y = -4*radius + jy*radius*np.sqrt(3)
126
127 vy = 2*sin(cnt*pi/3)
128 vx = 2*cos(cnt*pi/3)
129
130 massFact = 1
131
132 #for explicit solver, we need Lie group node:
133 nodeType=exu.NodeType.RotationRotationVector if isExplicitSolver else exu.NodeType.RotationEulerParameters
134
135 oMass = mbs.CreateRigidBody(referencePosition=[x,y,radius],
136 initialVelocity=[vx,vy,0],
137 initialAngularVelocity=[0,0,0],
138 nodeType=nodeType,
139 inertia=InertiaSphere(mass=massFact*mass, radius=radius),
140 gravity = [0,0,-g],
141 graphicsDataList=[graphics.Sphere(radius=radius,
142 color=graphics.colorList[cnt][0:3]+[graphics.material.indexDefault],
143 nTiles=24)],
144 )
145 listMasses.append(oMass)
146 mMass = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oMass))
147
148 if False: #object mode
149 for oMass2 in listMasses[:-1]:
150 mMass2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oMass2))
151 nData1 = mbs.AddNode(NodeGenericData(initialCoordinates=[0.1,0,0,0],
152 numberOfDataCoordinates=4))
153 oSSC = mbs.AddObject(ObjectContactSphereSphere(markerNumbers=[mMass, mMass2],
154 nodeNumber=nData1,
155 spheresRadii=[radius, radius],
156 contactStiffness = contactStiffness,
157 dynamicFriction=dynamicFriction,
158 impactModel = impactModel,
159 restitutionCoefficient = restitutionCoefficient,
160 visualization=VObjectContactSphereSphere(show=True),
161 ))
162 else:
163 #create contact between each sphere:
164 for oMass2 in listMasses[:-1]:
165 oSSC = mbs.CreateSphereSphereContact(itemNumbers=[oMass, oMass2],
166 spheresRadii=[radius, radius],
167 contactStiffness = contactStiffness,
168 dynamicFriction = dynamicFriction,
169 impactModel = impactModel,
170 restitutionCoefficient = restitutionCoefficient,
171 show=True,
172 )
173
174 if method=='trigs': #create 2 triangles
175 trianglePoints0 = exu.Vector3DList([[-size,-size,0],[size,-size,0],[-size,size,0]])
176 trianglePoints1 = exu.Vector3DList([[size,-size,0],[size,size,0],[-size,size,0]])
177 includeEdgesList = [5,3]
178
179 trigList = [trianglePoints0,trianglePoints1]
180 for k, trianglePoints in enumerate(trigList):
181 oSSC = mbs.CreateSphereTriangleContact(itemNumbers=[oMass, oGround],
182 trianglePoints=trianglePoints,
183 includeEdges=includeEdgesList[k],
184 sphereRadius=radius,
185 contactStiffness = contactStiffness,
186 dynamicFriction=dynamicFriction,
187 impactModel = impactModel,
188 restitutionCoefficient = restitutionCoefficient,
189 show = True
190 )
191 else: #alternative with quads
192 quadPoints = exu.Vector3DList([[-size,-size,0],[size,-size,0],[size,size,0],[-size,size,0]])
193 oSSC = mbs.CreateSphereQuadContact(itemNumbers=[oMass, oGround],
194 quadPoints=quadPoints,
195 includeEdges=15, #all edges
196 sphereRadius=radius,
197 contactStiffness = contactStiffness,
198 dynamicFriction=dynamicFriction,
199 impactModel = impactModel,
200 restitutionCoefficient = restitutionCoefficient,
201 show = True
202 )
203
204
205 sPos=mbs.AddSensor(SensorBody(bodyNumber=oMass, storeInternal=True,
206 outputVariableType=exu.OutputVariableType.Position))
207 sPosList.append(sPos)
208
209 #exu.Print(mbs)
210 mbs.Assemble()
211
212 simulationSettings = exu.SimulationSettings()
213 simulationSettings.solution.file.write = (not testIsActive) #only the SolutionViewer reads it (#2492)
214 simulationSettings.solution.file.writePeriod = 0.005
215 simulationSettings.solution.sensors.writePeriod = 0.001 #output interval
216 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
217 simulationSettings.timeIntegration.endTime = tEnd
218 #simulationSettings.timeIntegration.realtime.active = True
219 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-6
220 simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
221
222 simulationSettings.timeIntegration.stepInformation = 3 #remove flag 64 which shows step reduction warnings
223
224
225 simulationSettings.timeIntegration.newton.useModifiedNewton = True
226 simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenSparse
227
228 simulationSettings.show.statistics = True
229 simulationSettings.timeIntegration.verboseMode = 1
230 SC.visualizationSettings.view0.scene.drawCoordinateSystem = False
231 SC.visualizationSettings.general.showSolverInformation = False
232
233
234 if not testIsActive:
235 SC.renderer.Start() #start graphics visualization
236 if methodNum == 0:
237 SC.renderer.DoIdleTasks() #wait for pressing SPACE bar to continue
238
239 mbs.SolveDynamic(simulationSettings)
240
241 if not testIsActive:
242 #SC.renderer.DoIdleTasks()
243 SC.renderer.Stop() #safely close rendering window!
244
245 if False:
246 mbs.PlotSensor(sPosList, components=[2]*len(sPosList))
247
248 #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
249 ode2 = mbs.systemData.GetODE2Coordinates()
250 listSolutions.append(ode2)
251
252 testSolution += np.linalg.norm(ode2)
253
254diff = listSolutions[0] - listSolutions[1]
255exu.Print('solution diff=',np.linalg.norm(diff))
256
257for i, sol in enumerate(listSolutions):
258 exu.Print('solver=',str(methodList[i]),'\nsol=',sol[0:6])
259
260#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
261exu.Print('solution of createSphereQuadContact=',testSolution)
262exu.sys['testResult'] = testSolution
263#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++