fourBarMechanism3D.py

You can view and download this file on Github: fourBarMechanism3D.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  A simple 3D four bar mechanism #read full text output!
  5#           1) regular case does not work (redundant constraints/overconstrained joints; jacobian singluar)
  6#           2) use simulationSettings.linearSolverSettings.ignoreSingularJacobian = True
  7#           3) remove redundant constraints: change flags for GenericJoint at last joint [1,1,0,0,0,0] to obtain well defined mbs
  8#
  9# Author:   Johannes Gerstmayr
 10# Date:     2021-08-05
 11#
 12# 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.
 13#
 14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 15
 16import exudyn as exu
 17from exudyn.itemInterface import *
 18from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 19import exudyn.graphics as graphics #only import if it does not conflict
 20import numpy as np
 21from math import pi, sin, cos
 22
 23
 24useGraphics = True
 25
 26casesText = ['redundant constraints', 'redundant constraints with improved solver', 'non-redundant constraints']
 27cases = [0,1,2]
 28
 29for case in cases:
 30    caseText = casesText[case]
 31    print('\n\n************************************************')
 32    print('run four bar mechanism with case:\n  '+caseText)
 33    print('************************************************')
 34
 35    SC = exu.SystemContainer()
 36    mbs = SC.AddSystem()
 37
 38
 39    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 40    #physical parameters
 41    g =     [0.1,-9.81,0] #gravity + disturbance
 42    L = 1               #length
 43    w = 0.1             #width
 44    bodyDim=[L,w,w]     #body dimensions
 45    # p0 =    [0,0,0]
 46    pMid0 = np.array([0,L*0.5,0]) #center of mass, body0
 47    pMid1 = np.array([L*0.5,L,0]) #center of mass, body1
 48    pMid2 = np.array([L,L*0.5,0]) #center of mass, body2
 49
 50    #ground body
 51    graphicsCOM0 = graphics.Basis(origin=[0,0,0], length=4*w)
 52    oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[graphicsCOM0])))
 53    markerGround0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
 54    markerGround1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[L,0,0]))
 55
 56    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 57    #first link:
 58    iCube0 = InertiaCuboid(density=5000, sideLengths=bodyDim)
 59
 60    #graphics for body
 61    graphicsBody0 = graphics.RigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
 62                                         axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
 63                                         thickness = w, width = [1.2*w,1.2*w], color=graphics.color.red)
 64    graphicsBody1 = graphics.RigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
 65                                         axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
 66                                         thickness = w, width = [1.2*w,1.2*w], color=graphics.color.green)
 67    graphicsBody2 = graphics.RigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
 68                                         axis0=[0,0,1], axis1=[0,0,1], radius=[0.5*w,0.5*w],
 69                                         thickness = w, width = [1.2*w,1.2*w], color=graphics.color.steelblue)
 70    dictCube0 = mbs.CreateRigidBody(
 71                  inertia=iCube0, # includes COM
 72                  referencePosition=pMid0,
 73                  referenceRotationMatrix=RotationMatrixZ(0.5*pi),
 74                  gravity=g,
 75                  graphicsDataList=[graphicsBody0],
 76                  returnDict=True)
 77    [n0, b0] = [dictCube0['nodeNumber'], dictCube0['bodyNumber']]
 78
 79    markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*L,0,0]))
 80    markerBody0J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0.5*L,0,0]))
 81
 82    dictCube1 = mbs.CreateRigidBody(
 83                  inertia=iCube0, # includes COM
 84                  referencePosition=pMid1,
 85                  gravity=g,
 86                  graphicsDataList=[graphicsBody1],
 87                  returnDict=True)
 88    [n1, b1] = [dictCube1['nodeNumber'], dictCube1['bodyNumber']]
 89
 90    markerBody1J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[-0.5*L,0,0]))
 91    markerBody1J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[0.5*L,0,0]))
 92
 93    dictCube2 = mbs.CreateRigidBody(
 94                  inertia=iCube0, # includes COM
 95                  referencePosition=pMid2,
 96                  referenceRotationMatrix=RotationMatrixZ(-0.5*pi),
 97                  gravity=g,
 98                  graphicsDataList=[graphicsBody2],
 99                  returnDict=True)
