serialRobotKinematicTree.py
You can view and download this file on Github: serialRobotKinematicTree.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Example of a serial robot with minimal and redundant coordinates
5#
6# Author: Johannes Gerstmayr
7# Date: 2022-06-26
8#
9# 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.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13
14import exudyn as exu
15from exudyn.itemInterface import *
16from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
17import exudyn.graphics as graphics #only import if it does not conflict
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 = False
31
32#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33compensateStaticTorques = True #static torque compensation converges slowly!
34useKinematicTree = True
35#kinematic tree and redundant mbs agrees for stdDH version up to 1e-10, with compensateStaticTorques = False
36# KT: rotations at tEnd= 1.8464514676503092 , [0.4921990591981066, 0.2718999073958087, 0.818158053005264, -0.0030588904101585936, 0.26831938569719394, -0.0010660472359057434]
37# red. MBS:rotations at tEnd= 1.8464514674961 , [ 0.49219906 0.27189991 0.81815805 -0.00305889 0.26831939 -0.00106605]
38
39mode='newDH'
40# mode='newModDH' #modified DH parameters #position agrees for all digits: 1.846440411790352
41
42jointWidth=0.1
43jointRadius=0.06
44linkWidth=0.1
45
46graphicsBaseList = [graphics.Brick([0,0,-0.15], [0.12,0.12,0.1], graphics.color.grey)]
47graphicsBaseList +=[graphics.Cylinder([0,0,0], [0.5,0,0], 0.0025, graphics.color.red)]
48graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0025, graphics.color.green)]
49graphicsBaseList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0025, graphics.color.blue)]
50graphicsBaseList +=[graphics.Cylinder([0,0,-jointWidth], [0,0,jointWidth], linkWidth*0.5, graphics.colorList[0])] #belongs to first body
51
52ty = 0.03
53tz = 0.04
54zOff = -0.05
55toolSize= [0.05,0.5*ty,0.06]
56graphicsToolList = [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,tz], radius=ty*1.5, color=graphics.color.red)]
57graphicsToolList+= [graphics.Brick([0,ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
58graphicsToolList+= [graphics.Brick([0,-ty,1.5*tz+zOff], toolSize, graphics.color.grey)]
59
60#control parameters, per joint:
61fc=1
62Pcontrol = np.array([40000, 40000, 40000, 100, 100, 10])
63Dcontrol = np.array([400, 400, 100, 1, 1, 0.1])
64Pcontrol = fc*Pcontrol
65Dcontrol = fc*Dcontrol
66
67
68#changed to new robot structure July 2021:
69robot = Robot(gravity=[0,0,9.81],
70 base = RobotBase(visualization=VRobotBase(graphicsData=graphicsBaseList)),
71 tool = RobotTool(HT=HTtranslate([0,0,0.1]), visualization=VRobotTool(graphicsData=graphicsToolList)),
72 referenceConfiguration = []) #referenceConfiguration created with 0s automatically
73
74#modDHKK according to Khalil and Kleinfinger, 1986
75link0={'stdDH':[0,0,0,pi/2],
76 'modDHKK':[0,0,0,0],
77 'mass':20, #not needed!
78 'inertia':np.diag([1e-8,0.35,1e-8]), #w.r.t. COM! in stdDH link frame
79 'COM':[0,0,0]} #in stdDH link frame
80
81link1={'stdDH':[0,0,0.4318,0],
82 'modDHKK':[0.5*pi,0,0,0],
83 'mass':17.4,
84 'inertia':np.diag([0.13,0.524,0.539]), #w.r.t. COM! in stdDH link frame
85 'COM':[-0.3638, 0.006, 0.2275]} #in stdDH link frame
86
87link2={'stdDH':[0,0.15,0.0203,-pi/2],
88 'modDHKK':[0,0.4318,0,0.15],
89 'mass':4.8,
90 'inertia':np.diag([0.066,0.086,0.0125]), #w.r.t. COM! in stdDH link frame
91 'COM':[-0.0203,-0.0141,0.07]} #in stdDH link frame
92
93link3={'stdDH':[0,0.4318,0,pi/2],
94 'modDHKK':[-0.5*pi,0.0203,0,0.4318],
95 'mass':0.82,
96 'inertia':np.diag([0.0018,0.0013,0.0018]), #w.r.t. COM! in stdDH link frame
97 'COM':[0,0.019,0]} #in stdDH link frame
98
99link4={'stdDH':[0,0,0,-pi/2],
100 'modDHKK':[0.5*pi,0,0,0],
101 'mass':0.34,
102 'inertia':np.diag([0.0003,0.0004,0.0003]), #w.r.t. COM! in stdDH link frame
103 'COM':[0,0,0]} #in stdDH link frame
104
105link5={'stdDH':[0,0,0,0],
106 'modDHKK':[-0.5*pi,0,0,0],
107 'mass':0.09,
108 'inertia':np.diag([0.00015,0.00015,4e-5]), #w.r.t. COM! in stdDH link frame
109 'COM':[0,0,0.032]} #in stdDH link frame
110linkList=[link0, link1, link2, link3, link4, link5]
111
112#++++++++++++++++++++++++++
113#test example for graphicsData in VRobotLink
114s0=0.08
115wj = 0.12
116rj = 0.06
117
118if mode=='newDH':
119 for cnt, link in enumerate(linkList):
120 robot.AddLink(RobotLink(mass=link['mass'],
121 COM=link['COM'],
122 inertia=link['inertia'],
123 localHT=StdDH2HT(link['stdDH']),
124 PDcontrol=(Pcontrol[cnt], Dcontrol[cnt]),
125 visualization=VRobotLink(linkColor=graphics.colorList[cnt])
126 ))
127elif mode=='newModDH': #computes preHT and localHT, but ALSO converts inertia parameters from stdDH to modDHKK (NEEDED!)
128 for cnt, link in enumerate(linkList):
129 [preHT, localHT] = ModDHKK2HT(link['modDHKK'])
130 stdLocalHT = StdDH2HT(link['stdDH'])
131 HT = InverseHT(stdLocalHT) @ (localHT) #from stdHT back and forward in localHT of ModDHKK
132
133 rbi = RigidBodyInertia()
134 rbi.SetWithCOMinertia(link['mass'], link['inertia'], link['COM'])
135
136 rbi = rbi.Transformed(InverseHT(HT)) #inertia parameters need to be transformed to new modDHKK link frame
137
138 robot.AddLink(RobotLink(mass=rbi.mass,
139 COM=rbi.COM(),
140 inertia=rbi.InertiaCOM(),
141 preHT = preHT,
142 localHT=localHT,
143 PDcontrol=(Pcontrol[cnt], Dcontrol[cnt]),
144 visualization=VRobotLink(linkColor=graphics.colorList[cnt])
145 #visualization=VRobotLink(showCOM=True, showMBSjoint=False, graphicsData=gLinkList[cnt])
146 ))
147else:
148 raise ValueError('invalid mode!')
149
150#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
151#configurations and trajectory
152q0 = [0,0,0,0,0,0] #zero angle configuration
153
154#this set of coordinates only works with TSD, not with old fashion load control:
155# q1 = [0, pi/8, pi*0.75, 0,pi/8,0] #configuration 1
156# q2 = [pi,-pi, -pi*0.5,1.5*pi,-pi*2,pi*2] #configuration 2
157# q3 = [3*pi,0,-0.25*pi,0,0,0] #zero angle configuration
158
159#this set also works with load control:
160q1 = [0, pi/8, pi*0.5, 0,pi/8,0] #configuration 1
161q2 = [0.8*pi,-0.8*pi, -pi*0.5,0.75*pi,-pi*0.4,pi*0.4] #configuration 2
162q3 = [0.5*pi,0,-0.25*pi,0,0,0] #zero angle configuration
163
164#trajectory generated with optimal acceleration profiles:
165trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
166trajectory.Add(ProfileConstantAcceleration(q3,0.25))
167trajectory.Add(ProfileConstantAcceleration(q1,0.25))
168trajectory.Add(ProfileConstantAcceleration(q2,0.25))
169trajectory.Add(ProfileConstantAcceleration(q0,0.25))
170#traj.Add(ProfilePTP([1,1],syncAccTimes=False, maxVelocities=[1,1], maxAccelerations=[5,5]))
171
172# x = traj.EvaluateCoordinate(t,0)
173
174
175#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
176#test robot model
177#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
178
179
180jointList = [0]*robot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
181
182## user function to compute static torque compensation (slows down simulation!)
183def ComputeMBSstaticRobotTorques(robot):
184
185 if not useKinematicTree:
186 q=[]
187 for joint in jointList:
188 q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
189 else:
190 q = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Coordinates)
191
192 HT=robot.JointHT(q)
193 return robot.StaticTorques(HT)
194
195#++++++++++++++++++++++++++++++++++++++++++++++++
196#base, graphics, object and marker:
197
198objectGround = mbs.AddObject(ObjectGround(referencePosition=HT2translation(robot.GetBaseHT()),
199 #visualization=VObjectGround(graphicsData=graphicsBaseList)
200 ))
201
202
203#baseMarker; could also be a moving base!
204baseMarker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
205
206
207
208#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
209#build mbs robot model:
210if not useKinematicTree:
211 robotDict = robot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker,
212 createJointTorqueLoads=False,
213 )
214
215 jointList = robotDict['jointList'] #must be stored there for the load user function
216else:
217 robotDict = robot.CreateKinematicTree(mbs)
218 oKT = robotDict['objectKinematicTree']
219
220#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
221#user function which is called only once per step, speeds up simulation drastically
222def PreStepUF(mbs, t):
223 if compensateStaticTorques:
224 staticTorques = ComputeMBSstaticRobotTorques(robot)
225 #print("tau=", staticTorques)
226 else:
227 staticTorques = np.zeros(len(jointList))
228
229 [u,v,a] = trajectory.Evaluate(t)
230
231 #compute load for joint number
232 if not useKinematicTree:
233 for i in range(len(robot.links)):
234 joint = jointList[i]
235 phi = mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2] #z-rotation
236 omega = mbs.GetObjectOutput(joint, exu.OutputVariableType.AngularVelocityLocal)[2] #z-angular velocity
237 tsd = torsionalSDlist[i]
238 mbs.SetObjectParameter(tsd, 'offset', u[i])
239 mbs.SetObjectParameter(tsd, 'velocityOffset', v[i])
240 mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity
241 else:
242 #in case of kinematic tree, very simple operations!
243 mbs.SetObjectParameter(oKT, 'jointPositionOffsetVector', u)
244 mbs.SetObjectParameter(oKT, 'jointVelocityOffsetVector', v)
245 mbs.SetObjectParameter(oKT, 'jointForceVector', -staticTorques) #negative sign needed here!
246
247 return True
248
249mbs.SetPreStepUserFunction(PreStepUF)
250
251sListJointAngles = []
252sListTorques = []
253nJoints = len(jointList)
254if not useKinematicTree:
255 torsionalSDlist = robotDict['springDamperList']
256 sJointRotComponents = [0]*nJoints #only one component
257 sTorqueComponents = [0]*nJoints #only one component
258
259 #add sensors:
260 cnt = 0
261 for i in range(len(jointList)):
262 jointLink = jointList[i]
263 tsd = torsionalSDlist[i]
264 #using TSD:
265 sJointRot = mbs.AddSensor(SensorObject(objectNumber=tsd, storeInternal=True,
266 fileName="solution/joint" + str(i) + "Rot.txt",
267 outputVariableType=exu.OutputVariableType.Rotation,
268 writeToFile = sensorWriteToFile))
269 sListJointAngles += [sJointRot]
270 sJointAngVel = mbs.AddSensor(SensorObject(objectNumber=jointLink, storeInternal=True,
271 fileName="solution/joint" + str(i) + "AngVel.txt",
272 outputVariableType=exu.OutputVariableType.AngularVelocityLocal,
273 writeToFile = sensorWriteToFile))
274
275 cnt = 0
276 for iSD in robotDict['springDamperList']:
277 sTorque = mbs.AddSensor(SensorObject(objectNumber=iSD, storeInternal=True,
278 fileName="solution/jointTorque" + str(cnt) + ".txt",
279 outputVariableType=exu.OutputVariableType.TorqueLocal,
280 writeToFile = sensorWriteToFile))
281 sListTorques += [sTorque]
282
283 cnt+=1
284else:
285 sJointRotComponents = list(np.arange(0,nJoints))
286 sTorqueComponents = list(np.arange(0,nJoints))
287
288 sJointRot = mbs.AddSensor(SensorBody(bodyNumber=oKT,
289 storeInternal=True,
290 outputVariableType=exu.OutputVariableType.Coordinates))
291 sListJointAngles = [sJointRot]*6
292
293 #exu.OutputVariableType.Force is not the joint torque!
294 sTorques = mbs.AddSensor(SensorBody(bodyNumber=oKT, storeInternal=True,
295 outputVariableType=exu.OutputVariableType.Force))
296 sListTorques = [sTorques]*6
297
298
299mbs.Assemble()
300#mbs.systemData.Info()
301
302SC.visualizationSettings.connectors.showJointAxes = True
303SC.visualizationSettings.connectors.jointAxesLength = 0.02
304SC.visualizationSettings.connectors.jointAxesRadius = 0.002
305
306SC.visualizationSettings.nodes.showBasis = True
307SC.visualizationSettings.nodes.basisSize = 0.1
308SC.visualizationSettings.loads.show = False
309
310SC.visualizationSettings.openGL.multiSampling=4
311
312# tEnd = 1.25
313# h = 0.002
314tEnd = 1.25
315h = 0.001#*0.1*0.01
316
317#mbs.WaitForUserToContinue()
318simulationSettings = exu.SimulationSettings() #takes currently set values or default values
319
320simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
321simulationSettings.timeIntegration.endTime = tEnd
322simulationSettings.solutionSettings.solutionWritePeriod = 0.005
323simulationSettings.solutionSettings.sensorsWritePeriod = 0.005
324simulationSettings.solutionSettings.binarySolutionFile = True
325#simulationSettings.solutionSettings.writeSolutionToFile = False
326# simulationSettings.timeIntegration.simulateInRealtime = True
327# simulationSettings.timeIntegration.realtimeFactor = 0.25
328
329simulationSettings.timeIntegration.verboseMode = 1
330# simulationSettings.displayComputationTime = True
331simulationSettings.displayStatistics = True
332simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
333
334#simulationSettings.timeIntegration.newton.useModifiedNewton = True
335simulationSettings.timeIntegration.newton.useModifiedNewton = True
336
337simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
338SC.visualizationSettings.general.autoFitScene=False
339SC.visualizationSettings.window.renderWindowSize=[1920,1200]
340useGraphics = False
341
342if useGraphics:
343 exu.StartRenderer()
344 if 'renderState' in exu.sys:
345 SC.SetRenderState(exu.sys['renderState'])
346 mbs.WaitForUserToContinue()
347
348mbs.SolveDynamic(simulationSettings,
349 solverType=exu.DynamicSolverType.TrapezoidalIndex2,
350 showHints=True)
351#explicit integration also possible for KinematicTree:
352# mbs.SolveDynamic(simulationSettings,
353# solverType=exu.DynamicSolverType.RK33,
354# showHints=True)
355
356
357if useGraphics:
358 SC.visualizationSettings.general.autoFitScene = False
359 exu.StopRenderer()
360
361if False:
362 mbs.SolutionViewer()
363
364
365if not useKinematicTree:
366 #compute final torques:
367 measuredTorques=[]
368 for sensorNumber in sListTorques:
369 measuredTorques += [abs(mbs.GetSensorValues(sensorNumber)) ]
370 exu.Print("torques at tEnd=", VSum(measuredTorques))
371
372 measuredRot = []
373 for sensorNumber in sListJointAngles :
374 measuredRot += [(mbs.GetSensorValues(sensorNumber)) ]
375 exu.Print("rotations at tEnd=", VSum(measuredRot), ',', measuredRot)
376
377else:
378 q = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Coordinates)
379 exu.Print("rotations at tEnd=", VSum(q), ',', q)
380 f = mbs.GetObjectOutputBody(oKT, exu.OutputVariableType.Force)
381 exu.Print("torques at tEnd=", VSum(f), ',', f)
382
383
384
385
386#%%++++++++++++++++++++++++++++
387if True:
388 mbs.PlotSensor(sensorNumbers=sListJointAngles, components=sJointRotComponents,
389 title='joint angles', closeAll=True)
390 if useKinematicTree: #otherwise not available
391 mbs.PlotSensor(sensorNumbers=sListTorques, components=sTorqueComponents,
392 title='joint torques')
393 #sListJointAngles