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