100    [n2, b2] = [dictCube2['nodeNumber'], dictCube2['bodyNumber']]
101
102    markerBody2J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2, localPosition=[-0.5*L,0,0]))
103    markerBody2J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b2, localPosition=[ 0.5*L,0,0]))
104
105
106    #revolute joint option 1:
107    mbs.AddObject(GenericJoint(markerNumbers=[markerGround0, markerBody0J0],
108                                constrainedAxes=[1,1,1,1,1,0],
109                                visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
110
111    mbs.AddObject(GenericJoint(markerNumbers=[markerBody0J1, markerBody1J0],
112                                constrainedAxes=[1,1,1,1,1,0],
113                                visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
114
115    mbs.AddObject(GenericJoint(markerNumbers=[markerBody1J1, markerBody2J0],
116                                constrainedAxes=[1,1,1,1,1,0],
117                                visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
118
119    constrainedAxes3 = [1,1,1,1,1,0]
120    if case == 2:
121        constrainedAxes3 = [1,1,0,0,0,0] #only these constraints are needed for closing loop!
122        print('use non-redundant constraints for last joint:', constrainedAxes3)
123
124    mbs.AddObject(GenericJoint(markerNumbers=[markerBody2J1, markerGround1],
125                                constrainedAxes=constrainedAxes3,
126                                visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
127
128    #position sensor at tip of body1
129    sens1=mbs.AddSensor(SensorBody(bodyNumber=b1, localPosition=[0,0,0.5*L],
130                                   fileName='solution/sensorPos.txt',
131                                   outputVariableType = exu.OutputVariableType.Position))
132
133    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
134    #assemble system before solving
135    mbs.Assemble()
136    if False:
137        mbs.systemData.Info() #show detailed information
138    if False:
139        #from exudyn.utilities import DrawSystemGraph
140        mbs.DrawSystemGraph(useItemTypes=True) #draw nice graph of system
141
142    simulationSettings = exu.SimulationSettings() #takes currently set values or default values
143
144    tEnd = 10 #simulation time
145    h = 2e-3 #step size
146    simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
147    simulationSettings.timeIntegration.endTime = tEnd
148    simulationSettings.timeIntegration.verboseMode = 1
149    #simulationSettings.timeIntegration.simulateInRealtime = True
150    #simulationSettings.timeIntegration.realtimeFactor = 4
151
152    if case == 1:
153        simulationSettings.linearSolverSettings.ignoreSingularJacobian = True #for redundant constraints
154
155    simulationSettings.timeIntegration.newton.useModifiedNewton = True
156    simulationSettings.solutionSettings.writeSolutionToFile = False
157    #simulationSettings.solutionSettings.solutionWritePeriod = 0.005 #store every 5 ms
158
159    SC.visualizationSettings.window.renderWindowSize=[1200,1024]
160    SC.visualizationSettings.openGL.multiSampling = 4
161    SC.visualizationSettings.general.autoFitScene = False
162
163    SC.visualizationSettings.nodes.drawNodesAsPoint=False
164    SC.visualizationSettings.nodes.showBasis=True
165
166    if useGraphics:
167        exu.StartRenderer()
168        if 'renderState' in exu.sys: #reload old view
169            SC.SetRenderState(exu.sys['renderState'])
170
171        mbs.WaitForUserToContinue() #stop before simulating
172
173    try: #solver will raise exception in case 1
174        mbs.SolveDynamic(simulationSettings = simulationSettings)
175    except:
176        pass
177
178    # mbs.SolveDynamic(simulationSettings = simulationSettings,
179    #                  solverType=exu.DynamicSolverType.TrapezoidalIndex2)
180    if useGraphics:
181        SC.WaitForRenderEngineStopFlag() #stop before closing
182        exu.StopRenderer() #safely close rendering window!
183
184    #check redundant constraints and DOF:
185    mbs.ComputeSystemDegreeOfFreedom(verbose=useGraphics)
186
187
188if False:
189    sol = LoadSolutionFile('coordinatesSolution.txt')
190
191    mbs.SolutionViewer(sol)
192
193if False:
194
195    mbs.PlotSensor([sens1],[1])