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
22testIsActive = exu.sys.get('testIsActive', False)
23
24
25
26
27# create empty system
28SC = exu.SystemContainer() # sc = systems container
29mbs = SC.AddSystem() # mbs = multibodysystem
30
31
32
33L1 = 0.8
34L2 = 0.5
35L3 = 0.3
36w = 0.05
37
38ground = mbs.CreateGround(referencePosition=[L2,L1-L3,0],
39 graphicsDataList=[graphics.CheckerBoard(size=4)])
40
41#link with reference point at [0,0,0], COM at [0.5*L1,0,0]
42link0Inertia = InertiaCuboid(density=1000, sideLengths=[L1,w,w]) #w.r.t. center of mass = [0,0,0]
43link0Inertia = link0Inertia.Translated([0.5*L1,0,0]) #COM at [0.5*L1,0,0]
44
45
46pdControl = (2e4,4e2)
47#create tree links:
48link0 = TreeLink(linkInertia=link0Inertia,
49 jointType=exu.JointType.RevoluteZ,
50 jointHT=exu.HT(translation=[0,0,0.5]),
51 PDcontrol=pdControl,
52 )
53link1 = TreeLink(linkInertia=link0Inertia,
54 jointType=exu.JointType.RevoluteY,
55 jointHT=exu.HT(translation=[0.5,0,0.]),
56 PDcontrol=pdControl
57 )
58#create kinematic tree:
59oKT0 = mbs.CreateKinematicTree(
60 initialCoordinates=[pi,-0.5*pi],
61 listOfTreeLinks=[link0, link1],
62 colors=[graphics.color.blue,graphics.color.red],
63 gravity=[0,0,-9.81],
64 baseOffset=[1,0.5,0.],
65 linkRoundness=1,
66 )
67
68#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
69#create tree links
70link0 = TreeLink(linkInertia=link0Inertia,
71 jointType=exu.JointType.PrismaticX,
72 parent=-1,
73 jointHT=exu.HT(translation=[0,0,0.1]),
74 PDcontrol=(10*pdControl[0],10*pdControl[1]),
75 )
76
77link1 = TreeLink(linkInertia=link0Inertia,
78 jointType=exu.JointType.RevoluteZ,
79 parent=0,
80 jointHT=exu.HT(translation=[-0.25,0,0.4]),
81 PDcontrol=pdControl,
82 )
83link2 = TreeLink(linkInertia=link0Inertia,
84 jointType=exu.JointType.RevoluteZ,
85 parent=0,
86 jointHT=exu.HT(translation=[0.25,0,0.4]),
87 PDcontrol=pdControl,
88 )
89link3 = TreeLink(linkInertia=link0Inertia,
90 jointType=exu.JointType.RevoluteX,
91 parent=1,
92 jointHT=exu.HT(translation=[0,0.5,0.]),
93 PDcontrol=pdControl
94 )
95link4 = TreeLink(linkInertia=link0Inertia,
96 jointType=exu.JointType.RevoluteX,
97 parent=3,
98 jointHT=exu.HT(translation=[0,0.5,0.]),
99 PDcontrol=pdControl
100 )
101
102link5 = TreeLink(linkInertia=link0Inertia,
103 jointType=exu.JointType.RevoluteX,
104 parent=2,
105 jointHT=exu.HT(translation=[0,0.5,0.]),
106 PDcontrol=pdControl
107 )
108
109link6 = TreeLink(linkInertia=link0Inertia,
110 jointType=exu.JointType.RevoluteX,
111 parent=5,
112 jointHT=exu.HT(translation=[0,0.5,0.]),
113 PDcontrol=pdControl
114 )
115
116#create kinematic tree:
117oKT1 = mbs.CreateKinematicTree(
118 initialCoordinates=[0.1, 0.125*pi,-0.125*pi]+[0]*4,
119 listOfTreeLinks=[link0, link1, link2, link3, link4, link5, link6],
120 colors=[graphics.color.orange]+[graphics.color.blue,graphics.color.red]*3,
121 #gravity=[0,0,-9.81],
122 baseOffset=[-1,-0.5,0.],
123 linkRoundness=0.5,
124 baseGraphicsDataList=[graphics.Cylinder(pAxis=[-0.5,0,0.05],vAxis=[1,0,0],radius=0.05,
125 color=graphics.color.dodgerblue)]
126 )
127
128def PreStepUserFunction(mbs, t):
129 u = mbs.GetObjectParameter(oKT1,'jointPositionOffsetVector')
130 u[0] = SmoothStep(t, 0.1, 0.2, 0, 0.25)
131 u[1] = SmoothStep(t, 0.2, 0.3, 0, 0.25*pi)
132 u[2] = SmoothStep(t, 0.2, 0.3, 0, -0.25*pi)
133 u[3] = SmoothStep(t, 0.4, 0.5, 0, 0.5*pi)
134 u[4] = SmoothStep(t, 0.6, 0.7, 0, 0.5*pi)
135 u[5] = SmoothStep(t, 0.8, 0.9, 0, 0.5*pi)
136 u[6] = SmoothStep(t, 1.0, 1.1, 0, 0.5*pi)
137 mbs.SetObjectParameter(oKT1,'jointPositionOffsetVector',u)
138 return True
139
140mbs.SetPreStepUserFunction(PreStepUserFunction)
141
142
143tEnd = 1.2
144stepSize = 4e-3
145mbs.Assemble()
146
147simulationSettings = exu.SimulationSettings()
148simulationSettings.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster
149simulationSettings.solution.file.write=(not testIsActive) #only the SolutionViewer reads it (#2492)
150simulationSettings.solution.file.writePeriod=0.004
151simulationSettings.timeIntegration.numberOfSteps = tEnd/stepSize
152simulationSettings.timeIntegration.endTime = tEnd
153simulationSettings.timeIntegration.verboseMode = 1
154#simulationSettings.timeIntegration.realtime.active = True
155
156SC.visualizationSettings.view0.scene.drawWorldBasis = True
157SC.visualizationSettings.openGL.light0.shadow = 0.25
158SC.visualizationSettings.openGL.multiSampling = 4
159SC.visualizationSettings.view0.window.renderWindowSize=[1600,1200]
160
161if not testIsActive:
162 SC.renderer.Start()
163 SC.renderer.RestoreSavedState()
164 SC.renderer.DoIdleTasks()
165
166mbs.SolveDynamic(simulationSettings)
167
168if not testIsActive:
169 SC.renderer.Stop()
170
171 mbs.SolutionViewer()
172
173#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
174ode2 = mbs.systemData.GetODE2Coordinates()
175
176u = np.linalg.norm(ode2)
177exu.Print('solution of createKinematicTreeTest=',u)
178
179exu.sys['testResult'] = u
180#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++