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()