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
 96[n0,b0]=AddRigidBody(mainSys = mbs,
 97                     inertia = inertiaLeftShaft,
 98                     nodeType = exu.NodeType.RotationRxyz,
 99                     position = pMid0,
100                     angularVelocity = [0,0,omega],
101                     gravity = g,
102                     graphicsDataList = graphicsBodyLeft)
103
104#cross shaft
105#create simple rigid body
106b1 = mbs.CreateRigidBody(inertia = inertiaCylinderMiddle,
107                         nodeType= exu.NodeType.RotationRxyz,
108                         referencePosition = pMid2, #reference position x/y/z of COM
109                         referenceRotationMatrix=Ashaft2,
110                         initialAngularVelocity=[0,0,omega],
111                         initialVelocity=[0,0,0],
112                         gravity = g,
113                         graphicsDataList = [graphicsCrossShaft])
114
115#right rigid shaft
116b2 = mbs.CreateRigidBody(inertia = inertiaCylinderRight,
117                         nodeType= exu.NodeType.RotationRxyz,
118                         referencePosition = pMid1, #reference position x/y/z of COM
119                         referenceRotationMatrix=Ashaft2,
120                         initialAngularVelocity=[0,0,omega],
121                         initialVelocity=[0,0,0],
122                         gravity = g,
123                         graphicsDataList = graphicsBodyRight)
124
125
126
127# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
128#ground body and marker
129gGround = [graphics.CheckerBoard(point=[-1.5*r,0,0], normal=[1,0,0], size = 4)]
130oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=gGround)))
131
132
133
134# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
135#sensors
136sAngularVelocityLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
137                          outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
138
139sAngularVelocityRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
140                          outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
141
142sRotationLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
143                          outputVariableType=exu.OutputVariableType.Rotation))
144sRotationRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
145                          outputVariableType=exu.OutputVariableType.Rotation))
146
147
148
149# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
150# constant angular velocity constraint for flexible body
151#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
152if True: # if true: constant angular velocity constraint for flexible body
153    nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #ground node for coordinate constraint
154
155    mRotationAxis = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = n0,
156                                                          coordinate=5))
157
158    mMotorCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nGround,
159                                                          coordinate=5))
160
161    mbs.AddObject(CoordinateConstraint(markerNumbers = [mRotationAxis,mMotorCoordinate],
162                                        offset = -omega,
163                                        velocityLevel = True,
164                                        visualization = VCoordinateConstraint(show=False))) #gives equation omegaMarker1 = offset
165# #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
166
167
168
169#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
170#joints
171mbs.CreateRevoluteJoint(bodyNumbers=[oGround, b0], position=[0,0,-l/2], axis=[0,0,1],
172                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.01)
173
174mbs.CreateRevoluteJoint(bodyNumbers=[b1, b0], position=[0,0,0], axis=[1,0,0],
175                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.5)
176
177mbs.CreateRevoluteJoint(bodyNumbers=[b1, b2], position=[0,0,0], axis=[0,1,0],
178                          useGlobalFrame=False, axisRadius=0.02, axisLength=0.5*r2/r)
179
180mbs.CreateGenericJoint(bodyNumbers=[b2, oGround], position=[0,0,l/2],
181                        useGlobalFrame=False, constrainedAxes=[1,1,0,0,0,0], axesRadius=0.02, axesLength=0.01)
182
183
184
185#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
186#add graphics for rotations of shafts:
187pShow = np.array([0.2,0.,-l/2])
188rShow = 0.15
189gGround0 = [graphics.Cylinder(pAxis = [0,0,-0.02], vAxis = [0,0,0.02], radius=rShow, nTiles = 32, color=graphics.color.lightgrey)]
190gGround0 += [graphics.Arrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=graphics.color.steelblue)]
191oGroundBody0 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround0)))
192
193gGround1 = [graphics.Arrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=graphics.color.lawngreen)]
194oGroundBody1 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround1)))
195
196
197def PreStepUserFunction(mbs, t):
198    phi0 = mbs.GetObjectOutputBody(b0, variableType=exu.OutputVariableType.Rotation)[2]
199    A0 = RotationMatrixZ(phi0)
200    mbs.SetObjectParameter(oGroundBody0, 'referenceRotation', A0)
201
202    A1 = mbs.GetObjectOutputBody(b2, variableType=exu.OutputVariableType.RotationMatrix).reshape((3,3))
203    mbs.SetObjectParameter(oGroundBody1, 'referenceRotation', Ashaft2.T @ A1)
204    return True
205
206mbs.SetPreStepUserFunction(PreStepUserFunction)
207
208#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
209mbs.Assemble()
210
211if ComputeSystemDegreeOfFreedom:
212    exu.ComputeSystemDegreeOfFreedom (mbs, simulationSettings= exu.SimulationSettings(),
213                                      threshold= 1e-12, verbose= True, useSVD= False)
214
215
216
217#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
218#simulationSettings
219simulationSettings = exu.SimulationSettings() #takes currently set values or default values
220
221simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
222simulationSettings.timeIntegration.endTime = tEnd #0.2 for testing
223simulationSettings.solutionSettings.solutionWritePeriod = h
224simulationSettings.solutionSettings.sensorsWritePeriod = h
225simulationSettings.timeIntegration.verboseMode = 1
226
227simulationSettings.timeIntegration.newton.useModifiedNewton = True
228simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
229simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
230
231simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
232simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints =  simulationSettings.timeIntegration.generalizedAlpha.useNewmark
233
234simulationSettings.timeIntegration.simulateInRealtime = True
235simulationSettings.timeIntegration.realtimeFactor = 0.2
236
237SC.visualizationSettings.nodes.show = False
238SC.visualizationSettings.markers.show = False
239#SC.visualizationSettings.connectors.showJointAxes = True
240SC.visualizationSettings.connectors.jointAxesLength = 0.03
241SC.visualizationSettings.connectors.jointAxesRadius = 0.008
242SC.visualizationSettings.openGL.lineWidth=2 #maximum
243SC.visualizationSettings.openGL.lineSmooth=True
244SC.visualizationSettings.openGL.shadow=0.15
245SC.visualizationSettings.openGL.multiSampling = 4
246SC.visualizationSettings.openGL.light0position = [8,8,10,0]
247simulationSettings.solutionSettings.solutionInformation = "Example universal joint"
248SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
249
250SC.visualizationSettings.markers.defaultSize=0.05
251
252if overconstrainedSystem:
253    simulationSettings.linearSolverType = exu.LinearSolverType.EigenDense #use for overconstrained systems
254    simulationSettings.linearSolverSettings.ignoreSingularJacobian = True #use for overconstrained systems
255
256
257if simulation:
258    exu.StartRenderer()
259    if 'renderState' in exu.sys:
260        SC.SetRenderState(exu.sys['renderState'])
261    mbs.WaitForUserToContinue()
262
263    exu.SolveDynamic(mbs, simulationSettings)
264
265    SC.WaitForRenderEngineStopFlag()
266    exu.StopRenderer() #safely close rendering window!
267
268
269    if solutionViewer:
270        #%%+++++++++++++++++++++++++++++++
271        from exudyn.interactive import SolutionViewer
272        SolutionViewer(mbs)
273
274    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
275    if showPlotfigure:
276        from exudyn.plot import PlotSensor
277
278        plt.figure('Angular velocity')
279        PlotSensor(mbs, sensorNumbers=[sAngularVelocityLeft], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='blue', newFigure=False)
280        PlotSensor(mbs, sensorNumbers=[sAngularVelocityRight], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='orange', newFigure=False)
281
282        alpha=mbs.GetSensorStoredData(sRotationLeft)[:,0]
283        gamma1=mbs.GetSensorStoredData(sRotationLeft)[:,3]
284        beta=angle
285
286        omega2=omega*np.cos(beta)/(1-np.cos(gamma1+np.deg2rad(90))**2*np.sin(beta)**2)
287        plt.figure('Angular velocity')
288
289        time=mbs.GetSensorStoredData(sRotationLeft)[:,0]
290        plt.plot(time,omega2,'r--',label='analytical solution')
291        plt.legend()
292        plt.grid('on')
293        plt.show()