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