serialRobotInteractiveLimits.py
You can view and download this file on Github: serialRobotInteractiveLimits.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: 2023-03-22
9# Note: This example uses the redundant mbs approach; The kinematic tree approach would be much faster!
10#
11# 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.
12#
13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
15
16import exudyn as exu
17#a local run must not open a plot window when windows are suppressed (#2477)
18if exu.special.userInterface.suppressPlots: import matplotlib; matplotlib.use('Agg')
19from exudyn.itemInterface import *
20from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
21import exudyn.graphics as graphics
22from exudyn.rigidBodyUtilities import *
23from exudyn.robotics import *
24from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
25from exudyn.interactive import InteractiveDialog
26
27import numpy as np
28from numpy import linalg as LA
29from math import pi
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33
34sensorWriteToFile = True
35
36mbs.variables['controlActive'] = 1 #flag to deactivate control
37#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
38
39#now in the new structure
40
41mode='newDH'
42
43graphicsBaseList = [graphics.Brick([0,0,-0.4], [0.12*1,0.12*1,0.6], graphics.color.grey)]
44graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)]
45graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)]
46graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)]
47graphicsBaseList +=[graphics.Cylinder([0,0,-0.1], [0,0,0.1], 0.05, graphics.color.blue)]
48graphicsBaseList +=[graphics.CheckerBoard([0,0,-0.7], [0,0,1], size=2)]
49#newRobot.base.visualization['graphicsData']=graphicsBaseList
50
51ty = 0.03
52tz = 0.04
53zOff = -0.05
54toolSize= [0.05,0.5*ty,0.06]
55graphicsToolList = [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=graphics.color.red)]
56graphicsToolList+= [graphics.Brick([0,ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
57graphicsToolList+= [graphics.Brick([0,-ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
58
59
60#changed to new robot structure July 2021:
61newRobot = Robot(gravity=[0,0,0*9.81],
62 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
63 tool = RobotTool(HT=exu.HT(translation=[0,0,0.1]), visualization=VRobotTool(graphicsData=graphicsToolList)),
64 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
65
66#modDHKK according to Khalil and Kleinfinger, 1986
67link0={'stdDH':[0,0,0,pi/2],
68 'modDHKK':[0,0,0,0],
69 'mass':20, #not needed!
70 'inertia':np.diag([1e-8,0.35,1e-8]), #w.r.t. COM!
71 'COM':[0,0,0]}
72
73link1={'stdDH':[0,0,0.4318,0],
74 'modDHKK':[0.5*pi,0,0,0],
75 'mass':17.4,
76 'inertia':np.diag([0.13,0.524,0.539]), #w.r.t. COM!
77 'COM':[-0.3638, 0.006, 0.2275]}
78
79link2={'stdDH':[0,0.15,0.0203,-pi/2],
80 'modDHKK':[0,0.4318,0,0.15],
81 'mass':4.8,
82 'inertia':np.diag([0.066,0.086,0.0125]), #w.r.t. COM!
83 'COM':[-0.0203,-0.0141,0.07]}
84
85link3={'stdDH':[0,0.4318,0,pi/2],
86 'modDHKK':[-0.5*pi,0.0203,0,0.4318],
87 'mass':0.82,
88 'inertia':np.diag([0.0018,0.0013,0.0018]), #w.r.t. COM!
89 'COM':[0,0.019,0]}
90
91link4={'stdDH':[0,0,0,-pi/2],
92 'modDHKK':[0.5*pi,0,0,0],
93 'mass':0.34,
94 'inertia':np.diag([0.0003,0.0004,0.0003]), #w.r.t. COM!
95 'COM':[0,0,0]}
96
97link5={'stdDH':[0,0,0,0],
98 'modDHKK':[-0.5*pi,0,0,0],
99 'mass':0.09,
100 'inertia':np.diag([0.00015,0.00015,4e-5]), #w.r.t. COM!
101 'COM':[0,0,0.032]}
102linkList=[link0, link1, link2, link3, link4, link5]
103
104for link in linkList:
105 newRobot.AddLink(RobotLink(mass=link['mass'],
106 COM=link['COM'],
107 inertia=link['inertia'],
108 localHT=StdDH2HT(link['stdDH']),
109 ))
110
111cnt = 0
112for link in newRobot.links:
113 color = graphics.colorList[cnt]
114 color[3] = 0.75 #make transparent
115 link.visualization = VRobotLink(jointRadius=0.06, jointWidth=0.08, showMBSjoint=True, linkWidth=0.05,
116 linkColor=color, showCOM= False )
117 cnt+=1
118
119#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
120#configurations and trajectory
121q0 = [0,0,0,0,0,0] #zero angle configuration
122
123# q1 = [0, pi/8, pi*0.75, 0,pi/8,0] #configuration 1
124# q2 = [pi,-pi, -pi*0.5,1.5*pi,-pi*2,pi*2] #configuration 2
125# q3 = [3*pi,0,-0.25*pi,0,0,0] #zero angle configuration
126
127#trajectory generated with optimal acceleration profiles:
128trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
129# trajectory.Add(ProfileConstantAcceleration(q3,0.25))
130# trajectory.Add(ProfileConstantAcceleration(q1,0.25))
131# trajectory.Add(ProfileConstantAcceleration(q2,0.25))
132trajectory.Add(ProfileConstantAcceleration(q0,0.25))
133#traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5]))
134
135# x = traj.EvaluateCoordinate(t,0)
136
137
138#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
139#test robot model
140#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
141#control parameters, per joint:
142fc=1
143Pcontrol = 0.1*np.array([40000, 40000, 40000, 10*100, 10*100, 10*10])
144Dcontrol = np.array([400, 400, 100, 10*1, 10*1, 10*0.1])
145Pcontrol = fc*Pcontrol
146Dcontrol = fc*Dcontrol
147#soft:
148#Pcontrol = [4000, 4000, 4000, 100, 100, 10]
149#Dcontrol = [40, 40, 10, 1, 1, 0.1]
150
151#desired angles:
152qE = q0
153qE = q0
154tStart = [0,0,0, 0,0,0]
155duration = 0.1
156
157
158jointList = [0]*newRobot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
159
160def ComputeMBSstaticRobotTorques(newRobot):
161 q=[]
162 for joint in jointList:
163 q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
164 HT=newRobot.JointHT(q)
165 return newRobot.StaticTorques(HT)
166
167#++++++++++++++++++++++++++++++++++++++++++++++++
168#base, graphics, object and marker:
169
170objectGround = mbs.AddObject(ObjectGround(referencePosition=newRobot.GetBaseHT().translation,
171 #visualization=VObjectGround(graphicsData=graphicsBaseList)
172 ))
173
174
175#baseMarker; could also be a moving base!
176baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
177
178
179
180#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
181#build mbs robot model:
182robotDict = newRobot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
183
184jointList = robotDict['jointList'] #must be stored there for the load user function
185
186unitTorques0 = robotDict['unitTorque0List'] #(left body)
187unitTorques1 = robotDict['unitTorque1List'] #(right body)
188
189loadList0 = robotDict['jointTorque0List'] #(left body)
190loadList1 = robotDict['jointTorque1List'] #(right body)
191#print(loadList0, loadList1)
192
193
194#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
195#add CartesianSpringDamper for mouse drag
196gripperBody=robotDict['bodyList'][-1]
197gripperLength = 0.1 #in z-direction
198markerGripper = mbs.AddMarker(MarkerBodyPosition(bodyNumber=gripperBody, localPosition=[0,0,gripperLength]))
199markerGround0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=objectGround))
200
201# def UFcartesianSD(mbs, t, itemNumber, displacement, velocity, stiffness, damping, offset):
202# if mbs.variables['controlActive']:
203# return [0,0,0]
204# else:
205# p = SC.renderer.GetMouseCoordinates(True) #True=OpenGL coordinates; 2D
206# A = np.array(SC.renderer.GetState()['modelRotation'])
207# # print('p=',p)
208# # print('u=',displacement)
209# p3D = A@np.array([p[0],p[1],0.])
210
211# dp = displacement-p3D
212# f = [stiffness[0]*dp[0], stiffness[1]*dp[1], stiffness[2]*dp[2]]
213# return f
214
215
216kSD = 50000*0.1
217dSD = kSD*0.01 #damping included in robot
218gripperSD = mbs.AddObject(CartesianSpringDamper(markerNumbers=[markerGround0, markerGripper],
219 stiffness=[kSD,kSD,kSD],
220 damping=[dSD,dSD,dSD],
221 #springForceUserFunction=UFcartesianSD,
222 visualization=VCartesianSpringDamper(show=False), #do not show, looks weird
223 ))
224mbs.variables['gripperSD'] = gripperSD
225
226#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
227#test for joint limits:
228limits = []
229def UFtsd(mbs, t, itemNumber, rotation, angularVelocity, stiffness, damping, offset):
230 f = 0.
231 if False and mbs.variables['controlActive']:
232 f = stiffness*rotation + damping*angularVelocity
233 else:
234 limTSD = limits[itemNumber]
235 if rotation > limTSD[1]:
236 f = 50*stiffness*(rotation-limTSD[1])**2 + stiffness*(rotation-offset) + damping*angularVelocity
237 elif rotation < limTSD[0]:
238 f = -50*stiffness*(rotation-limTSD[0])**2 + stiffness*(rotation-offset) + damping*angularVelocity
239 else:
240 f = stiffness*(rotation-offset) + damping*angularVelocity
241 return f
242
243useUserFunction = 1
244if not useUserFunction:
245 UFtsd = 0
246
247#control robot
248compensateStaticTorques = False
249torsionalSDlist = []
250nGenericList = []
251limits = [[0.,0.]]*mbs.systemData.NumberOfObjects()
252limits += [[-0.75*pi,0.75*pi],
253 [ 0.0*pi,1.0*pi],
254 [-1.0*pi,0.4*pi],
255 [-0.5*pi,0.5*pi],
256 [-0.5*pi,0.5*pi],
257 [-0.5*pi,0.5*pi],
258 ]
259for i in range(len(jointList)):
260 joint = jointList[i]
261 markers = mbs.GetObject(joint)['markerNumbers'] #their frames are the joint's
262
263 nGeneric=mbs.AddNode(NodeGenericData(initialCoordinates=[0],
264 numberOfDataCoordinates=1)) #for infinite rotations
265 nGenericList += [nGeneric]
266 tsd = mbs.AddObject(TorsionalSpringDamper(markerNumbers=markers,
267 nodeNumber=nGeneric,
268 stiffness=Pcontrol[i],
269 damping=Dcontrol[i],
270 springTorqueUserFunction=UFtsd,
271 visualization=VTorsionalSpringDamper(drawSize=0.1)
272 ))
273 torsionalSDlist += [tsd]
274
275
276#user function which is called only once per step, speeds up simulation drastically
277def PreStepUF(mbs, t):
278
279 # print('nG=',end='')
280 # for i in nGenericList:
281 # q = mbs.GetNodeOutput(i, exu.OutputVariableType.Coordinates)
282 # print(round(q,4),', ',end='')
283 # print('')
284
285 #additional static torque compensation; P and D control in TSD:
286 if not mbs.variables['controlActive']:
287 p = SC.renderer.GetMouseCoordinates(True) #True=OpenGL coordinates; 2D
288 A = np.array(SC.renderer.GetState()['modelRotation'])
289 p3D = A@np.array([p[0],p[1],0.])
290
291 offset = p3D
292 mbs.SetObjectParameter(mbs.variables['gripperSD'], 'offset', offset)
293 mbs.SetObjectParameter(mbs.variables['gripperSD'], 'activeConnector', True)
294 else:
295 mbs.SetObjectParameter(mbs.variables['gripperSD'], 'activeConnector', False)
296
297 #with control:
298 if compensateStaticTorques:
299 staticTorques = ComputeMBSstaticRobotTorques(newRobot)
300 #print("tau=", staticTorques)
301 else:
302 staticTorques = np.zeros(len(jointList))
303
304
305 [u,v,a] = trajectory.Evaluate(t)
306
307 fact = 1.
308 if mbs.variables['controlActive']==2:
309 fact =1. #0.1 #soft reset ...
310
311 #compute load for joint number
312 for i in range(len(jointList)):
313 joint = jointList[i]
314 phi = mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2] #z-rotation
315 omega = mbs.GetObjectOutput(joint, exu.OutputVariableType.AngularVelocityLocal)[2] #z-angular velocity
316 #[u1,v1,a1] = MotionInterpolator(t, robotTrajectory, i)
317 tsd = torsionalSDlist[i]
318 if mbs.variables['controlActive'] or i>=3:
319 mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity; positive sign from Exudyn 1.2.38 onwards
320 mbs.SetObjectParameter(tsd, 'stiffness', Pcontrol[i]*fact)
321 mbs.SetObjectParameter(tsd, 'damping', Dcontrol[i]*1)
322
323 # with mouse drag:
324 for i in range(len(jointList)):
325 if not (mbs.variables['controlActive'] or i>=3):
326 tsd = torsionalSDlist[i]
327 #keep damping, but deactivate stiffness
328 mbs.SetObjectParameter(tsd, 'torque', 0)
329 #mbs.SetObjectParameter(tsd, 'stiffness', 0)
330 mbs.SetObjectParameter(tsd, 'damping', Dcontrol[i]*0.1) #keep small damping to improve drag!
331
332 return True
333
334mbs.SetPreStepUserFunction(PreStepUF)
335
336
337# mbs.variables['q0Current'] = q0[0]
338for i in range(6):
339 mbs.variables['q{}Current'.format(i)] = q0[i]
340
341#add sensors:
342cnt = 0
343for i in range(len(jointList)):
344 jointLink = jointList[i]
345 tsd = torsionalSDlist[i]
346 sJointRot = mbs.AddSensor(SensorObject(objectNumber=tsd,
347 fileName="solution/joint" + str(cnt) + "Rot.txt",
348 outputVariableType=exu.OutputVariableType.Rotation,
349 writeToFile = sensorWriteToFile))
350 sJointAngVel = mbs.AddSensor(SensorObject(objectNumber=jointLink,
351 fileName="solution/joint" + str(cnt) + "AngVel.txt",
352 outputVariableType=exu.OutputVariableType.AngularVelocityLocal,
353 writeToFile = sensorWriteToFile))
354 cnt+=1
355
356cnt = 0
357jointTorque0List = []
358for load0 in robotDict['jointTorque0List']:
359 sTorque = mbs.AddSensor(SensorLoad(loadNumber=load0, fileName="solution/jointTorque" + str(cnt) + ".txt",
360 writeToFile = sensorWriteToFile))
361 jointTorque0List += [sTorque]
362 cnt+=1
363
364
365
366
367def GetPoseString(q):
368 strx = ' x = ['
369 strphi = 'phi = ['
370 HT = newRobot.JointHT(q)[-1]
371 t = np.round(HT[0:3,-1], 4)
372 R = HT[0:3,0:3]
373 phi = np.round(RotationMatrix2RotXYZ(R),3)
374
375 for i in range(2):
376 strx += '{},\t'.format(t[i])
377 strphi += '{},\t'.format(phi[i])
378
379 strx += '{}]'.format(t[-1])
380 strphi += '{}]'.format(phi[-1])
381 diffLen = len(strx) - len(strphi)
382 if diffLen > 0:
383 strphi += ' '*diffLen
384 elif diffLen < 0:
385 strx += ' '*diffLen
386
387 return strx + '\n' + strphi
388
389#define items for dialog
390fAngle=2.
391dialogItems = [{'type':'label', 'text':'Angle 1', 'grid':(0,0,2), 'options':['L']},
392 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[0], 'steps':628, 'variable':'q0Current', 'grid':(0,1)},
393 {'type':'label', 'text':'Angle 2:', 'grid':(1,0)},
394 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[1], 'steps':628, 'variable':'q1Current', 'grid':(1,1)},
395 {'type':'label', 'text':'Angle 3:', 'grid':(2,0)},
396 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[2], 'steps':628, 'variable':'q2Current', 'grid':(2,1)},
397 {'type':'label', 'text':'Angle 4:', 'grid':(3,0)},
398 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[3], 'steps':628, 'variable':'q3Current', 'grid':(3,1)},
399 {'type':'label', 'text':'Angle 5:', 'grid':(4,0)},
400 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[4], 'steps':628, 'variable':'q4Current', 'grid':(4,1)},
401 {'type':'label', 'text':'Angle 6:', 'grid':(5,0)},
402 {'type':'slider', 'range': (-fAngle*3.14, fAngle*3.14), 'value':q0[5], 'steps':628, 'variable':'q5Current', 'grid':(5,1)},
403 {'type': 'label', 'text': 'Position:', 'grid': (6,0)},
404 {'type': 'label', 'text': '{}'.format(GetPoseString(q0)), 'grid': (6,1)},
405 {'type':'radio', 'textValueList':[('Mouse drag',0),('Control on',1),('Reset',2)], 'value':1, 'variable':'controlActive',
406 'grid': [(7,0),(7,1),(7,2)]}
407 #{'type':'button', 'text':'test button','callFunction':ButtonCall, 'grid':(7,0,2)},
408 ]
409
410
411mbs.Assemble()
412#mbs.systemData.Info()
413
414SC.visualizationSettings.connectors.showJointAxes = True
415SC.visualizationSettings.connectors.jointAxesLength = 0.02
416SC.visualizationSettings.connectors.jointAxesRadius = 0.002
417
418SC.visualizationSettings.nodes.showBasis = True
419SC.visualizationSettings.nodes.basisSize = 0.1
420SC.visualizationSettings.loads.show = False
421
422SC.visualizationSettings.openGL.multiSampling=4
423SC.visualizationSettings.openGL.light0.shadow=0.3
424SC.visualizationSettings.view0.camera.perspective=0.7
425
426tEnd = 1.25
427h = 0.0005
428
429#SC.renderer.DoIdleTasks()
430simulationSettings = exu.SimulationSettings() #takes currently set values or default values
431
432simulationSettings.solution.file.information = 'Hanging Robot Interactive Example'
433
434
435simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
436simulationSettings.timeIntegration.endTime = tEnd
437simulationSettings.solution.file.writePeriod = h*1
438simulationSettings.solution.sensors.writePeriod = h*10
439simulationSettings.solution.file.binary = True
440simulationSettings.timeIntegration.verboseMode = 0
441#simulationSettings.solution.file.write = False
442# simulationSettings.timeIntegration.realtime.active = True
443# simulationSettings.timeIntegration.realtime.factor = 0.25
444simulationSettings.solution.file.writeInitialValues = False
445
446simulationSettings.show.computationTime = False
447simulationSettings.show.statistics = False
448# simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenSparse
449# simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=False
450
451#simulationSettings.timeIntegration.newton.useModifiedNewton = True
452simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
453simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
454simulationSettings.timeIntegration.newton.useModifiedNewton = True
455
456SC.visualizationSettings.view0.window.renderWindowSize=[1920,1200]
457SC.visualizationSettings.general.graphicsUpdateInterval=0.005
458
459#this is an exemplariy simulation function, which adjusts some values for simulation
460def SimulationUF(mbs, dialog):
461 q = []
462 for i in range(6):
463 qi = mbs.variables['q{}Current'.format(i)] #not possible to update this variable
464 #qi = dialog.sliderVariables[i].get()
465 mbs.SetObjectParameter(torsionalSDlist[i],'offset',qi)
466 theta = mbs.GetObjectOutput(torsionalSDlist[i],exu.OutputVariableType.Rotation)
467 #q += [mbs.variables['q{}Current'.format(i)]]
468 q += [1.*theta] #current rotation
469 #dialog.dialogItems[-1]['text'] = GetPoseString(q)
470
471 if mbs.variables['controlActive'] == 2:
472 for i in range(6):
473 q[i] = 0
474 mbs.variables['q{}Current'.format(i)] = 0
475 dialog.widgets[2*i+1].set(q[i])
476
477 dialog.labelStringVariables[-1].set(GetPoseString(q))
478
479 if not mbs.variables['controlActive']:
480 for i in range(6):
481 #dialog.sliderVariables[i].set(q[i])
482 dialog.widgets[2*i+1].set(q[i])
483 #print(q)
484
485SC.visualizationSettings.general.autoFitScene = False #use loaded render state
486SC.renderer.Start()
487SC.renderer.RestoreSavedState()
488
489InteractiveDialog(mbs=mbs, simulationSettings=simulationSettings,
490 simulationFunction=SimulationUF,
491 title='Interactive window',
492 dialogItems=dialogItems, period=0.01, realtimeFactor=4, #realtime is only approx. (does not include time lost for computation ==> 2 is a good choice)
493 runOnStart=True,
494 addLabelStringVariables=True,
495 #addSliderVariables=True
496 )
497
498SC.renderer.Stop()
499
500if 0:
501 if useGraphics:
502 SC.renderer.Start()
503 SC.renderer.RestoreSavedState()
504 SC.renderer.DoIdleTasks()
505
506 mbs.SolveDynamic(simulationSettings, showHints=True)
507
508
509 if useGraphics:
510 SC.visualizationSettings.general.autoFitScene = False
511 SC.renderer.Stop()
512
513
514 mbs.SolutionViewer()
515
516 lastRenderState = SC.renderer.GetState() #store model view
517
518 #compute final torques:
519 measuredTorques=[]
520 for sensorNumber in jointTorque0List:
521 measuredTorques += [mbs.GetSensorValues(sensorNumber)[2]]
522 exu.Print("torques at tEnd=", np.sum(measuredTorques))
523
524
525
526 if True:
527 import matplotlib.pyplot as plt
528 import matplotlib.ticker as ticker
529 plt.rcParams.update({'font.size': 14})
530 plt.close("all")
531
532 doJointTorques = False
533 if doJointTorques:
534 for i in range(6):
535 data = np.loadtxt(OutputFilePath("solution/jointTorque" + str(i) + ".txt"), comments='#', delimiter=',')
536 plt.plot(data[:,0], data[:,3], PlotLineCode(i), label="joint torque"+str(i)) #z-rotation
537
538 plt.xlabel("time (s)")
539 plt.ylabel("joint torque (Nm)")
540 ax=plt.gca() # get current axes
541 ax.grid(True, 'major', 'both')
542 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
543 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
544 plt.tight_layout()
545 ax.legend(loc='center right')
546 plt.show()
547 # plt.savefig("solution/robotJointTorques.pdf")
548
549 doJointAngles = True
550 if doJointAngles:
551 plt.close("all")
552
553 for i in range(6):
554 data = np.loadtxt(OutputFilePath("solution/joint" + str(i) + "Rot.txt"), comments='#', delimiter=',')
555 plt.plot(data[:,0], data[:,1], PlotLineCode(i), label="joint"+str(i)) #z-rotation
556
557 plt.xlabel("time (s)")
558 plt.ylabel("joint angle (rad)")
559 ax=plt.gca()
560 ax.grid(True, 'major', 'both')
561 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
562 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
563 plt.tight_layout()
564 ax.legend()
565 plt.rcParams.update({'font.size': 16})
566 plt.show()
567 # plt.savefig("solution/robotJointAngles.pdf")