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)]
100[nTower,bTower]=AddRigidBody(mainSys = mbs,
101                      inertia = inertiaTower,
102                      nodeType = exu.NodeType.RotationEulerParameters,
103                      position = posTower,
104                      gravity = g,
105                      graphicsDataList = graphicsTower)
106
107## add joint for tower
108markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
109markerTowerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bTower, localPosition=[0,-0.5*H,0]))
110oJointTower = mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerTowerGround],
111                                    constrainedAxes=[1,1,1,1,1,1],
112                                    visualization=VGenericJoint(axesRadius=0.5*sJoint, axesLength=1.5*sJoint)))
113
114
115#++++++++++++++++++++++++++++
116## set up rigid body for carriage, initially located at midspan or arm
117Vcarr = Dcarr*Dcarr*Lcarr
118
119inertiaCarr = InertiaCuboid(100/Vcarr, [Lcarr,Dcarr,Dcarr])
120posCarr = posArm + np.array([0,-carrYoff,0])
121
122zRoll = np.array([0,0,tRoll])
123pRollCarr =  [None,None,None,None,None]
124pRollCarr[0] = [-0.5*Lcarr,0,-hookZoff]
125pRollCarr[1] = [-0.5*Lcarr,0, hookZoff]
126pRollCarr[2] = [ 0.5*Lcarr,0,-hookZoff]
127pRollCarr[3] = [ 0.5*Lcarr,0, hookZoff]
128pRollCarr[4] = [ 0.5*Lcarr,0, 0*hookZoff]
129
130## add graphics data for carriage
131graphicsCarr = []
132for p in pRollCarr:
133    graphicsCarr += [graphics.Cylinder(p-0.5*zRoll,zRoll,radius=rHook, color=colorRolls, addEdges=True)]
134
135graphicsCarr += [graphics.Brick([0,0,0],[1.2*Lcarr,0.2*Dcarr,1.2*Dcarr],color=graphics.color.grey[0:3]+[0.5], addEdges = True)]
136
137### add rigid body for carriage
138[nCarr,bCarr]=AddRigidBody(mainSys = mbs,
139                      inertia = inertiaCarr,
140                      nodeType = exu.NodeType.RotationEulerParameters,
141                      position = posCarr,
142                      angularVelocity=[0,0,0],
143                      gravity = g,
144                      graphicsDataList = graphicsCarr)
145
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)]
164[nArm,bArm]=AddRigidBody(mainSys = mbs,
165                      inertia = inertiaArm,
166                      nodeType = exu.NodeType.RotationEulerParameters,
167                      position = posArm,
168                      angularVelocity=[0,0.1*0,0],
169                      gravity = g,
170                      graphicsDataList = graphicsArm)
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
256[nHook,bHook]=AddRigidBody(mainSys = mbs,
257                      inertia = inertiaHook,
258                      nodeType = exu.NodeType.RotationEulerParameters,
259                      position = posHook,
260                      angularVelocity=[0,0,0],
261                      gravity = g,
262                      graphicsDataList = graphicsHook)
263
264
265## create list of markers for hook-reeving system
266markerListHook = []
267markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[-0.5*L,-carrYoff+2*rHook,0]))]
268markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[0]))]
269markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[0]))]
270markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[1]))]
271markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[1]))]
272markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[3]))]
273markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[3]))]
274markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[2]))]
275markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bHook, localPosition=pRollHook[2]))]
276markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCarr, localPosition=pRollCarr[4]))]
277markerListHook+= [mbs.AddMarker(MarkerBodyRigid(bodyNumber=bArm, localPosition=[ 0.5*L,-carrYoff+2*rHook,0]))]
278markerListHook+=[mNodeHook,mNodeGround]
279
280LrefRopeHook = 8*0.5*H+L+8*pi*rHook
281
282## create list of axes for reeving system of hook
283sheavesAxesHook = exu.Vector3DList()
284sheavesAxesHook.Append([0,0,1])
285sheavesAxesHook.Append([0,0,-1])
286sheavesAxesHook.Append([0,0,1]) #Hook0
287sheavesAxesHook.Append([0,0,1])
288sheavesAxesHook.Append([0,0,1]) #Hook1
289sheavesAxesHook.Append([0,0,-1])
290sheavesAxesHook.Append([0,0,-1]) #Hook2
291sheavesAxesHook.Append([0,0,-1])
292sheavesAxesHook.Append([0,0,-1]) #Hook3
293sheavesAxesHook.Append([0,0,-1])
294sheavesAxesHook.Append([0,0,1])
295
296radiiRollHook = []
297for i in range(len(sheavesAxesHook)):
298    radiiRollHook += [rHook]
299
300## create reeving system for hook
301oRScarr1=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerListHook,
302                                            hasCoordinateMarkers=True, coordinateFactors=[1,0],
303                                            stiffnessPerLength=stiffnessRope, dampingPerLength=dampingRope*0.1, referenceLength = LrefRopeHook,
304                                            dampingTorsional = dampingRopeTorsional*0.1, dampingShear = dampingRopeShear,
305                                            sheavesAxes=sheavesAxesHook, sheavesRadii=radiiRollHook,
306                                            visualization=VReevingSystemSprings(ropeRadius=rRope, color=graphics.color.dodgerblue)))
307
308
309## add sensors to show trace of hook
310sPosTCP = mbs.AddSensor(SensorNode(nodeNumber=nHook, storeInternal=True,
311                                   outputVariableType=exu.OutputVariableType.Position))
312sRotTCP = mbs.AddSensor(SensorNode(nodeNumber=nHook, storeInternal=True,
313                                   outputVariableType=exu.OutputVariableType.RotationMatrix))
314
315#%% +++++++++++++++++++++++++++++++
316# #add sensors
317# if True:
318#     sPos1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
319#                                           outputVariableType=exu.OutputVariableType.Position))
320#     sOmega1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
321#                                           outputVariableType=exu.OutputVariableType.AngularVelocity))
322#     sLength= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
323#                                           outputVariableType=exu.OutputVariableType.Distance))
324#     sLength_t= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
325#                                           outputVariableType=exu.OutputVariableType.VelocityLocal))
326
327## create pre-step user function to drive crane system over time
328def PreStepUserFunction(mbs, t):
329    if t <= 10:
330        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t,  0, 10, 0, 0.45*L))
331    elif t <= 20:
332        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 10, 20, 0, -8*0.40*H))
333    elif t <= 30:
334        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t, 20, 30, 0.45*L,-0.4*L))
335    elif t <= 40:
336        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 30, 40, -8*0.40*H,8*0.45*H))
337    else:
338        mbs.SetObjectParameter(ccCarr, 'offset', SmoothStep(t, 40, 57, -0.4*L, 0.45*L))
339        mbs.SetObjectParameter(ccHook, 'offset', SmoothStep(t, 40, 60, 8*0.45*H, 6*0.45*H))
340
341    return True
342
343## add pre-step user function to mbs
344mbs.SetPreStepUserFunction(PreStepUserFunction)
345
346#%%++++++++++++++++++++++++++++++++++++++++++++++++
347## assemble and add simulation settings
348mbs.Assemble()
349
350simulationSettings = exu.SimulationSettings() #takes currently set values or default values
351
352tEnd = 80
353h=0.001
354
355solutionFile = 'solution/coordsCrane.txt'
356
357simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
358simulationSettings.timeIntegration.endTime = tEnd
359simulationSettings.solutionSettings.writeSolutionToFile= True #set False for CPU performance measurement
360simulationSettings.solutionSettings.solutionWritePeriod= 0.2
361simulationSettings.solutionSettings.coordinatesSolutionFileName = solutionFile
362simulationSettings.solutionSettings.sensorsWritePeriod = 0.02
363# simulationSettings.timeIntegration.simulateInRealtime=True
364# simulationSettings.timeIntegration.realtimeFactor=5
365SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
366simulationSettings.parallel.numberOfThreads=4
367simulationSettings.displayComputationTime = True
368
369simulationSettings.timeIntegration.verboseMode = 1
370
371simulationSettings.timeIntegration.newton.useModifiedNewton = True
372
373if True:
374    #traces:
375    SC.visualizationSettings.sensors.traces.listOfPositionSensors = [sPosTCP]
376    SC.visualizationSettings.sensors.traces.listOfTriadSensors =[sRotTCP]
377    SC.visualizationSettings.sensors.traces.showPositionTrace=True
378    SC.visualizationSettings.sensors.traces.showTriads=True
379    SC.visualizationSettings.sensors.traces.triadSize=2
380    SC.visualizationSettings.sensors.traces.showVectors=False
381    SC.visualizationSettings.sensors.traces.showFuture=False
382    SC.visualizationSettings.sensors.traces.triadsShowEvery=5
383
384
385SC.visualizationSettings.nodes.show = True
386SC.visualizationSettings.nodes.drawNodesAsPoint  = False
387SC.visualizationSettings.nodes.showBasis = True
388SC.visualizationSettings.nodes.basisSize = 0.2
389
390SC.visualizationSettings.openGL.multiSampling = 4
391SC.visualizationSettings.openGL.shadow = 0.3*0
392SC.visualizationSettings.openGL.light0position = [-50,200,100,0]
393
394SC.visualizationSettings.window.renderWindowSize=[1920,1200]
395#SC.visualizationSettings.general.autoFitScene = False #use loaded render state
396
397## start renderer and dynamic simulation
398useGraphics = True
399if useGraphics:
400    exu.StartRenderer()
401    if 'renderState' in exu.sys:
402        SC.SetRenderState(exu.sys[ 'renderState' ])
403    mbs.WaitForUserToContinue()
404
405
406mbs.SolveDynamic(simulationSettings,
407                 #solverType=exu.DynamicSolverType.TrapezoidalIndex2 #in this case, drift shows up significantly!
408                 )
409
410if useGraphics:
411    SC.WaitForRenderEngineStopFlag()
412    exu.StopRenderer() #safely close rendering window!
413
414
415## optionally start solution viewer at end of simulation
416if True:
417    #%%++++++++++++
418
419    SC.visualizationSettings.general.autoFitScene = False
420    mbs.SolutionViewer() #loads solution file via name stored in mbs
421
422#%%++++++++++++
423## optionally plot sensors for crane
424if False:
425
426    mbs.PlotSensor(sPos1, components=[0,1,2], labels=['pos X','pos Y','pos Z'], closeAll=True)
427    mbs.PlotSensor(sOmega1, components=[0,1,2], labels=['omega X','omega Y','omega Z'])
428    mbs.PlotSensor(sLength, components=[0], labels=['length'])
429    mbs.PlotSensor(sLength_t, components=[0], labels=['vel'])
430
431
432#compute error for test suite:
433sol2 = mbs.systemData.GetODE2Coordinates();
434u = np.linalg.norm(sol2);
435exu.Print('solution of craneReevingSystem=',u)