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