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