serialRobotFlexible.py
You can view and download this file on Github: serialRobotFlexible.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Example of a serial robot with redundant coordinates
5#
6# Author: Johannes Gerstmayr
7# Date: 2020-02-16
8# Revised: 2021-07-09
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
14
15import exudyn as exu
16from exudyn.itemInterface import *
17from exudyn.utilities import *
18from exudyn.rigidBodyUtilities import *
19from exudyn.graphicsDataUtilities import *
20from exudyn.robotics import *
21from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
22from exudyn.FEM import *
23
24import numpy as np
25from numpy import linalg as LA
26from math import pi
27import sys
28import time
29
30SC = exu.SystemContainer()
31mbs = SC.AddSystem()
32
33sensorWriteToFile = True
34
35fileNames = ['testData/netgenRobotBase',
36 'testData/netgenRobotArm0',
37 'testData/netgenRobotArm1',
38 ] #for load/save of FEM data
39
40
41
42#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
43
44gravity=[0,0,-9.81]
45#geometry, arm lengths:t
46L = [0.075,0.4318,0.15,0.4318]
47W = [0,0,0.015,0]
48rArm = 0.025 #radius arm
49rFlange = 0.05 #radius of flange
50meshSize = rArm*0.5
51meshOrder = 2 #2 is more accurate!
52useFlexBody = False
53
54Lbase = 0.3
55flangeBaseR = 0.05 #socket of base radius
56flangeBaseL = 0.05 #socket of base length
57rBase = 0.08
58tBase = 0.01 #wall thickness
59
60#standard:
61nModes = 8
62
63rho = 1000
64Emodulus=1e9 #steel: 2.1e11
65nu=0.3
66dampingK = 1e-2 #stiffness proportional damping
67
68nFlexBodies = 1*int(useFlexBody)
69femList = [None]*nFlexBodies
70
71def GetCylinder(p0, axis, length, radius):
72 pnt0 = Pnt(p0[0], p0[1], p0[2])
73 pnt1 = pnt0 + Vec(axis[0]*length, axis[1]*length, axis[2]*length)
74 cyl = Cylinder(pnt0, pnt1, radius)
75 plane0 = Plane (pnt0, Vec(-axis[0], -axis[1], -axis[2]) )
76 plane1 = Plane (pnt1, Vec(axis[0], axis[1], axis[2]) )
77 return cyl*plane0*plane1
78
79
80fb=[] #flexible bodies list of dictionaries
81fb+=[{'p0':[0,0,-Lbase], 'p1':[0,0,0], 'axis0':[0,0,1], 'axis1':[0,0,1]}] #defines flanges
82
83fes = None
84#create flexible bodies
85#requires netgen / ngsolve
86#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
87if True and useFlexBody: #needs netgen/ngsolve to be installed to compute mesh, see e.g.: https://github.com/NGSolve/ngsolve/releases
88 femList[0] = FEMinterface()
89 import sys
90 #adjust path to your ngsolve installation (if not added to global path)
91 sys.path.append('C:/ProgramData/ngsolve/lib/site-packages')
92
93 import ngsolve as ngs
94 import netgen
95 from netgen.meshing import *
96
97 from netgen.geom2d import unit_square
98 #import netgen.libngpy as libng
99 from netgen.csg import *
100
101
102 #++++++++++++++++++++++++++++++++++++++++++++++++
103 #flange
104 geo = CSGeometry()
105
106
107 geo.Add(GetCylinder(fb[0]['p0'], fb[0]['axis0'], Lbase-flangeBaseL, rBase) -
108 GetCylinder([0,0,-Lbase+tBase], [0,0,1], Lbase-2*tBase-flangeBaseL, rBase-tBase) +
109 GetCylinder([0,0,-flangeBaseL-tBase*0.5], fb[0]['axis1'], flangeBaseL+tBase*0.5, flangeBaseR))
110
111 print('start meshing')
112 mesh = ngs.Mesh( geo.GenerateMesh(maxh=meshSize))
113 mesh.Curve(1)
114 print('finished meshing')
115
116 if False: #set this to true, if you want to visualize the mesh inside netgen/ngsolve
117 # import netgen
118 import netgen.gui
119 ngs.Draw(mesh)
120 for i in range(10000000):
121 netgen.Redraw() #this makes the window interactive
122 time.sleep(0.05)
123
124 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
125 #Use fem to import FEM model and create FFRFreducedOrder object
126 [bfM, bfK, fes] = femList[0].ImportMeshFromNGsolve(mesh, density=rho, youngsModulus=Emodulus, poissonsRatio=nu, meshOrder=meshOrder)
127 femList[0].SaveToFile(fileNames[0])
128
129#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
130# sys.exit()
131
132#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
133#compute flexible modes
134
135for i in range(nFlexBodies):
136 fem = femList[i]
137 fem.LoadFromFile(fileNames[i])
138
139 nodesPlane0 = fem.GetNodesInPlane(fb[i]['p0'], fb[i]['axis0'])
140 # print('body'+str(i)+'nodes0=', nodesPlane0)
141 lenNodesPlane0 = len(nodesPlane0)
142 weightsPlane0 = np.array((1./lenNodesPlane0)*np.ones(lenNodesPlane0))
143
144 nodesPlane1 = fem.GetNodesInPlane(fb[i]['p1'], fb[i]['axis1'])
145 # print('body'+str(i)+'nodes1=', nodesPlane1)
146 lenNodesPlane1 = len(nodesPlane1)
147 weightsPlane1 = np.array((1./lenNodesPlane1)*np.ones(lenNodesPlane1))
148
149 boundaryList = [nodesPlane0, nodesPlane1]
150
151 print("nNodes=",fem.NumberOfNodes())
152
153 print("compute flexible modes... ")
154 start_time = time.time()
155 fem.ComputeHurtyCraigBamptonModes(boundaryNodesList=boundaryList,
156 nEigenModes=nModes,
157 useSparseSolver=True,
158 computationMode = HCBstaticModeSelection.RBE2)
159 print("compute modes needed %.3f seconds" % (time.time() - start_time))
160
161
162 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
163 #compute stress modes for postprocessing (inaccurate for coarse meshes, just for visualization):
164 if fes != None:
165 mat = KirchhoffMaterial(Emodulus, nu, rho)
166 varType = exu.OutputVariableType.StressLocal
167 #varType = exu.OutputVariableType.StrainLocal
168 print("ComputePostProcessingModes ... (may take a while)")
169 start_time = time.time()
170
171 #without NGsolve, but only for linear elements
172 # fem.ComputePostProcessingModes(material=mat,
173 # outputVariableType=varType)
174 fem.ComputePostProcessingModesNGsolve(fes, material=mat,
175 outputVariableType=varType)
176
177 print(" ... needed %.3f seconds" % (time.time() - start_time))
178 # SC.visualizationSettings.contour.reduceRange=False
179 SC.visualizationSettings.contour.outputVariable = varType
180 SC.visualizationSettings.contour.outputVariableComponent = -1 #x-component
181 else:
182 SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.DisplacementLocal
183 SC.visualizationSettings.contour.outputVariableComponent = 1
184
185 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
186 print("create CMS element ...")
187 cms = ObjectFFRFreducedOrderInterface(fem)
188
189 objFFRF = cms.AddObjectFFRFreducedOrder(mbs, positionRef=[0,0,0],
190 initialVelocity=[0,0,0],
191 initialAngularVelocity=[0,0,0],
192 stiffnessProportionalDamping=dampingK,
193 gravity=gravity,
194 color=[0.1,0.9,0.1,1.],
195 )
196
197
198 #%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
199 #animate modes
200 if False:
201 from exudyn.interactive import AnimateModes
202 mbs.Assemble()
203 SC.visualizationSettings.nodes.show = False
204 SC.visualizationSettings.openGL.showFaceEdges = True
205 SC.visualizationSettings.openGL.multiSampling=4
206 #SC.visualizationSettings.window.renderWindowSize = [1600,1080]
207 # SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.DisplacementLocal
208 # SC.visualizationSettings.contour.outputVariableComponent = 0 #component
209
210
211 #%%+++++++++++++++++++++++++++++++++++++++
212 #animate modes of ObjectFFRFreducedOrder (only needs generic node containing modal coordinates)
213 SC.visualizationSettings.general.autoFitScene = False #otherwise, model may be difficult to be moved
214
215 nodeNumber = objFFRF['nGenericODE2'] #this is the node with the generalized coordinates
216 AnimateModes(SC, mbs, nodeNumber)
217 import sys
218 sys.exit()
219
220
221
222 if True:
223
224 mPlane0 = mbs.AddMarker(MarkerSuperElementRigid(bodyNumber=objFFRF['oFFRFreducedOrder'],
225 meshNodeNumbers=np.array(nodesPlane0), #these are the meshNodeNumbers
226 weightingFactors=weightsPlane0))
227 mPlane1 = mbs.AddMarker(MarkerSuperElementRigid(bodyNumber=objFFRF['oFFRFreducedOrder'],
228 meshNodeNumbers=np.array(nodesPlane1), #these are the meshNodeNumbers
229 weightingFactors=weightsPlane1))
230
231 if i==0:
232 baseMarker = mPlane1
233 oGround = mbs.AddObject(ObjectGround(referencePosition= [0,0,0]))
234 mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=fb[i]['p0']))
235 mbs.AddObject(GenericJoint(markerNumbers=[mGround, mPlane0],
236 constrainedAxes = [1,1,1,1,1,1],
237 visualization=VGenericJoint(axesRadius=rFlange*0.5, axesLength=rFlange)))
238
239
240#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
241#%%+++++++++++++++++++++++++++++++++++++++++++++++++++++
242#robotics part
243graphicsBaseList = []
244if not useFlexBody:
245 #graphicsBaseList +=[GraphicsDataOrthoCubePoint([0,0,-0.15], [0.12,0.12,0.1], color4grey)]
246
247 graphicsBaseList +=[GraphicsDataCylinder([0,0,-Lbase], [0,0,Lbase-flangeBaseL], rBase, color4blue)]
248 graphicsBaseList +=[GraphicsDataCylinder([0,0,-flangeBaseL], [0,0,flangeBaseL], flangeBaseR, color4blue)]
249 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0.25,0,0], 0.00125, color4red)]
250 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0.25,0], 0.00125, color4green)]
251 graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0,0.25], 0.00125, color4blue)]
252
253#base graphics is fixed to ground!!!
254graphicsBaseList +=[GraphicsDataCheckerBoard([0,0,-Lbase], size=2.5)]
255#newRobot.base.visualization['graphicsData']=graphicsBaseList
256
257ty = 0.03
258tz = 0.04
259zOff = -0.05
260toolSize= [0.05,0.5*ty,0.06]
261graphicsToolList = [GraphicsDataCylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=color4red)]
262graphicsToolList+= [GraphicsDataOrthoCubePoint([0,ty,1.5*tz+zOff], toolSize, color4grey)]
263graphicsToolList+= [GraphicsDataOrthoCubePoint([0,-ty,1.5*tz+zOff], toolSize, color4grey)]
264
265
266#changed to new robot structure July 2021:
267newRobot = Robot(gravity=gravity,
268 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
269 tool = RobotTool(HT=HTtranslate([0,0,0.1]), visualization=VRobotTool(graphicsData=graphicsToolList)),
270 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
271
272#modKKDH according to Khalil and Kleinfinger, 1986
273link0={'stdDH':[0,L[0],0,pi/2],
274 'modKKDH':[0,0,0,0],
275 'mass':20, #not needed!
276 'inertia':np.diag([1e-8,0.35,1e-8]), #w.r.t. COM! in stdDH link frame
277 'COM':[0,0,0]} #in stdDH link frame
278
279link1={'stdDH':[0,0,L[1],0],
280 'modKKDH':[0.5*pi,0,0,0],
281 'mass':17.4,
282 'inertia':np.diag([0.13,0.524,0.539]), #w.r.t. COM! in stdDH link frame
283 'COM':[-0.3638, 0.006, 0.2275]} #in stdDH link frame
284
285link2={'stdDH':[0,L[2],W[2],-pi/2],
286 'modKKDH':[0,0.4318,0,0.15],
287 'mass':4.8,
288 'inertia':np.diag([0.066,0.086,0.0125]), #w.r.t. COM! in stdDH link frame
289 'COM':[-0.0203,-0.0141,0.07]} #in stdDH link frame
290
291link3={'stdDH':[0,L[3],0,pi/2],
292 'modKKDH':[-0.5*pi,0.0203,0,0.4318],
293 'mass':0.82,
294 'inertia':np.diag([0.0018,0.0013,0.0018]), #w.r.t. COM! in stdDH link frame
295 'COM':[0,0.019,0]} #in stdDH link frame
296
297link4={'stdDH':[0,0,0,-pi/2],
298 'modKKDH':[0.5*pi,0,0,0],
299 'mass':0.34,
300 'inertia':np.diag([0.0003,0.0004,0.0003]), #w.r.t. COM! in stdDH link frame
301 'COM':[0,0,0]} #in stdDH link frame
302
303link5={'stdDH':[0,0,0,0],
304 'modKKDH':[-0.5*pi,0,0,0],
305 'mass':0.09,
306 'inertia':np.diag([0.00015,0.00015,4e-5]), #w.r.t. COM! in stdDH link frame
307 'COM':[0,0,0.032]} #in stdDH link frame
308linkList=[link0, link1, link2, link3, link4, link5]
309
310#control parameters, per joint:
311Pcontrol = np.array([40000, 40000, 40000, 100, 100, 10])
312Dcontrol = np.array([400, 400, 100, 1, 1, 0.1])
313
314for i, link in enumerate(linkList):
315 newRobot.AddLink(RobotLink(mass=link['mass'],
316 COM=link['COM'],
317 inertia=link['inertia'],
318 localHT=StdDH2HT(link['stdDH']),
319 PDcontrol=(Pcontrol[i], Dcontrol[i]),
320 ))
321
322showCOM = False
323for cnt, link in enumerate(newRobot.links):
324 color = color4list[cnt]
325 color[3] = 0.75 #make transparent
326 link.visualization = VRobotLink(jointRadius=0.055, jointWidth=0.055*2, showMBSjoint=False,
327 linkWidth=2*0.05, linkColor=color, showCOM= showCOM )
328
329#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
330#configurations and trajectory
331q0 = [0,0,0,0,0,0] #zero angle configuration
332
333#this set of coordinates only works with TSD, not with old fashion load control:
334# q1 = [0, pi/8, pi*0.75, 0,pi/8,0] #configuration 1
335# q2 = [pi,-pi, -pi*0.5,1.5*pi,-pi*2,pi*2] #configuration 2
336# q3 = [3*pi,0,-0.25*pi,0,0,0] #zero angle configuration
337
338#this set also works with load control:
339q1 = [0, pi/8, pi*0.5, 0,pi/8,0] #configuration 1
340q2 = [0.8*pi,0.5*pi, -pi*0.5,0.75*pi,-pi*0.4,pi*0.4] #configuration 2
341q3 = [0.5*pi,0,-0.25*pi,0,0,0] #zero angle configuration
342
343#trajectory generated with optimal acceleration profiles:
344trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
345trajectory.Add(ProfileConstantAcceleration(q3,0.25))
346trajectory.Add(ProfileConstantAcceleration(q1,0.25))
347trajectory.Add(ProfileConstantAcceleration(q2,0.25))
348trajectory.Add(ProfileConstantAcceleration(q0,0.25))
349#traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5]))
350
351# x = traj.EvaluateCoordinate(t,0)
352
353
354#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
355#test robot model
356#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
357#desired angles:
358qE = q0
359qE = [pi*0.5,-pi*0.25,pi*0.75, 0,0,0]
360tStart = [0,0,0, 0,0,0]
361duration = 0.1
362
363
364jointList = [0]*newRobot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
365
366def ComputeMBSstaticRobotTorques(newRobot):
367 q=[]
368 for joint in jointList:
369 q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
370 HT=newRobot.JointHT(q)
371 return newRobot.StaticTorques(HT)
372
373#++++++++++++++++++++++++++++++++++++++++++++++++
374#base, graphics, object and marker:
375
376objectGround = mbs.AddObject(ObjectGround(referencePosition=HT2translation(newRobot.GetBaseHT()),
377 #visualization=VObjectGround(graphicsData=graphicsBaseList)
378 ))
379
380if not useFlexBody:
381 baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
382
383#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
384#build mbs robot model:
385robotDict = newRobot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
386
387jointList = robotDict['jointList'] #must be stored there for the load user function
388
389unitTorques0 = robotDict['unitTorque0List'] #(left body)
390unitTorques1 = robotDict['unitTorque1List'] #(right body)
391
392loadList0 = robotDict['jointTorque0List'] #(left body)
393loadList1 = robotDict['jointTorque1List'] #(right body)
394#print(loadList0, loadList1)
395#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
396#control robot
397compensateStaticTorques = True
398
399torsionalSDlist = robotDict['springDamperList']
400
401#user function which is called only once per step, speeds up simulation drastically
402def PreStepUF(mbs, t):
403 if compensateStaticTorques:
404 staticTorques = ComputeMBSstaticRobotTorques(newRobot)
405 else:
406 staticTorques = np.zeros(len(jointList))
407
408 [u,v,a] = trajectory.Evaluate(t)
409
410 #compute load for joint number
411 for i in range(len(jointList)):
412 joint = jointList[i]
413 phi = mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2] #z-rotation
414 omega = mbs.GetObjectOutput(joint, exu.OutputVariableType.AngularVelocityLocal)[2] #z-angular velocity
415 tsd = torsionalSDlist[i]
416 mbs.SetObjectParameter(tsd, 'offset', u[i])
417 mbs.SetObjectParameter(tsd, 'velocityOffset', v[i])
418 mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity
419
420 return True
421
422mbs.SetPreStepUserFunction(PreStepUF)
423
424
425if useFlexBody:
426 baseType = 'Flexible'
427else:
428 baseType = 'Rigid'
429
430#add sensors:
431cnt = 0
432jointTorque0List = []
433jointRotList = []
434for i in range(len(jointList)):
435 jointLink = jointList[i]
436 tsd = torsionalSDlist[i]
437 #using TSD:
438 sJointRot = mbs.AddSensor(SensorObject(objectNumber=tsd,
439 fileName='solution/joint' + str(i) + 'Rot'+baseType+'.txt',
440 outputVariableType=exu.OutputVariableType.Rotation,
441 writeToFile = sensorWriteToFile))
442 jointRotList += [sJointRot]
443
444 sJointAngVel = mbs.AddSensor(SensorObject(objectNumber=jointLink,
445 fileName='solution/joint' + str(i) + 'AngVel'+baseType+'.txt',
446 outputVariableType=exu.OutputVariableType.AngularVelocityLocal,
447 writeToFile = sensorWriteToFile))
448
449 sTorque = mbs.AddSensor(SensorObject(objectNumber=tsd,
450 fileName='solution/joint' + str(i) + 'Torque'+baseType+'.txt',
451 outputVariableType=exu.OutputVariableType.TorqueLocal,
452 writeToFile = sensorWriteToFile))
453
454 sHandPos = mbs.AddSensor(SensorBody(bodyNumber=robotDict['bodyList'][-1],
455 fileName='solution/handPos'+baseType+'.txt',
456 outputVariableType=exu.OutputVariableType.Position,
457 writeToFile = sensorWriteToFile))
458
459 sHandVel = mbs.AddSensor(SensorBody(bodyNumber=robotDict['bodyList'][-1],
460 fileName='solution/handVel'+baseType+'.txt',
461 outputVariableType=exu.OutputVariableType.Velocity,
462 writeToFile = sensorWriteToFile))
463
464 jointTorque0List += [sTorque]
465
466
467mbs.Assemble()
468#mbs.systemData.Info()
469
470SC.visualizationSettings.connectors.showJointAxes = True
471SC.visualizationSettings.connectors.jointAxesLength = 0.02
472SC.visualizationSettings.connectors.jointAxesRadius = 0.002
473
474SC.visualizationSettings.nodes.show = False
475# SC.visualizationSettings.nodes.showBasis = True
476# SC.visualizationSettings.nodes.basisSize = 0.1
477SC.visualizationSettings.loads.show = False
478
479SC.visualizationSettings.openGL.multiSampling=4
480
481tEnd = 2
482h = 0.002
483
484#mbs.WaitForUserToContinue()
485simulationSettings = exu.SimulationSettings() #takes currently set values or default values
486
487simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
488simulationSettings.timeIntegration.endTime = tEnd
489simulationSettings.solutionSettings.solutionWritePeriod = h*1
490simulationSettings.solutionSettings.sensorsWritePeriod = 0.004
491simulationSettings.solutionSettings.binarySolutionFile = True
492#simulationSettings.solutionSettings.writeSolutionToFile = False
493# simulationSettings.timeIntegration.simulateInRealtime = True
494# simulationSettings.timeIntegration.realtimeFactor = 0.25
495
496simulationSettings.timeIntegration.verboseMode = 1
497# simulationSettings.displayComputationTime = True
498simulationSettings.displayStatistics = True
499simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
500
501#simulationSettings.timeIntegration.newton.useModifiedNewton = True
502simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
503simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
504simulationSettings.timeIntegration.newton.useModifiedNewton = True
505
506simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
507SC.visualizationSettings.general.autoFitScene=False
508SC.visualizationSettings.window.renderWindowSize=[1200,1200]
509SC.visualizationSettings.openGL.shadow = 0.25
510SC.visualizationSettings.openGL.light0position = [-2,5,10,0]
511useGraphics = True
512
513if useGraphics:
514 exu.StartRenderer()
515 if 'renderState' in exu.sys:
516 SC.SetRenderState(exu.sys['renderState'])
517 mbs.WaitForUserToContinue()
518
519mbs.SolveDynamic(simulationSettings, showHints=True)
520
521
522if useGraphics:
523 SC.visualizationSettings.general.autoFitScene = False
524 exu.StopRenderer()
525
526
527mbs.SolutionViewer()
528
529lastRenderState = SC.GetRenderState() #store model view
530
531#compute final torques:
532measuredTorques=[]
533for sensorNumber in jointTorque0List:
534 measuredTorques += [abs(mbs.GetSensorValues(sensorNumber))]
535exu.Print('torques at tEnd=', VSum(measuredTorques))
536
537
538#%%+++++++++++++++++++++
539if True:
540
541 import exudyn.plot
542 exudyn.plot.PlotSensorDefaults().fontSize = 12
543
544 title = baseType + ' base'
545 mbs.PlotSensor(sensorNumbers=jointTorque0List, components=0, title='joint torques, '+title, closeAll=True,
546 fileName='solution/robotJointTorques'+baseType+'.pdf'
547 )
548 mbs.PlotSensor(sensorNumbers=jointRotList, components=0, title='joint angles, '+title,
549 fileName='solution/robotJointAngles'+baseType+'.pdf'
550 )
551
552 fPos = 'flexible base, Pos '
553 fVel = 'flexible base, Vel '
554 rPos = 'rigid base, Pos '
555 rVel = 'rigid base, Vel '
556 if baseType=='Flexible':
557 mbs.PlotSensor(sensorNumbers=[sHandPos]*3+['solution/handPosRigid.txt']*3, components=[0,1,2]*2,
558 labels=[fPos+'X', fPos+'Y', fPos+'Z', rPos+'X', rPos+'Y', rPos+'Z'],
559 fileName='solution/robotPosition'+baseType+'.pdf'
560 )
561 mbs.PlotSensor(sensorNumbers=[sHandVel]*3+['solution/handVelRigid.txt']*3, components=[0,1,2]*2,
562 labels=[fVel+'X', fVel+'Y', fVel+'Z', rVel+'X', rVel+'Y', rVel+'Z'],
563 fileName='solution/robotVelocity'+baseType+'.pdf'
564 )