craneReevingSystem.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  A crane model using two objects ReevingSystemSprings for the rope and hoisting mechanism
  5#           Tower as well as arm are modeled as rigid bodies connected by joints, such that the can move
  6#
  7# Model:    Crane with reeving system
  8#
  9# Author:   Johannes Gerstmayr
 10# Date:     2022-06-16
 11#
 12# 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.
 13#
 14# *clean example*
 15#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 16
 17## import exudyn package, utils and math packages
 18import exudyn as exu
 19from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 20import exudyn.graphics as graphics #only import if it does not conflict
 21
 22import numpy as np
 23from math import sin, cos, sqrt,pi
 24
 25## create system mbs
 26SC = exu.SystemContainer()
 27mbs = SC.AddSystem()
 28
 29## create ground with checker board background
 30gGround = graphics.CheckerBoard(point=[0,0,0], normal = [0,1,0], size=60, nTiles=12)
 31oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
 32
 33## define parameters of crane
 34tRoll = 0.05    #thickness rolls (graphics)
 35rHook = 0.2     #radius of Hook rolls
 36rCarr = 0.3     #radius of Carriage rolls
 37g = [0,-9.81,0]
 38colorRolls = graphics.color.red
 39
 40H = 40 #crane height
 41L = 30 #boom length
 42Dtower = 1.5
 43Darm = 1
 44Lcarr = 1
 45Dcarr = 0.6
 46Lhook = 0.5
 47Dhook = 0.5
 48
 49## define parameters of rope
 50rRope = 0.025 #drawing radius
 51A = rRope**2*pi
 52EArope = 1e9*A
 53print('EArope=',EArope)
 54stiffnessRope = EArope #stiffness per length
 55dampingRope = 0.1*stiffnessRope #dampiung per length
 56dampingRopeTorsional = 1e-4*dampingRope
 57dampingRopeShear = 0.1*dampingRope*0.001
 58
 59##
 60#further crane parameters: (height=Y, arm=X)
 61carrYoff = 0.3*Darm
 62hookZoff = 0.4*Dhook
 63hookYoff = 0.5*H
 64#hookZoffVec = np.array([0,0,hookZoff])
 65
 66posTower = np.array([0,0.5*H,0])
 67posArm = 2*posTower + np.array([0.5*L,0,0])
 68
 69sJoint = 0.5 #overal joint size for main joints
 70
 71#++++++++++++++++++++++++++++
 72## add ground node and ground marker for constraints
 73nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
 74mNodeGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0))#andy coordinate is zero
 75
 76## add nodes and markers for prescribed motion in reeving system
 77nCoordCarr = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0], initialCoordinates=[0],
 78                                         initialCoordinates_t=[0], numberOfODE2Coordinates=1))
 79nCoordHook = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0], initialCoordinates=[0],
 80                                         initialCoordinates_t=[0], numberOfODE2Coordinates=1))
 81mNodeCarr = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nCoordCarr, coordinate=0))
 82mNodeHook = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nCoordHook, coordinate=0))
 83
 84## add 1D mass object for dynamics of main rope drum
 85mbs.AddObject(Mass1D(physicsMass=1, nodeNumber=nCoordCarr))
 86mbs.AddObject(Mass1D(physicsMass=1, nodeNumber=nCoordHook))
 87
 88## add coordinate constraint for prescribed motion using offset later on
 89ccCarr = mbs.AddObject(CoordinateConstraint(markerNumbers=[mNodeGround, mNodeCarr], offset=0))
 90ccHook = mbs.AddObject(CoordinateConstraint(markerNumbers=[mNodeGround, mNodeHook], offset=0))
 91
 92#++++++++++++++++++++++++++++
 93## set up rigid body for tower
 94Vtower = Dtower*Dtower*H
 95inertiaTower = InertiaCuboid(2000/Vtower, [Dtower,H,Dtower])
 96
 97## model tower as body, which may be moved as well ...
 98graphicsTower = [graphics.Brick([0,0,0],[Dtower,H,Dtower],color=graphics.color.grey, addEdges = True)]
 99graphicsTower += [graphics.Cylinder([0,0.5*H-Darm*0.5,0],[0,0.5*Darm,0],radius=1.1*Darm, color=graphics.color.grey)]
