reevingSystemSpringsTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  A simple 3D reeving system;
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2022-06-16
  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 * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics
 16
 17testIsActive = exu.sys.get('testIsActive', False)
 18
 19import numpy as np
 20from math import sin, cos, sqrt,pi
 21
 22SC = exu.SystemContainer()
 23mbs = SC.AddSystem()
 24
 25gGround = graphics.CheckerBoard(point=[8,-8,-2], size=32, nTiles=10)
 26oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
 27
 28t = 0.1
 29r0 = 0.5
 30g = [0,-9.81,0]
 31
 32posList = [[0,0,0], 
 33           [7,-20,1], 
 34           [12,0,2],
 35           ]
 36rList = [r0*1.,4*r0,r0,5.9*r0,r0,r0,r0]
 37
 38# posList = [[0,0,0], 
 39#            [5,-20,1], 
 40#            [8,0,2],
 41#            [12,-10,0], 
 42#            [16,0,0] 
 43#            ]
 44# rList = [r0*0.,6*r0,r0,5.9*r0,r0,r0,r0]
 45dirList = [-1,1,-1,1,-1,1,1]
 46
 47
 48n = len(posList)
 49nodeList = []
 50bodyList = []
 51markerList = []
 52sheavesRadii = []
 53sheavesAxes = exu.Vector3DList()
 54Lref = 0
 55pLast = [0,0,0]
 56
 57for i, pos in enumerate(posList):
 58    r = rList[i]
 59
 60    graphicsRoll = graphics.Cylinder(pAxis=[0,0,-0.5*t], vAxis=[0,0,t], nTiles=32, radius=r, color=graphics.color.dodgerblue, alternatingColor=graphics.color.darkgrey)
 61    
 62    inertiaRoll = InertiaCylinder(density=1000,length=t,outerRadius=r, axis=2)
 63    inertiaRoll = inertiaRoll.Translated([0,-r,0])
 64
 65    dictR = mbs.CreateRigidBody(referencePosition=pos,  
 66                                inertia=inertiaRoll,  
 67                                gravity=g,  
 68                                graphicsDataList=[graphicsRoll, graphics.Basis(inertiaRoll.COM(), length=0.5)],  
 69                                returnDict=True)  
 70    nR = dictR['nodeNumber']  
 71    bR = dictR['bodyNumber']
 72
 73    mR = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nR))
 74    nodeList += [nR]
 75    bodyList += [bR]
 76    markerList += [mR]
 77
 78    sheavesAxes.Append([0,0,dirList[i]*1])
 79    sheavesRadii += [r]
 80
 81    if i != 0:
 82        Lref += np.linalg.norm(np.array(pos)-pLast)
 83    if i > 0 and i < len(posList)-1:
 84        #note that in this test example, Lref is slightly too long, leading to negative spring forces (compression) if not treated nonlinearly with default settings in ReevingSystemSprings
 85        Lref += r*pi #0.8*r*pi would always lead to tension
 86
 87    #mR = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bR, localPosition=[0,-r,0]))
 88    #mbs.AddLoad(Force(markerNumber=mR, loadVector=[0,-inertiaRoll.mass*9.81,0]))
 89        
 90    pLast = pos
 91
 92
 93markerGround0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[0]))
 94markerGroundMid = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[2]))
 95markerGroundLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=posList[-1]))
 96
 97#sMarkerR = mbs.AddSensor(SensorMarker(markerNumber=markerR, outputVariableType=exu.OutputVariableType.Position))
 98
 99#%%++++++++++++++++++++++++++++++++++++++++++++++++
