chainDriveExample.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test of a chain drive for ContactCurveCircles
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2024-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 *
 15import exudyn.graphics as graphics
 16import numpy as np
 17
 18#**function: unique drawing for chain plate; reference position is at first roller;
 19#link is oriented into x-axis, thickness in z-axis, height in y-axis (height of rectangular part);
 20#zOff is the center of the thickness of the plate
 21#returns a single graphicsData object
 22def GraphicsChainPlate(lengthLink, heightLink, thicknessLink, radiusLink,
 23                       pOff=[0,0,0], color=[-1,-1,-1,-1], nTiles=16,
 24                       addEdges = False, addFaces=True):
 25    if heightLink > 2*radiusLink:
 26        raise ValueError('GraphicsChainPlate: height must by <= 2*radius!')
 27    from numpy import sin, cos, arcsin
 28
 29    gList = []
 30    vertices = []
 31    segments = []
 32    lastIndex = 2*(nTiles+1)-1
 33    phiStart = arcsin(0.5*heightLink/radiusLink)
 34
 35    #check if rectangular section can be drawn:
 36    if lengthLink - 2*cos(phiStart)*radiusLink < 0:
 37        phiStart = pi/2
 38
 39    phiEnd = 2*pi-phiStart
 40    phiRange = phiEnd-phiStart
 41
 42    for i in range(nTiles+1):
 43        phi = i/nTiles*phiRange+phiStart
 44        vertices.append([radiusLink*cos(phi),radiusLink*sin(phi)])
 45
 46    for i in range(nTiles+1):
 47        phi = i/nTiles*phiRange+pi+phiStart
 48        vertices.append([radiusLink*cos(phi)+lengthLink,radiusLink*sin(phi)])
 49
 50    segments = np.vstack((np.arange(2*(nTiles+1)),
 51                          np.arange(2*(nTiles+1))+1)).T
 52    segments[-1,-1] = 0
 53
 54    return graphics.SolidExtrusion(vertices, segments.tolist(), thicknessLink, pOff=np.array(pOff)-[0,0,0.5*thicknessLink],
 55                            color=color,addEdges=addEdges,addFaces=addFaces)
 56
 57
 58#**function: unique drawing for chain plate; reference position is at first roller;
 59#link is oriented into x-axis, thickness in z-axis, height in y-axis (height of rectangular part);
 60#zOff is the center of the thickness of the plate
 61#returns a single graphicsData object
 62def GraphicsChainLink(lengthLink, heightLink, thicknessLink, radiusLink, innerWidthLink,
 63                      radiusRoller, radiusPin, withPin, pOff=[0,0,0],
 64                      colorLink=graphics.color.grey, colorRoller=[0.7,0.7,0.8,1], colorPin=graphics.color.darkgrey,
 65                      nTiles=16, addEdges = False, addFaces=True):
 66
 67    gcl = []
 68    gcl += [graphics.Cylinder([0,0,-0.5*(withPin)],[0,0,withPin], radius=radiusPin,
 69                              color=colorRoller, nTiles=nTiles, addEdges=addEdges, addFaces=addFaces)]
 70    gcl += [GraphicsChainPlate(lengthLink, heightLink, thicknessLink, radiusLink,
 71                               pOff=[0,0,0.5*(innerWidthLink+thicknessLink)], color=colorLink,
 72                               nTiles=nTiles, addEdges=addEdges, addFaces=addFaces)]
 73    gcl += [GraphicsChainPlate(lengthLink, heightLink, thicknessLink, radiusLink,
 74                               pOff=[0,0,-0.5*(innerWidthLink+thicknessLink)], color=colorLink,
 75                               nTiles=nTiles, addEdges=addEdges, addFaces=addFaces)]
 76    gcl += [graphics.Cylinder([0,0,-0.5*(innerWidthLink)],[0,0,innerWidthLink], radius=radiusRoller,
 77                              color=colorRoller, nTiles=nTiles, addEdges=addEdges, addFaces=addFaces)]
 78    return gcl
 79
 80#unique drawing for roller chain link:
 81
 82
 83useGraphics = True #without test
 84SC = exu.SystemContainer()
 85mbs = SC.AddSystem()
 86
 87pi = np.pi
 88mass = 1
 89nTeeth = 16
 90nLinks3 = 7 #free links between sprockets
 91phiTooth = 2*pi/nTeeth
 92rRoller = 0.005
 93lLink = 0.03                                #Length of link; l = 2*r*sin(phi/2)
 94wLink = 0.012                                #inner with of link
 95tpLink = 0.002                              #thickness of plate
 96hpLink = 0.012                              #height of plate
 97rpLink = 1.5*rRoller                        #radius of plate
 98rSprocketPitch = lLink/(2*sin(phiTooth/2))  #radius to center of holes in gear, given by chain link length and nTeeth!
 99rSprocketInner = rSprocketPitch - rRoller
