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