You can view and download this file on Github: fourBarMechanism3D.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details: A simple 3D four bar mechanism #read full text output!
# 1) regular case does not work (redundant constraints/overconstrained joints; jacobian singluar)
# 2) use simulationSettings.linearSolverSettings.ignoreSingularJacobian = True
# 3) remove redundant constraints: change flags for GenericJoint at last joint [1,1,0,0,0,0] to obtain well defined mbs
#
# Author: Johannes Gerstmayr
# Date: 2021-08-05
#
# 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 * #includes graphics and rigid body utilities
import numpy as np
from math import pi, sin, cos
useGraphics = True
casesText = ['redundant constraints', 'redundant constraints with improved solver', 'non-redundant constraints']
cases = [0,1,2]
for case in cases:
caseText = casesText[case]
print('\n\n************************************************')
print('run four bar mechanism with case:\n '+caseText)
print('************************************************')
SC = exu.SystemContainer()
mbs = SC.AddSystem()
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
#physical parameters
g = [0.1,-9.81,0] #gravity + disturbance
L = 1 #length
w = 0.1 #width
bodyDim=[L,w,w] #body dimensions
# p0 = [0,0,0]
pMid0 = np.array([0,L*0.5,0]) #center of mass, body0
pMid1 = np.array([L*0.5,L,0]) #center of mass, body1
pMid2 = np.array([L,L*0.5,0]) #center of mass, body2
#ground body
graphicsCOM0 = GraphicsDataBasis(origin=[0,0,0], length=4*w)
oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[graphicsCOM0])))
markerGround0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
markerGround1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[L,0,0]))
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
#first link:
iCube0 = InertiaCuboid(density=5000, sideLengths=bodyDim)
#graphics for body
graphicsBody0 = GraphicsDataRigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
thickness = w, width = [1.2*w,1.2*w], color=color4red)
graphicsBody1 = GraphicsDataRigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
thickness = w, width = [1.2*w,1.2*w], color=color4green)
graphicsBody2 = GraphicsDataRigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
thickness = w, width = [1.2*w,1.2*w], color=color4steelblue)
[n0,b0]=AddRigidBody(mainSys = mbs,
inertia = iCube0, #includes COM
nodeType = exu.NodeType.RotationEulerParameters,
position = pMid0,
rotationMatrix = RotationMatrixZ( 0.5*pi),
gravity = g,
graphicsDataList = [graphicsBody0])
markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*L,0,0]))
markerBody0J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[ 0.5*L,0,0]))
[n1,b1]=AddRigidBody(mainSys = mbs,
inertia = iCube0, #includes COM
nodeType = exu.NodeType.RotationEulerParameters,
position = pMid1,
rotationMatrix = RotationMatrixZ(0.),
gravity = g,
graphicsDataList = [graphicsBody1])
markerBody1J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[-0.5*L,0,0]))
markerBody1J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[ 0.5*L,0,0]))
[n2,b2]=AddRigidBody(mainSys = mbs,
inertia = iCube0, #includes COM
nodeType = exu.NodeType.RotationEulerParameters,
position = pMid2,
rotationMatrix = RotationMatrixZ(-0.5*pi),
gravity = g,
graphicsDataList = [graphicsBody2])
markerBody2J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2, localPosition=[-0.5*L,0,0]))
markerBody2J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2, localPosition=[ 0.5*L,0,0]))
#revolute joint option 1:
mbs.AddObject(GenericJoint(markerNumbers=[markerGround0, markerBody0J0],
constrainedAxes=[1,1,1,1,1,0],
visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
mbs.AddObject(GenericJoint(markerNumbers=[markerBody0J1, markerBody1J0],
constrainedAxes=[1,1,1,1,1,0],
visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
mbs.AddObject(GenericJoint(markerNumbers=[markerBody1J1, markerBody2J0],
constrainedAxes=[1,1,1,1,1,0],
visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
constrainedAxes3 = [1,1,1,1,1,0]
if case == 2:
constrainedAxes3 = [1,1,0,0,0,0] #only these constraints are needed for closing loop!
print('use non-redundant constraints for last joint:', constrainedAxes3)
mbs.AddObject(GenericJoint(markerNumbers=[markerBody2J1, markerGround1],
constrainedAxes=constrainedAxes3,
visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
#position sensor at tip of body1
sens1=mbs.AddSensor(SensorBody(bodyNumber=b1, localPosition=[0,0,0.5*L],
fileName='solution/sensorPos.txt',
outputVariableType = exu.OutputVariableType.Position))
#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
#assemble system before solving
mbs.Assemble()
if False:
mbs.systemData.Info() #show detailed information
if False:
#from exudyn.utilities import DrawSystemGraph
mbs.DrawSystemGraph(useItemTypes=True) #draw nice graph of system
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
tEnd = 10 #simulation time
h = 2e-3 #step size
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.timeIntegration.verboseMode = 1
#simulationSettings.timeIntegration.simulateInRealtime = True
#simulationSettings.timeIntegration.realtimeFactor = 4
if case == 1:
simulationSettings.linearSolverSettings.ignoreSingularJacobian = True #for redundant constraints
simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.solutionSettings.writeSolutionToFile = False
#simulationSettings.solutionSettings.solutionWritePeriod = 0.005 #store every 5 ms
SC.visualizationSettings.window.renderWindowSize=[1200,1024]
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.general.autoFitScene = False
SC.visualizationSettings.nodes.drawNodesAsPoint=False
SC.visualizationSettings.nodes.showBasis=True
if useGraphics:
exu.StartRenderer()
if 'renderState' in exu.sys: #reload old view
SC.SetRenderState(exu.sys['renderState'])
mbs.WaitForUserToContinue() #stop before simulating
try: #solver will raise exception in case 1
mbs.SolveDynamic(simulationSettings = simulationSettings)
except:
pass
# mbs.SolveDynamic(simulationSettings = simulationSettings,
# solverType=exu.DynamicSolverType.TrapezoidalIndex2)
if useGraphics:
SC.WaitForRenderEngineStopFlag() #stop before closing
exu.StopRenderer() #safely close rendering window!
#check redundant constraints and DOF:
mbs.ComputeSystemDegreeOfFreedom(verbose=True)
if False:
sol = LoadSolutionFile('coordinatesSolution.txt')
mbs.SolutionViewer(sol)
if False:
mbs.PlotSensor([sens1],[1])