100wSprocket = wLink*0.95
101addSecondWheel = True
102contactStiffness = 1e4*5
103contactDamping = 1e1
104
105exu.Print('Chain geometry:')
106exu.Print('  lLink  =',lLink)
107exu.Print('  rRoller=',rRoller)
108exu.Print('  rPitch =',rSprocketPitch)
109exu.Print('  rInner =',rSprocketPitch)
110
111tEnd = 10     #end time of simulation
112stepSize = 1e-4
113g = 9.81
114
115gGround = graphics.CheckerBoard(point = [0,-rSprocketInner*1.5,-wSprocket*2], size = 0.5)
116nGround = mbs.AddNode(NodePointGround())
117mCoordinateGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0))
118
119oGround = mbs.CreateGround(referencePosition=[0,0,0],graphicsDataList=[gGround])
120mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround))
121
122#ground node
123gListSprocket = [graphics.Cylinder([0,0,-0.5*wSprocket],[0,0,wSprocket],
124                 radius=rSprocketPitch, color=graphics.color.grey, nTiles=64)]
125
126pList = []
127nSprocket = nTeeth*4 #8
128for i in range(nSprocket):
129    angle = (i-0.5)/(nSprocket)*2*pi
130    r = rSprocketInner + 2*rRoller*(i%4 > 1)
131    x =-r*cos(angle)
132    y =-r*sin(angle)
133    pList.append([x,y])
134
135segments = np.vstack((np.arange(nSprocket),
136                        np.arange(nSprocket)+1)).T
137segments[-1,-1] = 0
138segmentsData = np.zeros((nSprocket,4))
139segmentsData[:,0:2] = pList
140segmentsData[:,2:4] = np.roll(pList,2) #roll is element wise on rows and columns, therefore 2>shift one row
141
142print(segmentsData[:5,:])
143
144gListSprocket = [
145    graphics.SolidExtrusion(pList, segments.tolist(), wSprocket, pOff=[0,0,-0.5*wSprocket],
146                            color=graphics.color.lightgrey,addEdges=True,addFaces=True),
147    graphics.Cylinder(pAxis=[0,0,-wSprocket], vAxis=[0,0,2*wSprocket], radius = rRoller, color=graphics.color.grey),
148    graphics.Brick(size=[0.5*wSprocket,wSprocket*2,wSprocket*1.05], color=graphics.color.red)
149]
150
151
152
153#sprocket wheel:
154sprocket = mbs.CreateRigidBody(referencePosition=[0,0,0],
155                                inertia=InertiaCylinder(density=100,length=wSprocket,outerRadius=rSprocketPitch,axis=2),
156                                gravity = [0,-g,0],
157                                graphicsDataList=gListSprocket,
158                                create2D=True
159                                )
160mSprocket = mbs.AddMarker(MarkerBodyRigid(bodyNumber=sprocket))
161mbs.AddObject(RevoluteJoint2D(markerNumbers=[mGround, mSprocket],
162                                visualization=VRevoluteJoint2D(drawSize=rRoller)) )
163
164#add prescribed velocity:
165if True:
166    def UFvelocityDrive(mbs, t, itemNumber, lOffset): #time derivative of UFoffset
167        vStart = 0
168        vEnd = pi
169        return SmoothStep(t, 0, 0.5, vStart, vEnd)
170
171    nodeSprocket = mbs.GetObject(sprocket)['nodeNumber']
172    mCoordinateWheel = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nodeSprocket, coordinate=2))
173    velControl = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround, mCoordinateWheel],
174                                        velocityLevel=True, offsetUserFunction_t= UFvelocityDrive,
175                                        visualization=VCoordinateConstraint(show = False)))#UFvelocityDrive
176else:
177    mbs.AddLoad(Torque(markerNumber=mSprocket, loadVector=[0,0,-0.1]))
178
179sVel1 = mbs.AddSensor(SensorBody(bodyNumber=sprocket, storeInternal=True, outputVariableType=exu.OutputVariableType.AngularVelocity))
180
181if addSecondWheel:
182    oGround2 = mbs.CreateGround(referencePosition=[0,-nLinks3*lLink,0],graphicsDataList=[])
183    mGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround2))
184    sprocket2 = mbs.CreateRigidBody(referencePosition=[0,-nLinks3*lLink,0],
185                                    inertia=InertiaCylinder(density=100,length=wSprocket,outerRadius=rSprocketPitch,axis=2),
186                                    gravity = [0,-g,0],
187                                    graphicsDataList=gListSprocket,
188                                    create2D=True
189                                    )
190    # nn = mbs.GetObject(sprocket)['nodeNumber']
191    mSprocket2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=sprocket2))
192    mbs.AddObject(SphericalJoint(markerNumbers=[mGround2, mSprocket2],
193                                 constrainedAxes=[1,0,0],
194                                 visualization=VSphericalJoint(jointRadius=0.25*rRoller)) )
195
196    mbs.AddObject(LinearSpringDamper(markerNumbers=[mGround2, mSprocket2],
197                                     stiffness = 1000, damping=100,
198                                     axisMarker0=[0,1,0], force = 10,
199                                     visualization=VLinearSpringDamper(drawSize=0.01),
200                                     ))
201
202    dTSD = 0.01
203    mbs.AddObject(TorsionalSpringDamper(markerNumbers = [mGround, mSprocket2], damping = dTSD))
204    #mbs.AddLoad(Torque(markerNumber=mSprocket2, loadVector=[0,0,0.2]))
205    sVel2 = mbs.AddSensor(SensorBody(bodyNumber=sprocket2, storeInternal=True, outputVariableType=exu.OutputVariableType.AngularVelocity))
206
207
208#graphics for chain link:
209gChainLink1 = GraphicsChainLink(lLink, hpLink, tpLink, rpLink, wLink,
210                      rRoller, 0.3*rRoller, wLink+4.5*tpLink, pOff=[0,0,0],
211                      nTiles=32, addEdges = True)
212gChainLink2 = GraphicsChainLink(lLink, hpLink, tpLink, rpLink, wLink+tpLink*2,
213                      rRoller, 0.3*rRoller, wLink+4.5*tpLink, pOff=[0,0,0],
214                      nTiles=32, addEdges = True)
215
216#create geometry of hanging chain:
217posList = []
218rotList = []
219for i in range(nLinks3):
220    x=-rSprocketPitch
221    y=-(nLinks3-i)*lLink
222    posList.append([x,y])
223    rotList.append(pi/2)
224
225for i in range(int(nTeeth/2)):
226    angle = i/(nTeeth/2)*pi
227    x=-rSprocketPitch*cos(angle)
228    y= rSprocketPitch*sin(angle)
229    posList.append([x,y])
230    rotList.append(pi/2-angle-0.5/(nTeeth/2)*pi)
231
232for i in range(nLinks3):
233    x= rSprocketPitch
234    y=-(i+0)*lLink
235    posList.append([x,y])
236    rotList.append(-pi/2)
237
238for i in range(int(nTeeth/2)):
239    angle = -(i+0)/(nTeeth/2)*pi
240    x= rSprocketPitch*cos(angle)
241    y= rSprocketPitch*sin(angle)
242    posList.append([x,y-nLinks3*lLink])
243    rotList.append(-pi/2+angle-0.5/(nTeeth/2)*pi)
244
245oLinksList = []
246mLinksList = []
247
248prevMarker = None
249firstMarker = None
250for i in range(len(posList)):
251    pos = posList[i]
252    #note: in 2D case, the reference point = COM, which is not correct for chain; for correct representation, move ref. point to COM of link!
253    link = mbs.CreateRigidBody(referencePosition=pos+[0],
254                               inertia=InertiaCuboid(1000,sideLengths=[lLink,wLink,wLink]),
255                               referenceRotationMatrix=RotationMatrixZ(rotList[i]),
256                               gravity = [0,-g,0],
257                               graphicsDataList=gChainLink1 if i%2==0 else gChainLink2,
258                               create2D=True
259                               )
260    # nn = mbs.GetObject(sprocket)['nodeNumber']
261    oLinksList += [link]
262    mLinksList += [mbs.AddMarker(MarkerBodyRigid(bodyNumber=link))]
263
264    if prevMarker is not None:
265        mbs.AddObject(RevoluteJoint2D(markerNumbers=[prevMarker, mLinksList[-1]],
266                                      visualization=VRevoluteJoint2D(drawSize=rRoller)) )
267    else:
268        firstMarker = mLinksList[-1]
269
270    prevMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=link, localPosition=[lLink,0,0]))
271
272#close chain:
273mbs.AddObject(RevoluteJoint2D(markerNumbers=[prevMarker, firstMarker],
274                                visualization=VRevoluteJoint2D(drawSize=rRoller)) )
275
276
277if False: #put one link to ground:
278    posLast = mbs.GetMarkerOutput(prevMarker, variableType=exu.OutputVariableType.Position,
279                                  configuration=exu.ConfigurationType.Reference)
280
281    oGround2 = mbs.CreateGround(referencePosition=posLast)
282    mGround2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround2))
283    mbs.AddObject(RevoluteJoint2D(markerNumbers=[prevMarker, mGround2],
284                                  visualization=VRevoluteJoint2D(drawSize=rRoller)) )
285
286nGenericData = mbs.AddNode(NodeGenericData(initialCoordinates=[-1,0,0]*nSprocket,
287                                           numberOfDataCoordinates=3*nSprocket))
288mbs.AddObject(ObjectContactCurveCircles(markerNumbers=[mSprocket]+mLinksList, nodeNumber=nGenericData,
289                                        circlesRadii=[rRoller]*len(mLinksList), segmentsData=exu.MatrixContainer(segmentsData),
290                                        contactStiffness=contactStiffness, contactDamping=contactDamping))
291
292if addSecondWheel:
293    nGenericData2 = mbs.AddNode(NodeGenericData(initialCoordinates=[-1,0,0]*nSprocket,
294                                               numberOfDataCoordinates=3*nSprocket))
295    mbs.AddObject(ObjectContactCurveCircles(markerNumbers=[mSprocket2]+mLinksList, nodeNumber=nGenericData2,
296                                            circlesRadii=[rRoller]*len(mLinksList), segmentsData=exu.MatrixContainer(segmentsData),
297                                            contactStiffness=contactStiffness, contactDamping=contactDamping))
298
299
300mbs.Assemble()
301
302simulationSettings = exu.SimulationSettings()
303simulationSettings.solutionSettings.writeSolutionToFile = True
304simulationSettings.solutionSettings.solutionWritePeriod = 0.01
305simulationSettings.solutionSettings.sensorsWritePeriod = stepSize  #output interval
306simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
307simulationSettings.timeIntegration.endTime = tEnd
308#simulationSettings.timeIntegration.simulateInRealtime = True
309# simulationSettings.timeIntegration.discontinuous.iterationTolerance = 1e-2
310# simulationSettings.timeIntegration.discontinuous.useRecommendedStepSize = False
311
312simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
313simulationSettings.timeIntegration.newton.useModifiedNewton = True
314#simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 1
315
316simulationSettings.displayStatistics = True
317simulationSettings.timeIntegration.verboseMode = 1
318
319SC.visualizationSettings.window.renderWindowSize=[1600,2000]
320SC.visualizationSettings.openGL.multiSampling=4
321#SC.visualizationSettings.openGL.facesTransparent=True
322SC.visualizationSettings.openGL.shadow=0.3
323SC.visualizationSettings.loads.show = False
324SC.visualizationSettings.connectors.contactPointsDefaultSize = 0.001
325SC.visualizationSettings.connectors.showContact = True
326
327SC.renderer.Start()              #start graphics visualization
328SC.renderer.DoIdleTasks()    #wait for pressing SPACE bar to continue
329
330mbs.SolveDynamic(simulationSettings)
331
332SC.renderer.DoIdleTasks()#wait for pressing 'Q' to quit
333SC.renderer.Stop()               #safely close rendering window!
334
335#plot results:
336if False:
337    mbs.PlotSensor([sVel1,sVel2], components=[2,2], closeAll=True)
338    import matplotlib.pyplot as plt
339    plt.show(block=True) #for figures to stay open at end of plot routines
340
341#+++++++++++++++++++++++++++++++++++++++++++++++++++++
342
343mbs.SolutionViewer()