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