kinematicTreeAndMBStest.py
You can view and download this file on Github: kinematicTreeAndMBStest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN python utility library
3#
4# Details: several tests for class Robot and kinematicTree; tests compare minimum coordinate and redundant coordinate formulations
5#
6# Author: Johannes Gerstmayr
7# Date: 2022-05-29
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13import exudyn as exu
14from exudyn.utilities import *
15from exudyn.FEM import *
16
17import numpy as np
18
19useGraphics = True
20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
22try: #only if called from test suite
23 from modelUnitTests import exudynTestGlobals #for globally storing test results
24 useGraphics = exudynTestGlobals.useGraphics
25except:
26 class ExudynTestGlobals:
27 pass
28 exudynTestGlobals = ExudynTestGlobals()
29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
31from math import pi, sin, cos#, sqrt
32from copy import copy, deepcopy
33from exudyn.rigidBodyUtilities import Skew, Skew2Vec
34from exudyn.robotics import *
35
36SC = exu.SystemContainer()
37mbs = SC.AddSystem()
38
39useGraphics = False
40performTest = True
41
42printSensors = True
43#useGraphics = False
44#exudynTestGlobals.testError = 0. #not filled, done via result
45exudynTestGlobals.testResult = 0. #values added up
46
47useMBS = True
48useKinematicTree = True
49
50# case = '3Dmechanism'
51case = 'invertedPendulum'
52#case = 'treeStructure'
53
54
55#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56#this function is used to compare class Robot internal structures with ObjectKinematicTree sensor
57compareKT = False
58jacobian = np.eye(3)
59def CompareKinematicTreeAndRobot(newRobot, locPos):
60 #compare with Python class Robot functions for validation:
61 #from exudyn.kinematicTree import *
62 global jacobian
63 #get coordinates (), INCLUDES reference values:
64 q = mbs.GetObjectOutput(oKT,exu.OutputVariableType.Coordinates)
65 q_t = mbs.GetObjectOutput(oKT,exu.OutputVariableType.Coordinates_t)
66 #print('q=',q)
67
68 baseHT = newRobot.GetBaseHT()
69 allHT = newRobot.JointHT(q)
70
71 print('compare solution for n links=', newRobot.NumberOfLinks())
72 for i in range(newRobot.NumberOfLinks()):
73 #link = newRobot.GetLink(i)
74 #compare position; we need a sensor to access variables
75 s=mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=i, localPosition=locPos,
76 outputVariableType=exu.OutputVariableType.Position))
77 mbs.systemIsConsistent = True #adding new sensor requires re-assemble, which is not done here
78 pRobot = HT2translation(allHT[i]) + HT2rotationMatrix(allHT[i]) @ locPos
79 pKT = mbs.GetSensorValues(s)
80 # print('joint pos: Robot=', HT2translation(allHT[i]) + HT2rotationMatrix(allHT[i]) @ locPos,
81 # ', KT=', mbs.GetSensorValues(s))
82 print('position diff=', (pRobot-pKT).round(15))
83
84 #compare velocity
85 sVel=mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=i, localPosition=locPos,
86 outputVariableType=exu.OutputVariableType.Velocity))
87 sOmega=mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=i, localPosition=locPos,
88 outputVariableType=exu.OutputVariableType.AngularVelocity))
89 mbs.systemIsConsistent = True #adding new sensor requires re-assemble, which is not done here
90
91 jacobian = newRobot.Jacobian(allHT[0:i+1], toolPosition=HT2translation(allHT[i]@HTtranslate(locPos)), mode='all')
92 #print('jac=', jacobian.round(3))
93
94 vOmegaRobot = jacobian @ q_t[0:i+1]
95 vOmegaKT = list(mbs.GetSensorValues(sVel)) + list(mbs.GetSensorValues(sOmega))
96 #print('vel: Robot=', vOmegaRobot, ', KT=', vOmegaKT)
97 print('vel diff=', (vOmegaRobot-vOmegaKT).round(14))
98
99
100#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
101#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
102#some spatial mechanism with revolute and prismatic joints:
103
104if case == '3Dmechanism' or performTest:
105 mbs.Reset()
106 gGround = GraphicsDataCheckerBoard(point= [0,0,-2], size = 4)
107 objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
108 visualization=VObjectGround(graphicsData=[gGround])))
109 baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
110
111 L = 0.5 #length
112 w = 0.1 #width of links
113 pControl = 200*0
114 dControl = pControl*0.02
115
116 # pControl=None
117 # dControl=None
118
119 gravity3D = [0,-9.81,0]
120 #gravity3D = [0,-9.81,0] #note that this system is extremely sensitive to disturbances: adding 1e-15 will change solution by 1e-7
121 graphicsBaseList = [GraphicsDataOrthoCubePoint(size=[L*4, 0.8*w, 0.8*w], color=color4grey)] #rail
122
123 newRobot = Robot(gravity=gravity3D,
124 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
125 tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[
126 GraphicsDataOrthoCubePoint(size=[w, L, w], color=color4orange)])),
127 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
128
129 #cart:
130 Jlink = InertiaCuboid(density=5000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
131 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
132 jointType='Px', preHT=HT0(),
133 PDcontrol=(pControl, dControl),
134 visualization=VRobotLink(linkColor=color4lawngreen))
135 newRobot.AddLink(link)
136
137 if True:
138 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
139 Jlink = Jlink.Translated([0,0.5*L,0])
140 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
141 jointType='Rz', preHT=HT0(),
142 PDcontrol=(pControl, dControl),
143 visualization=VRobotLink(linkColor=color4blue))
144 newRobot.AddLink(link)
145
146
147 if True:
148 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
149 Jlink = Jlink.Translated([0,0.5*L,0])
150 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
151 # jointType='Rz', preHT=HTtranslateY(L),
152 jointType='Rz', preHT=HTtranslateY(L)@HTrotateY(0.25*pi),
153 PDcontrol=(pControl, dControl),
154 visualization=VRobotLink(linkColor=color4red))
155 newRobot.AddLink(link)
156
157 if False:
158 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
159 Jlink = Jlink.Translated([0,0.5*L,0])
160 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
161 jointType='Px', preHT=HT0(),
162 PDcontrol=(pControl, dControl),
163 visualization=VRobotLink(linkColor=color4lawngreen))
164 newRobot.AddLink(link)
165
166 if True:
167 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
168 Jlink = Jlink.Translated([0,0.5*L,0])
169 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
170 jointType='Rz', preHT=HTtranslateY(L)@HTrotateZ(-0.5*pi),
171 PDcontrol=(pControl, dControl),
172 #visualization=VRobotLink(linkColor=color4brown))
173 visualization=VRobotLink(linkColor=[-1,-1,-1,1]))
174 newRobot.AddLink(link)
175
176 if False:
177 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
178 Jlink = Jlink.Translated([0,0.5*L,0])
179 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
180 jointType='Rz', preHT=HTtranslateY(L)@HTrotateZ(-0.5*pi),
181 PDcontrol=(pControl, dControl),
182 visualization=VRobotLink(linkColor=color4brown))
183 newRobot.AddLink(link)
184
185 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
186 Jlink = Jlink.Translated([0,0.5*L,0])
187 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
188 jointType='Rz', preHT=HTtranslateY(L)@HTrotateZ(-0.5*pi),
189 PDcontrol=(pControl, dControl),
190 visualization=VRobotLink(linkColor=color4brown))
191 newRobot.AddLink(link)
192
193 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
194 Jlink = Jlink.Translated([0,0.5*L,0])
195 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
196 jointType='Rz', preHT=HTtranslateY(L)@HTrotateZ(-0.5*pi),
197 PDcontrol=(pControl, dControl),
198 visualization=VRobotLink(linkColor=color4brown))
199 newRobot.AddLink(link)
200
201 sMBS = []
202 locPos = [0.1,0.2,0.3]
203 # locPos = [0,0,0]
204 nLinks = newRobot.NumberOfLinks()
205 if useMBS:
206 robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
207 bodies = robDict['bodyList']
208
209 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[0], localPosition=locPos, storeInternal=True,
210 outputVariableType=exu.OutputVariableType.Position))]
211 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[0], localPosition=locPos, storeInternal=True,
212 outputVariableType=exu.OutputVariableType.Acceleration))]
213 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[1], localPosition=locPos, storeInternal=True,
214 outputVariableType=exu.OutputVariableType.Acceleration))]
215 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[2], localPosition=locPos, storeInternal=True,
216 outputVariableType=exu.OutputVariableType.Acceleration))]
217
218 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
219 outputVariableType=exu.OutputVariableType.Position))]
220 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
221 outputVariableType=exu.OutputVariableType.Rotation))]
222 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
223 outputVariableType=exu.OutputVariableType.Velocity))]
224 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
225 outputVariableType=exu.OutputVariableType.AngularVelocity))]
226 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
227 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))]
228 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
229 outputVariableType=exu.OutputVariableType.Acceleration))]
230 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
231 outputVariableType=exu.OutputVariableType.AngularAcceleration))]
232
233 sKT = []
234 if useKinematicTree:
235 dKT = newRobot.CreateKinematicTree(mbs)
236 oKT = dKT['objectKinematicTree']
237
238 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=0, localPosition=locPos, storeInternal=True,
239 outputVariableType=exu.OutputVariableType.Position))]
240 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=0, localPosition=locPos, storeInternal=True,
241 outputVariableType=exu.OutputVariableType.Acceleration))]
242 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=1, localPosition=locPos, storeInternal=True,
243 outputVariableType=exu.OutputVariableType.Acceleration))]
244 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=2, localPosition=locPos, storeInternal=True,
245 outputVariableType=exu.OutputVariableType.Acceleration))]
246
247 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
248 outputVariableType=exu.OutputVariableType.Position))]
249 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
250 outputVariableType=exu.OutputVariableType.Rotation))]
251 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
252 outputVariableType=exu.OutputVariableType.Velocity))]
253 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
254 outputVariableType=exu.OutputVariableType.AngularVelocity))]
255 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
256 outputVariableType=exu.OutputVariableType.AngularVelocityLocal))]
257 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
258 outputVariableType=exu.OutputVariableType.Acceleration))]
259 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
260 outputVariableType=exu.OutputVariableType.AngularAcceleration))]
261
262
263 sTitles = [
264 'Position link 0',
265 'Acceleration link 0',
266 'Acceleration link 1',
267 'Acceleration link 2',
268 'Tip Position',
269 'Tip Rotation',
270 'Tip Velocity',
271 'Tip AngularVelocity',
272 'Tip AngularVelocityLocal',
273 'Tip Acceleration',
274 'Tip AngularAcceleration',
275 ]
276
277 #exu.Print(mbs)
278 mbs.Assemble()
279
280 simulationSettings = exu.SimulationSettings()
281
282 tEnd = 0.5
283 if not performTest:
284 tEnd = 2*0.5
285
286 h = 1e-3 #0.1
287 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
288 simulationSettings.timeIntegration.endTime = tEnd
289 # simulationSettings.timeIntegration.numberOfSteps = 1#int(tEnd/h)
290 # simulationSettings.timeIntegration.endTime = h*1#tEnd
291 simulationSettings.solutionSettings.solutionWritePeriod = 0.01
292 simulationSettings.solutionSettings.sensorsWritePeriod = 0.001
293 simulationSettings.timeIntegration.verboseMode = 1
294 #simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
295 simulationSettings.timeIntegration.newton.useModifiedNewton=True
296
297 # simulationSettings.displayComputationTime = True
298 simulationSettings.displayStatistics = True
299 # simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
300
301 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
302
303 SC.visualizationSettings.general.autoFitScene=False
304 SC.visualizationSettings.window.renderWindowSize = [1600,1200]
305 SC.visualizationSettings.general.drawCoordinateSystem=True
306 SC.visualizationSettings.general.drawWorldBasis=True
307 SC.visualizationSettings.openGL.multiSampling=4
308 SC.visualizationSettings.nodes.showBasis = True
309 SC.visualizationSettings.nodes.basisSize = 0.5
310 if useGraphics:
311 exu.StartRenderer()
312 if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
313
314 mbs.WaitForUserToContinue() #press space to continue
315
316 # mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
317 mbs.SolveDynamic(simulationSettings)
318
319 if useGraphics: #use this to reload the solution and use SolutionViewer
320 #sol = LoadSolutionFile('coordinatesSolution.txt')
321
322 mbs.SolutionViewer() #can also be entered in IPython ...
323
324 if useGraphics:
325 SC.WaitForRenderEngineStopFlag()
326 exu.StopRenderer() #safely close rendering window!
327
328
329 if len(sMBS) == len(sKT):
330 if useGraphics:
331
332 mbs.PlotSensor(closeAll=True)
333
334 for i in range(len(sMBS)):
335 mbs.PlotSensor(sensorNumbers=[sMBS[i]]*3+[sKT[i]]*3, components=[0,1,2]*2, title=sTitles[i])
336 else:
337 u = 0.
338 for i in range(len(sMBS)):
339 v = mbs.GetSensorValues(sMBS[i])
340 if printSensors:
341 exu.Print('sensor MBS '+str(i)+'=',list(v))
342 u += np.linalg.norm(v)
343 v = mbs.GetSensorValues(sKT[i])
344 if printSensors:
345 exu.Print('sensor KT '+str(i)+' =',list(v))
346 u += np.linalg.norm(v)
347
348 exu.Print("solution of kinematicTreeAndMBStest 1=", u)
349 exudynTestGlobals.testResult += u
350
351 if compareKT:
352 CompareKinematicTreeAndRobot(newRobot, [0.1,0.3,0.2])
353
354
355#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
356#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
357if case == 'invertedPendulum' or performTest:
358 mbs.Reset()
359 gGround = GraphicsDataCheckerBoard(point= [0,0,-2], size = 12)
360 objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
361 visualization=VObjectGround(graphicsData=[gGround])))
362 baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
363
364 L = 0.5 #length
365 w = 0.1 #width of links
366 pControl = 200*0
367 dControl = pControl*0.02
368
369 gravity3D = [10*0,-9.81,0]
370 graphicsBaseList = [GraphicsDataOrthoCubePoint(size=[L*4, 0.8*w, 0.8*w], color=color4grey)] #rail
371
372 newRobot = Robot(gravity=gravity3D,
373 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
374 tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[
375 GraphicsDataOrthoCubePoint(size=[w, L, w], color=color4orange)])),
376 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
377
378 #cart:
379 Jlink = InertiaCuboid(density=5000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
380 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
381 jointType='Px', preHT=HT0(),
382 PDcontrol=(pControl, dControl),
383 visualization=VRobotLink(linkColor=color4lawngreen))
384 newRobot.AddLink(link)
385
386 nChainLinks = 5
387
388 for i in range(nChainLinks):
389 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
390 Jlink = Jlink.Translated([0,0.5*L,0])
391 preHT = HT0()
392 if i > 0:
393 preHT = HTtranslateY(L)
394
395 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
396 jointType='Rz', preHT=preHT,
397 PDcontrol=(pControl, dControl),
398 visualization=VRobotLink(linkColor=color4blue))
399 newRobot.AddLink(link)
400
401 newRobot.referenceConfiguration[0] = 0.5*0
402 # for i in range(nChainLinks):
403 # newRobot.referenceConfiguration[i+1] = (2*pi/360) * 5
404 newRobot.referenceConfiguration[1] = -(2*pi/360) * 5 #-0.5*pi
405 # newRobot.referenceConfiguration[2] = (2*pi/360) * 12 #-0.5*pi
406
407
408 sMBS = []
409 # locPos = [0.1,0.2,0.3]
410 locPos = [0,0,0]
411 nLinks = newRobot.NumberOfLinks()
412 if useMBS:
413 robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
414 bodies = robDict['bodyList']
415
416 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
417 outputVariableType=exu.OutputVariableType.Position))]
418
419 sKT = []
420 if useKinematicTree:
421 dKT = newRobot.CreateKinematicTree(mbs)
422 oKT = dKT['objectKinematicTree']
423
424 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
425 outputVariableType=exu.OutputVariableType.Position))]
426
427 #exu.Print(mbs)
428 mbs.Assemble()
429
430 simulationSettings = exu.SimulationSettings()
431
432 tEnd = 0.5
433 if not performTest:
434 tEnd = 0.5
435 h = 1e-3 #0.1
436 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
437 simulationSettings.timeIntegration.endTime = tEnd
438 # simulationSettings.timeIntegration.numberOfSteps = 1#int(tEnd/h)
439 # simulationSettings.timeIntegration.endTime = h*1#tEnd
440 simulationSettings.solutionSettings.solutionWritePeriod = 0.01
441 simulationSettings.solutionSettings.sensorsWritePeriod = 0.001*10
442 simulationSettings.timeIntegration.verboseMode = 1
443 #simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
444 simulationSettings.timeIntegration.newton.useModifiedNewton=True
445
446 # simulationSettings.displayComputationTime = True
447 # simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
448
449 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
450
451 SC.visualizationSettings.general.autoFitScene=False
452 SC.visualizationSettings.window.renderWindowSize = [1600,1200]
453 SC.visualizationSettings.general.drawCoordinateSystem=True
454 SC.visualizationSettings.general.drawWorldBasis=True
455 SC.visualizationSettings.openGL.multiSampling=4
456 SC.visualizationSettings.nodes.showBasis = True
457 SC.visualizationSettings.nodes.basisSize = 0.5
458 if useGraphics:
459
460 exu.StartRenderer()
461 if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
462
463 mbs.WaitForUserToContinue() #press space to continue
464
465 # mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
466 mbs.SolveDynamic(simulationSettings)
467
468 if useGraphics: #use this to reload the solution and use SolutionViewer
469 #sol = LoadSolutionFile('coordinatesSolution.txt')
470
471 mbs.SolutionViewer() #can also be entered in IPython ...
472
473 if useGraphics:
474 SC.WaitForRenderEngineStopFlag()
475 exu.StopRenderer() #safely close rendering window!
476 else:
477 #check results for test suite:
478 u = 0.
479 for i in range(len(sMBS)):
480 v = mbs.GetSensorValues(sMBS[i])
481 if printSensors:
482 exu.Print('sensor MBS '+str(i)+'=',v)
483 u += np.linalg.norm(v)
484 v = mbs.GetSensorValues(sKT[i])
485 if printSensors:
486 exu.Print('sensor KT '+str(i)+' =',v)
487 u += np.linalg.norm(v)
488
489 exu.Print("solution of kinematicTreeAndMBStest 2=", u)
490 exudynTestGlobals.testResult += u
491
492 if compareKT:
493 # CompareKinematicTreeAndRobot(newRobot, [0.1,0.3,0.2])
494 CompareKinematicTreeAndRobot(newRobot, [0.,0.,0.])
495
496
497#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
498#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
499if case == 'treeStructure' or performTest:
500 mbs.Reset()
501 gGround = GraphicsDataCheckerBoard(point= [0,0,-2], size = 12)
502 objectGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
503 visualization=VObjectGround(graphicsData=[gGround])))
504 baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
505
506 L = 0.5 #length
507 w = 0.1 #width of links
508 pControl = 200*0
509 dControl = pControl*0.02
510
511 gravity3D = [10*0,-9.81,0]
512 graphicsBaseList = [GraphicsDataOrthoCubePoint(size=[L*4, 0.8*w, 0.8*w], color=color4grey)] #rail
513
514 newRobot = Robot(gravity=gravity3D,
515 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
516 #tool = RobotTool(HT=HTtranslate([0,0.5*L,0]), visualization=VRobotTool(graphicsData=[GraphicsDataOrthoCubePoint(size=[w, L, w], color=color4orange)])),
517 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
518
519 #cart:
520 Jlink = InertiaCuboid(density=5000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
521 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
522 jointType='Px', preHT=HT0(),
523 parent = -1,
524 PDcontrol=(pControl, dControl),
525 visualization=VRobotLink(linkColor=color4lawngreen))
526 rootLink = newRobot.AddLink(link)
527
528 nChainLinks = 5
529
530 parentLink = rootLink
531 for i in range(nChainLinks):
532 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
533 Jlink = Jlink.Translated([0,0.5*L,0])
534 preHT = HTtranslateX(0.5*L)
535 if i > 0:
536 preHT = HTtranslateY(L)@HTrotateZ(-5*(2*pi/360))
537
538 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
539 jointType='Rz', preHT=preHT,
540 parent = parentLink,
541 PDcontrol=(pControl, dControl),
542 visualization=VRobotLink(linkColor=color4blue))
543 parentLink = newRobot.AddLink(link)
544
545 parentLink = rootLink
546 for i in range(nChainLinks):
547 Jlink = InertiaCuboid(density=1000, sideLengths=[w,L,w]) #w.r.t. reference center of mass
548 Jlink = Jlink.Translated([0,0.5*L,0])
549 preHT = HTtranslateX(-0.5*L)
550 if i > 0:
551 preHT = HTtranslateY(L)@HTrotateZ(5*(2*pi/360))
552
553 link = RobotLink(Jlink.Mass(), Jlink.COM(), Jlink.InertiaCOM(),
554 jointType='Rz', preHT=preHT,
555 parent = parentLink,
556 PDcontrol=(pControl, dControl),
557 visualization=VRobotLink(linkColor=color4blue))
558 parentLink = newRobot.AddLink(link)
559
560 #newRobot.referenceConfiguration[0] = 0.5*0
561 # for i in range(nChainLinks):
562 # newRobot.referenceConfiguration[i+1] = (2*pi/360) * 5
563 #newRobot.referenceConfiguration[1] = -(2*pi/360) * 5 #-0.5*pi
564
565
566 sMBS = []
567 # locPos = [0.1,0.2,0.3]
568 locPos = [0,0,0]
569 nLinks = newRobot.NumberOfLinks()
570 if useMBS:
571 robDict = newRobot.CreateRedundantCoordinateMBS(mbs=mbs, baseMarker=baseMarker, createJointTorqueLoads=False)
572 bodies = robDict['bodyList']
573
574 sMBS+=[mbs.AddSensor(SensorBody(bodyNumber=bodies[nLinks-1], localPosition=locPos, storeInternal=True,
575 outputVariableType=exu.OutputVariableType.Position))]
576
577 sKT = []
578 if useKinematicTree:
579 dKT = newRobot.CreateKinematicTree(mbs)
580 oKT = dKT['objectKinematicTree']
581
582 sKT+=[mbs.AddSensor(SensorKinematicTree(objectNumber=oKT, linkNumber=nLinks-1, localPosition=locPos, storeInternal=True,
583 outputVariableType=exu.OutputVariableType.Position))]
584
585 #exu.Print(mbs)
586 mbs.Assemble()
587
588 simulationSettings = exu.SimulationSettings()
589
590 tEnd = 0.25
591 if not performTest:
592 tEnd = 5
593 h = 1e-3 #0.1
594 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
595 simulationSettings.timeIntegration.endTime = tEnd
596 # simulationSettings.timeIntegration.numberOfSteps = 1#int(tEnd/h)
597 # simulationSettings.timeIntegration.endTime = h*1#tEnd
598 simulationSettings.solutionSettings.solutionWritePeriod = 0.01
599 simulationSettings.solutionSettings.sensorsWritePeriod = 0.001*10
600 simulationSettings.timeIntegration.verboseMode = 1
601 #simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
602 simulationSettings.timeIntegration.newton.useModifiedNewton=True
603
604 # simulationSettings.displayComputationTime = True
605 # simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
606
607 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.95 #SHOULD work with 0.9 as well
608
609 SC.visualizationSettings.general.autoFitScene=False
610 SC.visualizationSettings.window.renderWindowSize = [1600,1200]
611 SC.visualizationSettings.general.drawCoordinateSystem=True
612 SC.visualizationSettings.general.drawWorldBasis=True
613 SC.visualizationSettings.openGL.multiSampling=4
614 SC.visualizationSettings.nodes.showBasis = True
615 SC.visualizationSettings.nodes.basisSize = 0.5
616 if useGraphics:
617
618 exu.StartRenderer()
619 if 'renderState' in exu.sys: SC.SetRenderState(exu.sys['renderState']) #load last model view
620
621 mbs.WaitForUserToContinue() #press space to continue
622
623 # mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.ExplicitMidpoint)
624 mbs.SolveDynamic(simulationSettings)
625
626 if useGraphics: #use this to reload the solution and use SolutionViewer
627 #sol = LoadSolutionFile('coordinatesSolution.txt')
628
629 mbs.SolutionViewer() #can also be entered in IPython ...
630
631 if useGraphics:
632 SC.WaitForRenderEngineStopFlag()
633 exu.StopRenderer() #safely close rendering window!
634 else:
635 #check results for test suite:
636 u = 0.
637 for i in range(len(sMBS)):
638 v = mbs.GetSensorValues(sMBS[i])
639 if printSensors:
640 exu.Print('sensor MBS '+str(i)+'=',v)
641 u += np.linalg.norm(v)
642 v = mbs.GetSensorValues(sKT[i])
643 if printSensors:
644 exu.Print('sensor KT '+str(i)+' =',v)
645 u += np.linalg.norm(v)
646
647 exu.Print("solution of kinematicTreeAndMBStest 3=", u)
648 exudynTestGlobals.testResult += u
649
650
651 if compareKT:
652 CompareKinematicTreeAndRobot(newRobot, [0.1,0.3,0.2])
653
654exudynTestGlobals.testResult *= 1e-7 #result is too sensitive to small (1e-15) disturbances, so different results for 32bits and linux
655exu.Print("solution of kinematicTreeAndMBStest all=", exudynTestGlobals.testResult)