You can view and download this file on Github: universalJoint.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details: universal joint with rigid bodies
# and compared with analytical solution
#
# Author: Michael Pieber
# Date: 2023-09-20
#
# 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.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
import exudyn as exu
from exudyn.itemInterface import *
from exudyn.utilities import *
import numpy as np
import matplotlib.pyplot as plt
plt.close("all")
fontSize = 14
simulation = True
showPlotfigure=False
ComputeSystemDegreeOfFreedom = False #possible to show the degreeOfFreedom
overconstrainedSystem = False #possible to simulate, even if overconstrained
solutionViewer = False
SC = exu.SystemContainer()
mbs = SC.AddSystem()
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#parameters & dimensions
l=1 #m length of rigid shaft
r=0.3 #m radius of rigid shaft
omega=10 #rad/s angular velocity
angle=np.deg2rad(45) #vary between 0 and <90 degree; change angle of the joint
Ashaft2 = RotationVector2RotationMatrix([angle,0,0])
#left rigid shaft
pMid0 = [0,0,0] #center of mass of left rigid shaft
massShaft=9 #kg
thetaLeft = np.eye(3)*1e2
inertiaLeftShaft = RigidBodyInertia(massShaft, thetaLeft)
#right rigid shaft
inertiaCylinderRight = InertiaCylinder(density=1000, length=l,outerRadius=r,axis=2)
pMid1 = [0,-l/2*np.sin(angle),l/2+l/2*np.cos(angle)] #center of mass of right rigid shaft
#cross shaft
inertiaCylinderMiddle = InertiaCylinder(density=1000, length=r,outerRadius=2*r,axis=2)
pMid2 = [0,0,l/2] #center of mass of cross shaft
g = [0,0,0] #gravity
#parameters for simulation
tEnd=20*np.pi/omega #simulation time (one rotation)
h=1e-3 #stepsize
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#graphics for body
graphicsCrossShaft = GraphicsDataOrthoCubePoint(size=[0.2*r]*3, color=color4grey)
graphicsBodyLeft = [GraphicsDataCylinder(pAxis=[0,0,-l/2], vAxis=[0,0,2*l/2-r],
radius=r/8, color=color4steelblue, nTiles=32, alternatingColor=color4blue)]
graphicsBodyLeft += [GraphicsDataOrthoCubePoint(centerPoint=[0,0,l/2-r], size=[2*r,0.25*r,0.25*r], color=color4steelblue)]
graphicsBodyLeft += [GraphicsDataOrthoCubePoint(centerPoint=[r*0.875,0,l/2-0.5*r], size=[0.25*r,0.25*r,r*1.25], color=color4steelblue)]
graphicsBodyLeft += [GraphicsDataOrthoCubePoint(centerPoint=[-r*0.875,0,l/2-0.5*r], size=[0.25*r,0.25*r,r*1.25], color=color4steelblue)]
d2 = r*1.5
r2 = r*0.75
graphicsBodyRight = [GraphicsDataCylinder(pAxis=[0,0,-l/2+d2], vAxis=[0,0,2*l/2-d2],
radius=r/8, color=color4lawngreen, nTiles=32, alternatingColor=color4green)]
graphicsBodyRight += [GraphicsDataOrthoCubePoint(centerPoint=[0,0,-l/2+d2], size=[0.25*r,2*r2,0.25*r], color=color4lawngreen)]
graphicsBodyRight += [GraphicsDataOrthoCubePoint(centerPoint=[0,r2*0.875,-l/2+0.5*d2], size=[0.25*r,0.25*r,d2+r*0.25], color=color4lawngreen)]
graphicsBodyRight += [GraphicsDataOrthoCubePoint(centerPoint=[0,-r2*0.875,-l/2+0.5*d2], size=[0.25*r,0.25*r,d2+r*0.25], color=color4lawngreen)]
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#add rigid bodies
#left rigid shaft
#create simple rigid body
[n0,b0]=AddRigidBody(mainSys = mbs,
inertia = inertiaLeftShaft,
nodeType = exu.NodeType.RotationRxyz,
position = pMid0,
angularVelocity = [0,0,omega],
gravity = g,
graphicsDataList = graphicsBodyLeft)
#cross shaft
#create simple rigid body
b1 = mbs.CreateRigidBody(inertia = inertiaCylinderMiddle,
nodeType= exu.NodeType.RotationRxyz,
referencePosition = pMid2, #reference position x/y/z of COM
referenceRotationMatrix=Ashaft2,
initialAngularVelocity=[0,0,omega],
initialVelocity=[0,0,0],
gravity = g,
graphicsDataList = [graphicsCrossShaft])
#right rigid shaft
b2 = mbs.CreateRigidBody(inertia = inertiaCylinderRight,
nodeType= exu.NodeType.RotationRxyz,
referencePosition = pMid1, #reference position x/y/z of COM
referenceRotationMatrix=Ashaft2,
initialAngularVelocity=[0,0,omega],
initialVelocity=[0,0,0],
gravity = g,
graphicsDataList = graphicsBodyRight)
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#ground body and marker
gGround = [GraphicsDataCheckerBoard(point=[-1.5*r,0,0], normal=[1,0,0], size = 4)]
oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=gGround)))
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#sensors
sAngularVelocityLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
sAngularVelocityRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
outputVariableType=exu.OutputVariableType.AngularVelocityLocal))
sRotationLeft = mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,
outputVariableType=exu.OutputVariableType.Rotation))
sRotationRight = mbs.AddSensor(SensorBody(bodyNumber=b2, storeInternal=True,
outputVariableType=exu.OutputVariableType.Rotation))
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# constant angular velocity constraint for flexible body
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if True: # if true: constant angular velocity constraint for flexible body
nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #ground node for coordinate constraint
mRotationAxis = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = n0,
coordinate=5))
mMotorCoordinate = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nGround,
coordinate=5))
mbs.AddObject(CoordinateConstraint(markerNumbers = [mRotationAxis,mMotorCoordinate],
offset = -omega,
velocityLevel = True,
visualization = VCoordinateConstraint(show=False))) #gives equation omegaMarker1 = offset
# #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#joints
mbs.CreateRevoluteJoint(bodyNumbers=[oGround, b0], position=[0,0,-l/2], axis=[0,0,1],
useGlobalFrame=False, axisRadius=0.02, axisLength=0.01)
mbs.CreateRevoluteJoint(bodyNumbers=[b1, b0], position=[0,0,0], axis=[1,0,0],
useGlobalFrame=False, axisRadius=0.02, axisLength=0.5)
mbs.CreateRevoluteJoint(bodyNumbers=[b1, b2], position=[0,0,0], axis=[0,1,0],
useGlobalFrame=False, axisRadius=0.02, axisLength=0.5*r2/r)
mbs.CreateGenericJoint(bodyNumbers=[b2, oGround], position=[0,0,l/2],
useGlobalFrame=False, constrainedAxes=[1,1,0,0,0,0], axesRadius=0.02, axesLength=0.01)
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#add graphics for rotations of shafts:
pShow = np.array([0.2,0.,-l/2])
rShow = 0.15
gGround0 = [GraphicsDataCylinder(pAxis = [0,0,-0.02], vAxis = [0,0,0.02], radius=rShow, nTiles = 32, color=color4lightgrey)]
gGround0 += [GraphicsDataArrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=color4steelblue)]
oGroundBody0 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround0)))
gGround1 = [GraphicsDataArrow(pAxis = [0,0,0.02], vAxis=[rShow*1.2,0,0], radius=0.01, color=color4lawngreen)]
oGroundBody1 = mbs.AddObject(ObjectGround(referencePosition=pShow, visualization=VObjectGround(graphicsData=gGround1)))
def PreStepUserFunction(mbs, t):
phi0 = mbs.GetObjectOutputBody(b0, variableType=exu.OutputVariableType.Rotation)[2]
A0 = RotationMatrixZ(phi0)
mbs.SetObjectParameter(oGroundBody0, 'referenceRotation', A0)
A1 = mbs.GetObjectOutputBody(b2, variableType=exu.OutputVariableType.RotationMatrix).reshape((3,3))
mbs.SetObjectParameter(oGroundBody1, 'referenceRotation', Ashaft2.T @ A1)
return True
mbs.SetPreStepUserFunction(PreStepUserFunction)
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
mbs.Assemble()
if ComputeSystemDegreeOfFreedom:
exu.ComputeSystemDegreeOfFreedom (mbs, simulationSettings= exu.SimulationSettings(),
threshold= 1e-12, verbose= True, useSVD= False)
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#simulationSettings
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd #0.2 for testing
simulationSettings.solutionSettings.solutionWritePeriod = h
simulationSettings.solutionSettings.sensorsWritePeriod = h
simulationSettings.timeIntegration.verboseMode = 1
simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = simulationSettings.timeIntegration.generalizedAlpha.useNewmark
simulationSettings.timeIntegration.simulateInRealtime = True
simulationSettings.timeIntegration.realtimeFactor = 0.2
SC.visualizationSettings.nodes.show = False
SC.visualizationSettings.markers.show = False
#SC.visualizationSettings.connectors.showJointAxes = True
SC.visualizationSettings.connectors.jointAxesLength = 0.03
SC.visualizationSettings.connectors.jointAxesRadius = 0.008
SC.visualizationSettings.openGL.lineWidth=2 #maximum
SC.visualizationSettings.openGL.lineSmooth=True
SC.visualizationSettings.openGL.shadow=0.15
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.openGL.light0position = [8,8,10,0]
simulationSettings.solutionSettings.solutionInformation = "Example universal joint"
SC.visualizationSettings.general.graphicsUpdateInterval = 0.02
SC.visualizationSettings.markers.defaultSize=0.05
if overconstrainedSystem:
simulationSettings.linearSolverType = exu.LinearSolverType.EigenDense #use for overconstrained systems
simulationSettings.linearSolverSettings.ignoreSingularJacobian = True #use for overconstrained systems
if simulation:
exu.StartRenderer()
if 'renderState' in exu.sys:
SC.SetRenderState(exu.sys['renderState'])
mbs.WaitForUserToContinue()
exu.SolveDynamic(mbs, simulationSettings)
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
if solutionViewer:
#%%+++++++++++++++++++++++++++++++
from exudyn.interactive import SolutionViewer
SolutionViewer(mbs)
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if showPlotfigure:
from exudyn.plot import PlotSensor
plt.figure('Angular velocity')
PlotSensor(mbs, sensorNumbers=[sAngularVelocityLeft], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='blue', newFigure=False)
PlotSensor(mbs, sensorNumbers=[sAngularVelocityRight], components=[2], xLabel='time in s', yLabel='angular velocity in rad/s',colors='orange', newFigure=False)
alpha=mbs.GetSensorStoredData(sRotationLeft)[:,0]
gamma1=mbs.GetSensorStoredData(sRotationLeft)[:,3]
beta=angle
omega2=omega*np.cos(beta)/(1-np.cos(gamma1+np.deg2rad(90))**2*np.sin(beta)**2)
plt.figure('Angular velocity')
time=mbs.GetSensorStoredData(sRotationLeft)[:,0]
plt.plot(time,omega2,'r--',label=r'analytical solution')
plt.legend()
plt.grid('on')
plt.show()