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