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