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