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