100dictTower = mbs.CreateRigidBody(
101              inertia=inertiaTower,
102              referencePosition=posTower,
103              gravity=g,
104              graphicsDataList=graphicsTower,
105              returnDict=True)
106[nTower, bTower] = [dictTower['nodeNumber'], dictTower['bodyNumber']]
107
108## add joint for tower
109markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
110markerTowerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bTower, localPosition=[0,-0.5*H,0]))
111oJointTower = mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerTowerGround],
112                                    constrainedAxes=[1,1,1,1,1,1],
113                                    visualization=VGenericJoint(axesRadius=0.5*sJoint, axesLength=1.5*sJoint)))
114
115
116#++++++++++++++++++++++++++++
117## set up rigid body for carriage, initially located at midspan or arm
118Vcarr = Dcarr*Dcarr*Lcarr
119
120inertiaCarr = InertiaCuboid(100/Vcarr, [Lcarr,Dcarr,Dcarr])
121posCarr = posArm + np.array([0,-carrYoff,0])
122
123zRoll = np.array([0,0,tRoll])
124pRollCarr =  [None,None,None,None,None]
125pRollCarr[0] = [-0.5*Lcarr,0,-hookZoff]
126pRollCarr[1] = [-0.5*Lcarr,0, hookZoff]
127pRollCarr[2] = [ 0.5*Lcarr,0,-hookZoff]
128pRollCarr[3] = [ 0.5*Lcarr,0, hookZoff]
129pRollCarr[4] = [ 0.5*Lcarr,0, 0*hookZoff]
130
131## add graphics data for carriage
132graphicsCarr = []
133for p in pRollCarr:
134    graphicsCarr += [graphics.Cylinder(p-0.5*zRoll,zRoll,radius=rHook, color=colorRolls, addEdges=True)]
135
136graphicsCarr += [graphics.Brick([0,0,0],[1.2*Lcarr,0.2*Dcarr,1.2*Dcarr],color=graphics.color.grey[0:3]+[0.5], addEdges = True)]
137
138### add rigid body for carriage
139dictCarr = mbs.CreateRigidBody(
140              inertia=inertiaCarr,
141              referencePosition=posCarr,
142              gravity=g,
143              graphicsDataList=graphicsCarr,
144              returnDict=True)
145[nCarr, bCarr] = [dictCarr['nodeNumber'], dictCarr['bodyNumber']]
146
147#++++++++++++++++++++++++++++
148## set up arm
149Varm = Darm*Darm*L
150inertiaArm = InertiaCuboid(2000/Varm, [L,Darm,Darm])
151
152pRollArm =  [None,None,None]
153pRollArm[0] = [-0.5*L, rCarr,0]
154pRollArm[1] = [ 0.5*L,0     ,0]
155pRollArm[2] = [-0.5*L,-rCarr,0]
156rRollArm = [0,rCarr,0]
157
158## add tower as rigid body
159graphicsArm = []
160for i,p in enumerate(pRollArm):
161    graphicsArm += [graphics.Cylinder(p-0.5*zRoll,zRoll,radius=max(0.1*rCarr,rRollArm[i]), color=colorRolls, addEdges=True)]
162
163graphicsArm += [graphics.Brick([-0.1*L,0,0],[L*1.2,Darm,Darm],color=[0.3,0.3,0.9,0.5], addEdges = True)]
164dictArm = mbs.CreateRigidBody(
165              inertia=inertiaArm,
166              referencePosition=posArm,
167              gravity=g,
168              graphicsDataList=graphicsArm,
169              returnDict=True)
170[nArm, bArm] = [dictArm['nodeNumber'], dictArm['bodyNumber']]
171
172## create revolute joint between tower and arm
173markerTowerArm = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bTower, localPosition=[0,0.5*H,0]))
174markerArmTower = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[-0.5*L,0,0]))
175oJointArm = mbs.AddObject(GenericJoint(markerNumbers=[markerTowerArm, markerArmTower],
176                                    constrainedAxes=[1,1,1,1,0,1],
177                                    visualization=VGenericJoint(axesRadius=0.5*sJoint, axesLength=1.5*sJoint)))
178
179## create prismatic joint between arm and carriage
180markerArmCarr = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[0,-carrYoff,0]))
181markerCarrArm = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=[0,0,0]))
182oJointCarr = mbs.AddObject(GenericJoint(markerNumbers=[markerArmCarr, markerCarrArm],
183                                    constrainedAxes=[0,1,1,1,1,1],
184                                    visualization=VGenericJoint(axesRadius=0.5*sJoint, axesLength=1.5*sJoint)))
185
186#++++++++++++++++++++++++++++
187## set up marker lists and local axes for sheaves for reeving system for motion of carriage at tower
188markerListCarriage1 = []
189markerListCarriage1+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=pRollArm[0]))]
190markerListCarriage1+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=pRollArm[1]))]
191markerListCarriage1+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=[ 0.5*Lcarr,0,0]))]
192markerListCarriage1+=[mNodeCarr,mNodeGround]
193
194LrefRopeCarriage1 = L+pi*rCarr+0.5*L-0.5*Lcarr
195
196sheavesAxes1 = exu.Vector3DList()
197for i, radius in enumerate(rRollArm):
198    sheavesAxes1.Append([0,0,-1])
199
200## set up marker lists and local axes for sheaves for reeving system for motion of carriage at arm end
201markerListCarriage2 = []
202markerListCarriage2+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=pRollArm[2]))]
203markerListCarriage2+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=[-0.5*Lcarr,0,0]))]
204markerListCarriage2+=[mNodeCarr,mNodeGround]
205
206LrefRopeCarriage2 = 0.5*L-0.5*Lcarr
207
208
209#needs just two points:
210sheavesAxes2 = exu.Vector3DList()
211sheavesAxes2.Append([0,0,-1])
212sheavesAxes2.Append([0,0,-1])
213
214## create first reeving system object for carriage
215oRScarr1=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerListCarriage1,
216                                            hasCoordinateMarkers=True, coordinateFactors=[-1,0],#negative direction X
217                                            stiffnessPerLength=stiffnessRope, dampingPerLength=dampingRope,
218                                            referenceLength = LrefRopeCarriage1,
219                                            dampingTorsional = dampingRopeTorsional, dampingShear = dampingRopeShear*0,
220                                            sheavesAxes=sheavesAxes1, sheavesRadii=rRollArm,
221                                            visualization=VReevingSystemSprings(ropeRadius=rRope, color=graphics.color.lawngreen)))
222
223## create second reeving system object for carriage
224oRScarr2=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerListCarriage2,
225                                            hasCoordinateMarkers=True, coordinateFactors=[1,0], #positive direction X
226                                            stiffnessPerLength=stiffnessRope, dampingPerLength=dampingRope,
227                                            referenceLength = LrefRopeCarriage2,
228                                            dampingTorsional = dampingRopeTorsional*0,
229                                            sheavesAxes=sheavesAxes2, sheavesRadii=[0,0],
230                                            visualization=VReevingSystemSprings(ropeRadius=rRope, color=graphics.color.lawngreen)))
231
232#++++++++++++++++++++++++++++
233## set up inertia and parameters for hook
234Vhook = Dhook*Dhook*Lhook
235
236inertiaHook = InertiaCuboid(100/Vhook, [Lhook,Dhook,Dhook])
237inertiaHook = inertiaHook.Translated([0,-Dhook,0])
238posHook = posCarr + np.array([0,-hookYoff,0])
239
240
241pRollHook =  [None,None,None,None]
242pRollHook[0] = [-0.5*Lhook,0,-hookZoff]
243pRollHook[1] = [-0.5*Lhook,0, hookZoff]
244pRollHook[2] = [ 0.5*Lhook,0,-hookZoff]
245pRollHook[3] = [ 0.5*Lhook,0, hookZoff]
246
247## set up graphics for hook
248graphicsHook = []
249for p in pRollHook:
250    graphicsHook += [graphics.Cylinder(p-0.5*zRoll,zRoll,radius=rHook, color=colorRolls, addEdges=True)]
251
252graphicsHook += [graphics.Brick([0,0,0],[Lhook,0.2*Dhook,Dhook],color=graphics.color.grey[0:3]+[0.5], addEdges = True)]
253graphicsHook += [graphics.Brick([0,-Dhook,0],[4*Lhook,2*Dhook,2*Dhook],color=graphics.color.grey[0:3]+[0.5], addEdges = True)]
254
255## add rigid body for hook
256dictHook = mbs.CreateRigidBody(
257              inertia=inertiaHook,
258              referencePosition=posHook,
259              initialAngularVelocity=[0,0,0],
260              gravity=g,
261              graphicsDataList=graphicsHook,
262              returnDict=True)
263[nHook, bHook] = [dictHook['nodeNumber'], dictHook['bodyNumber']]
264
265
266## create list of markers for hook-reeving system
267markerListHook = []
268markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[-0.5*L,-carrYoff+2*rHook,0]))]
269markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[0]))]
270markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[0]))]
271markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[1]))]
272markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[1]))]
273markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[3]))]
274markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[3]))]
275markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[2]))]
276markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[2]))]
277markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[4]))]
278markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[ 0.5*L,-carrYoff+2*rHook,0]))]
279markerListHook+=[mNodeHook,mNodeGround]
280
281LrefRopeHook = 8*0.5*H+L+8*pi*rHook
282
283## create list of axes for reeving system of hook
284sheavesAxesHook = exu.Vector3DList()
285sheavesAxesHook.Append([0,0,1])
286sheavesAxesHook.Append([0,0,-1])
287sheavesAxesHook.Append([0,0,1]) #Hook0
288sheavesAxesHook.Append([0,0,1])
289sheavesAxesHook.Append([0,0,1]) #Hook1
290sheavesAxesHook.Append([0,0,-1])
291sheavesAxesHook.Append([0,0,-1]) #Hook2
292sheavesAxesHook.Append([0,0,-1])
293sheavesAxesHook.Append([0,0,-1]) #Hook3
294sheavesAxesHook.Append([0,0,-1])
295sheavesAxesHook.Append([0,0,1])
296
297radiiRollHook = []
298for i in range(len(sheavesAxesHook)):
299    radiiRollHook += [rHook]
300
301## create reeving system for hook
302oRScarr1=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerListHook,
303                                            hasCoordinateMarkers=True, coordinateFactors=[1,0],
304                                            stiffnessPerLength=stiffnessRope, dampingPerLength=dampingRope*0.1, referenceLength = LrefRopeHook,
305                                            dampingTorsional = dampingRopeTorsional*0.1, dampingShear = dampingRopeShear,
306                                            sheavesAxes=sheavesAxesHook, sheavesRadii=radiiRollHook,
307                                            visualization=VReevingSystemSprings(ropeRadius=rRope, color=graphics.color.dodgerblue)))
308
309
310## add sensors to show trace of hook
311sPosTCP = mbs.AddSensor(SensorNode(nodeNumber=nHook, storeInternal=True,
312                                   outputVariableType=exu.OutputVariableType.Position))
313sRotTCP = mbs.AddSensor(SensorNode(nodeNumber=nHook, storeInternal=True,
314                                   outputVariableType=exu.OutputVariableType.RotationMatrix))
315
316#%% +++++++++++++++++++++++++++++++
317# #add sensors
318# if True:
319#     sPos1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
320#                                           outputVariableType=exu.OutputVariableType.Position))
321#     sOmega1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
322#                                           outputVariableType=exu.OutputVariableType.AngularVelocity))
323#     sLength= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
324#                                           outputVariableType=exu.OutputVariableType.Distance))
325#     sLength_t= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
326#                                           outputVariableType=exu.OutputVariableType.VelocityLocal))
327
328## create pre-step user function to drive crane system over time
329def PreStepUserFunction(mbs, t):
330    if t <= 10:
331        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t,  0, 10, 0, 0.45*L))
332    elif t <= 20:
333        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 10, 20, 0, -8*0.40*H))
334    elif t <= 30:
335        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t, 20, 30, 0.45*L,-0.4*L))
336    elif t <= 40:
337        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 30, 40, -8*0.40*H,8*0.45*H))
338    else:
339        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t, 40, 57, -0.4*L, 0.45*L))
340        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 40, 60, 8*0.45*H, 6*0.45*H))
341
342    return True
343
344## add pre-step user function to mbs
345mbs.SetPreStepUserFunction(PreStepUserFunction)
346
347#%%++++++++++++++++++++++++++++++++++++++++++++++++
348## assemble and add simulation settings
349mbs.Assemble()
350
351simulationSettings = exu.SimulationSettings() #takes currently set values or default values
352
353tEnd = 80
354h=0.001
355
356solutionFile = 'solution/coordsCrane.txt'
357
358simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
359simulationSettings.timeIntegration.endTime = tEnd
360simulationSettings.solutionSettings.writeSolutionToFile= True #set False for CPU performance measurement
361simulationSettings.solutionSettings.solutionWritePeriod= 0.2
362simulationSettings.solutionSettings.coordinatesSolutionFileName = solutionFile
363simulationSettings.solutionSettings.sensorsWritePeriod = 0.02
364# simulationSettings.timeIntegration.simulateInRealtime=True
365# simulationSettings.timeIntegration.realtimeFactor=5
366SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
367simulationSettings.parallel.numberOfThreads=4
368simulationSettings.displayComputationTime = True
369
370simulationSettings.timeIntegration.verboseMode = 1
371
372simulationSettings.timeIntegration.newton.useModifiedNewton = True
373
374if True:
375    #traces:
376    SC.visualizationSettings.sensors.traces.listOfPositionSensors = [sPosTCP]
377    SC.visualizationSettings.sensors.traces.listOfTriadSensors =[sRotTCP]
378    SC.visualizationSettings.sensors.traces.showPositionTrace=True
379    SC.visualizationSettings.sensors.traces.showTriads=True
380    SC.visualizationSettings.sensors.traces.triadSize=2
381    SC.visualizationSettings.sensors.traces.showVectors=False
382    SC.visualizationSettings.sensors.traces.showFuture=False
383    SC.visualizationSettings.sensors.traces.triadsShowEvery=5
384
385
386SC.visualizationSettings.nodes.show = True
387SC.visualizationSettings.nodes.drawNodesAsPoint  = False
388SC.visualizationSettings.nodes.showBasis = True
389SC.visualizationSettings.nodes.basisSize = 0.2
390
391SC.visualizationSettings.openGL.multiSampling = 4
392SC.visualizationSettings.openGL.shadow = 0.3*0
393SC.visualizationSettings.openGL.light0position = [-50,200,100,0]
394
395SC.visualizationSettings.window.renderWindowSize=[1920,1200]
396#SC.visualizationSettings.general.autoFitScene = False #use loaded render state
397
398## start renderer and dynamic simulation
399useGraphics = True
400if useGraphics:
401    exu.StartRenderer()
402    if 'renderState' in exu.sys:
403        SC.SetRenderState(exu.sys[ 'renderState' ])
404    mbs.WaitForUserToContinue()
405
406
407mbs.SolveDynamic(simulationSettings,
408                 #solverType=exu.DynamicSolverType.TrapezoidalIndex2 #in this case, drift shows up significantly!
409                 )
410
411if useGraphics:
412    SC.WaitForRenderEngineStopFlag()
413    exu.StopRenderer() #safely close rendering window!
414
415
416## optionally start solution viewer at end of simulation
417if True:
418    #%%++++++++++++
419
420    SC.visualizationSettings.general.autoFitScene = False
421    mbs.SolutionViewer() #loads solution file via name stored in mbs
422
423#%%++++++++++++
424## optionally plot sensors for crane
425if False:
426
427    mbs.PlotSensor(sPos1, components=[0,1,2], labels=['pos X','pos Y','pos Z'], closeAll=True)
428    mbs.PlotSensor(sOmega1, components=[0,1,2], labels=['omega X','omega Y','omega Z'])
429    mbs.PlotSensor(sLength, components=[0], labels=['length'])
430    mbs.PlotSensor(sLength_t, components=[0], labels=['vel'])
431
432
433#compute error for test suite:
434sol2 = mbs.systemData.GetODE2Coordinates();
435u = np.linalg.norm(sol2);
436exu.Print('solution of craneReevingSystem=',u)