contactCurveExample.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Simple test for ObjectContactCurveCircles
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2025-05-10
  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#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12import exudyn as exu
 13from exudyn.utilities import *
 14import exudyn.graphics as graphics
 15import numpy as np
 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
 29SC = exu.SystemContainer()
 30mbs = SC.AddSystem()
 31
 32L = 2
 33a = 0.1
 34b = 0.1
 35rPin = a*0.75
 36
 37oGround = mbs.CreateGround(graphicsDataList=[graphics.CheckerBoard(point=[0,0,-b], size=3*L)])
 38
 39contactStiffness = 2e5*10
 40contactDamping = 5e2*10
 41
 42inertiaBrick=InertiaCuboid(1000, sideLengths=[L,a,b])
 43
 44#Example 1: arm moving in given curve:
 45if True:
 46    pOff = np.array([-L,0,0])
 47    inertiaBrick1 = inertiaBrick.Translated([-0.5*L,0,0])
 48    lever = mbs.CreateRigidBody(
 49                                referencePosition=pOff+[1*L+1*rPin,-0.5*L+rPin,0],
 50                                inertia=inertiaBrick1,
 51                                gravity = [0,-g,0],
 52                                graphicsDataList=[graphics.Brick(centerPoint=[-0.5*L,0,0], size=[L,a,b], color=graphics.color.dodgerblue),
 53                                                  graphics.Basis(inertiaBrick1.COM()),
 54                                                  graphics.Sphere(point=[-L,0,0], radius=rPin, nTiles=16, color=graphics.color.darkgrey)],
 55                                #create2D=True, #not possible here, as COM must be 0 with 2D
 56                                )
 57
 58    mBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=lever, localPosition=[-L,0,0]))
 59
 60    #create 2D points for contact curve (linear segments)
 61    pList = []
 62    n = 64
 63
 64    for i in range(2*n+2):
 65        phi = -1*pi+i/(n-1)*1*pi
 66        r = L*0.5
 67        if i >= n:
 68            phi = 0.*pi-phi
 69            r = 0.5*L-2*rPin
 70        x = r*sin(phi)
 71        y = -r*cos(phi)
 72        if i == n:
 73            x = 0.25*L
 74            y = -0.5*L
 75        if i == n+1:
 76            x = 0.25*L
 77            y = -0.5*L+2*rPin
 78
 79        pList.append([x,y])
 80
 81
 82    #transform points into contact segments:
 83    nPoints = len(pList)
 84    segmentsData = np.zeros((nPoints,4))
 85    segmentsData[:,0:2] = pList
 86    segmentsData[:,2:4] = np.roll(pList,2) #roll is element wise on rows and columns, therefore 2>shift one row
 87
 88    segmentsData = segmentsData[1:,:] #we do not like to close curve, so remove first segment
 89    nSeg = len(segmentsData)
 90
 91    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=pOff+[0,0,0]))
 92    nGenericData = mbs.AddNode(NodeGenericData(initialCoordinates=[-1,0,0]*nSeg,
 93                                               numberOfDataCoordinates=3*nSeg))
 94
 95    mbs.AddObject(ObjectContactCurveCircles(markerNumbers=[mGround]+[mBody],
 96                                            nodeNumber=nGenericData,
 97                                            circlesRadii=[rPin],
 98                                            segmentsData=exu.MatrixContainer(segmentsData),
 99                                            contactStiffness=contactStiffness, contactDamping=contactDamping,
100                                            visualization=VObjectContactCurveCircles(show=True, color=graphics.color.blue)
101                                            ))
102
103    mbs.CreateCoordinateConstraint(bodyNumbers=[lever,None],coordinates=[1,None])
104    mbs.CreateCoordinateConstraint(bodyNumbers=[lever,None],coordinates=[2,None])
105
106    def UFoffset_t(mbs, t, itemNumber, lOffset): #time derivative of UFoffset
107        return 2.2*L*sin(2*pi*t)
108
109    def UFoffset(mbs, t, itemNumber, lOffset): #time derivative of UFoffset
110        return -0.7*(cos(2*pi*t)-1)
111
112    mbs.CreateCoordinateConstraint(bodyNumbers=[lever,None],coordinates=[0,None],
113                                   #velocityLevel=True,
114                                   offsetUserFunction_t=UFoffset_t,
115                                   offsetUserFunction=UFoffset,
116                                   )
117
118#Example 2: pendulum in bucket:
119if True:
120    pOff = np.array([L*1.5,0,0])
121
122    lever = mbs.CreateRigidBody(referencePosition=pOff+[-0.9*L,rPin,0],
123                                inertia=inertiaBrick,
124                                gravity = [0,-g,0],
125                                create2D=True,
126                                graphicsDataList=[graphics.Brick(size=[L,a,b], color=graphics.color.orange),
127                                                  graphics.Sphere(point=[0.5*L,0,0], radius=rPin, nTiles=16, color=graphics.color.darkgrey)],
128                                )
129
130    mBody = mbs.AddMarker(MarkerBodyRigid(bodyNumber=lever, localPosition=[0.5*L,0,0]))
131
132    #create 2D points for contact curve (linear segments)
133    pList = []
134
135    pList.append([-1,0.5])
136    pList.append([-1,0.])
137    pList.append([1,0.])
138    pList.append([1,0.5])
139
140    #transform points into contact segments:
141    nPoints = len(pList)
142    segmentsData = np.zeros((nPoints,4))
143    segmentsData[:,0:2] = pList
144    segmentsData[:,2:4] = np.roll(pList,2) #roll is element wise on rows and columns, therefore 2>shift one row
145
146    segmentsData = segmentsData[1:,:]
147    nSeg = len(segmentsData)
148
149    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=pOff+[0,0,0]))
150    nGenericData = mbs.AddNode(NodeGenericData(initialCoordinates=[-1,0,0]*nSeg,
151                                               numberOfDataCoordinates=3*nSeg))
152
153    mbs.AddObject(ObjectContactCurveCircles(markerNumbers=[mGround]+[mBody],
154                                            nodeNumber=nGenericData,
155                                            circlesRadii=[rPin],
156                                            segmentsData=exu.MatrixContainer(segmentsData),
157                                            contactStiffness=contactStiffness, contactDamping=contactDamping,
158                                            visualization=VObjectContactCurveCircles(show=True, color=graphics.color.blue)
159                                            ))
160
161
162mbs.Assemble()
163
164stepSize=0.001
165tEnd = 1
166simulationSettings = exu.SimulationSettings()
167simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
168simulationSettings.solutionSettings.solutionWritePeriod = 0.01
169simulationSettings.solutionSettings.sensorsWritePeriod = stepSize  #output interval
170simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
171simulationSettings.timeIntegration.endTime = tEnd
172# simulationSettings.timeIntegration.simulateInRealtime = True
173#simulationSettings.timeIntegration.realtimeFactor = 0.5
174# simulationSettings.timeIntegration.discontinuous.iterationTolerance = 1e-2
175# simulationSettings.timeIntegration.discontinuous.useRecommendedStepSize = False
176
177#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
178simulationSettings.timeIntegration.newton.useModifiedNewton = True
179#simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 1
180
181simulationSettings.timeIntegration.verboseMode = 1
182
183SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
184SC.visualizationSettings.window.renderWindowSize=[1600,2000]
185SC.visualizationSettings.openGL.multiSampling=4
186#SC.visualizationSettings.openGL.facesTransparent=True
187SC.visualizationSettings.openGL.shadow=0.3*useGraphics
188SC.visualizationSettings.loads.show = False
189SC.visualizationSettings.connectors.showContact = True
190
191
192mbs.SolveDynamic(simulationSettings)
193
194#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
195ode2 = mbs.systemData.GetODE2Coordinates()
196
197u = 0.1*np.linalg.norm(ode2)
198exu.Print('solution of contactCurveExample=',u)
199
200exudynTestGlobals.testResult = u
201#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
202
203if useGraphics:
204    mbs.SolutionViewer()