reevingSystemOpen.py
You can view and download this file on Github: reevingSystemOpen.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Example to calculate rope line of reeving system
5#
6# Author: Johannes Gerstmayr
7# Date: 2022-02-02
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.itemInterface import *
15from exudyn.utilities import *
16from exudyn.beams import *
17from exudyn.robotics import *
18
19import numpy as np
20from math import sin, cos, pi, sqrt , asin, acos, atan2
21import copy
22
23SC = exu.SystemContainer()
24mbs = SC.AddSystem()
25
26#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27#+++ MOTION PLANNING and TRAJECTORIES +++++++++++++++++++++++++++++++++++++++++++++
28#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29#**function: Compute parameters for optimal trajectory using given duration and distance
30#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
31#**input: duration in seconds and distance in meters or radians
32#**output: returns [vMax, accMax] with maximum velocity and maximum acceleration to achieve given trajectory
33def ConstantAccelerationParameters(duration, distance):
34 accMax = 4*distance/duration**2
35 vMax = (accMax * distance)**0.5
36 return [vMax, accMax]
37
38#**function: Compute angle / displacement s, velocity v and acceleration a
39#**input:
40# t: current time to compute values
41# tStart: start time of profile
42# sStart: start offset of path
43# duration: duration of profile
44# distance: total distance (of path) of profile
45#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
46#**output: [s, v, a] with path s, velocity v and acceleration a for constant acceleration profile; before tStart, solution is [0,0,0] while after duration, solution is [sStart+distance, 0, 0]
47def ConstantAccelerationProfile(t, tStart, sStart, duration, distance):
48 [vMax, accMax] = ConstantAccelerationParameters(duration, distance)
49
50 s = sStart
51 v = 0
52 a = 0
53
54 x = t-tStart
55 if x < 0:
56 s=0
57 elif x < 0.5*duration:
58 s = sStart + 0.5*accMax*x**2
59 v = x*accMax
60 a = accMax
61 elif x < duration:
62 s = sStart + distance - 0.5*accMax * (duration-x)**2
63 v = (duration - x)*accMax
64 a = -accMax
65 else:
66 s = sStart + distance
67
68 return [s, v, a]
69
70#**function: Compute joint value, velocity and acceleration for given robotTrajectory['PTP'] of point-to-point type, evaluated for current time t and joint number
71#**input:
72# t: time to evaluate trajectory
73# robotTrajectory: dictionary to describe trajectory; in PTP case, either use 'time' points, or 'time' and 'duration', or 'time' and 'maxVelocity' and 'maxAccelerations' in all consecutive points; 'maxVelocities' and 'maxAccelerations' must be positive nonzero values that limit velocities and accelerations;
74# joint: joint number for which the trajectory shall be evaluated
75#**output: for current time t it returns [s, v, a] with path s, velocity v and acceleration a for current acceleration profile; outside of profile, it returns [0,0,0] !
76#**notes: DEPRECATED, DO NOT USE - moved to robotics.motion
77#**example:
78# q0 = [0,0,0,0,0,0] #initial configuration
79# q1 = [8,5,2,0,2,1] #other configuration
80# PTP =[]
81# PTP+=[{'q':q0,
82# 'time':0}]
83# PTP+=[{'q':q1,
84# 'time':0.5}]
85# PTP+=[{'q':q1,
86# 'time':1e6}] #forever
87# RT={'PTP':PTP}
88# [u,v,a] = MotionInterpolator(t=0.5, robotTrajectory=RT, joint=1)
89def MotionInterpolator(t, robotTrajectory, joint):
90
91 n = len(robotTrajectory['PTP'])
92 if n < 2:
93 print("ERROR in MotionInterpolator: trajectory must have at least 2 points!")
94
95 i = 0
96 while (i < n) and (t >= robotTrajectory['PTP'][i]['time']):
97 i += 1
98
99 if (i==0) or (i==n):
100 return [0,0,0] #outside of trajectory
101
102 #i must be > 0 and < n now!
103 q0 = robotTrajectory['PTP'][i-1] #must always exist
104 q1 = robotTrajectory['PTP'][i] #must always exist
105
106 return ConstantAccelerationProfile(t, q0['time'], q0['q'][joint],
107 q1['time'] - q0['time'],
108 q1['q'][joint] - q0['q'][joint])
109
110
111
112#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
113#settings:
114useGraphics= True
115useContact = True
116useFriction = True
117# kProp = 10
118dryFriction = 2*0.5
119contactStiffness = 1e5
120contactDamping = 2e-3*contactStiffness
121
122wheelMass = 1
123wheelInertia = 0.01
124
125rotationDampingWheels = 0.01 #proportional to rotation speed
126torque = 1
127
128#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
129#create circles
130#complicated shape:
131nANCFnodes = 1*50
132stepSize = 5e-4
133tEnd = 10
134R=0.45
135preStretch=-0.002*0 #not needed here, system is open!
136circleList = [
137 [[ 0,-3 ],R,'L'],
138 [[ 0,0 ],R,'L'],
139 [[0.5,-0.8],R,'R'],
140 [[ 1,0 ],R,'L'],
141 [[ 1,-3 ],R,'L'],
142 ]
143
144#create motion profile:
145point0={'q':[0], #initial configuration
146 'time':0}
147point1={'q':[2.5/R],
148 'time':2}
149point2={'q':[-2.5/R],
150 'time':4}
151point3={'q':[0],
152 'time':5}
153pointLast={'q':[0], #add this a second time to stay this forever
154 'time':1e6} #forever
155RT={'PTP':[point0,point1,point2,point3,pointLast]}
156
157#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
158#create geometry:
159reevingDict = CreateReevingCurve(circleList, drawingLinesPerCircle = 64,
160 removeLastLine=False, #allows closed curve
161 numberOfANCFnodes=nANCFnodes, graphicsNodeSize= 0.01)
162
163del circleList[-1] #remove circles not needed for contact/visualization
164del circleList[0] #remove circles not needed for contact/visualization
165
166gList=[]
167if False: #visualize reeving curve, without simulation
168 gList = reevingDict['graphicsDataLines'] + reevingDict['graphicsDataCircles']
169
170oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
171 visualization=VObjectGround(graphicsData= gList)))
172
173#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
174#create ANCF elements:
175
176gVec = np.array([0,-9.81,0]) # gravity
177E=1e9 # Young's modulus of ANCF element in N/m^2
178rhoBeam=1000 # density of ANCF element in kg/m^3
179b=0.002 # width of rectangular ANCF element in m
180h=0.002 # height of rectangular ANCF element in m
181A=b*h # cross sectional area of ANCF element in m^2
182I=b*h**3/12 # second moment of area of ANCF element in m^4
183dEI = 1e-3*E*I #bending proportional damping
184dEA = 1e-2*E*A #axial strain proportional damping
185
186dimZ = b #z.dimension
187
188cableTemplate = Cable2D(#physicsLength = L / nElements, #set in GenerateStraightLineANCFCable2D(...)
189 physicsMassPerLength = rhoBeam*A,
190 physicsBendingStiffness = E*I,
191 physicsAxialStiffness = E*A,
192 physicsBendingDamping = dEI,
193 physicsAxialDamping = dEA,
194 physicsReferenceAxialStrain = preStretch, #prestretch
195 visualization=VCable2D(drawHeight=h),
196 )
197
198ancf = PointsAndSlopes2ANCFCable2D(mbs, reevingDict['ancfPointsSlopes'], reevingDict['elementLengths'],
199 cableTemplate, massProportionalLoad=gVec,
200 #fixedConstraintsNode0=[1,1,1,1], fixedConstraintsNode1=[1,1,1,1],
201 firstNodeIsLastNode=False, graphicsSizeConstraints=0.01)
202
203#+++++++++++++++++++++++++++++++++++++++
204#add weights
205node0 = ancf[0][0]
206nodeL = ancf[0][-1]
207massLoad=2
208
209bMass0 = mbs.AddObject(ObjectMassPoint2D(physicsMass=massLoad,
210 nodeNumber=node0,
211 visualization=VMassPoint2D(graphicsData=[GraphicsDataSphere(radius=0.1, nTiles=32)])))
212bMassL = mbs.AddObject(ObjectMassPoint2D(physicsMass=massLoad,
213 nodeNumber=nodeL,
214 visualization=VMassPoint2D(graphicsData=[GraphicsDataSphere(radius=0.1, nTiles=32)])))
215
216mBody0=mbs.AddMarker(MarkerBodyPosition(bodyNumber=bMass0))
217mbs.AddLoad(Force(markerNumber=mBody0, loadVector=massLoad*gVec))
218mBodyL=mbs.AddMarker(MarkerBodyPosition(bodyNumber=bMassL))
219mbs.AddLoad(Force(markerNumber=mBodyL, loadVector=massLoad*gVec))
220
221#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
222#add contact:
223if useContact:
224
225 gContact = mbs.AddGeneralContact()
226 gContact.verboseMode = 1
227 gContact.frictionProportionalZone = 0.5
228 gContact.ancfCableUseExactMethod = False
229 gContact.ancfCableNumberOfContactSegments = 4
230 gContact
231 ssx = 32#32 #search tree size
232 ssy = 32#32 #search tree size
233 ssz = 1 #search tree size
234 gContact.SetSearchTreeCellSize(numberOfCells=[ssx,ssy,ssz])
235 #gContact.SetSearchTreeBox(pMin=np.array([-1,-1,-1]), pMax=np.array([4,1,1])) #automatically computed!
236
237 halfHeight = 0.5*h*0
238 dimZ= 0.01 #for drawing
239 # wheels = [{'center':wheelCenter0, 'radius':rWheel0-halfHeight, 'mass':mWheel},
240 # {'center':wheelCenter1, 'radius':rWheel1-halfHeight, 'mass':mWheel},
241 # {'center':rollCenter1, 'radius':rRoll-halfHeight, 'mass':mRoll}, #small mass for roll, not to influence beam
242 # ]
243 sWheelRot = [] #sensors for angular velocity
244
245 nGround = mbs.AddNode(NodePointGround())
246 mCoordinateGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0))
247
248 for i, wheel in enumerate(circleList):
249 p = [wheel[0][0], wheel[0][1], 0] #position of wheel center
250 r = wheel[1]
251
252 rot0 = 0 #initial rotation
253 pRef = [p[0], p[1], rot0]
254 gList = [GraphicsDataCylinder(pAxis=[0,0,-dimZ],vAxis=[0,0,-dimZ], radius=r,
255 color= color4lightgrey, nTiles=64),
256 GraphicsDataArrow(pAxis=[0,0,0], vAxis=[0.9*r,0,0], radius=0.01*r, color=color4orange)]
257
258 omega0 = 0 #initial angular velocity
259 v0 = np.array([0,0,omega0])
260
261 nMass = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=pRef, initialVelocities=v0,
262 visualization=VNodeRigidBody2D(drawSize=dimZ*2)))
263 oMass = mbs.AddObject(ObjectRigidBody2D(physicsMass=wheelMass, physicsInertia=wheelInertia,
264 nodeNumber=nMass, visualization=
265 VObjectRigidBody2D(graphicsData=gList)))
266 mNode = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nMass))
267 mGroundWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=p))
268
269 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mGroundWheel, mNode]))
270 sWheelRot += [mbs.AddSensor(SensorNode(nodeNumber=nMass,
271 fileName='solution/wheel'+str(i)+'angVel.txt',
272 outputVariableType=exu.OutputVariableType.AngularVelocity))]
273
274 #add drive with prescribed velocity:
275 def UFvelocityDrive(mbs, t, itemNumber, lOffset): #time derivative of UFoffset
276 v = 10*t
277 vMax = 5
278 return max(v, vMax)
279
280 def UFmotionDrive(mbs, t, itemNumber, lOffset):
281 [u,v,a] = MotionInterpolator(t, robotTrajectory=RT, joint=0)
282 return u
283
284
285 velocityControl = False
286 if i == 1:
287 mCoordinateWheel = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nMass, coordinate=2))
288 if velocityControl:
289 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround, mCoordinateWheel],
290 velocityLevel=True, offsetUserFunction_t=UFvelocityDrive))
291 else: #position control
292 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround, mCoordinateWheel],
293 offsetUserFunction=UFmotionDrive))
294
295 if i > 0: #friction on rolls:
296 mCoordinateWheel = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nMass, coordinate=2))
297 mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mCoordinateGround, mCoordinateWheel],
298 damping=rotationDampingWheels,
299 visualization=VCoordinateSpringDamper(show=False)))
300
301 frictionMaterialIndex=0
302 gContact.AddSphereWithMarker(mNode, radius=r, contactStiffness=contactStiffness,
303 contactDamping=contactDamping, frictionMaterialIndex=frictionMaterialIndex)
304
305
306
307 for oIndex in ancf[1]:
308 gContact.AddANCFCable(objectIndex=oIndex, halfHeight=halfHeight, #halfHeight should be h/2, but then cylinders should be smaller
309 contactStiffness=contactStiffness, contactDamping=contactDamping,
310 frictionMaterialIndex=0)
311
312 frictionMatrix = np.zeros((2,2))
313 frictionMatrix[0,0]=int(useFriction)*dryFriction
314 frictionMatrix[0,1]=0 #no friction between some rolls and cable
315 frictionMatrix[1,0]=0 #no friction between some rolls and cable
316 gContact.SetFrictionPairings(frictionMatrix)
317
318
319mbs.Assemble()
320
321simulationSettings = exu.SimulationSettings() #takes currently set values or default values
322
323simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
324simulationSettings.solutionSettings.coordinatesSolutionFileName = 'solution/coordinatesSolution.txt'
325simulationSettings.solutionSettings.writeSolutionToFile = True
326simulationSettings.solutionSettings.solutionWritePeriod = 0.005
327simulationSettings.solutionSettings.sensorsWritePeriod = 0.001
328#simulationSettings.displayComputationTime = True
329simulationSettings.parallel.numberOfThreads = 4 #use 4 to speed up for > 100 ANCF elements
330simulationSettings.displayStatistics = True
331
332simulationSettings.timeIntegration.endTime = tEnd
333simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
334simulationSettings.timeIntegration.stepInformation= 3+4+32#+128+256
335#simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-4
336simulationSettings.timeIntegration.newton.relativeTolerance = 1e-6
337#simulationSettings.timeIntegration.discontinuous.iterationTolerance = 10
338simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep=True
339simulationSettings.timeIntegration.newton.maxIterations=16
340simulationSettings.timeIntegration.verboseMode = 1
341# simulationSettings.timeIntegration.adaptiveStepRecoveryIterations = 9
342simulationSettings.timeIntegration.adaptiveStepRecoverySteps = 40
343
344simulationSettings.timeIntegration.newton.useModifiedNewton = True
345simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
346simulationSettings.displayStatistics = True
347
348SC.visualizationSettings.general.circleTiling = 24
349SC.visualizationSettings.loads.show=False
350SC.visualizationSettings.nodes.defaultSize = 0.01
351SC.visualizationSettings.openGL.multiSampling = 4
352SC.visualizationSettings.openGL.lineWidth = 2
353
354# SC.visualizationSettings.general.useGradientBackground = True
355simulationSettings.solutionSettings.solutionInformation = 'elevator'
356# SC.visualizationSettings.general.textSize = 14
357
358SC.visualizationSettings.window.renderWindowSize = [1024,2000]
359
360if False:
361 SC.visualizationSettings.contour.outputVariableComponent=0
362 SC.visualizationSettings.contour.outputVariable=exu.OutputVariableType.ForceLocal
363
364#visualize contact:
365if False:
366 SC.visualizationSettings.contact.showSearchTree =True
367 SC.visualizationSettings.contact.showSearchTreeCells =True
368 SC.visualizationSettings.contact.showBoundingBoxes = True
369
370if useGraphics:
371 exu.StartRenderer()
372 mbs.WaitForUserToContinue()
373
374if True:
375 doDynamic = True
376 if doDynamic :
377 exu.SolveDynamic(mbs, simulationSettings) #183 Newton iterations, 0.114 seconds
378 else:
379 exu.SolveStatic(mbs, simulationSettings) #183 Newton iterations, 0.114 seconds
380
381
382if useGraphics and True:
383 SC.visualizationSettings.general.autoFitScene = False
384 SC.visualizationSettings.general.graphicsUpdateInterval=0.02
385 from exudyn.interactive import SolutionViewer
386 sol = LoadSolutionFile('solution/coordinatesSolution.txt', safeMode=True)#, maxRows=100)
387 SolutionViewer(mbs, sol)
388
389
390if useGraphics:
391 SC.WaitForRenderEngineStopFlag()
392 exu.StopRenderer() #safely close rendering window!
393
394 # if True:
395 # from exudyn.plot import PlotSensor
396 # PlotSensor(mbs, sensorNumbers=[sAngVel[0],sAngVel[1]], components=2, closeAll=True)
397 # PlotSensor(mbs, sensorNumbers=sMeasureRoll, components=1)