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