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                   )