universalJoint.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  universal joint with rigid bodies 
  5#           and compared with analytical solution
  6#
  7# Author:   Michael Pieber
  8# Date:     2023-09-20
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15#a local run must not open a plot window when windows are suppressed (#2477)
 16if exu.special.userInterface.suppressPlots: import matplotlib; matplotlib.use('Agg')
 17from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 18import exudyn.graphics as graphics
 19
 20import numpy as np
 21
 22import matplotlib.pyplot as plt
 23
 24
 25plt.close("all")
 26fontSize = 14
 27simulation = True
 28showPlotfigure=False
 29
 30
 31ComputeSystemDegreeOfFreedom = False #possible to show the degreeOfFreedom
 32overconstrainedSystem = False #possible to simulate, even if overconstrained
 33
 34solutionViewer = False
 35
 36
 37SC = exu.SystemContainer()
 38mbs = SC.AddSystem()
 39
 40
 41#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 42#parameters & dimensions
 43l=1 #m length of rigid shaft
 44r=0.3 #m radius of rigid shaft
 45
 46omega=10 #rad/s angular velocity
 47
 48angle=np.deg2rad(45) #vary between 0 and <90 degree; change angle of the joint
 49Ashaft2 = RotationVector2RotationMatrix([angle,0,0])
 50
 51#left rigid shaft
 52pMid0 = [0,0,0] #center of mass of left rigid shaft
 53massShaft=9 #kg
 54thetaLeft = np.eye(3)*1e2
 55inertiaLeftShaft = RigidBodyInertia(massShaft, thetaLeft)
 56
 57#right rigid shaft
 58inertiaCylinderRight = InertiaCylinder(density=1000, length=l,outerRadius=r,axis=2)
 59pMid1 = [0,-l/2*np.sin(angle),l/2+l/2*np.cos(angle)] #center of mass of right rigid shaft
 60
 61#cross shaft
 62inertiaCylinderMiddle = InertiaCylinder(density=1000, length=r,outerRadius=2*r,axis=2)
 63pMid2 = [0,0,l/2] #center of mass of cross shaft
 64
 65g = [0,0,0] #gravity
 66
 67
 68#parameters for simulation
 69tEnd=int(20*np.pi/omega) #simulation time
 70h=1e-3 #stepsize
 71
 72
 73
 74
 75#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 76#graphics for body
 77graphicsCrossShaft = graphics.Brick(size=[0.2*r]*3, color=graphics.color.grey)
 78
 79graphicsBodyLeft = [graphics.Cylinder(pAxis=[0,0,-l/2], vAxis=[0,0,2*l/2-r], 
 80                                         radius=r/8, color=graphics.color.steelblue, nTiles=32, alternatingColor=graphics.color.blue)]
 81graphicsBodyLeft += [graphics.Brick(centerPoint=[0,0,l/2-r], size=[2*r,0.25*r,0.25*r], color=graphics.color.steelblue)]
 82graphicsBodyLeft += [graphics.Brick(centerPoint=[r*0.875,0,l/2-0.5*r], size=[0.25*r,0.25*r,r*1.25], color=graphics.color.steelblue)]
 83graphicsBodyLeft += [graphics.Brick(centerPoint=[-r*0.875,0,l/2-0.5*r], size=[0.25*r,0.25*r,r*1.25], color=graphics.color.steelblue)]
 84
 85d2 = r*1.5
 86r2 = r*0.75
 87graphicsBodyRight = [graphics.Cylinder(pAxis=[0,0,-l/2+d2], vAxis=[0,0,2*l/2-d2], 
 88                                          radius=r/8, color=graphics.color.lawngreen, nTiles=32, alternatingColor=graphics.color.green)]
 89graphicsBodyRight += [graphics.Brick(centerPoint=[0,0,-l/2+d2], size=[0.25*r,2*r2,0.25*r], color=graphics.color.lawngreen)]
 90graphicsBodyRight += [graphics.Brick(centerPoint=[0,r2*0.875,-l/2+0.5*d2], size=[0.25*r,0.25*r,d2+r*0.25], color=graphics.color.lawngreen)]
 91graphicsBodyRight += [graphics.Brick(centerPoint=[0,-r2*0.875,-l/2+0.5*d2], size=[0.25*r,0.25*r,d2+r*0.25], color=graphics.color.lawngreen)]
 92
 93
 94# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 95#add rigid bodies
 96#left rigid shaft
 97#create simple rigid body
 98dictLeftShaft = mbs.CreateRigidBody(
 99                  inertia=inertiaLeftShaft, 
100                  nodeType=exu.NodeType.RotationRxyz, 
101                  referencePosition=pMid0, 
102                  initialAngularVelocity=[0,0,omega], 
103                  gravity=g,
104                  graphicsDataList=graphicsBodyLeft,
105                  returnDict=True)
106[n0, b0] = [dictLeftShaft['nodeNumber'], dictLeftShaft['bodyNumber']]
107
108#cross shaft
109#create simple rigid body
110b1 = mbs.CreateRigidBody(inertia = inertiaCylinderMiddle,
111                         nodeType= exu.NodeType.RotationRxyz,
112                         referencePosition = pMid2, #reference position x/y/z of COM
113                         referenceRotationMatrix=Ashaft2,
114                         initialAngularVelocity=[0,0,omega],
115                         initialVelocity=[0,0,0],
116                         gravity = g,
117                         graphicsDataList = [graphicsCrossShaft])
118
119#right rigid shaft
120b2 = mbs.CreateRigidBody(inertia = inertiaCylinderRight,
121                         nodeType= exu.NodeType.RotationRxyz,
122                         referencePosition = pMid1, #reference position x/y/z of COM
123                         referenceRotationMatrix=Ashaft2,
124                         initialAngularVelocity=[0,0,omega],
125                         initialVelocity=[0,0,0],
126                         gravity = g,
127                         graphicsDataList = graphicsBodyRight)
128
129
130
131# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
132#ground body and marker
133gGround = [graphics.CheckerBoard(point=[-1.5*r,0,0], normal=[1,0,0], size = 4)]
134oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=gGround)))
135
136
137
138# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
139#sensors
140sAngularVelocityLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
141                          outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
142
143sAngularVelocityRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
144                          outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
145
146sRotationLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
147                          outputVariableType=exu.OutputVariableType.Rotation))
148sRotationRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
149                          outputVariableType=exu.OutputVariableType.Rotation))
150
151
152
153# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
154# constant angular velocity constraint for flexible body
155#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
156if True: # if true: constant angular velocity constraint for flexible body
157    nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #ground node for coordinate constraint
158
159    mRotationAxis = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = n0, 
160                                                          coordinate=5))
161
162    mMotorCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nGround, 
163                                                          coordinate=5))
164
165    mbs.AddObject(CoordinateConstraint(markerNumbers = [mRotationAxis,mMotorCoordinate],
166                                        offset = -omega, 
167                                        velocityLevel = True,
168                                        visualization = VCoordinateConstraint(show=False))) #gives equation omegaMarker1 = offset
169# #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
170
171
172
173#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
174#joints
175mbs.CreateRevoluteJoint(itemNumbers=[oGround, b0], position=[0,0,-l/2], axis=[0,0,1],
176                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.01)
177
178mbs.CreateRevoluteJoint(itemNumbers=[b1, b0], position=[0,0,0], axis=[1,0,0],
179                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.5)
180
181mbs.CreateRevoluteJoint(itemNumbers=[b1, b2], position=[0,0,0], axis=[0,1,0],
182                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.5*r2/r)
183    
184mbs.CreateGenericJoint(itemNumbers=[b2, oGround], position=[0,0,l/2], 
185                        useGlobalFrame=False, constrainedAxes=[1,1,0,0,0,0], axesRadius=0.02, axesLength=0.01)
186
187
188
189#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
190#add graphics for rotations of shafts:
191pShow = np.array([0.2,0.,-l/2])
192rShow = 0.15
193gGround0 = [graphics.Cylinder(pAxis = [0,0,-0.02], vAxis = [0,0,0.02], radius=rShow, nTiles = 32, color=graphics.color.lightgrey)]
194gGround0 += [graphics.Arrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=graphics.color.steelblue)]
195oGroundBody0 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround0)))
196
197gGround1 = [graphics.Arrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=graphics.color.lawngreen)]
198oGroundBody1 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround1)))
199
200#for visualization of arrows (no sync between two rotations ...)
201def PreStepUserFunction(mbs, t):
202    phi0 = mbs.GetObjectOutputBody(b0, variableType=exu.OutputVariableType.Rotation)[2]
203    A0 = RotationMatrixZ(phi0)
204    mbs.SetObjectParameter(oGroundBody0, 'referenceRotation', A0)
205    
206    A1 = mbs.GetObjectOutputBody(b2, variableType=exu.OutputVariableType.RotationMatrix).reshape((3,3))
207    mbs.SetObjectParameter(oGroundBody1, 'referenceRotation', Ashaft2.T @ A1)
208    return True
209
210mbs.SetPreStepUserFunction(PreStepUserFunction)
211
212#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
213mbs.Assemble()
214
215if ComputeSystemDegreeOfFreedom:
216    exu.ComputeSystemDegreeOfFreedom (mbs, simulationSettings= exu.SimulationSettings(),
217                                      threshold= 1e-12, verbose= True, useSVD= False)
218
219
220
221#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
222#simulationSettings
223simulationSettings = exu.SimulationSettings() #takes currently set values or default values
224
225simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
226simulationSettings.timeIntegration.endTime = tEnd #0.2 for testing
227simulationSettings.solution.file.writePeriod = h
228simulationSettings.solution.sensors.writePeriod = h
229simulationSettings.timeIntegration.verboseMode = 1
230
231simulationSettings.timeIntegration.newton.useModifiedNewton = True
232simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
233simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = True
234
235simulationSettings.timeIntegration.realtime.active = True
236simulationSettings.timeIntegration.realtime.factor = 0.2
237
238SC.visualizationSettings.nodes.show = False
239SC.visualizationSettings.markers.show = False
240#SC.visualizationSettings.connectors.showJointAxes = True
241SC.visualizationSettings.connectors.jointAxesLength = 0.03
242SC.visualizationSettings.connectors.jointAxesRadius = 0.008
243SC.visualizationSettings.openGL.lineWidth=2 #maximum
244
245SC.visualizationSettings.openGL.light0.shadow=0.15
246SC.visualizationSettings.openGL.multiSampling = 4
247SC.visualizationSettings.openGL.light0.position = [8,8,10,0]
248simulationSettings.solution.file.information = "Example universal joint"
249SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
250
251SC.visualizationSettings.markers.defaultSize=0.05
252
253if overconstrainedSystem:
254    simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenDense #use for overconstrained systems
255    simulationSettings.linearSolver.ignoreSingularJacobian = True #use for overconstrained systems
256
257
258if simulation:
259    SC.visualizationSettings.general.autoFitScene = False
260    SC.renderer.Start()
261    SC.renderer.SetModelView(zoom=0.9022491,rotationVector=[0.7122599,0.116426,1.713679],centerPoint=[0.2592883,0.05991498,0])
262    SC.renderer.DoIdleTasks()
263    
264    mbs.SolveDynamic(simulationSettings)
265    
266    SC.renderer.DoIdleTasks()
267    SC.renderer.Stop() #safely close rendering window!
268
269
270    if solutionViewer:
271        #%%+++++++++++++++++++++++++++++++
272        from exudyn.interactive import SolutionViewer
273        SolutionViewer(mbs)
274
275    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
276    if showPlotfigure:      
277        from exudyn.plot import PlotSensor
278       
279        plt.figure('Angular velocity')
280        PlotSensor(mbs, sensorNumbers=[sAngularVelocityLeft], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='blue', newFigure=False)
281        PlotSensor(mbs, sensorNumbers=[sAngularVelocityRight], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='orange', newFigure=False)
282
283        alpha=mbs.GetSensorStoredData(sRotationLeft)[:,0]  
284        gamma1=mbs.GetSensorStoredData(sRotationLeft)[:,3] 
285        beta=angle
286                
287        omega2=omega*np.cos(beta)/(1-np.cos(gamma1+np.deg2rad(90))**2*np.sin(beta)**2)
288        plt.figure('Angular velocity')
289        
290        time=mbs.GetSensorStoredData(sRotationLeft)[:,0] 
291        plt.plot(time,omega2,'r--',label='analytical solution')
292        plt.legend()
293        plt.grid('on')
294        plt.show()