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