serialRobotTSD.py
You can view and download this file on Github: serialRobotTSD.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
22
23import numpy as np
24from numpy import linalg as LA
25from math import pi
26
27SC = exu.SystemContainer()
28mbs = SC.AddSystem()
29
30sensorWriteToFile = True
31
32#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
34mode='newDH'
35
36jointWidth=0.1
37jointRadius=0.06
38linkWidth=0.1
39
40graphicsBaseList = [GraphicsDataOrthoCubePoint([0,0,-0.15], [0.12,0.12,0.1], color4grey)]
41graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0.5,0,0], 0.0025, color4red)]
42graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0.5,0], 0.0025, color4green)]
43graphicsBaseList +=[GraphicsDataCylinder([0,0,0], [0,0,0.5], 0.0025, color4blue)]
44graphicsBaseList +=[GraphicsDataCylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, color4list[0])] #belongs to first body
45
46ty = 0.03
47tz = 0.04
48zOff = -0.05
49toolSize= [0.05,0.5*ty,0.06]
50graphicsToolList = [GraphicsDataCylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=color4red)]
51graphicsToolList+= [GraphicsDataOrthoCubePoint([0,ty,1.5*tz+zOff], toolSize, color4grey)]
52graphicsToolList+= [GraphicsDataOrthoCubePoint([0,-ty,1.5*tz+zOff], toolSize, color4grey)]
53
54
55#changed to new robot structure July 2021:
56newRobot = Robot(gravity=[0,0,9.81],
57 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
58 tool = RobotTool(HT=HTtranslate([0,0,0.1]), visualization=VRobotTool(graphicsData=graphicsToolList)),
59 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
60
61#modDHKK according to Khalil and Kleinfinger, 1986
62link0={'stdDH':[0,0,0,pi/2],
63 'modDHKK':[0,0,0,0],
64 'mass':20, #not needed!
65 'inertia':np.diag([1e-8,0.35,1e-8]), #w.r.t. COM! in stdDH link frame
66 'COM':[0,0,0]} #in stdDH link frame
67
68link1={'stdDH':[0,0,0.4318,0],
69 'modDHKK':[0.5*pi,0,0,0],
70 'mass':17.4,
71 'inertia':np.diag([0.13,0.524,0.539]), #w.r.t. COM! in stdDH link frame
72 'COM':[-0.3638, 0.006, 0.2275]} #in stdDH link frame
73
74link2={'stdDH':[0,0.15,0.0203,-pi/2],
75 'modDHKK':[0,0.4318,0,0.15],
76 'mass':4.8,
77 'inertia':np.diag([0.066,0.086,0.0125]), #w.r.t. COM! in stdDH link frame
78 'COM':[-0.0203,-0.0141,0.07]} #in stdDH link frame
79
80link3={'stdDH':[0,0.4318,0,pi/2],
81 'modDHKK':[-0.5*pi,0.0203,0,0.4318],
82 'mass':0.82,
83 'inertia':np.diag([0.0018,0.0013,0.0018]), #w.r.t. COM! in stdDH link frame
84 'COM':[0,0.019,0]} #in stdDH link frame
85
86link4={'stdDH':[0,0,0,-pi/2],
87 'modDHKK':[0.5*pi,0,0,0],
88 'mass':0.34,
89 'inertia':np.diag([0.0003,0.0004,0.0003]), #w.r.t. COM! in stdDH link frame
90 'COM':[0,0,0]} #in stdDH link frame
91
92link5={'stdDH':[0,0,0,0],
93 'modDHKK':[-0.5*pi,0,0,0],
94 'mass':0.09,
95 'inertia':np.diag([0.00015,0.00015,4e-5]), #w.r.t. COM! in stdDH link frame
96 'COM':[0,0,0.032]} #in stdDH link frame
97linkList=[link0, link1, link2, link3, link4, link5]
98
99for link in linkList:
100 newRobot.AddLink(RobotLink(mass=link['mass'],
101 COM=link['COM'],
102 inertia=link['inertia'],
103 localHT=StdDH2HT(link['stdDH']),
104 ))
105
106cnt = 0
107for link in newRobot.links:
108 color = color4list[cnt]
109 color[3] = 0.75 #make transparent
110 link.visualization = VRobotLink(jointRadius=jointRadius, jointWidth=jointWidth, showMBSjoint=False,
111 linkWidth=linkWidth, linkColor=color, showCOM= True )
112 cnt+=1
113
114#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
115#configurations and trajectory
116q0 = [0,0,0,0,0,0] #zero angle configuration
117
118#this set of coordinates only works with TSD, not with old fashion load control:
119# q1 = [0, pi/8, pi*0.75, 0,pi/8,0] #configuration 1
120# q2 = [pi,-pi, -pi*0.5,1.5*pi,-pi*2,pi*2] #configuration 2
121# q3 = [3*pi,0,-0.25*pi,0,0,0] #zero angle configuration
122
123#this set also works with load control:
124q1 = [0, pi/8, pi*0.5, 0,pi/8,0] #configuration 1
125q2 = [0.8*pi,-0.8*pi, -pi*0.5,0.75*pi,-pi*0.4,pi*0.4] #configuration 2
126q3 = [0.5*pi,0,-0.25*pi,0,0,0] #zero angle configuration
127
128#trajectory generated with optimal acceleration profiles:
129trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
130trajectory.Add(ProfileConstantAcceleration(q3,0.25))
131trajectory.Add(ProfileConstantAcceleration(q1,0.25))
132trajectory.Add(ProfileConstantAcceleration(q2,0.25))
133trajectory.Add(ProfileConstantAcceleration(q0,0.25))
134#traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5]))
135
136# x = traj.EvaluateCoordinate(t,0)
137
138
139#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
140#test robot model
141#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
142#control parameters, per joint:
143fc=1
144Pcontrol = np.array([40000, 40000, 40000, 100, 100, 10])
145Dcontrol = np.array([400, 400, 100, 1, 1, 0.1])
146Pcontrol = fc*Pcontrol
147Dcontrol = fc*Dcontrol
148#soft:
149# Pcontrol = [4000, 4000, 4000, 100, 100, 10]
150# Dcontrol = [40, 40, 10, 1, 1, 0.1]
151
152#desired angles:
153qE = q0
154qE = [pi*0.5,-pi*0.25,pi*0.75, 0,0,0]
155tStart = [0,0,0, 0,0,0]
156duration = 0.1
157
158
159jointList = [0]*newRobot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
160
161def ComputeMBSstaticRobotTorques(newRobot):
162 q=[]
163 for joint in jointList:
164 q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
165 HT=newRobot.JointHT(q)
166 return newRobot.StaticTorques(HT)
167
168#++++++++++++++++++++++++++++++++++++++++++++++++
169#base, graphics, object and marker:
170
171objectGround = mbs.AddObject(ObjectGround(referencePosition=HT2translation(newRobot.GetBaseHT()),
172 #visualization=VObjectGround(graphicsData=graphicsBaseList)
173 ))
174
175
176#baseMarker; could also be a moving base!
177baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
178
179
180
181#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
182#build mbs robot model:
183robotDict = newRobot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
184
185jointList = robotDict['jointList'] #must be stored there for the load user function
186
187unitTorques0 = robotDict['unitTorque0List'] #(left body)
188unitTorques1 = robotDict['unitTorque1List'] #(right body)
189
190loadList0 = robotDict['jointTorque0List'] #(left body)
191loadList1 = robotDict['jointTorque1List'] #(right body)
192#print(loadList0, loadList1)
193#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
194#control robot
195compensateStaticTorques = True
196torsionalSDlist = []
197
198for i in range(len(jointList)):
199 joint = jointList[i]
200 rot0 = mbs.GetObject(joint)['rotationMarker0']
201 rot1 = mbs.GetObject(joint)['rotationMarker1']
202 markers = mbs.GetObject(joint)['markerNumbers']
203 nGeneric=mbs.AddNode(NodeGenericData(initialCoordinates=[0],
204 numberOfDataCoordinates=1)) #for infinite rotations
205 tsd = mbs.AddObject(TorsionalSpringDamper(markerNumbers=markers,
206 nodeNumber=nGeneric,
207 rotationMarker0=rot0,
208 rotationMarker1=rot1,
209 stiffness=Pcontrol[i],
210 damping=Dcontrol[i],
211 visualization=VTorsionalSpringDamper(drawSize=0.1)
212 ))
213 torsionalSDlist += [tsd]
214
215
216#user function which is called only once per step, speeds up simulation drastically
217def PreStepUF(mbs, t):
218 if compensateStaticTorques:
219 staticTorques = ComputeMBSstaticRobotTorques(newRobot)
220 #print("tau=", staticTorques)
221 else:
222 staticTorques = np.zeros(len(jointList))
223
224 [u,v,a] = trajectory.Evaluate(t)
225
226 #compute load for joint number
227 for i in range(len(jointList)):
228 joint = jointList[i]
229 phi = mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2] #z-rotation
230 omega = mbs.GetObjectOutput(joint, exu.OutputVariableType.AngularVelocityLocal)[2] #z-angular velocity
231 #[u1,v1,a1] = MotionInterpolator(t, robotTrajectory, i)
232 u1 = u[i]
233 v1 = v[i]
234 tsd = torsionalSDlist[i]
235 mbs.SetObjectParameter(tsd, 'offset', u1)
236 mbs.SetObjectParameter(tsd, 'velocityOffset', v1)
237 mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity
238
239 return True
240
241mbs.SetPreStepUserFunction(PreStepUF)
242
243
244
245#add sensors:
246cnt = 0
247jointTorque0List = []
248for i in range(len(jointList)):
249 jointLink = jointList[i]
250 tsd = torsionalSDlist[i]
251 #using TSD:
252 sJointRot = mbs.AddSensor(SensorObject(objectNumber=tsd,
253 fileName="solution/joint" + str(i) + "Rot.txt",
254 outputVariableType=exu.OutputVariableType.Rotation,
255 writeToFile = sensorWriteToFile))
256
257 sJointAngVel = mbs.AddSensor(SensorObject(objectNumber=jointLink,
258 fileName="solution/joint" + str(i) + "AngVel.txt",
259 outputVariableType=exu.OutputVariableType.AngularVelocityLocal,
260 writeToFile = sensorWriteToFile))
261
262 sTorque = mbs.AddSensor(SensorObject(objectNumber=tsd,
263 fileName="solution/joint" + str(i) + "Torque.txt",
264 outputVariableType=exu.OutputVariableType.TorqueLocal,
265 writeToFile = sensorWriteToFile))
266
267 jointTorque0List += [sTorque]
268
269
270mbs.Assemble()
271#mbs.systemData.Info()
272
273SC.visualizationSettings.connectors.showJointAxes = True
274SC.visualizationSettings.connectors.jointAxesLength = 0.02
275SC.visualizationSettings.connectors.jointAxesRadius = 0.002
276
277SC.visualizationSettings.nodes.showBasis = True
278SC.visualizationSettings.nodes.basisSize = 0.1
279SC.visualizationSettings.loads.show = False
280
281SC.visualizationSettings.openGL.multiSampling=4
282
283tEnd = 1.25
284h = 0.002
285
286#mbs.WaitForUserToContinue()
287simulationSettings = exu.SimulationSettings() #takes currently set values or default values
288
289simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
290simulationSettings.timeIntegration.endTime = tEnd
291simulationSettings.solutionSettings.solutionWritePeriod = h*1
292simulationSettings.solutionSettings.sensorsWritePeriod = h*10
293simulationSettings.solutionSettings.binarySolutionFile = True
294#simulationSettings.solutionSettings.writeSolutionToFile = False
295# simulationSettings.timeIntegration.simulateInRealtime = True
296# simulationSettings.timeIntegration.realtimeFactor = 0.25
297
298simulationSettings.timeIntegration.verboseMode = 1
299# simulationSettings.displayComputationTime = True
300simulationSettings.displayStatistics = True
301simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
302
303#simulationSettings.timeIntegration.newton.useModifiedNewton = True
304simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
305simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
306simulationSettings.timeIntegration.newton.useModifiedNewton = True
307
308simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
309SC.visualizationSettings.general.autoFitScene=False
310SC.visualizationSettings.window.renderWindowSize=[1920,1200]
311useGraphics = False
312
313if useGraphics:
314 exu.StartRenderer()
315 if 'renderState' in exu.sys:
316 SC.SetRenderState(exu.sys['renderState'])
317 mbs.WaitForUserToContinue()
318
319mbs.SolveDynamic(simulationSettings, showHints=True)
320
321
322if useGraphics:
323 SC.visualizationSettings.general.autoFitScene = False
324 exu.StopRenderer()
325
326
327mbs.SolutionViewer()
328
329lastRenderState = SC.GetRenderState() #store model view
330
331#compute final torques:
332measuredTorques=[]
333for sensorNumber in jointTorque0List:
334 measuredTorques += [abs(mbs.GetSensorValues(sensorNumber))]
335exu.Print("torques at tEnd=", VSum(measuredTorques))
336
337
338if True:
339 import matplotlib.pyplot as plt
340 import matplotlib.ticker as ticker
341 plt.rcParams.update({'font.size': 14})
342 plt.close("all")
343
344 doJointTorques = False
345 if doJointTorques:
346 for i in range(6):
347 data = np.loadtxt("solution/jointTorque" + str(i) + ".txt", comments='#', delimiter=',')
348 plt.plot(data[:,0], data[:,3], PlotLineCode(i), label="joint torque"+str(i)) #z-rotation
349
350 plt.xlabel("time (s)")
351 plt.ylabel("joint torque (Nm)")
352 ax=plt.gca() # get current axes
353 ax.grid(True, 'major', 'both')
354 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
355 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
356 plt.tight_layout()
357 ax.legend(loc='center right')
358 plt.show()
359 # plt.savefig("solution/robotJointTorques.pdf")
360
361 doJointAngles = True
362 if doJointAngles:
363 plt.close("all")
364
365 for i in range(6):
366 data = np.loadtxt("solution/joint" + str(i) + "Rot.txt", comments='#', delimiter=',')
367 # data = np.loadtxt("solution/joint" + str(i) + "AngVel.txt", comments='#', delimiter=',')
368 plt.plot(data[:,0], data[:,1], PlotLineCode(i), label="joint"+str(i)) #z-rotation
369
370 plt.xlabel("time (s)")
371 plt.ylabel("joint angle (rad)")
372 ax=plt.gca()
373 ax.grid(True, 'major', 'both')
374 ax.xaxis.set_major_locator(ticker.MaxNLocator(10))
375 ax.yaxis.set_major_locator(ticker.MaxNLocator(10))
376 plt.tight_layout()
377 ax.legend()
378 plt.rcParams.update({'font.size': 16})
379 plt.show()
380 # plt.savefig("solution/robotJointAngles.pdf")