createKinematicTreeTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Model of a four bar mechanism under gravity
  5#           Used as test case for CreateKinematicTree
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2025-06-5
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15from exudyn.utilities import *
 16import exudyn.graphics as graphics
 17import numpy as np
 18
 19#import exudyn.rigidBodyUtilities as erb
 20#from exudyn.rigidBodyUtilities import HT0, HT2rotationMatrix, HT2translation
 21
 22useGraphics = True #without test
 23#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 24#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 25try: #only if called from test suite
 26    from modelUnitTests import exudynTestGlobals #for globally storing test results
 27    useGraphics = exudynTestGlobals.useGraphics
 28except:
 29    class ExudynTestGlobals:
 30        pass
 31    exudynTestGlobals = ExudynTestGlobals()
 32#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33
 34
 35
 36
 37# create empty system
 38SC = exu.SystemContainer() # sc = systems container
 39mbs = SC.AddSystem() # mbs = multibodysystem
 40
 41
 42
 43L1 = 0.8
 44L2 = 0.5
 45L3 = 0.3
 46w = 0.05
 47
 48ground = mbs.CreateGround(referencePosition=[L2,L1-L3,0],
 49                          graphicsDataList=[graphics.CheckerBoard(size=4)])
 50
 51#link with reference point at [0,0,0], COM at [0.5*L1,0,0]
 52link0Inertia = InertiaCuboid(density=1000, sideLengths=[L1,w,w]) #w.r.t. center of mass = [0,0,0]
 53link0Inertia = link0Inertia.Translated([0.5*L1,0,0]) #COM at [0.5*L1,0,0]
 54
 55
 56pdControl = (2e4,4e2)
 57#create tree links:
 58link0 = TreeLink(linkInertia=link0Inertia,
 59                 jointType=exu.JointType.RevoluteZ,
 60                 jointHT=HTtranslate([0,0,0.5]),
 61                 PDcontrol=pdControl,
 62                 )
 63link1 = TreeLink(linkInertia=link0Inertia,
 64                 jointType=exu.JointType.RevoluteY,
 65                 jointHT=HTtranslate([0.5,0,0.]),
 66                 PDcontrol=pdControl
 67                 )
 68#create kinematic tree:
 69oKT0 = mbs.CreateKinematicTree(
 70                              initialCoordinates=[pi,-0.5*pi],
 71                              listOfTreeLinks=[link0, link1],
 72                              colors=[graphics.color.blue,graphics.color.red],
 73                              gravity=[0,0,-9.81],
 74                              baseOffset=[1,0.5,0.],
 75                              linkRoundness=1,
 76                              )
 77
 78#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 79#create tree links
 80link0 = TreeLink(linkInertia=link0Inertia,
 81                 jointType=exu.JointType.PrismaticX,
 82                 parent=-1,
 83                 jointHT=HTtranslate([0,0,0.1]),
 84                 PDcontrol=(10*pdControl[0],10*pdControl[1]),
 85                 )
 86
 87link1 = TreeLink(linkInertia=link0Inertia,
 88                 jointType=exu.JointType.RevoluteZ,
 89                 parent=0,
 90                 jointHT=HTtranslate([-0.25,0,0.4]),
 91                 PDcontrol=pdControl,
 92                 )
 93link2 = TreeLink(linkInertia=link0Inertia,
 94                 jointType=exu.JointType.RevoluteZ,
 95                 parent=0,
 96                 jointHT=HTtranslate([0.25,0,0.4]),
 97                 PDcontrol=pdControl,
 98                 )
 99link3 = TreeLink(linkInertia=link0Inertia,
100                 jointType=exu.JointType.RevoluteX,
101                 parent=1,
102                 jointHT=HTtranslate([0,0.5,0.]),
103                 PDcontrol=pdControl
104                 )
105link4 = TreeLink(linkInertia=link0Inertia,
106                 jointType=exu.JointType.RevoluteX,
107                 parent=3,
108                 jointHT=HTtranslate([0,0.5,0.]),
109                 PDcontrol=pdControl
110                 )
111
112link5 = TreeLink(linkInertia=link0Inertia,
113                 jointType=exu.JointType.RevoluteX,
114                 parent=2,
115                 jointHT=HTtranslate([0,0.5,0.]),
116                 PDcontrol=pdControl
117                 )
118
119link6 = TreeLink(linkInertia=link0Inertia,
120                 jointType=exu.JointType.RevoluteX,
121                 parent=5,
122                 jointHT=HTtranslate([0,0.5,0.]),
123                 PDcontrol=pdControl
124                 )
125
126#create kinematic tree:
127oKT1 = mbs.CreateKinematicTree(
128                              initialCoordinates=[0.1, 0.125*pi,-0.125*pi]+[0]*4,
129                              listOfTreeLinks=[link0, link1, link2, link3, link4, link5, link6],
130                              colors=[graphics.color.orange]+[graphics.color.blue,graphics.color.red]*3,
131                              #gravity=[0,0,-9.81],
132                              baseOffset=[-1,-0.5,0.],
133                              linkRoundness=0.5,
134                              baseGraphicsDataList=[graphics.Cylinder(pAxis=[-0.5,0,0.05],vAxis=[1,0,0],radius=0.05,
135                                                                   color=graphics.color.dodgerblue)]
136                              )
137
138def PreStepUserFunction(mbs, t):
139    u = mbs.GetObjectParameter(oKT1,'jointPositionOffsetVector')
140    u[0] = SmoothStep(t, 0.1, 0.2, 0, 0.25)
141    u[1] = SmoothStep(t, 0.2, 0.3, 0, 0.25*pi)
142    u[2] = SmoothStep(t, 0.2, 0.3, 0, -0.25*pi)
143    u[3] = SmoothStep(t, 0.4, 0.5, 0, 0.5*pi)
144    u[4] = SmoothStep(t, 0.6, 0.7, 0, 0.5*pi)
145    u[5] = SmoothStep(t, 0.8, 0.9, 0, 0.5*pi)
146    u[6] = SmoothStep(t, 1.0, 1.1, 0, 0.5*pi)
147    mbs.SetObjectParameter(oKT1,'jointPositionOffsetVector',u)
148    return True
149
150mbs.SetPreStepUserFunction(PreStepUserFunction)
151
152
153tEnd = 1.2
154stepSize = 4e-3
155mbs.Assemble()
156
157simulationSettings = exu.SimulationSettings()
158simulationSettings.solutionSettings.writeSolutionToFile=True
159simulationSettings.solutionSettings.solutionWritePeriod=0.004
160simulationSettings.timeIntegration.numberOfSteps = tEnd/stepSize
161simulationSettings.timeIntegration.endTime = tEnd
162simulationSettings.timeIntegration.verboseMode = 1
163#simulationSettings.timeIntegration.simulateInRealtime = True
164
165SC.visualizationSettings.general.drawWorldBasis = True
166SC.visualizationSettings.openGL.shadow = 0.25
167SC.visualizationSettings.openGL.multiSampling = 4
168SC.visualizationSettings.window.renderWindowSize=[1600,1200]
169
170if useGraphics:
171    SC.renderer.Start()
172    if 'renderState' in exu.sys:
173        SC.renderer.SetState(exu.sys['renderState'])
174    SC.renderer.DoIdleTasks()
175
176mbs.SolveDynamic(simulationSettings)
177
178if useGraphics:
179    SC.renderer.Stop()
180
181    mbs.SolutionViewer()
182
183#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
184ode2 = mbs.systemData.GetODE2Coordinates()
185
186u = np.linalg.norm(ode2)
187exu.Print('solution of createKinematicTreeTest=',u)
188
189exudynTestGlobals.testResult = u
190#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++