100#add joints:
101oJoint0 = mbs.AddObject(GenericJoint(markerNumbers=[markerGround0, markerList[0]],
102                                    constrainedAxes=[1,1,1,1,1,1],
103                                    visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
104oJointLast = mbs.AddObject(GenericJoint(markerNumbers=[markerGroundLast, markerList[-1]],
105                                    constrainedAxes=[1,1,1,1,1,1],
106                                    visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
107
108if len(posList) > 3:
109    mbs.AddObject(GenericJoint(markerNumbers=[markerGroundMid, markerList[2]],
110                                        constrainedAxes=[1,1,1,1,1,1],
111                                        visualization=VGenericJoint(axesRadius=0.5*t, axesLength=1.5*t)))
112
113#%%++++++++++++++++++++++++++++++++++++++++++++++++
114#add reeving system spring
115stiffness = 1e5 #stiffness per length
116damping = 0.5*stiffness #dampiung per length
117oRS=mbs.AddObject(ReevingSystemSprings(markerNumbers=markerList, hasCoordinateMarkers=False, 
118                                   stiffnessPerLength=stiffness, dampingPerLength=damping, 
119                                   referenceLength = Lref,
120                                   dampingTorsional = 0.01*damping, dampingShear = 0.1*damping,
121                                   sheavesAxes=sheavesAxes, sheavesRadii=sheavesRadii,
122                                   #regularizationForce = -1, #purely linear system
123                                   visualization=VReevingSystemSprings(ropeRadius=0.05, color=graphics.color.dodgerblue)))
124
125#%%++++++++++++++++++++++++++++++++++++++++++++++++
126#add sensors 
127if True:
128    sPos1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
129                                          outputVariableType=exu.OutputVariableType.Position))
130    sOmega1 = mbs.AddSensor(SensorNode(nodeNumber=nodeList[1], storeInternal=True,
131                                          outputVariableType=exu.OutputVariableType.AngularVelocity))
132    sLength= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
133                                          outputVariableType=exu.OutputVariableType.Distance))
134    sLength_t= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
135                                          outputVariableType=exu.OutputVariableType.VelocityLocal))
136    sForce= mbs.AddSensor(SensorObject(objectNumber=oRS, storeInternal=True,
137                                          outputVariableType=exu.OutputVariableType.ForceLocal))
138
139#%%++++++++++++++++++++++++++++++++++++++++++++++++
140#simulate:
141mbs.Assemble()
142
143simulationSettings = exu.SimulationSettings() #takes currently set values or default values
144
145tEnd = 2
146if not testIsActive:
147    tEnd = 2 #200
148h=0.01  #use small step size to detext contact switching
149
150simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
151simulationSettings.timeIntegration.endTime = tEnd
152#simulationSettings.solution.file.write= True #set False for CPU performance measurement
153simulationSettings.solution.sensors.writePeriod = 0.01
154
155simulationSettings.timeIntegration.verboseMode = 1
156
157simulationSettings.timeIntegration.newton.useModifiedNewton = True
158
159SC.visualizationSettings.nodes.show = True
160SC.visualizationSettings.nodes.drawNodesAsPoint  = False
161SC.visualizationSettings.nodes.showBasis = True
162SC.visualizationSettings.nodes.basisSize = 0.2
163
164SC.visualizationSettings.openGL.multiSampling = 4
165
166#SC.visualizationSettings.general.autoFitScene = False #use loaded render state
167# testIsActive = False
168if not testIsActive:
169    SC.renderer.Start()
170    SC.renderer.RestoreSavedState()
171    SC.renderer.DoIdleTasks()
172
173
174mbs.SolveDynamic(simulationSettings, 
175                 #solverType=exu.DynamicSolverType.TrapezoidalIndex2 #in this case, drift shows up significantly!
176                 )
177
178if not testIsActive:
179    SC.renderer.DoIdleTasks()
180    SC.renderer.Stop() #safely close rendering window!
181
182if True:
183    
184
185    mbs.PlotSensor(sPos1, components=[0,1,2], labels=['pos X','pos Y','pos Z'], closeAll=True)
186    mbs.PlotSensor(sOmega1, components=[0,1,2], labels=['omega X','omega Y','omega Z'])
187    mbs.PlotSensor(sLength, components=[0], labels=['length'])
188    mbs.PlotSensor(sLength_t, components=[0], labels=['vel'])
189    mbs.PlotSensor(sForce, components=[0], labels=['force'])
190
191
192
193
194#compute error for test suite:
195sol2 = mbs.systemData.GetODE2Coordinates(); 
196u = np.linalg.norm(sol2); 
197exu.Print('solution of ReevingSystemSprings=',u)
198
199exu.sys['testResult'] = u